Angular velocity in local frame

Discussion in 'Simulation' started by jpsb, Oct 27, 2017.

  1. Hello!

    I have a floating body (free joint), and I have a the following defined in the xml to help with visualization for the x, y, z axis, to be displayed in red, green and blue, respectively.

    <body name='main_body' pos='0 0 1'>
    <!-- ======= coordinate system ======= -->
    <geom contype="0" conaffinity="0" type="capsule" fromto="0 0 0 0.2 0 0" size="0.005" rgba="1 0 0 1" mass="0"/>
    <geom contype="0" conaffinity="0" type="capsule" fromto="0 0 0 0 0.2 0" size="0.005" rgba="0 1 0 1" mass="0"/>
    <geom contype="0" conaffinity="0" type="capsule" fromto="0 0 0 0 0 0.2" size="0.005" rgba="0 0 1 1" mass="0"/>

    <inertial pos="0 0 0.1520153" mass="1.4699013" fullinertia="0.0031887 0.0032245 0.0013857 -0.0000038 -0.0000881 0.0000846" />
    <geom type="mesh" mesh="main body mesh"/>
    <joint type="free" damping="0.01" pos='0 0 0.1520153'/>
    </body>

    What I want to do is to obtain the angular velocity of the body expressed in local coordinates, and for that I'm using the mj_objectVelocity function, with the flag = 1 for local frame, as in the code bellow.

    mjtNum* vel_local = mj_stackAlloc(d, 6);
    mj_objectVelocity(m, d, mjOBJ_BODY, id_mainbody, vel_local, 1);

    mjtNum* ang_vel = mj_stackAlloc(d, 3);
    ang_vel[0] = vel_local[0];
    ang_vel[1] = vel_local[1];
    ang_vel[2] = vel_local[2];

    mju_printMat(ang_vel, 3, 1);


    However, during simulation, when it is clear that the body is rotating mainly around the x axis (the red capsule), for exemple, the printing in the screen with the biggest value is always in the second position, as bellow.

    0.45237021
    2.79366970
    -0.22335321

    And when it is clear that the body is rotating around the y axis, the opposite happens i.e., the largest value is in the first position. However, for the z axis it seens to be correct, the sign and position on the printing.

    Why this is happening and how can I fix it??

    Thanks vary much!