Unable to exert demanded torque with actuator

[ajoardar007]

Hello!

I am a graduate student at the University of Waterloo, and I am using MuJoCo in a course demonstration.

Background:

I am trying to model a position PID controller using a mobile manipulator. You can find the Colab Notebook with the associated link STL-files here. It was based of of Mujoco's tutorial notebook (Link). All you need to do is just run it and give it permissions to read the STL-files.

The joint is able to move to the set-point (1 rad) using the provided position controller using the only the PD-gains (second-last code-block) (actuator position name="wam/J1/P"). Exerted torque starts at 900 Nm, and gradually tappers to 0 Nm, as it should. However, due to the lack of support for an I-gain, I decided to implement a PID-controller using an effort actuator.

download.1.mp4

My setup

MuJoCo version: 3.4.0,
API: Python,
Architecture: Google Colab (Link)

My question

The effort actuator (motor name='wam/J1/F') is only able to move the joint to 0.589 rad, regardless of whether the set-point is 1 or 2 rads (the joint-limits are +/- 2.6 rads). Looking at the commanded and exerted joint torque (after x42 gearbox), the command is 19k Nm while the exerted is 0 Nm. it had tappered off from 500 Nm to 0 Nm very quickly. Next, I tried to exert a constant torque of 42 Nm.

download.mp4

Upon replacing the PID-inputs to the joint controller with a 0Nm input, the command and exerted joint torques is the same. However, when the commanded output is 42 Nm, the exerted torque quickly tappers from 42 Nm to 0 Nm.

I tried moving the MB (actuator motor name="smt/pose/x") with an output of 100N and the sensors reported 100N at all sample times.

There are 4 test cases:

1. In-built joint position controller using a position actuator on joint J1,
   • Works as intended
2. A PID-based position controller using a motor-actuator on joint J1,
   • Does NOT work as intended
3. Constant force-input command to motor-actuator on joint Smt X-axis,
   • Works as intended
4. Constant force-input command to motor-actuator on joint J1.
   • Does NOT work as intended

The commands to exert them are located in the section between two lines of #####, labelled as # Actuator.
The print-statement to check if they are being exerted and the change to the corresponding joint are located in the section between two lines of #####, labelled as # Print statements.

Enable each of them in turn to check their effects.

