Hi guys, I'm trying to use mocap body to guide a super simple 2dof reacher to move while it doesn't work. I use Code: sim.data.set_mocap_pos to change mocap body's position in running time and mocap can move as I want. However the reacher doesn't move at all. Following is how I set the waypoints for reacher's fingertip: Code: for i in range(WAYPOINTS_NUM): delta_rad = (i + 1) * math.pi / 1000 fingertip_target_x = math.cos(delta_rad) * 0.21 fingertip_target_y = math.sin(delta_rad) * 0.21 fingertip_target = np.array([fingertip_target_x, fingertip_target_y, START_POINT[2]]) sim.data.set_mocap_pos('mocap', fingertip_target) for _ in range(10): sim.step() mjViewer.render() Following is my xml file: Code: <mujoco model="reacher"> <compiler angle="radian" inertiafromgeom="true"/> <default> <default class="reacher"> <joint armature="1" damping="50" frictionloss="0" stiffness="0"/> </default> <!-- joint armature="1" damping="1" limited="true"/ --> <geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/> </default> <option gravity="0 0 -9.81" integrator="RK4" timestep="0.01"/> <equality> <weld body1="mocap" body2="fingertip" solimp="0.9 0.95 0.001" solref="0.02 1"/> </equality> <worldbody> <!-- fingertip mocap --> <body mocap="true" name="mocap" pos="0.21 0.0 0.01"> <geom contype="0" conaffinity="0" rgba="1 1 1 1" size=".01" type="sphere"/> </body> <!-- Arena --> <geom conaffinity="0" contype="0" name="ground" pos="0 0 0" rgba="0.9 0.9 0.9 1" size="1 1 10" type="plane"/> <geom conaffinity="0" fromto="-.3 -.3 .01 .3 -.3 .01" name="sideS" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/> <geom conaffinity="0" fromto=" .3 -.3 .01 .3 .3 .01" name="sideE" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/> <geom conaffinity="0" fromto="-.3 .3 .01 .3 .3 .01" name="sideN" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/> <geom conaffinity="0" fromto="-.3 -.3 .01 -.3 .3 .01" name="sideW" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/> <!-- Arm --> <geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/> <body name="body0" childclass="reacher" pos="0 0 .01"> <geom fromto="0 0 0 0.1 0 0" name="link0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/> <joint axis="0 0 1" limited="false" name="joint0" pos="0 0 0" type="hinge"/> <body name="body1" pos="0.1 0 0"> <joint axis="0 0 1" limited="true" name="joint1" pos="0 0 0" range="-3.0 3.0" type="hinge"/> <geom fromto="0 0 0 0.1 0 0" name="link1" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/> <body name="fingertip" pos="0.11 0 0"> <geom contype="0" name="fingertip" rgba="0.0 0.8 0.6 1" size=".01" type="sphere"/> </body> </body> </body> <!-- Target --> <body name="target" pos=".1 -.1 .01"> <joint armature="0" axis="1 0 0" damping="0" limited="true" name="target_x" pos="0 0 0" range="-.27 .27" ref=".1" stiffness="0" type="slide"/> <joint armature="0" axis="0 1 0" damping="0" limited="true" name="target_y" pos="0 0 0" range="-.27 .27" ref="-.1" stiffness="0" type="slide"/> <geom conaffinity="0" contype="0" name="target" pos="0 0 0" rgba="0.9 0.2 0.2 1" size=".009" type="sphere"/> </body> </worldbody> <actuator> <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint0"/> <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint1"/> </actuator> </mujoco> Thanks! Any help would be appreciated!
The weld constraint is trying to make the 3D position and 3D orientation of the bodies match, which is not possible here (since your arm only has 2 DOFs), which is leading to some weird compromise solution. I am working on improving the handling of weld constraints in the next release. Anyway, you don't want a weld constraint in this situation. Use a connect constraint instead: <connect body1="mocap" body2="fingertip" anchor="0 0 0" solimp="0.9 0.95 0.001" solref="0.02 1"/> I tried it and it does inverse kinematics nicely, making the arm move around with the mocap body.
Hi Emo, thanks for your help. However, 'connect' doesn't work for me either. The python code I attached in the first post is how I plan the path and control mocap body. Do you have idea what's the reason?
I don't use any of the Python wrappers and am not sure what they do... in particular, is the mocap body actually moving? You can load the modified model in the simulate code sample that comes with MuJoCo (without any Python wrappers) and move the mocap body with the mouse, and you should see the arm tracking it. If that doesn't work, let me know. If it does work, then there is some issue with Python not setting the mocap fields (or something else).
Hi Emo, thanks for help. Now as my 6dof arm can work, I will let 2dof reacher go. Anyway, very appreciate your help!