Sim runs into stability issue for my mobile manipulator when I switch to position command for actuators #2571

ShangqunYu · 2025-04-12T04:12:34Z

ShangqunYu
Apr 12, 2025

Intro

Hi!
I am a graduate student at UMASS, I use MuJoCo for my research on object manipulation via using mobile manipulator.
When I switch to position control for my arms, it seems like the simulation runs into some stability issue. When I load the scene, the robot just keep moving around even though I have not control the base, when I was doing the torque command everything looks fine, I wonder if I have set something incorrectly.

My setup

You can down the example code and replicate the result.
`

  <default class="visual">
    <geom group="2" type="mesh" contype="0" conaffinity="0" density="0"/>
  </default>
  <default class="collision">
    <geom group="3" type="mesh" density="2700"/>

    <default class="pad_box_0">
      <geom friction="1.5 0.1 0.0002" solimp="0.95 0.99 0.001" solref="0.001 1" priority="1" rgba="0.45 0.45 0.45 1"/>
    </default>
    <default class="pad_box_1">
      <geom friction="1.5 0.1 0.0002" solimp="0.95 0.99 0.001" solref="0.001 1" priority="1" rgba="0.55 0.55 0.55 1"/>
    </default>
  </default>

  <default class="wheel_collision">
    <geom group="3" type="mesh"/>
    <default class="caster">
      <geom type="sphere" size=".02" pos="-0.24 0 .02" condim="1" priority="1"/>
    </default>
    <default class="rubber">
      <geom condim="6" friction="2 0.02 0.1" priority="1" solref="0.02 1" solimp="0.99 0.99 0.01" conaffinity="1"/>
    </default>
  </default>
  <default class="position">
        <position ctrlrange="-3.1415 3.1415" kp="50" kv="2" forcerange="-150 150"/>
        <default class="position_small">
          <position kp="50" kv="2.5" forcerange="-28 28"/>
        </default>
  </default>
</default>

<!-- Materials -->
<material name="grey" specular="0.5" shininess="0.25" rgba="0.51764705882 0.53764705882 0.54764705882 1"/>
<material name="black" specular="0.5" shininess="0.25" rgba="0.1 0.1 0.1 1"/>
<material name="Wheel_Rubber_Gray" rgba="0.418986 0.424087 0.429225 1"/>

  <body name="robot_base" pos="0 0 0.248" quat="0 0 0 1">
    <joint name="root_joint" type="free"/>
    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="base_link_underpan"/>
    <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="base_link_underpan"/>
    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="body_base_link"/>
    <!-- <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="body_base_link"/> -->

    <body name="platform_base_link" pos="0 -0.11663 0.271">
      <inertial pos="-5.42644e-05 -0.060115 0.00743041" quat="0.474164 0.524628 0.474438 0.524254" mass="3.36802" diaginertia="0.0403717 0.0296204 0.015987"/>
      <joint name="platform_joint" pos="0 0 0" axis="0 0 1" type="slide" range="0 1" actuatorfrcrange="-160 160" damping="2.4" armature="0.2"/>
      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="platform_base_link"/>
      <!-- <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="platform_base_link"/> -->
      <geom pos="0.1 -0.1103 0.031645" quat="4.28004e-05 0.382684 -1.77285e-05 0.923879" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_base_link1"/>
      <geom pos="0.1 -0.1103 0.031645" quat="4.28004e-05 0.382684 -1.77285e-05 0.923879" type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_base_link1"/>
      <geom pos="-0.1 -0.1103 0.031645" quat="0.923879 0 -0.382684 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="r_base_link1"/>
      <geom pos="-0.1 -0.1103 0.031645" quat="0.923879 0 -0.382684 0" type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="r_base_link1"/>

      <body name="l_link1" pos="0.232229 -0.110312 0.163874" quat="-0.923879 0 -0.382684 8.56007e-05">
        <inertial pos="-2.9675e-07 0.021107 -0.025188" quat="0.988853 -0.148894 -1.495e-05 -0.000114968" mass="0.59353" diaginertia="0.0012661 0.00124667 0.000496239"/>
        <joint name="l_shoulder_yaw" pos="0 0 0" axis="0 0 1" range="-3.11 3.11" actuatorfrcrange="-60 60"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_shoulder_yaw"/>
        <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_shoulder_yaw"/>
        <body name="l_link2" quat="0.499998 0.5 -0.5 0.500002">
          <inertial pos="0.15226 2.3916e-07 -0.0062027" quat="5.16616e-05 0.800119 6.89106e-05 0.599841" mass="0.86416" diaginertia="0.00207595 0.0020527 0.000509376"/>
          <joint name="l_shoulder_roll" pos="0 0 0" axis="0 0 1" range="-2.27 2.27" actuatorfrcrange="-60 60"/>
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_shoulder_roll"/>
          <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_shoulder_roll"/>
          <body name="l_link3" pos="0.256 0 0" quat="0.707105 0 0 0.707108">
            <inertial pos="4.7194e-06 -0.059592 0.010268" quat="0.424687 0.564983 -0.565532 0.424981" mass="0.28963" diaginertia="0.000656042 0.00063737 0.000114628"/>
            <joint name="l_elbow_roll" pos="0 0 0" axis="0 0 1" range="-2.36 2.36" actuatorfrcrange="-30 30"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_elbow_roll"/>
            <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_elbow_roll"/>
            <body name="l_link4" pos="0 -0.21 -0.00030007" quat="0.707105 0.707108 0 0">
              <inertial pos="2.9186e-07 -0.018042 -0.02154" quat="0.990403 0.138211 6.75064e-06 -8.85845e-05" mass="0.23942" diaginertia="0.00028595 0.000275323 0.000107307"/>
              <joint name="l_elbow_yaw" pos="0 0 0" axis="0 0 1" range="-3.11 3.11" actuatorfrcrange="-10 10"/>
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_elbow_yaw"/>
              <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_elbow_yaw"/>
              <body name="l_link5" quat="0.707105 -0.707108 0 0">
                <inertial pos="4.2301e-06 -0.059381 0.0070671" quat="0.419742 0.568638 -0.56922 0.420067" mass="0.21879" diaginertia="0.000358601 0.00035053 8.07893e-05"/>
                <joint name="l_wrist_roll" pos="0 0 0" axis="0 0 1" range="-2.23 2.23" actuatorfrcrange="-10 10"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_wrist_roll"/>
                <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_wrist_roll"/>
                <body name="l_link6" pos="0 -0.115 -0.00029971" quat="0.707105 -0.707108 0 0">
                  <inertial pos="0.0139139 0.00340438 -0.00528981" quat="0.770992 0.16454 0.137053 0.599762" mass="0.41281" diaginertia="0.00134184 0.00115682 0.000547946"/>
                  <joint name="l_wrist_yaw" pos="0 0 0" axis="0 0 1" range="-6.28 6.28" actuatorfrcrange="-10 10"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="l_wrist_yaw"/>
                  <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="l_wrist_yaw"/>
                  
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>

  <body name="link_right_wheel"  pos="-0.226 -0.17843 -0.1965" quat="0.707 0 0.707 0">
    <joint name="joint_right_wheel" class="wheel"/>
    <geom mesh="link_right_wheel_0" material="Wheel_Rubber_Gray" class="visual" mass="0.15"/>
    <geom mesh="link_right_wheel_1" material="black" class="visual"/>
    <geom class="wheel_collision" type="cylinder" size=".05 .0125" pos="0 0 0.0125"/>
  </body>
  <body name="link_left_wheel" pos="0.226 -0.17843 -0.1965" quat="0.707 0 0.707 0">
    <joint name="joint_left_wheel" class="wheel"/>
    <geom mesh="link_left_wheel_0" material="Wheel_Rubber_Gray" class="visual" mass="0.15"/>
    <geom mesh="link_left_wheel_1" material="black" class="visual"/>
    <geom class="wheel_collision" type="cylinder" size=".05 .0125" pos="0 0 -0.0125"/>
  </body>


    <body name="link_swivel_wheel_3_1" pos="0 0.22157 -0.16254">
      <inertial pos="0.00067987 -0.0050012 -0.013414" quat="-0.134505 0.694593 -0.137471 0.693217" mass="0.088835" diaginertia="4.7479e-05 3.81021e-05 3.16789e-05"/>
      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="link_swivel_wheel_3_1"/>
      <geom type="mesh" rgba="0.79216 0.81961 0.93333 1" mesh="link_swivel_wheel_3_1"/>
      <body name="link_swivel_wheel_3_2" pos="-0.0035 -0.022 -0.04516">
        <inertial pos="0.0010651 0 0" quat="0.679224 0.733931 0 0" mass="0.12664" diaginertia="0.00011169 6.0841e-05 6.08367e-05"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.79216 0.81961 0.93333 1" mesh="link_swivel_wheel_3_2"/>
        <geom type="sphere" size="0.038" pos="0 0 0" condim="1" priority="1"/>
      </body>
    </body>
  </body>

<fixed name="forward">
  <joint joint="joint_left_wheel" coef=".5"/>
  <joint joint="joint_right_wheel" coef=".5"/>
</fixed>
<fixed name="turn">
  <joint joint="joint_left_wheel" coef="-.5"/>
  <joint joint="joint_right_wheel" coef=".5"/>
</fixed>

<position name="r_shoulder_yaw" joint="r_shoulder_yaw" class="position" ctrlrange="-3.1 3.1" ctrllimited="true"/>
<position name="r_shoulder_roll" joint="r_shoulder_roll" class="position"  ctrlrange="-2.27 2.27" ctrllimited="true"/>
<position name="r_elbow_roll" joint="r_elbow_roll" class="position"  ctrlrange="-2.36 2.36" ctrllimited="true"/>
<position name="r_elbow_yaw" joint="r_elbow_yaw" class="position_small"  ctrlrange="-3.1 3.1" ctrllimited="true"/>
<position name="r_wrist_roll" joint="r_wrist_roll" class="position_small"  ctrlrange="-2.23 2.23" ctrllimited="true"/>
<position name="r_wrist_yaw" joint="r_wrist_yaw" class="position_small"  ctrlrange="-6.28 6.28" ctrllimited="true"/>

<motor name="forward" tendon="forward" ctrlrange="-1 1" gear="5" forcerange="-100 100"/>
<motor name="turn" tendon="turn" ctrlrange="-1 1" gear="5" forcerange="-200 200"/>

`

example.zip

My question

Any tips of how to fix the issue will be greatly appreciated.

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())

Confirmations

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

yuvaltassa · 2025-04-15T13:26:27Z

yuvaltassa
Apr 15, 2025
Maintainer

Did you try adding some damping (kv) to your actuators?

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Sim runs into stability issue for my mobile manipulator when I switch to position command for actuators #2571

Uh oh!

{{title}}

Uh oh!

Replies: 1 comment

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

Sim runs into stability issue for my mobile manipulator when I switch to position command for actuators #2571

Uh oh!

ShangqunYu Apr 12, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 1 comment

Uh oh!

yuvaltassa Apr 15, 2025 Maintainer

ShangqunYu
Apr 12, 2025

yuvaltassa
Apr 15, 2025
Maintainer