d-> xpos and d->xipos has dramatic inconsistency

Discussion in 'Simulation' started by yongxf, Jul 9, 2016.

  1. yongxf

    yongxf

    Hi all,

    I try to return the Cartesian space position for the COM of a object. In my case, the body only contains one geom, and the body description is shown as (global coordinate is used in this case):
    Code:
    <body name='object'>
        <geom name='object_gemo' type='cylinder' fromto='0 0 0.21 0 0 0.071'  size='0.035' />
                <joint armature="0" damping="0" limited="false" type="free" name="root" pos="0 0 0.071" stiffness="0"/>
    </body>

    I tried to get the pose of the object by the following three methods, as shown in below.

    The problem is that position output of d->xpos has different sign. Theoretically, the z direction of the COM should be roughly 0.15m. Also notice quaternion are exactly same.

    Is there anything that I missed? Or is it a bug?

    Code:
    print the body/bodycom/geom position: 
d->xpos
-0.00016614 -0.00034757 -0.15073589
d->xipos
0.00038298 0.00035072 0.15073580
d->geom_xpos
0.00038298 0.00035072 0.15073580



print the body/bodycom/geom orientation: which is number:10
d->xmat
0.99999714 -0.00190845 0.00144290 0.00190842 0.99999818 0.00002156 -0.00144294 -0.00001881 0.99999896
d->ximat
0.99999714 -0.00190845 0.00144290 0.00190842 0.99999818 0.00002156 -0.00144294 -0.00001881 0.99999896
d->geom_xmat
0.99999714 -0.00190845 0.00144290 0.00190842 0.99999818 0.00002156 -0.00144294 -0.00001881 0.99999896
     
    yongxf, Jul 9, 2016
    #1
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    Not sure what you are doing. I tested the following simple model:

    <mujoco>
    <compiler coordinate="global"/>

    <worldbody>
    <body>
    <geom type="cylinder" fromto="0 0 0.21 0 0 0.071" size="0.035"/>
    <joint type="free" pos="0 0 0.071"/>
    </body>
    </worldbody>
    </mujoco>

    and it behaves exactly as expected, namely xpos, xipos and geom_xpos are all equal to (0, 0, 0.14)

    Can you attach the entire XML file you are loading?
     
    Emo Todorov, Jul 10, 2016
    #2
  3. yongxf

    yongxf

    Thanks for your reply. My xml file is attached here:

    The object id is 10, and totally 11 bodies included here. So just check the last row of d->xpos and so on.

    Thanks!
    Best,
     

    Attached Files:

    yongxf, Jul 10, 2016
    #3
  4. Emo Todorov

    Emo Todorov Administrator Staff Member

    There is probably a bug in the way you access the data in mjData. Can you show me the code you use to print these numbers?

    I checked your model, everything looks fine at the initial time step. Here are the corresponding fields of mjData, printed out with the mj_printData function:

    XPOS
    0 0 0
    0 0 0.035
    0.075 0 0.07
    0.13 0 0.07
    0 0.075 0.07
    0 0.13 0.07
    -0.075 0 0.07
    -0.13 0 0.07
    0 -0.075 0.07
    0 -0.13 0.07
    0 0 0.14

    XIPOS
    0 0 0
    0 0 0.035
    0.075 0 0.07
    0.13 0 0.07
    0 0.075 0.07
    0 0.13 0.07
    -0.075 0 0.07
    -0.13 0 0.07
    0 -0.075 0.07
    0 -0.13 0.07
    0 0 0.14

    GEOM_XPOS
    0 0 0
    0 0 0.035
    0.075 0 0.07
    0.13 0 0.07
    0 0.075 0.07
    0 0.13 0.07
    -0.075 0 0.07
    -0.13 0 0.07
    0 -0.075 0.07
    0 -0.13 0.07
    0 0 0.14
     
    Emo Todorov, Jul 10, 2016
    #4
  5. yongxf

    yongxf

    Oh I know the reason. This is a bug in my code.
    I accidentally use the address of d->xpos, and changed the contents afterwards.

    Code:
    mjtNum *object_xpos = &((d->xpos)[object_id * 3]); 
/* handreds line of code*/
mju_sub3(object_xpos, zero_vector3, object_xpos); // P = -P
mju_mulMatVec(object_xpos, object_xmat_T, object_xpos, 3, 3); // compute -R'P (R*P)
    Thanks for trying my model and clarifying this.
    Best,
     
    yongxf, Jul 10, 2016
    #5