Allowing Base Motion with Mocap Weld-Constrained Arms #2635
-
IntroHi! I am a Lead ML Engineer at ArenaX Lab., I use MuJoCo for my research on robotics manipulation control using reinforcement learning. My setupMuJoCo 3.1.6 My questionI have a robot model with a mobile base (driven by three wheel motors) and two 7-DOF arms. The end-effectors are controlled using mocap bodies with weld equality constraints, following the approach used in Gymnasium-Robotics' FetchEnv. However, when I move the mobile base, the arms remain pinned in place rather than moving with the base, due to the weld constraints. To address this, I temporarily deactivate the welds during base movement and reactivate them afterward. While this partially resolves the issue, it’s not fully reliable. I also tried using an IK controller to update the mocap targets, but that didn’t work. Is there a more robust or efficient way to support mobile base movement while maintaining mocap-based end-effector control? Minimal model and/or code that explain my questionIf you encountered the issue in a complex model, please simplify it as much as possible (while still reproducing the issue). Model: minimal XML<mujoco>
<include file="robot.xml" />
<include file="shared.xml" />
<worldbody>
<body mocap="true" name="l_arm_mocap" pos="0.01831491 0.3605796 0.38745686"
quat="-0.13003985 0.98767075 -0.08640984 0.01137859">
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7"
size="0.005 0.005 0.005" type="box"></geom>
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 0.005"
type="box"></geom>
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 0.005"
type="box"></geom>
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 0.005"
type="box"></geom>
</body>
<body mocap="true" name="r_arm_mocap" pos="0.01831507 -0.36058052 0.38745713"
quat="0.13001045 0.98767462 0.08641017 0.01137606">
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7"
size="0.005 0.005 0.005" type="box"></geom>
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 0.005"
type="box"></geom>
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 0.005"
type="box"></geom>
<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 0.005"
type="box"></geom>
</body>
</worldbody>
<equality>
<weld body1="l_arm_mocap" body2="l_wrist_out_link" solimp="0.95 0.99 0.001"
solref="0.01 1"></weld>
<weld body1="r_arm_mocap" body2="r_wrist_out_link" solimp="0.95 0.99 0.001"
solref="0.01 1"></weld>
</equality>
</mujoco>
Code: import mujoco
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
def _step_action(self, action):
action = np.clip(action, -1.0, 1.0)
# Mobile base
mobile_base_action = action[:3]
if any(mobile_base_action != 0):
self.data.eq_active[-2:] = 0
mujoco_utils.reset_mocap_welds(self.model, self.data)
mujoco.mj_forward(self.model, self.data)
self.mobile_base.move(mobile_base_action)
mujoco.mj_step(self.model, self.data, nstep=200)
mujoco_utils.reset_mocap2body_xpos(self.model, self.data)
reset_mocapbody2body_xpos(self.model, self.data)
self.data.eq_active[-2:] = 1
mujoco.mj_forward(self.model, self.data)
l_delta_pos = action[3:6] * 0.01
l_delta_euler = action[6:9] * 5 # between -5 and 5 degrees
l_gripper_cmd = action[9]
l_mocap_quat = self.data.mocap_quat[self.l_gripper_mocap_id]
l_mocap_quat = rotate_quaternion(l_mocap_quat, [1, 0, 0], l_delta_euler[0])
l_mocap_quat = rotate_quaternion(l_mocap_quat, [0, 1, 0], l_delta_euler[1])
l_mocap_quat = rotate_quaternion(l_mocap_quat, [0, 0, 1], l_delta_euler[2])
mujoco_utils.reset_mocap2body_xpos(self.model, self.data)
self.data.mocap_pos[self.l_gripper_mocap_id] += l_delta_pos
self.data.mocap_quat[self.l_gripper_mocap_id] = l_mocap_quat
# Do the same for right arm
mujoco.mj_step(self.model, self.data, nstep=self.frame_skip)
if self.render_mode == "human":
self.render()
return self._get_obs()Confirmations
|
Beta Was this translation helpful? Give feedback.
Replies: 1 comment 7 replies
-
|
Hi @matinmoezzi, if it helps you can check out the mobile manipulator example in the mink library. |
Beta Was this translation helpful? Give feedback.
-
|
Fascinating question. The short answer is no, there is no way to use mocap bodies for this. The problem is that they have no velocity by definition, and you need these things to move with your base. What you are doing is like, well actually it's not like anything real or sensible; don't do it. But there is different a way to do exactly what you want. You can make pseudo-mocap bodies using free joints with crazy high armature, like 1e6 or more (don't worry, it only appears in denominators). These will now maintain whatever position and importantly velocity you give them. No need to disable your welds. Let me know if this makes sense to you, I'd be curious to see how well it works. I think it's exactly what you want. |
Beta Was this translation helpful? Give feedback.
-
|
@matinmoezzi Yes, just append a |
Beta Was this translation helpful? Give feedback.
-
Hi @yuvaltassa Thank you for your comment. I tried what you suggested, and it kind of works. But I'm still not sure how to use the velocity of the pseudo-mocap bodies to move the base. Also, there's the same issue I’ve seen with regular mocap setups—when I move one arm, the other gets stuck in its position, which doesn’t feel physically accurate. Do you know of any examples with a similar setup, like a mobile base with two arms? |
Beta Was this translation helpful? Give feedback.
-
|
Sorry, I might have misunderstood. I thought your motions were in an egocentric reference frame, ie that you want these bodies to move together with the base. Is that not the case? I was expecting that you'd take the motion of the base and use that to set the motion of the pseudo-mocap bodies, and that any additional egocentric motion would be added on top. Perhaps you need to clarify how the tracking data is defined and what motions you are trying to achieve. |
Beta Was this translation helpful? Give feedback.
-
|
Yes, that's the case. Now I got your point. I need to get the velocity of the base when it's moving and add the velocities to the pseudo-mocap bodies. What I’m aiming for is that both the arms and the base can move freely, and their motions are determined using inverse kinematics. For example, when I move the base, the arms—since they’re attached to it—should move naturally along with it. Likewise, if I move the arms, the base should respond accordingly. The idea is that neither the base nor the arms are fixed while the other is moving—they should influence each other’s motion through the IK solution. |
Beta Was this translation helpful? Give feedback.
Hi @matinmoezzi, if it helps you can check out the mobile manipulator example in the mink library.