Question: Why can't I exert a constant joint torque using motor name='wam/J1/F'? Maybe that will help answer why my PID controller isn't working as it should.
Cheers,

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>
  <!-- DEFAULT -->
  <!--#########################
      ### Compiler Specific ###
      ######################### -->
  <compiler coordinate="local" angle="radian" meshdir="../content/drive/MyDrive/TA/WAM STL Files/"/>
  <!-- Options: -->
  <option
      timestep="1e-3"
      iterations="10"
      tolerance="1e-10"
      impratio="1"
      gravity="0 0 -9.81"
      solver="Newton"
      cone="elliptic"
      integrator="implicitfast"
  />

  <!-- Visual -->
  <visual>
      <map fogstart="3" fogend="5" force="0.1" znear="0.1"/>
      <quality shadowsize="1024" offsamples="16"/>
      <!-- Visualizing Contact patch -->
      <!-- Visualizing COM sphere -->
      <!-- Visualizing Joint axis -->
      <!-- Visualizing Frame axis -->
      <scale  forcewidth="0.05"
              contactwidth="0.05" contactheight="0.05"
              connect="0.05"
              com="0.1"
              jointlength="1" jointwidth="0.05"
              framelength="1" framewidth="0.1"/>
  </visual>

  <asset>
      <!-- [ENV] Ground PLane -->
      <texture name="groundplane" type="2d" builtin="checker" rgb1=".25 .26 .25"
          rgb2=".22 .22 .22" width="100" height="100" mark="edge" markrgb=".3 .3 .3"/>
      <material name="MatGnd" texture="groundplane"  texrepeat="5 5" specular="1" shininess=".001" reflectance="0.00001"/>
      <material name="body" emission="0.2"/>

      <!-- Load WAM Link meshes -->
      <mesh file="base_link_convex.stl"/>
      <mesh file="shoulder_link_fine.stl"/>
      <mesh file="shoulder_link_convex_decomposition_p1.stl"/>
      <mesh file="shoulder_link_convex_decomposition_p2.stl"/>
      <mesh file="shoulder_link_convex_decomposition_p3.stl"/>
  </asset>


  <default>
      <!-- The class-name being specified under default maps back to the class-names being specified for a body (childclass/class) , or  -->
      <!-- These names have to be unique; it must not clash with class-names for un-intended bodies/joints/geometries -->
      <default class="summit">
      </default>
      <default class="summit/whl">
          <!-- class-names for joints -->
          <joint axis="0 1 0" damping="0.55" frictionloss="0.1"/>
          <!-- Bydefault, hinge-type is the default joint type -->
          <!-- https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-joint -->
      </default>
      <default class="summit/col">
          <!-- class-names for geometry-type -->
          <geom type="mesh" contype="1" conaffinity="1" group="0" rgba="0.5 0.6 0.7 0.3"/>
      </default>
      <default class="summit/whl/viz">
          <!-- class-names for geometry-type -->
          <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.3 0.3 0.3 1" material="body"/>
      </default>

      <!-- WAM -->
      <default class="wam">
      </default>
      <default class="wam/col">
          <geom type="mesh" contype="1" conaffinity="1" group="0" rgba="0.5 0.6 0.7 0.6"/>
      </default>
      <default class="wam/viz">
          <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1"/>
      </default>
      <default class="wam/shoulder_yaw_link/viz">
          <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.8 0.8 0.8 1"/>
      </default>
  </default>

  <worldbody>
      <!-- Light source -->
      <light name="top" pos="0 0 2"/>

      <!-- Ground plane -->
      <geom name="ground" type="plane" pos="0 0 0" size="1 1 1" conaffinity="1" contype="1" material="MatGnd" friction="0.0001 0.001 0.001" group="1"/>

      <body name="Summit" pos="0 0 0.02" euler="0 0 0">
          <!-- Summit Body -->
          <body name="smt/base_link" pos="0 0 0.28">
              <geom name="body" type="box" size=".31 .215 .2" rgba="1 0 0 0.1"/>
              <inertial pos="0 0 0.37" mass="125" diaginertia="1.391 6.853 6.125" />
          </body>

          <!-- world placement -->
          <joint name="smt/world_x" armature="0.0001" axis="1 0 0" damping="1e+11" pos="0 0 0" type="slide"></joint>
          <joint name="smt/world_y" armature="0.0001" axis="0 1 0" damping="1e+11" pos="0 0 0" type="slide"></joint>
          <joint name="smt/world_z" armature="0.0001" axis="0 0 1" damping="1e+0"  pos="0 0 0" type="slide"></joint>

          <!-- control summit base -->
          <joint name="smt/pose/x" type="slide" pos="0 0 0.4" axis="1 0 0"  damping="15" frictionloss="0.1"/>
          <joint name="smt/pose/y" type="slide" pos="0 0 0.4" axis="0 1 0"  damping="15" frictionloss="0.1"/>
          <joint name="smt/orie/z" type="hinge" pos="0 0 0.4" axis="0 0 1"  damping="10" frictionloss="0.1"/>

          <!-- Place imu-sensors here. Have them rotated to match the MB control-axis | w x y z-->
          <site name="imu_sensor_site_mb" type="box" size="0.05 0.05 0.05" rgba="0.9 0 0 0"/>
          <!-- Mobile whls - Fixed -->
          <body name="smt/whl/LF_link_1" pos="0 0 0">
              <geom name="smt/col/whl/LF_cylinder"           class="summit/col" pos="0.222 0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
              <geom name="smt/viz/whl/LF_cylinder"           class="summit/whl/viz" pos="0.222 0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder"/>
              <!-- <inertial                                                        pos="0 0 0" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/> -->
              <joint name="smt/whl_LF_1"                     class="summit/whl" pos="0.222 0.22 0.128"/>
          </body>
          <body name="smt/whl/LR_link_1" pos="0 0 0">
              <geom name="smt/col/whl/LR_cylinder"           class="summit/col" pos="-0.223 0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
              <geom name="smt/viz/whl/LR_cylinder"           class="summit/whl/viz" pos="-0.223 0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder"/>
              <!-- <inertial                                                        pos="0 0 0" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/> -->
              <joint name="smt/whl_LR_1"                     class="summit/whl" pos="-0.223 0.22 0.128"/>
          </body>
          <body name="smt/whl/RF_link_1" pos="0 0 0">
              <geom name="smt/col/whl/RF_cylinder"           class="summit/col" pos="0.222 -0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
              <geom name="smt/viz/whl/RF_cylinder"           class="summit/whl/viz" pos="0.222 -0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder"/>
              <!-- <inertial                                                        pos="0 0 0" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/> -->
              <joint name="smt/whl_RF_1"                     class="summit/whl" pos="0.222 -0.22 0.128"/>
          </body>
          <body name="smt/whl/RR_link_1" pos="0 0 0">
              <geom name="smt/col/whl/RR_cylinder"           class="summit/col" pos="-0.223 -0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder" friction="0.0001 0.001 0.001"/>
              <geom name="smt/viz/whl/RR_cylinder"           class="summit/whl/viz" pos="-0.223 -0.22 0.128" quat="0.7071068 0.7071068 0 0" size="0.13 0.05" type="cylinder"/>
              <!-- <inertial                                                        pos="0 0 0" mass="10.0" diaginertia="0.04411 0.02467 0.02467"/> -->
              <joint name="smt/whl_RR_1"                     class="summit/whl" pos="-0.223 -0.22 0.128"/>
          </body>

          <!-- === WAM 7DOF HAND LINK === === === === -->
          <!-- Place WAM Module Here -->
          <body name="wam_7dof_bhand" pos="0.14 0 0.405" quat="1 0 0 0">
              <body name="wam/base_link" childclass="wam" pos="0 0 0">
                  <body name="wam/base" childclass="wam" pos="0 0 0">
                      <!-- ### Inertial Ref: (*1, pg.10) unit: [m, kg, kg * m^2]
                          Note:
                              - we have to offset the stl model frame to the inertial frame in doc
                              - pos = <z, x, y> + <0,0,0.346>
                              - **the axis should be based on the orientation notation at the bottom left corner
                      -->
                      <inertial   pos="-0.14071720 -0.02017671 0.07995294" mass="9.97059584" fullinertia="0.11760385 0.10916849 0.18294303 0.02557874 0.00161433 0.00640270" />
                      <geom       class="wam/viz" mesh="base_link_convex"/>
                      <geom       class="wam/col" mesh="base_link_convex"/>
                  </body>
                  <site name="imu_sensor_site_wam_base_link" type="box" size="0.01 0.01 0.01" rgba="0.9 0 0 0"/>

                  <body name="wam/shoulder_yaw_link" pos="0 0 0.346">
                      <!-- ### Inertial Ref: (*1, pg.12) unit: [m, kg, kg * m^2]
                          Note:
                              - frame correction to the stl: <x, z, -y>
                      -->
                      <inertial               pos="-0.00443422 -0.00066489 -0.12189039" mass="10.76768767" fullinertia="0.13488033 0.09046330 0.11328369 -0.00012485 0.00213041 -0.00068555" />
                      <joint name="wam/J1"    range="-2.6 2.6" damping="200" frictionloss="300" type="hinge" limited="true" pos="0 0 0" axis="0 0 1"/>
                      <geom                   class="wam/shoulder_yaw_link/viz" mesh="shoulder_link_fine"/>
                      <geom                   class="wam/col" mesh="shoulder_link_convex_decomposition_p1"/>
                      <geom                   class="wam/col" mesh="shoulder_link_convex_decomposition_p2"/>
                      <geom                   class="wam/col" mesh="shoulder_link_convex_decomposition_p3"/>
                      <!-- Place imu sensors here. -->
                      <site name="imu_sensor_site_wam_shoulder_yaw_link" type="box" size="0.01 0.01 0.01" rgba="0.9 0 0 0"/>
                  </body>
              </body>
          </body>
      </body>
  </worldbody>
  <sensor>
  <!-- Mobile base -->
      <!-- Get Linear acceleration in MB local-frame -->
      <accelerometer name="accelerometer_mb" site="imu_sensor_site_mb"/>
      <!-- Get Linear velocity in MB local-frame -->
      <velocimeter name="velocimeter_mb" site="imu_sensor_site_mb"/>
      <!-- Get Angular velocity in MB local-frame -->
      <gyro name="gyroscope_mb" site="imu_sensor_site_mb"/>
      <!-- Get MB gloabl-position -->
      <!-- “body” refers to the inertial frame of the body, while “xbody” refers to the regular frame of the body (usually centered at the joint with the parent body) -->
      <framepos name="global_pos_mb" objtype="xbody" objname="smt/base_link"/>
      <!-- Get Joint effort -->
      <jointactuatorfrc name="joint_effort_smt_pose_x" joint="smt/pose/x"/>
      <!-- Get MB gloabl-quaternion -->
      <framequat name="global_quat_mb" objtype="xbody" objname="smt/base_link"/>
      <!-- Get wheel joint velocity -->
      <!-- LF Wheel -->
      <!-- <jointvel name="joint_vel_mb_lf" joint="smt/whl_LF"/> -->
      <!-- LF Wheel -->
      <!-- <jointvel name="joint_vel_mb_rf" joint="smt/whl_RF"/> -->
      <!-- LF Wheel -->
      <!-- <jointvel name="joint_vel_mb_rr" joint="smt/whl_RR"/> -->
      <!-- LF Wheel -->
      <!-- <jointvel name="joint_vel_mb_lr" joint="smt/whl_LR"/> -->

      <!-- Wam Shoulder Yaw link -->
      <!-- Get joint position -->
      <jointpos name="joint_pos_wam_shoulder_yaw" joint="wam/J1"/>
      <!-- Get joint velocity -->
      <jointvel name="joint_vel_wam_shoulder_yaw" joint="wam/J1"/>
      <!-- Get Joint effort -->
      <jointactuatorfrc name="joint_effort_wam_shoulder_yaw" joint="wam/J1"/>
  </sensor>
  <actuator>
      <!-- https://youtube.com/playlist?list=PLc7bpbeTIk758Ad3fkSywdxHWpBh9PM0G&si=2aflX9GOzkbmAmPP -->
      <motor name="smt/pose/x"  joint="smt/pose/x" ctrllimited="true" ctrlrange="-10.0 10.0" gear="100.0"/>
      <motor name="smt/pose/y"  joint="smt/pose/y" ctrllimited="true" ctrlrange="-10.0 10.0" gear="100.0"/>
      <motor name="smt/orie/z"  joint="smt/orie/z" ctrllimited="true" ctrlrange="-10.0 10.0" gear="50.0"/>
      <position name="wam/J1/P" joint="wam/J1" kp="900" kv="10"/>
      <motor name='wam/J1/F' ctrllimited="true" joint='wam/J1' ctrlrange='-12.62 12.62' gear="42"      />
  </actuator>

