Setting data xpos not changing simulation position

Discussion in 'Support requests' started by Sridhar Thiagarajan, Jul 19, 2019.

  1. Code:
    _data->xpos[object_id * 3] = request.object_pose.position.x;
          _data->xpos[object_id * 3 + 1] = request.object_pose.position.y;
          _data->xpos[object_id * 3 + 2]  = request.object_pose.position.z;
    
          _data->xquat[object_id * 4]    = request.object_pose.orientation.x;
          _data->xquat[object_id * 4 + 1] = request.object_pose.orientation.y;
          _data->xquat[object_id * 4 + 2] = request.object_pose.orientation.z;
          _data->xquat[object_id * 4 + 3] = request.object_pose.orientation.w;
    
          // Update simulation data
          mj_forward(_model, _data);
    I am unable to update the position of a cube in simulation, and no changes take place.

    The cube is defined as

    <mujoco>
    <worldbody>
    <body name="cube" pos="0.68 0.0 1.5">
    <joint name="object0:joint" type="free" damping="0.01"></joint>
    <geom size="0.025 0.025 0.025" rgba="1 0 0 1" type="box" condim="3" name="cube" mass="2"></geom>

    </body>
    </worldbody>
    </mujoco>
     
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    The fields you are setting are results from the computation, not inputs. So your changes are erased when mj_forward is called. You should change mjData.qpos instead. It is a system vector that contains all position coordinates.
     
  3. Thanks! That works for a free joint.

    I am trying to do the same for a composite object (get and set position of composite elements). It is a child of the worldbody (freejoint). However, each of the composite element has more than one joint.

    Is qpos only for a single joint bodies, what would be the alternative for these types?
     
    Last edited: Jul 23, 2019
  4. Emo Todorov

    Emo Todorov Administrator Staff Member

    qpos is a system vector with all positional degrees of freedom. free joints have 7 numbers in that vector (3D position followed by 4D quaterion orientation). ball joints have 4 numbers (quaternion). you can read about it here:

    http://www.mujoco.org/book/index.html#Floating

    each joint in the model has the field jnt_qposadr specifying the offset of that joint's data in the system qpos vector. similarly for velocities and jnt_dofadr. keep in mind that position and velocities can have different dimensionality due to quaternions.
     
  5. Thanks for the clarification! What about when we have a composite body such as this -

    <worldbody>

    <body name="B3_5" pos="0.675 0.05 1.42">
    <freejoint/>
    <composite type="cloth" count="8 8 1" spacing="0.05" flatinertia="0.01">
    <joint kind="main" damping="0.001"/>
    <!--<joint kind= "twist" damping="0.001"/>-->
    <skin material="matcarpet" texcoord="true" inflate="0.005" subgrid="2"/>-->
    <geom type="capsule" size="0.015 0.01" rgba=".8 .2 .1 1" mass="0.002"/>
    </composite>
    </body>

    </worldbody>

    It is a child of the world body connected by a freejoint (the composite as a whole). However, when I try to access each of the individual elements of this composite body, they have 2 joints (as expected, since they are connected). Is it possible to set the location of these individual bodies in world coordinates arbitrarly? If I set the joint position as above, they might result in a different behavior since these joints probably refer to the connecting joints between the individual bodies.
     
  6. Emo Todorov

    Emo Todorov Administrator Staff Member

    When you have parent-child relations among bodies, the child joints specify relative positions and forward kinematics is needed to compute the absolute world positions.

    Note that cloth-like objects can be simulated in two ways: cloth and 2d grid. The grid object is composed of particles that are children of the world -- so their joint coordinates specify world coordinates directly. The grid-based cloth is also more stable and efficient to simulate, although its behavior is somewhat different. I suggest trying both.