Being new to Mujoco, I just created a dummy scene in mujoco for which I now try to control some of the objects in it. The scene contains a fixed table on which a movable book is places, which can be moved by controlling the actuated spatula frame:
xml = """
<mujoco>
<worldbody>
<light name="top" pos="0 0 1"/>
<body name="table" pos="0 0 0.025">
<geom name="plate" type="box" size="0.25 0.2 0.025" rgba=".8 .8 .8 1"/>
<geom name="bound0" type="box" size=".25 .01 .05" pos="0 -.19 .075"/>
<geom name="bound1" type="box" size=".01 .19 .05" pos="-.24 .01 .075"/>
</body>
<body name="booklink" pos="0 0 0.065">
<freejoint/>
<geom name="book" type="box" size=".1 .05 .0125" rgba="0 1 0 1" mass=".1"/>
</body>
<body name="spatulalink" pos="0 0 .2">
<joint name="transX" type="slide" axis="1 0 0" limited="true" range="-.6 .6"/>
<joint name="transY" type="slide" axis="0 1 0" limited="true" range="-.6 .6"/>
<joint name="transZ" type="slide" axis="0 0 1" limited="true" range="-.6 .6"/>
<joint name="hingeX" type="hinge" axis="1 0 0" limited="true" range="-3.2 3.2"/>
<joint name="hingeY" type="hinge" axis="0 1 0" limited="true" range="-3.2 3.2"/>
<joint name="hingeZ" type="hinge" axis="0 0 1" limited="true" range="-3.2 3.2"/>
<geom name="spatula" type="box" size=".05 .05 5e-4" rgba="1 1 0 1" friction=".1 .1 .1" mass=".1"/>
</body>
</worldbody>
<actuator>
<position ctrllimited="true" ctrlrange="-.6 .6" joint="transX"/>
<position ctrllimited="true" ctrlrange="-.6 .6" joint="transY"/>
<position ctrllimited="true" ctrlrange="-.6 .6" joint="transZ"/>
<position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeX"/>
<position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeY"/>
<position ctrllimited="true" ctrlrange="-3.2 3.2" joint="hingeZ"/>
</actuator>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
Now my question is how I can properly run a trajectory in this environment during which I control the robot. Naively, I tried to put some controls by directly writing into data.qpos and data.qvel. This creates the desired motions, but the objects tunnel through each other, i.e., there are collisions of the geometries, which I of course do not want. You can see a picture of this here (you see the yellow spatula object going through the boundary in collision):
Now, after some reading, I now learned that the correct way of controlling actuators / joints in Mujoco is instead to set the controls directly, which in my case would be data.crtl = some_position_vector. I do this as follows:
# Construct a BSpline to interpolate the via-points
spline_ref = BSpline_reference(data.qpos[7:], data.qvel[6:], data.time)
spline_ref.append(path, times, data.time)
with mujoco.viewer.launch_passive(model, data) as viewer:
# Close the viewer automatically after 30 wall-seconds.
start = time.time()
sim_steps = int(n_steps // tau)
i = 0
while viewer.is_running(): #and i < sim_steps:
# Get a target position from the reference spline using current sim state
qreal = data.qpos[7:]
qDot_real = data.qvel[6:]
qref, qDotref = spline_ref.getReference(qreal, qDot_real, data.time)
data.ctrl = q_ref
mujoco.mj_step(model, data)
# Pick up changes to the physics state, apply perturbations, update options from GUI.
viewer.sync()
# Slow down sim. for visualization
time.sleep(1e-2)
i += 1
Somehow, the movement I get from this is completely different than when I set data.qpos directly. Can someone explain to me how the controls can be set properly?
What you have in your model are position actuators, which apply a torque or force of
kp * (q_desired - q) - kv * qdot. So the position is not set magically as it would when you set qpos directly.The defaults are
kp="1"andkv="0".kpmight be too small for your system.Similarly to how you would on a real robot, you should tune the gains on your position actuators so you get the responsiveness you want without introducing unwanted oscillations.
Note that the kv parameter will essentially create a reference velocity of zero. If you want to track a reference velocity, as well as position, you could add extra velocity actuators to your model, and set the ctrl entries for those to your qDotref.