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>
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.
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?
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.
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.
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.