Assigning an Object Handle to Robot's Initial Position.

Typically: "How do I... ", "How can I... " questions
Post Reply
sradmard
Posts: 11
Joined: 06 Oct 2016, 00:39

Assigning an Object Handle to Robot's Initial Position.

Post by sradmard »

Hi,

I am trying to publish odom data and the associated tf of my robot in ROS. I am currently publishing my tf for the odom as follows.
transformations[1]=getTransformStamped(modelBase,'base_link',-1,'odom')
simExtRosInterface_sendTransforms(transformations)

However, this would place the 'odom' frame at the world origin. This is not applicable when my robot starting positin is not at the world origin in v-rep. Therefore, instead of -1, I would like to assign an object handle to the robot's initial pose. Is there an easy way to do it through the script without needing to define a stationary dummy frame at the base of the robot?

Thank you so much in advance.

coppelia
Site Admin
Posts: 7966
Joined: 14 Dec 2012, 00:25

Re: Assigning an Object Handle to Robot's Initial Position.

Post by coppelia »

Hello,

try with something like:

Code: Select all

function getTransformStamped(objHandle,name,relToMatrix,relToName)
    local t=simGetSystemTime()
    local m=simBuildIdentityMatrix()
    if relToMatrix then
        m=simCopyMatrix(relToMatrix)
        simInvertMatrix(m)
    end
    local objM=simGetObjectMatrix(objHandle,-1)
    local objRelM=simMultiplyMatrices(m,objM)
    local p={objRelM[4],objRelM[8],objRelM[12]}
    local o=simGetQuaternionFromMatrix(objRelM)
    return {
        header={
            stamp=t,
            frame_id=relToName
        },
        child_frame_id=name,
        transform={
            translation={x=p[1],y=p[2],z=p[3]},
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
        }
    }
end
So instead of providing the handle of the object you want the data relative to, simply provide the matrix of that object. See the other matrix API functions.

Cheers

sradmard
Posts: 11
Joined: 06 Oct 2016, 00:39

Re: Assigning an Object Handle to Robot's Initial Position.

Post by sradmard »

Hi,
Thank you so much for your reply. However, I still believe that the problem is not solved. Instead, I have to manually provide the initial position/orientation matrix of my robot with respect to the world in the child script. I also have to update that matrix every time my robot's initial position/orientation changes. I don't see this being different than creating a dummy frame at the base of the robot and moving it every time the robot's initial position/orientation is different.
I am looking for a way to make it automatic if possible. Is there any suggestion on that?
Should I make that dummy frame stationary and the parent frame of my entire robot, so that every time that I want to change the initial position/orientation of my robot I just change that dummy frame?
Thank you again.

Post Reply