</mujoco>

Code:

import mujoco
import mediapy as media

model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

# Make a camera.
cam = mujoco.MjvCamera()
mujoco.mjv_defaultCamera(cam)

# enable joint visualization option:
scene_option = mujoco.MjvOption()
# Show joint axis
scene_option.flags[mujoco.mjtVisFlag.mjVIS_JOINT] = True
# Show body frames
scene_option.frame = mujoco.mjtFrame.mjFRAME_GEOM

duration = 3.8  # (seconds)
framerate = 60  # (Hz)

# Simulate and display video.
frames = []
mujoco.mj_resetData(model, data)
cam = mujoco.MjvCamera()

# WAM joint state set-point
# Position
w_p_sp = np.array([[1],[1],[1],[1],[1],[1],[1]], dtype=np.float32)
# Velocity
w_v_sp = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)

# Error
# Initial position error
err_p = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)
# Initial velocity error
err_v = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)
# Initial cummulative position error
err_cumm_p = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)

# Effort contribution due to
# Position error
eff_p = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)
# Velocity error
eff_v = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)
# Cummulative position error
eff_cumm_p = np.array([[0],[0],[0],[0],[0],[0],[0]], dtype=np.float32)

id_new_sen_smt_x_eff = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_SENSOR,'joint_effort_smt_pose_x')
id_new_sen_smt_pos = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_SENSOR,'global_pos_mb')
id_new_w0_p = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_SENSOR,'joint_pos_wam_shoulder_yaw')
id_new_w0_v = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_SENSOR,'joint_vel_wam_shoulder_yaw')
id_new_w0_e = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_SENSOR,'joint_effort_wam_shoulder_yaw')
# Sensor index
sensor_index_sen_smt_x_eff = model.sensor_adr[id_new_sen_smt_x_eff]
sensor_index_sen_smt_pos = model.sensor_adr[id_new_sen_smt_pos]
sensor_index_w0_p = model.sensor_adr[id_new_w0_p]
sensor_index_w0_v = model.sensor_adr[id_new_w0_v]
sensor_index_w0_e = model.sensor_adr[id_new_w0_e]
# Sensor dimension
sensor_dim_sen_smt_x_eff = model.sensor_dim[id_new_sen_smt_x_eff]
sensor_dim_sen_smt_pos = model.sensor_dim[id_new_sen_smt_pos]
sensor_dim_w0_p = model.sensor_dim[id_new_w0_p]
sensor_dim_w0_v = model.sensor_dim[id_new_w0_v]
sensor_dim_w0_e = model.sensor_dim[id_new_w0_e]

