equality constraints break or closed loop crash or not accurate when simulation

[yijing2050]

Intro

Hi!

I am a ([ researcher) in a research laboratory. I use MuJoCo for my research on arm robot.

My setup

    <exclude name="e14" body1="link21" body2="link3" />
    <exclude name="e15" body1="link22" body2="link3" />
    <exclude name="e16" body1="link21" body2="link1" />
    <exclude name="e17" body1="link22" body2="link1" />
    <exclude name="e18" body1="link41" body2="link5" />
    <exclude name="e19" body1="link42" body2="link5" />
    <exclude name="e20" body1="link41" body2="link3" />
    <exclude name="e21" body1="link42" body2="link3" />

    <exclude name="e22" body1="link31" body2="link2" />
    <exclude name="e23" body1="link31" body2="link4" />

    <!-- <exclude name="e24" body1="Link_dg1" body2="link2" /> -->

    <connect name="c1" active="true"  body1="link3" body2="link4"   anchor="0.40069 0 0" solimp="0.9999 1 0 " solref="0.005 1"/>
    <connect name="c2" active="true"  body1="link5" body2="link6"  anchor="0.18708 0 0" solimp="0.9999 1 0" solref="0.005 1"/>

    <connect name="c3" active="true"  body1="link21" body2="link3"   anchor="1.6 0 0" solimp="0.9999 1 0" solref="0.005 1"/>
    <connect name="c4" active="true"  body1="link22" body2="link3"  anchor="1.6 0 0" solimp="0.9999 1 0" solref="0.005 1"/>
    <connect name="c5" active="true"  body1="link41" body2="link5"   anchor="1.6 0 0" solimp="0.9999 1 0.001" solref="0.005 1"/>
    <connect name="c6" active="true"  body1="link42" body2="link5"  anchor="1.6 0 0" solimp="0.9999 1 0.001" solref="0.005 1"/>
    <connect name="c7" active="true"  body1="link31" body2="link4"  anchor="0.32087 0 0" solimp="1 1 0" solref="0.005 1"/>

    <!-- <connect name="c1" active="true"  body1="link3" body2="link4"   anchor="0.40069 0 0" solimp="0.95 0.99 0.001 " solref="0.01 1"/>
    <connect name="c2" active="true"  body1="link5" body2="link6"  anchor="0.18708 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c3" active="true"  body1="link21" body2="link3"   anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c4" active="true"  body1="link22" body2="link3"  anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c5" active="true"  body1="link41" body2="link5"   anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c6" active="true"  body1="link42" body2="link5"  anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c7" active="true"  body1="link31" body2="link4"  anchor="0.32087 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>

-->

    <!-- passive joint -->
    <!-- <position name="joint2" joint="joint2" ctrllimited="true" ctrlrange="0 1.18" kp="1e8"  />
    <position name="joint3" joint="joint3" ctrllimited="true" ctrlrange="-1.18 0" kp="1e8"  />
    <position name="joint41" joint="joint41" ctrllimited="true" ctrlrange="-1.16 0" kp="1e6"  />
    <position name="joint42" joint="joint42" ctrllimited="true" ctrlrange="0 1.16" kp="1e6"  />
    <position name="joint31" joint="joint31" ctrllimited="true" ctrlrange="0 1.2" kp="1e8"  />
    <position name="joint_dg1" joint="joint_dg1" ctrllimited="true" ctrlrange="0 1.04" kp="1e8"  />
    <position name="joint4" joint="joint4" ctrllimited="true" ctrlrange="-2.19 0" kp="1e6"  />
    <position name="joint5" joint="joint5" ctrllimited="true" ctrlrange="0 1.15" kp="1e6"  />
    <position name="joint_dg2" joint="joint_dg2" ctrllimited="true" ctrlrange="0 0.94" kp="1e7"  />
    <position name="joint7" joint="joint7" ctrllimited="true" ctrlrange="0 0.92" kp="1e6"  />
    <position name="joint21" joint="joint21" ctrllimited="true" ctrlrange="0 1.18" kp="1e8"  />
    <position name="joint22" joint="joint22" ctrllimited="true" ctrlrange="0 1.18" kp="1e8"  /> -->

My question

The equality constraints break or the closed loop crash when simulating.
for example:
if control as this:
actuation = np.array([-0.21954719, 0.3609375 , 0.09375 , 0.535 ])
data.ctrl[:] = actuation[:],this is most unstable, and the closed loops often crash.
In the actuator section of my XML file, I've set a very high proportional coefficient (kp) for position control based on multiple trials. If it's too small, we
meshes.zip
can't control the robotic arm properly, but if it's too large, it also causes problems. Could you advise on how to properly configure this kp value? Hope to receive your reply soon. Thank you!
meshes.zip

Minimal model and/or code that explain my question

If you encountered the issue in a complex model, please simplify it as much as possible (while still reproducing the issue).

Model:

minimal XML

<mujoco>
  <worldbody>
    <light pos="0 0 1"/>
    <geom type="sphere" size="1" rgba="1 0 0 1"/>
  </worldbody>
</mujoco>

Code:

import mujoco
import mediapy as media
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

with mujoco.Renderer(model) as renderer:
  mujoco.mj_forward(model, data)
  renderer.update_scene(data)

  media.show_image(renderer.render())

meshes.zip

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.

[yijing2050]

Complementary xml file：

    <exclude name="e14" body1="link21" body2="link3" />
    <exclude name="e15" body1="link22" body2="link3" />
    <exclude name="e16" body1="link21" body2="link1" />
    <exclude name="e17" body1="link22" body2="link1" />
    <exclude name="e18" body1="link41" body2="link5" />
    <exclude name="e19" body1="link42" body2="link5" />
    <exclude name="e20" body1="link41" body2="link3" />
    <exclude name="e21" body1="link42" body2="link3" />

    <exclude name="e22" body1="link31" body2="link2" />
    <exclude name="e23" body1="link31" body2="link4" />

    <!-- <exclude name="e24" body1="Link_dg1" body2="link2" /> -->

    <connect name="c1" active="true"  body1="link3" body2="link4"   anchor="0.40069 0 0" solimp="0.9999 1 0 " solref="0.005 1"/>
    <connect name="c2" active="true"  body1="link5" body2="link6"  anchor="0.18708 0 0" solimp="0.9999 1 0" solref="0.005 1"/>

    <connect name="c3" active="true"  body1="link21" body2="link3"   anchor="1.6 0 0" solimp="0.9999 1 0" solref="0.005 1"/>
    <connect name="c4" active="true"  body1="link22" body2="link3"  anchor="1.6 0 0" solimp="0.9999 1 0" solref="0.005 1"/>
    <connect name="c5" active="true"  body1="link41" body2="link5"   anchor="1.6 0 0" solimp="0.9999 1 0.001" solref="0.005 1"/>
    <connect name="c6" active="true"  body1="link42" body2="link5"  anchor="1.6 0 0" solimp="0.9999 1 0.001" solref="0.005 1"/>
    <connect name="c7" active="true"  body1="link31" body2="link4"  anchor="0.32087 0 0" solimp="1 1 0" solref="0.005 1"/>

    <!-- <connect name="c1" active="true"  body1="link3" body2="link4"   anchor="0.40069 0 0" solimp="0.95 0.99 0.001 " solref="0.01 1"/>
    <connect name="c2" active="true"  body1="link5" body2="link6"  anchor="0.18708 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c3" active="true"  body1="link21" body2="link3"   anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c4" active="true"  body1="link22" body2="link3"  anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c5" active="true"  body1="link41" body2="link5"   anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c6" active="true"  body1="link42" body2="link5"  anchor="1.6 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>
    <connect name="c7" active="true"  body1="link31" body2="link4"  anchor="0.32087 0 0" solimp="0.95 0.99 0.001" solref="0.01 1"/>

-->

    <!-- passive joint -->
    <!-- <position name="joint2" joint="joint2" ctrllimited="true" ctrlrange="0 1.18" kp="1e8"  />
    <position name="joint3" joint="joint3" ctrllimited="true" ctrlrange="-1.18 0" kp="1e8"  />
    <position name="joint41" joint="joint41" ctrllimited="true" ctrlrange="-1.16 0" kp="1e6"  />
    <position name="joint42" joint="joint42" ctrllimited="true" ctrlrange="0 1.16" kp="1e6"  />
    <position name="joint31" joint="joint31" ctrllimited="true" ctrlrange="0 1.2" kp="1e8"  />
    <position name="joint_dg1" joint="joint_dg1" ctrllimited="true" ctrlrange="0 1.04" kp="1e8"  />
    <position name="joint4" joint="joint4" ctrllimited="true" ctrlrange="-2.19 0" kp="1e6"  />
    <position name="joint5" joint="joint5" ctrllimited="true" ctrlrange="0 1.15" kp="1e6"  />
    <position name="joint_dg2" joint="joint_dg2" ctrllimited="true" ctrlrange="0 0.94" kp="1e7"  />
    <position name="joint7" joint="joint7" ctrllimited="true" ctrlrange="0 0.92" kp="1e6"  />
    <position name="joint21" joint="joint21" ctrllimited="true" ctrlrange="0 1.18" kp="1e8"  />
    <position name="joint22" joint="joint22" ctrllimited="true" ctrlrange="0 1.18" kp="1e8"  /> -->

[yijing2050]

[yijing2050]

Especially during path planning, I need to perform state validity checks including self-collision and collision with environmental objects. The robotic arm often collapses easily (physically breaks apart), likely due to model parameter issues—but I'm unsure how to optimize these model parameters in the xml file. I hope you can provide some suggestions and assistance. Thank you.