google-deepmind
Mysterious behavior when grasping while controlling gripper poses using caterisan control
Intro
Hi!
I am a Ph.d student at SDU, I use MuJoCo for my research on manipulation of deformable objects and reinforcement learning.
I am looking for any help, advice, insight or comments that might shine some light on my problem :)
Thanks in advance!
My setup
I am running
- Ubuntu 24.04
- MuJoCo 3.3.7
What's happening? What did you expect?
I have tried to use both the mocap and the actuator based control and get the results shown in the videos below
| Mocap Control | Actuator Control |
|---|---|
Here the mocap object behaves perfectly well, but there seems to be some odd phenomenon happening when I use the actuators. As you can see, when I rotate using the actuators on the revolute joints, it almost looks like the box is sliding forward? Like if the friction suddenly changes direction and moves the box ahead of the grippers fingers.
Steps for reproduction
In order to reproduce this I have made attempted to only use the most default parameters and models. The script and model can be seen below
Simulation of both cases (change the `init` function when running)
import time
import glfw
import mujoco
import mujoco as mj
import mujoco.viewer
import numpy as np
from robot_descriptions import robotiq_2f85_mj_description
_XML = """
<mujoco model="empty scene">
<compiler angle="radian" autolimits="true" />
<option timestep="0.002"
integrator="implicitfast"
solver="Newton"
gravity="0 0 -9.82"
cone="elliptic"
sdf_iterations="5"
sdf_initpoints="30"
noslip_iterations="2"
>
</option>
<statistic center="0.3 0 0.3" extent="0.8" meansize="0.08" />
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0" />
<rgba haze="0.15 0.25 0.35 1" />
<global azimuth="120" elevation="-20" offwidth="2000" offheight="2000" />
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512"
height="3072" />
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4"
rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"
reflectance="0.2" />
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="floor" size="0 0 0.5" type="plane" material="groundplane" solimp="0.0 0.0 0.0 0.0 1" />
</worldbody>
</mujoco>
"""
def add_act_freejoint(
spec: mj.MjSpec,
x_lim: tuple[float] = (-1, 1),
y_lim: tuple[float] = (-1, 1),
z_lim: tuple[float] = (-1, 1),
roll_lim: tuple[float] = (-np.pi, np.pi),
pitch_lim: tuple[float] = (-np.pi, np.pi),
yaw_lim: tuple[float] = (-np.pi, np.pi),
kp_pos: float = 1000,
kp_ori: float = 1000,
kv_pos: float = 1000,
kv_ori: float = 1000,
) -> mj.MjSpec:
base = spec.worldbody.first_body()
base.add_joint(name="x", axis=[1, 0, 0], type=mj.mjtJoint.mjJNT_SLIDE, range=x_lim)
base.add_joint(name="y", axis=[0, 1, 0], type=mj.mjtJoint.mjJNT_SLIDE, range=y_lim)
base.add_joint(name="z", axis=[0, 0, 1], type=mj.mjtJoint.mjJNT_SLIDE, range=z_lim)
base.add_joint(
name="roll", axis=[1, 0, 0], type=mj.mjtJoint.mjJNT_HINGE, range=roll_lim
)
base.add_joint(
name="pitch",
axis=[0, 1, 0],
type=mj.mjtJoint.mjJNT_HINGE,
range=pitch_lim,
)
base.add_joint(
name="yaw", axis=[0, 0, 1], type=mj.mjtJoint.mjJNT_HINGE, range=yaw_lim
)
spec.add_actuator(
name="x", target="x", trntype=mj.mjtTrn.mjTRN_JOINT, ctrlrange=x_lim
).set_to_position(kp=kp_pos, kv=kv_pos)
spec.add_actuator(
name="y", target="y", trntype=mj.mjtTrn.mjTRN_JOINT, ctrlrange=y_lim
).set_to_position(kp=kp_pos, kv=kv_pos)
spec.add_actuator(
name="z", target="z", trntype=mj.mjtTrn.mjTRN_JOINT, ctrlrange=z_lim
).set_to_position(kp=kp_pos, kv=kv_pos)
spec.add_actuator(
name="roll",
target="roll",
trntype=mj.mjtTrn.mjTRN_JOINT,
ctrlrange=roll_lim,
forcerange=[-10, 10],
).set_to_position(kp=kp_ori, kv=kv_ori)
spec.add_actuator(
name="pitch",
target="pitch",
trntype=mj.mjtTrn.mjTRN_JOINT,
ctrlrange=pitch_lim,
forcerange=[-10, 10],
).set_to_position(kp=kp_ori, kv=kv_ori)
spec.add_actuator(
name="yaw",
target="yaw",
trntype=mj.mjtTrn.mjTRN_JOINT,
ctrlrange=yaw_lim,
forcerange=[-10, 10],
).set_to_position(kp=kp_ori, kv=kv_ori)
return spec
class MjSim:
def __init__(self):
self._model, self._data = self.init_mocap()
# self._model, self._data = self.init()
def init(self):
gripper = mj.MjSpec.from_file(robotiq_2f85_mj_description.MJCF_PATH)
print(robotiq_2f85_mj_description.MJCF_PATH)
scene = mj.MjSpec.from_string(_XML)
K = 100
# add a "freejoint" which I can actuate
gripper = add_act_freejoint(gripper, kp_ori=K, kv_ori=K, kp_pos=K, kv_pos=K)
gripper.worldbody.first_body().gravcomp = 1
scene.worldbody.add_frame(
name=f"{gripper.modelname}_1",
pos=[0, 0, 0.275],
euler=np.array([np.pi, 0, 0]),
).attach_body(gripper.worldbody.first_body(), f"{gripper.modelname}_1/")
prop = scene.worldbody.add_body(name="prop")
prop.add_geom(
name="prop",
type=mj.mjtGeom.mjGEOM_BOX,
size=[0.018, 0.018, 0.018],
mass=0.3,
)
prop.add_freejoint()
m = scene.compile()
d = mj.MjData(m)
# step once to compute the poses of objects
mj.mj_step(m, d)
return m, d
def init_mocap(self):
gripper = mj.MjSpec.from_file(robotiq_2f85_mj_description.MJCF_PATH)
print(robotiq_2f85_mj_description.MJCF_PATH)
scene = mj.MjSpec.from_string(_XML)
scene.worldbody.add_body(
name="mocap", mocap=True, pos=[0, 0, 0.3], euler=[np.pi, 0, 0]
).add_geom(
name="mocap",
type=mj.mjtGeom.mjGEOM_BOX,
size=[0.02, 0.02, 0.02],
contype=0,
conaffinity=0,
)
# add a "freejoint" which I can actuate
# gripper = add_act_freejoint(gripper, kp_ori=K, kv_ori=K, kp_pos=K, kv_pos=K)
gripper.worldbody.first_body().gravcomp = 1
gripper.worldbody.first_body().add_freejoint()
scene.worldbody.add_frame(
name=f"{gripper.modelname}_1",
pos=[0, 0, 0.25],
euler=np.array([np.pi, 0, 0]),
).attach_body(gripper.worldbody.first_body(), f"{gripper.modelname}_1/")
scene.add_equality(
name="weld",
type=mj.mjtEq.mjEQ_WELD,
objtype=mj.mjtObj.mjOBJ_BODY,
name1="mocap",
name2=gripper.worldbody.first_body().name,
# name2=f"{left.modelname}/link_left_wrist_x",
data=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
solref=[0.0000001, 1],
)
prop = scene.worldbody.add_body(name="prop")
prop.add_geom(
name="prop",
type=mj.mjtGeom.mjGEOM_BOX,
size=[0.018, 0.018, 0.018],
mass=0.3,
)
prop.add_freejoint()
m = scene.compile()
d = mj.MjData(m)
# step once to compute the poses of objects
mj.mj_step(m, d)
return m, d
def run(self):
with mujoco.viewer.launch_passive(
model=self.model,
data=self.data,
show_left_ui=False,
show_right_ui=True,
) as viewer:
while viewer.is_running():
step_start = time.time()
mj.mj_step(self.model, self.data)
viewer.sync()
# Rudimentary time keeping, will drift relative to wall clock.
time_until_next_step = self.model.opt.timestep - (
time.time() - step_start
)
if time_until_next_step > 0:
time.sleep(time_until_next_step)
@property
def data(self) -> mj.MjData:
return self._data
@property
def model(self) -> mj.MjModel:
return self._model
def keyboard_callback(self, key: int):
if key is glfw.KEY_SPACE:
print("You pressed space...")
if __name__ == "__main__":
sim = MjSim()
sim.run()
Minimal model for reproduction
The minimal model can be found in the previous section.
Code required for reproduction
The minimal code can be found in the previous section.
Confirmations
- [x] I searched the latest documentation thoroughly before posting.
- [x] I searched previous Issues and Discussions, I am certain this has not been raised before.
What accelerator are you using?
Hi @phansel, I not entirely sure what you refer to when you ask about the accelerator, can you maybe clarify your question a bit more?
I am still having this issue, so I'm just pinging it to maybe attract some help :)
This is curious indeed. Are you sure you are not actuating the object by mistake?
I did encounter similar behavior once. I was testing a pick and place environment with a panda robot to pick up a cube. In some positions with the cube in the gripper, the cube would move up (defying gravity) while moving the gripper sideways. IIRC I was using mocap based IK and the cube was definitely not actuated. I moved on to use a pybound C++ controller and the strange behavior was gone. I didn't open an issue though, because I could not reproduce it.
This is curious indeed. Are you sure you are not actuating the object by mistake?
Thank you for the quick response! Yes I am quite sure, I am just running the code that I attached in the issue which uses "off the shelf" components from mujoco_menagerie and a world MJCF which is also specified in the issue. @DavidPL1 good to hear that I am not the only one encountering this issue, hopefully a fix will solve both our problems :)
Please let me know if you need any further information from my side to cast light on this issue :)