# Get actuator ID
id_new_eff_smt_pose_x = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_ACTUATOR,'smt/pose/x')
id_new_w0_f = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_ACTUATOR,'wam/J1/F')
id_new_w0_p = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_ACTUATOR,'wam/J1/P')

with mujoco.Renderer(model, height=480, width=640) as renderer:
  # Control viewport camera pose
  cam.azimuth = 90.0 # (deg) w.r.t the ground plane
  # Distance seperating the camera from the look-at point
  cam.distance = 2.0 # m
  cam.elevation = -60 # (deg)
  # The point the camera is looking at
  cam.lookat = np.array([0.0, -0.000000, 0.5])
  while data.time < duration:
    # Compute error
    # Position
    err_p[0,0] = data.sensordata[sensor_index_w0_p:(sensor_index_w0_p+sensor_dim_w0_p)]-w_p_sp[0,0]
    # Velocity
    err_v[0,0] = data.sensordata[sensor_index_w0_v:(sensor_index_w0_v+sensor_dim_w0_v)]-w_v_sp[0,0]

    # Go to set-point
    eff_p[0,0] = -900*err_p[0,0]
    eff_v[0,0] = -10*err_v[0,0]
    eff_cumm_p[0,0] = -2.5*err_cumm_p[0,0]

    ############################################################################
    # Actuator

    # # Test Position actuator on J1: 1 rad position
    # data.ctrl[id_new] = w_p_sp[0,0]

    # # Test PID on J1: 1 rad position
    # data.ctrl[id_new_w0_f] = eff_p[0,0]+eff_v[0,0]+eff_cumm_p[0,0]

    # # Test constant effort input
    # # Exert 1N of motor force on Smt X-axis
    # data.ctrl[id_new_eff_smt_pose_x] = 1

    # # Exert 1N of motor force on J1
    # data.ctrl[id_new_w0_f] = 1

    ############################################################################

    # Update cummulative error
    err_cumm_p = err_cumm_p+err_p

    mujoco.mj_step(model, data)
    if len(frames) < data.time * framerate:

      ##########################################################################
      # Print statements

      # # Test Position actuator: 1 rad position
      # print("Angular Position of J1: "+str(data.sensordata[sensor_index_w0_p:(sensor_index_w0_p+sensor_dim_w0_p)]))
      # print("Exerted Joint effort by J1: "+str(data.sensordata[sensor_index_w0_e:(sensor_index_w0_e+sensor_dim_w0_e)]))
      
      # # Test PID on J1: 1 rad position
      # print("Angular Position of J1: "+str(data.sensordata[sensor_index_w0_p:(sensor_index_w0_p+sensor_dim_w0_p)]))
      # print("Exerted Joint effort by J1: "+str(data.sensordata[sensor_index_w0_e:(sensor_index_w0_e+sensor_dim_w0_e)]))
      # print("Commanded Joint effort by J1: "+str(data.ctrl[id_new_w0_f]*42))
      # print("Angular Position error of J1: "+str(err_p[0,0]))
      # print("P-gain effort contribution on J1: "+str(eff_p[0,0]))
      # print("I-gain effort contribution on J1: "+str(eff_cumm_p[0,0]))
      # print("D-gain effort contribution on J1: "+str(eff_v[0,0]))

      # # Test constant effort input
      # Exert 1N of motor force on Smt X-axis
      # print("X-axis force on Summit-frame in World-frame: "+str(data.sensordata[sensor_index_sen_smt_x_eff:(sensor_index_sen_smt_x_eff+sensor_dim_sen_smt_x_eff)]))

      # # Exert 1N of motor force on J1
      # print("Exerted Joint effort by J1: "+str(data.sensordata[sensor_index_w0_e:(sensor_index_w0_e+sensor_dim_w0_e)]))

      ##########################################################################
      
      renderer.update_scene(data, cam, scene_option=scene_option)
      pixels = renderer.render()
      frames.append(pixels)

media.show_video(frames, fps=framerate)

Notes:
Please change the location of STL files in the XML file to where ever you are keeping it.
WAM STL Files.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.