mujoco
mujoco copied to clipboard
Contacts seem unstable for mesh geom using adhesion
I am trying to use the new adhesion feature to build a suction gripper. Some of the objects are built using mesh. But I find contacts between mesh and other geom can be unstable (the rendered contact points in the simulator appear on and off)
Here is a model which explains my question:
minimal XML
<mujoco model="base">
<compiler boundmass="1e-05" boundinertia="1e-11" coordinate="local" meshdir="meshes" angle="radian" eulerseq="xyz"/>
<option timestep="0.002" density="1.2" viscosity="2e-05" impratio="20.0" cone="elliptic"/>
<visual>
<headlight ambient="0.3 0.3 0.3" diffuse="0.6 0.6 0.6" specular="0 0 0"/>
<map znear="0.01"/>
</visual>
<default>
<default class="/">
<joint damping=".3" axis="0 1 0"/>
<!--
By adding 3mm of margin yet making solimp barely apply any force until 3mm penetration, we get
3mm of "action at a distance". This is important so that small changes in distance don't lead
to loss of adhesion
-->
<geom type="box" friction=".5" margin=".003" solimp="0 .99 .003 .9 6"/>
<default class="tangram">
<geom type="mesh" friction="1 10 0.5" conaffinity="0" density="500.0" margin=".003" solimp="0 .99 .003 .9 6"/>
</default>
<default class="mechanical">
<geom rgba=".5 .5 .8 0.5"/>
<tendon rgba=".5 .5 .8 1"/>
</default>
<default class="active_adhesion">
<geom rgba=".8 .5 .5 1"/>
</default>
<default class="object">
<geom rgba=".5 .8 .5 1" density="100"/>
</default>
</default>
</default>
<asset>
<mesh name="large_triangle" class="/" file="large_triangle.stl"/>
</asset>
<worldbody>
<geom name="floor" class="/" type="plane" size="1 1 0.1" rgba="0.7 0.7 0.7 1"/>
<camera name="sideview" class="/" pos="-2.5 0 0.5" quat="0.5468 0.4482 -0.4482 -0.5468"/>
<camera name="frontview" class="/" pos="0 -2.5 1.2" quat="0.86 0.51 0 0"/>
<body name="arm0" pos="0 0 .3" childclass="mechanical">
<joint name="arm0x" type="slide" axis="1 0 0" damping="3"/>
<joint name="arm0y" type="slide" axis="0 1 0" damping="3"/>
<geom type="cylinder" size=".015 .01"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 0 -0.05"/>
<body name="arm1" pos="0 0 -.05" childclass="mechanical">
<joint name="arm1"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 -.12 0 -.07"/>
<body name="arm2" pos="-.12 0 -.07">
<joint name="arm2"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 .12 0 -.07"/>
<body name="arm4" pos=".12 0 -.08">
<joint name="arm4" damping=".01" pos="0 -.03 0"/>
<geom type="box" size="0.015 0.015 0.01"/>
<body name="4boxes" pos="0 0 -.02" childclass="active_adhesion">
<site name="force_sensor" group="3"/>
<joint name="arm3" damping=".01" axis="0 0 1"/>
<geom size=".01 .01 .01" pos="0.01 0.01 0"/>
<geom size=".01 .01 .01" pos="0.01 -.01 0"/>
<geom size=".01 .01 .01" pos="-.01 0.01 0"/>
<geom size=".01 .01 .01" pos="-.01 -.01 0"/>
</body>
</body>
</body>
</body>
</body>
<body name="box" pos="-.1 0 .03">
<freejoint/>
<geom size=".02 .02 .02" class="object"/>
</body>
<body name="large_triangle0" pos="-.04 -0.04 .03">
<geom name="object_geom_0" class="tangram" rgba="0.54881350392732475 0.71518936637241948 0.60276337607164387 0.7" mesh="large_triangle"/>
<freejoint name="object_joint_0"/>
</body>
</worldbody>
<equality>
<joint joint1="arm1" joint2="arm2" polycoef="0 -.5 0 0 0"/>
<joint joint1="arm4" joint2="arm1" />
</equality>
<actuator>
<position name="arm" joint="arm2" ctrlrange="-.8 1" ctrllimited="true" kp="10"/>
<motor name="arm_slidex" joint="arm0x" ctrlrange="-1 1" ctrllimited="true"/>
<motor name="arm_slidey" joint="arm0y" ctrlrange="-1 1" ctrllimited="true"/>
<motor name="gripper_rot" joint="arm3" ctrlrange="-0.05 0.05" ctrllimited="true"/>
<adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="20"/>
</actuator>
</mujoco>
Here is a screenshot / video, illustrating my question: https://drive.google.com/file/d/12RMvHJ0IrMp_IRXDOmjQy-77OdfNlk92/view?usp=sharing
Could you try enabling the experimental multiccd
option?
<option>
<flag multiccd="enable"/>
</option>
Thank you so much! Now the issue is magically fixed I think.
I think it improves a lot. But it looks like the contact is still not stable enough (maybe?). I have another video that illustrates the problem. (https://drive.google.com/drive/folders/1VOApl8zFhZvVUAowxQZZn6jvL6TOUNOQ?usp=sharing)
In the video, the gripper has full contact with the object. The object can move while being pressed. And if I add more adhesive forces, the movement gets more obvious, making the object look kind of slippery.
<mujoco model="base">
<!-- <compiler boundmass="1e-05" boundinertia="1e-11" coordinate="local" meshdir="meshes" angle="radian" eulerseq="xyz"/> -->
<compiler boundmass="1e-05" boundinertia="1e-11" coordinate="local" meshdir="meshes" angle="radian" eulerseq="xyz"/>
<option timestep="0.002" density="1.2" viscosity="2e-05" impratio="20.0" cone="elliptic"/>
<option>
<flag multiccd="enable"/>
</option>
<visual>
<headlight ambient="0.3 0.3 0.3" diffuse="0.6 0.6 0.6" specular="0 0 0"/>
<map znear="0.01"/>
</visual>
<default>
<joint damping=".3" axis="0 1 0"/>
<!--
By adding 3mm of margin yet making solimp barely apply any force until 3mm penetration, we get
3mm of "action at a distance". This is important so that small changes in distance don't lead
to loss of adhesion
-->
<geom rgba="0 0 1 1" size="0.001" solref="0.001 1" type="sphere"/>
<default class="tangram">
<geom type="mesh" friction="1 10 0.5" conaffinity="0" density="500.0" margin=".003" solimp="0 .99 .003 .9 6"/>
</default>
<default class="mechanical">
<geom rgba=".5 .5 .8 0.5"/>
<tendon rgba=".5 .5 .8 1"/>
</default>
<default class="active_adhesion">
<geom type="box" rgba=".8 .5 .5 1" friction=".5" margin=".003" solimp="0 .99 .003 .9 6"/>
</default>
</default>
<asset>
<mesh name="square" file="square.stl"/>
</asset>
<worldbody>
<geom name="floor" type="plane" size="1 1 0.1" rgba="0.7 0.7 0.7 1"/>
<camera name="sideview" pos="-2.5 0 0.5" quat="0.54679999999999995 0.44819999999999999 -0.44819999999999999 -0.54679999999999995"/>
<camera name="frontview" pos="0 -2.5 1.2" quat="0.85999999999999999 0.51000000000000001 0 0"/>
<body name="arm0" pos="0 0 .3" childclass="mechanical">
<joint name="arm0x" type="slide" damping=".3" axis="1 0 0"/>
<joint name="arm0y" type="slide" damping=".3" axis="0 1 0"/>
<geom type="cylinder" size=".015 .01"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 0 -0.05"/>
<body name="arm1" pos="0 0 -.05" childclass="mechanical">
<joint name="arm1"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 -.12 0 -.07"/>
<body name="arm2" pos="-.12 0 -.07">
<joint name="arm2"/>
<geom type="cylinder" size=".015 .01" zaxis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 .12 0 -.07"/>
<body name="arm4" pos=".12 0 -.08">
<joint name="arm4" damping=".3" pos="0 -.03 0"/>
<geom type="box" size="0.015 0.015 0.01"/>
<body name="4boxes" pos="0 0 -.02" childclass="active_adhesion">
<site name="force_sensor" group="3"/>
<joint name="arm3" damping=".3" axis="0 0 1"/>
<geom size=".01 .01 .01" pos="0.01 0.01 0"/>
<geom size=".01 .01 .01" pos="0.01 -.01 0"/>
<geom size=".01 .01 .01" pos="-.01 0.01 0"/>
<geom size=".01 .01 .01" pos="-.01 -.01 0"/>
</body>
</body>
</body>
</body>
</body>
<body name="square0" pos="-0.04 -0.04 0">
<geom name="object_geom_6" class="tangram" rgba="0.11827442586893322 0.63992102132752382 0.1433532874090464 0.7" mesh="square"/>
<freejoint name="object_joint_6"/>
</body>
</worldbody>
<equality>
<joint joint1="arm1" joint2="arm2" polycoef="0 -.5 0 0 0"/>
<joint joint1="arm4" joint2="arm1" />
<!-- <joint joint1="arm3" joint2="arm1"/> -->
</equality>
<actuator>
<position name="arm" joint="arm2" ctrlrange="-.8 1" ctrllimited="true" kp="10"/>
<position name="arm_slidex" joint="arm0x" ctrlrange="-1 1" ctrllimited="true"/>
<position name="arm_slidey" joint="arm0y" ctrlrange="-1 1" ctrllimited="true"/>
<motor name="gripper_rot" joint="arm3" ctrlrange="-0.05 0.05" ctrllimited="true"/>
<adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="20"/>
</actuator>
</mujoco>
First, it's okay for contacts to be unstable if the behaviour itself is stable.
In your case it seems you might be pressing quite hard on a free objects from two sides which will always be tricky.
You can try using the noslip solver <option noslip_iterations="n"/>
for n>0
.
Let us know if that helps.
It doesn't help much. Occasionally the mesh object would shake while being lifted by the gripper in the air. Since all the shape I need is of some very basic geometry, is there a way to have a triangle without using mesh? I used SketchUp to build stl file. Is there a way to get a better stl?
Apologies if this is still an issue for you. Closing since there hasn't been any activity on this issue in a while.