openai
Designing Closed Kinematic Chains: Equality Connect Constraints Not Working
Description I am trying to recreate a Jansen Linkage in MuJoCo. To do this I thought that I could add a Connect Constraint to welded bodies on the various ends of linkages to create the closed kinematic chains. When I run the simulation, none of the constraints are satisfied and the sim.model.eq_data shows the values > 0.
To Reproduce
<mujoco model="jansen">
<compiler angle="degree" coordinate="local"/>
<option iterations="20" timestep="0.001"/>
<worldbody>
<light cutoff="4.0" diffuse="1 1 1" dir="-0.9 -0.3 -2.5" directional="true" exponent="20" pos="0.9 0.3 2.5" specular="0 0 0"/>
<geom name="ground" pos="0 0 0" rgba="0.25 0.26 0.25 1" size="10 10 2" type="plane" euler="0 0 0"/>
<body name="jansen" pos="0 0 5.0" euler="0 0 0">
<body name="hip">
<geom mass="2.0" pos="0 0 0" size="0.01 0.025" type="capsule" euler="0 90 0"/>
</body>
<body name="outer" euler="0 0 0">
<joint axis="1 0 0" damping="0.05" limited="false" name="outer_leg_joint"/>
<body name="link_a" pos="-0.05 0 0" euler="0 0 0">
<geom density="1000" name="link_a_geom" pos="0 0 -0.075" size="0.0125 0.075" type="capsule"/>
<geom mass="1.0" name="link_a_COM" pos="0 0 -0.075" size="0.02" type="sphere"/>
<body name="link_b" pos="0 0 -0.175" euler="0 0 0">
<geom density="1000" name="link_b_geom" pos="0 0 -0.3095" size="0.0125 0.3095" type="capsule"/>
<geom mass="1.0" name="link_b_COM" pos="0 0 -0.3095" size="0.02" type="sphere"/>
<joint axis="1 0 0" damping="0.05" limited="false" name="leg_joint_a"/>
<body name="link_c" pos="0 0 -0.644" euler="0 0 0">
<geom density="1000" name="link_c_geom" pos="0 0 -0.19649999999999998" size="0.0125 0.19649999999999998" type="capsule"/>
<geom mass="1.0" name="link_c_COM" pos="0 0 -0.19649999999999998" size="0.02" type="sphere"/>
<joint axis="1 0 0" damping="0.05" limited="false" name="leg_joint_b"/>
<body name="link_d" pos="0 0 -0.418" euler="0 0 0">
<geom density="1000" name="link_d_geom" pos="0 0 -0.2075" size="0.0125 0.2075" type="capsule"/>
<geom mass="1.0" name="link_d_COM" pos="0 0 -0.2075" size="0.02" type="sphere"/>
<joint axis="1 0 0" damping="0.05" limited="false" name="leg_joint_c"/>
<body name="link_e" pos="0 0 -0.44" euler="0 0 0">
<geom density="1000" name="link_e_geom" pos="0 0 -0.25" size="0.0125 0.25" type="capsule"/>
<geom mass="1.0" name="link_e_COM" pos="0 0 -0.25" size="0.02" type="sphere"/>
<joint axis="1 0 0" damping="0.05" limited="false" name="leg_joint_d"/>
<body name="link_e_end" pos="0 0 -0.525" euler="0 0 0"/>
</body>
</body>
</body>
</body>
<body name="link_a_end" pos="0 0 -0.15"/>
</body>
</body>
<body name="inner" euler="0 0 0"/>
</body>
</worldbody>
<equality>
<connect anchor="0 0 0" body1="link_a_end" active="true" body2="link_e_end" name="equality_constraint" solref="2 100"/>
<connect anchor="0 0 0" body1="link_e_end" active="true" body2="link_a_end" name="equality_constraint2" solref="0.02 100"/>
</equality>
</mujoco>
Expected behavior I thought I would see some force that tries to satisfy this constraint that is a simulated spring damper force.
Desktop (please complete the following information):
- OS: [macOS 10.15.7]
- Python Version [3.6.6]
- Mujoco Version [2.00]
- mujoco-py version [ 2.0.2.13]
Additional context Note: This is not the full Jensan Linkage, just part to see how to support closing the loops. I haven't been able to find any examples of closed linkages, but the equality section says it should be able to support this feature.
Thanks for the help
@suvansh Not with MuJoCo, I do not think that this capability is possible, however, you can do it with Drake. https://drake.mit.edu/
@suvansh @mfogelson
Here is a working example
<mujoco model="jansen">
<compiler angle="radian" coordinate="global"/>
<default>
<geom contype="0" conaffinity="1" size="0.1"/>
<joint damping="0.5" limited="true" range="-1.25 1.25"/>
</default>
<worldbody>
<light cutoff="4.0" diffuse="1 1 1" dir="-0.9 -0.9 -2.5" directional="true" exponent="20" pos="0.9 0.9 2.5" specular="0 0 0"/>
<geom name="ground" pos="0 0 0" rgba="0.25 0.26 0.25 1" size="10 10 2" type="plane" contype="1"/>
<body name="top" euler="0 0 0">
<geom type="capsule" fromto="0 0 3 0 0 2"/>
<joint axis="0 1 0" name="top" pos="0 0 3"/>
<body name="right" euler="-1.57 0 0">
<geom type="capsule" fromto="0 0 2 1 0 2" rgba=".8 .7 9 1"/>
<joint axis="0 1 0" name="right" pos="0 0 2"/>
<body name="down" euler="-1.57 0 0">
<geom type="capsule" fromto="1 0 2 1 0 1" rgba=".8 .7 9 1"/>
<joint axis="0 1 0" name="down" pos="1 0 2"/>
<body name="left" euler="-1.57 0 0">
<geom type="capsule" fromto="1 0 1 0 0 1"/>
<joint axis="0 1 0" name="left" pos="1 0 1"/>
<body name="up" euler="-1.57 0 0">
<geom type="capsule" fromto="0 0 1 0 0 2"/>
<joint axis="0 1 0" name="up" pos="0 0 1"/>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<equality>
<connect anchor="0 0 2" body1="top" body2="up" name="equality_constraint"/>
</equality>
</mujoco>
