Hi, I'm using mocap body to control my gripper base like following: <equality> <weld body1="mocap" body2="right_gripper_base" solimp="0.9 0.95 0.001" solref="0.02 1"></weld> </equality> I also have two fingers connected with gripper base through two slide joints like following: <body name="right_gripper_base" pos="0 0 0.025"> <inertial pos="0 0 0" quat="-0.5 0.5 0.5 0.5" mass="0.3" diaginertia="3e-08 2e-08 2e-08" /> <geom quat="0 0 0.707107 0.707107" class="viz" mesh="electric_gripper_base" /> <geom size="0.029 0.05" quat="0 0 0.707107 0.707107" type="cylinder" /> <body name="r_gripper_l_finger" pos="0 -0.0015 0.02"> <inertial pos="0 0 0" quat="0 0 0 -1" mass="0.02" diaginertia="0.01 0.01 0.01" /> <joint name="r_gripper_l_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" range="0 0.020833" limited="true"/> <geom class="viz" mesh="extended_narrow" euler="1.57 1.57 0" rgba="0.1 0.1 0.1 1"/> <geom size="0.005 0.00675 0.05635" contype="0" pos="0 0.01725 0.0615" quat="0 0 0 -1" type="box" rgba="0.1 0.1 0.1 1"/> <geom size="0.005 0.025 0.0085" contype="0" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box" rgba="0.1 0.1 0.1 1"/> <body name="r_gripper_l_finger_tip" pos="0 0.01725 0.1127"> <inertial pos="0 0 0" quat="0 0 0 1" mass="0.01" diaginertia="0.01 0.01 0.01" /> <geom pos="-.007 -.01 0.003" class="viz" mesh="half_round_tip" euler="-1.57 0 0" friction="1 0.05 0.01"/> <geom size="0.008 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 1" type="cylinder" friction="1 0.05 0.01"/> </body> </body> <body name="r_gripper_r_finger" pos="0 0.0015 0.02"> <inertial pos="0 0 0" mass="0.02" diaginertia="0.01 0.01 0.01" /> <joint name="r_gripper_r_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" range="-0.020833 0" limited="true"/> <geom class="viz" mesh="extended_narrow" euler="1.57 -1.57 0" rgba="0.1 0.1 0.1 1"/> <geom size="0.005 0.00675 0.05635" contype="0" pos="0 -0.01725 0.0615" type="box" rgba="0.1 0.1 0.1 1"/> <geom size="0.005 0.025 0.0085" contype="0" pos="0.005 0.003 0.0083" type="box" rgba="0.1 0.1 0.1 1"/> <body name="r_gripper_r_finger_tip" pos="0 -0.01725 0.1127"> <inertial pos="0 0 0" mass="0.01" diaginertia="0.01 0.01 0.01" /> <geom pos=".007 .01 0.003" class="viz" mesh="half_round_tip" euler="-1.57 3.14 0" friction="1 0.05 0.01"/> <geom size="0.008 0.0185" pos="0 0.0045 -0.015" type="cylinder" friction="1 0.05 0.01"/> </body> </body> However, I found when I set pos of mocap body to a position very close to a table body, the fingers will go off the gripper base as there exists collision between fingers with table. I am wondering if there is a way to avoid this. Thanks!
You have two soft constraints fighting each other: contact vs. equality between the mocap body and object. Which one wins depends on the solref and solimp settings for the two constraints. Pick the constraint that you would like to have priority, and give it smaller solref[0] and larger solimp[0] and solimp[1]. You have to experiment with the combination of parameters for a specific model. My preference is to have contacts win in these situations, i.e. allow the mocap body and base to become dislocated but always respect contacts. An example of this is the MPL model available on the Forum.