A small question about how to calculate joint torque to decrease joint velocities and accelerations more quickly?

[Yingfan99327]

Hi,

I'm a student and I'm trying to use MuJoCo for robotic simulation.

I'm looking for some help with the joint torque command.

I used the motor-type actuators in panda robot simulation. Here is my problem.

# joint velocities and joint accelerations read from data.qvel and data.qacc at time t
qvel =  [ 3.249e-02  5.866e-01  2.5216e-02 -2.1146e+00 -5.746e-02 -1.286e+00 -1.1046e-04] 
qacc =  [ 6.3916e-02  5.0266e+00 -9.9666e-02 -8.999e+00 1.756e-01  3.289e+00  8.253e-04] 
# joint torque computed from impedance controller at time t
data.ctrl = [-3.259e+03 -5.652e+04 -2.531e+03  2.154e+05 5.769e+03  1.280e+05  6.300e+00]

Then after mj_step, time_step = 0.001

# joint velocities and joint accelerations read from data.qvel and data.qacc at time t+1
qvel = [ 3.255e-02  5.917e-01  2.511e-02 -2.1237e+00 -5.731e-02 -1.280e+00 -1.097e-04] 
qacc = [ 6.361e-02  5.066e+00 -1.017e-01 -8.971e+00 1.783e-01  3.368e+00  8.287e-04] 

It could be seen that the joint acceleration decreases due to the joint torque with right signs,but what I expect is that the joint acceleration could be decreased much more quickly so that the joint velocities could reach 0 more quickly.

And, yes, the joint torque seems very high due to the high damping coefficients I set for the robot. However, I set the ctrl range in the xml file, I guess after I set the ctrl range, the actual torque sent to the joint will be automatically clipped into this range, right? Is this the reason why the joint acceleration changes not too much after mj_step?

<actuator>
      <motor class="panda" name="panda_joint1" joint="joint1" ctrlrange="-87 87" gear="1"/>
      <motor class="panda" name="panda_joint2" joint="joint2" ctrlrange="-87 87" gear="1"/>
      <motor class="panda" name="panda_joint3" joint="joint3" ctrlrange="-87 87" gear="1"/>
      <motor class="panda" name="panda_joint4" joint="joint4" ctrlrange="-87 87" gear="1"/>
      <motor class="panda" name="panda_joint5" joint="joint5" ctrlrange="-12 12" gear="1"/>
      <motor class="panda" name="panda_joint6" joint="joint6" ctrlrange="-12 12" gear="1"/>
      <motor class="panda" name="panda_joint7" joint="joint7" ctrlrange="-12 12" gear="1"/>
</actuator>

So, how could I enable the joint velocities and accelerations decrease to 0 quickly with the right joint torque?

Thanks and wishing you all the best.

[yuvaltassa]

Did you try removing the ctrlrange?

[Yingfan99327]

And as I read from this issue #1095, can I use the method mentioned in this issue,

data.joint("my_joint").qfrc_constraint + data.joint("my_joint").qfrc_smooth

to measure the real joint torque applied to each joint after I individually set the joint torque equal to the results from the joint impedance controller? Namely,

data.ctrl = tau_from_joint_impedance_controller
mj_step(model, data)
mj_forward(model, data)
print(data.joint("my_joint").qfrc_constraint + data.joint("my_joint").qfrc_smooth)

Because I suspect, because the joint torque computed is so large, with the ctrl range limits set in each actuator, the actual joint torques applied to each actuator is actually very small, which result in the very slow decreasing speed of joint accelerations and joint velocties.

Updates:
I did the test and the results are shown as follows.

# tau send to joints at time t
tau = [ -3.51731978e+04 
           -8.42908942e+05  
           -1.90472514e+04  
            2.47023430e+06 
            4.78634244e+04  
            1.07535504e+06  
            7.45977433e+01] 
# qfrc_constraint + qfrc_smooth after calling mj_step and mj_forward
joint1_torque: [-0.51316584] 
joint2_torque: [33.07041124] 
joint3_torque: [-0.55552377] 
joint4_torque: [-15.67936145] 
joint5_torque: [-0.43479885] 
joint6_torque: [2.14140804] 
joint7_torque: [0.02682055] 

[Yingfan99327]

Hi, Yuval,

Thanks for your reply. I removed the ctrlrange defined in the actuators, but it presented the same result.

Here is a minimal example of the panda xml file I used in my simulation. I used the model from the mujoco_menagerie and listed the main model definition as follows,

# actuator part:
<actuator>
    <motor class="panda" name="panda_joint1" joint="joint1" gear="1"/>
    <motor class="panda" name="panda_joint2" joint="joint2" gear="1"/>
    <motor class="panda" name="panda_joint3" joint="joint3" gear="1"/>
    <motor class="panda" name="panda_joint4" joint="joint4" gear="1"/>
    <motor class="panda" name="panda_joint5" joint="joint5" gear="1"/>
    <motor class="panda" name="panda_joint6" joint="joint6" gear="1"/>
    <motor class="panda" name="panda_joint7" joint="joint7" gear="1"/>
</actuator>

# assets part
<mujocoinclude>

    <compiler angle="radian"/>
    <!-- <option timestep="0.002" noslip_iterations="20"/> -->
    <option timestep="0.001"/>
    <size nuser_actuator="5"/>

    <asset>
      <material class="panda" name="white" rgba="1 1 1 1"/>
      <material class="panda" name="off_white" rgba="0.901961 0.921569 0.929412 1"/>
      <material class="panda" name="dark_grey" rgba="0.25 0.25 0.25 1"/>
      <material class="panda" name="green" rgba="0 1 0 1"/>
      <material class="panda" name="light_blue" rgba="0.039216 0.541176 0.780392 1"/>

      <!-- Collision meshes -->
      <mesh name="link0_c" file="meshes/menagerie_franka_meshes/link0.stl"/>
      <mesh name="link1_c" file="meshes/menagerie_franka_meshes/link1.stl"/>
      <mesh name="link2_c" file="meshes/menagerie_franka_meshes/link2.stl"/>
      <mesh name="link3_c" file="meshes/menagerie_franka_meshes/link3.stl"/>
      <mesh name="link4_c" file="meshes/menagerie_franka_meshes/link4.stl"/>
      <mesh name="link5_c0" file="meshes/menagerie_franka_meshes/link5_collision_0.obj"/>
      <mesh name="link5_c1" file="meshes/menagerie_franka_meshes/link5_collision_1.obj"/>
      <mesh name="link5_c2" file="meshes/menagerie_franka_meshes/link5_collision_2.obj"/>
      <mesh name="link6_c" file="meshes/menagerie_franka_meshes/link6.stl"/>
      <mesh name="link7_c" file="meshes/menagerie_franka_meshes/link7.stl"/>

      <!-- Visual meshes -->
      <mesh file="meshes/menagerie_franka_meshes/link0_0.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_3.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_4.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_5.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_7.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_8.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_9.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_10.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link0_11.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link3_0.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link3_1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link3_2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link3_3.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link4_0.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link4_1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link4_2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link4_3.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link5_0.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link5_1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link5_2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_0.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_3.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_4.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_5.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_6.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_7.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_8.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_9.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_10.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_11.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_12.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_13.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_14.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_15.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link6_16.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_0.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_1.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_2.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_3.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_4.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_5.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_6.obj"/>
      <mesh file="meshes/menagerie_franka_meshes/link7_7.obj"/>
    </asset>

    <default>
        <default class="panda">
          <material specular="0.5" shininess="0.25"/>
          <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
<!--          <general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>-->
<!--          <default class="finger">-->
<!--            <joint axis="0 1 0" type="slide" range="0 0.04"/>-->
<!--          </default>-->

          <default class="panda/visual">
            <geom type="mesh" contype="0" conaffinity="0" group="2"/>
          </default>
          <default class="panda/collision">
            <geom type="mesh" group="3"/>
          </default>
          <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
        </default>
    </default>

<compiler meshdir="" texturedir=""/>
    <!-- <compiler angle="radian"/>  -->
    <size njmax='1000' nconmax='1000'/> 

    <visual>
      <global offwidth="1000" offheight="1000"/>
    </visual>

    <compiler angle="radian" meshdir="assets" autolimits="true"/>
    <option integrator="implicitfast" impratio="10"/>

<worldbody>

        <body name="panda_robot" pos='0 -1.5 0' quat="0.707107 0 0 0.707107">
            <body name="link0" childclass="panda">
              <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974" fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
              <geom mesh="link0_0" material="off_white" class="panda/visual"/>
              <geom mesh="link0_1" material="dark_grey" class="panda/visual"/>
              <geom mesh="link0_2" material="off_white" class="panda/visual"/>
              <geom mesh="link0_3" material="dark_grey" class="panda/visual"/>
              <geom mesh="link0_4" material="off_white" class="panda/visual"/>
              <geom mesh="link0_5" material="dark_grey" class="panda/visual"/>
              <geom mesh="link0_7" material="white" class="panda/visual"/>
              <geom mesh="link0_8" material="white" class="panda/visual"/>
              <geom mesh="link0_9" material="dark_grey" class="panda/visual"/>
              <geom mesh="link0_10" material="off_white" class="panda/visual"/>
              <geom mesh="link0_11" material="white" class="panda/visual"/>
              <geom mesh="link0_c" class="panda/collision"/>

                <body name="link1" pos="0 0 0.333">
                <inertial mass="4.970684" pos="0.003875 0.002081 -0.04762" fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
                <joint name="joint1" range="-2.8973 2.8973" limited="true" frictionloss="0.05"/>
                <geom material="white" mesh="link1" class="panda/visual"/>
                <geom mesh="link1_c" class="panda/collision"/>
                <body name="link2" quat="0.707107 -0.707107 0 0">
                  <inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495" fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
                  <joint name="joint2" range="-1.7628 1.7628" limited="true" frictionloss="0.05"/>
                  <geom material="white" mesh="link2" class="panda/visual"/>
                  <geom mesh="link2_c" class="panda/collision"/>
                    <body name="link3" pos="0 -0.316 0" quat="0.707107 0.707107 0 0">
                    <joint name="joint3" range="-2.8973 2.8973" limited="true" frictionloss="0.05"/>
                    <inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2" fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
                    <geom mesh="link3_0" material="white" class="panda/visual"/>
                    <geom mesh="link3_1" material="white" class="panda/visual"/>
                    <geom mesh="link3_2" material="white" class="panda/visual"/>
                    <geom mesh="link3_3" material="dark_grey" class="panda/visual"/>
                    <geom mesh="link3_c" class="panda/collision"/>
                      <body name="link4" pos="0.0825 0 0" quat="0.707107 0.707107 0 0">
                      <inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2" fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
                      <joint name="joint4" range="-3.0718 -0.0698" limited="true" frictionloss="0.05"/>
                      <geom mesh="link4_0" material="white" class="panda/visual"/>
                      <geom mesh="link4_1" material="white" class="panda/visual"/>
                      <geom mesh="link4_2" material="dark_grey" class="panda/visual"/>
                      <geom mesh="link4_3" material="white" class="panda/visual"/>
                      <geom mesh="link4_c" class="panda/collision"/>
                        <body name="link5" pos="-0.0825 0.384 0" quat="0.707107 -0.707107 0 0">
                        <inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2" fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
                        <joint name="joint5" range="-2.8973 2.8973" limited="true" frictionloss="0.05"/>
                        <geom mesh="link5_0" material="dark_grey" class="panda/visual"/>
                        <geom mesh="link5_1" material="white" class="panda/visual"/>
                        <geom mesh="link5_2" material="white" class="panda/visual"/>
                        <geom mesh="link5_c0" class="panda/collision"/>
                        <geom mesh="link5_c1" class="panda/collision"/>
                        <geom mesh="link5_c2" class="panda/collision"/>
                          <body name="link6" quat="0.707107 0.707107 0 0">
                          <inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2" fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
                          <joint name="joint6" range="-0.0175 3.7525" limited="true" frictionloss="0.05"/>
                          <geom mesh="link6_0" material="off_white" class="panda/visual"/>
                          <geom mesh="link6_1" material="white" class="panda/visual"/>
                          <geom mesh="link6_2" material="dark_grey" class="panda/visual"/>
                          <geom mesh="link6_3" material="white" class="panda/visual"/>
                          <geom mesh="link6_4" material="white" class="panda/visual"/>
                          <geom mesh="link6_5" material="white" class="panda/visual"/>
                          <geom mesh="link6_6" material="white" class="panda/visual"/>
                          <geom mesh="link6_7" material="light_blue" class="panda/visual"/>
                          <geom mesh="link6_8" material="light_blue" class="panda/visual"/>
                          <geom mesh="link6_9" material="dark_grey" class="panda/visual"/>
                          <geom mesh="link6_10" material="dark_grey" class="panda/visual"/>
                          <geom mesh="link6_11" material="white" class="panda/visual"/>
                          <geom mesh="link6_12" material="green" class="panda/visual"/>
                          <geom mesh="link6_13" material="white" class="panda/visual"/>
                          <geom mesh="link6_14" material="dark_grey" class="panda/visual"/>
                          <geom mesh="link6_15" material="dark_grey" class="panda/visual"/>
                          <geom mesh="link6_16" material="white" class="panda/visual"/>
                          <geom mesh="link6_c" class="panda/collision"/>
                            <body name="link7" pos="0.088 0 0" quat="0.707107 0.707107 0 0">
                            <inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2" fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
                            <joint name="joint7" range="-2.8973 2.8973" limited="true" frictionloss="0.05"/>
                            <geom mesh="link7_0" material="white" class="panda/visual"/>
                            <geom mesh="link7_1" material="dark_grey" class="panda/visual"/>
                            <geom mesh="link7_2" material="dark_grey" class="panda/visual"/>
                            <geom mesh="link7_3" material="dark_grey" class="panda/visual"/>
                            <geom mesh="link7_4" material="dark_grey" class="panda/visual"/>
                            <geom mesh="link7_5" material="dark_grey" class="panda/visual"/>
                            <geom mesh="link7_6" material="dark_grey" class="panda/visual"/>
                            <geom mesh="link7_7" material="white" class="panda/visual"/>
                            <geom mesh="link7_c" class="panda/collision"/>
                              <body name="attachment" pos="0 0 0.107">
                              <site name="flange"/>
                              </body>
                            </body>
                          </body>
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
        </body>

As for the joint impedance controller, I just implemented it with its classic theory as follows,

tau = M * qacc_desired + D * (qvel_desired - qvel_current) + K * (qpos_desired - qpos_current) + data.qfrc_bias[:self.joint_num]

where qvel_current, qpos_current are read from the data, and qacc_desired, qvel_desired and qpos_desired are planned.

If you have free time, could you help me check it and give me some advice on these problems including the issues I mentioned before like #1909.

Thanks very much for your help and time. Really appreciate that.

Btw, I also want to show great thanks to this wonderful simulation tool, your and other developers' quick and detailed responses to issues which helps me get more familiar with this powerful simulation tool to do robotic simulation, for instance, some basic questions like how to calculate the derivatives of the jacobian matrix, how to measure the contact force ect. Really appreciate.

[yuvaltassa]

I suspect your actuators are inheriting the ctrlrange from here

[Yingfan99327]

Yes, I noticed it so I commented out this line as I showed in the xml file above, but it also didn't work. Or just commented out this is not the right way to remove the inheritance relationship?

[yuvaltassa]

In that case I'm not sure...

Anyway, why are you implementing you own actuator rather than using the built-in ones?

[Yingfan99327]

Well, I just hope to make the simulation close to the real scenes, I mean, hope that the joint position limits and joint torque limits listed in the official web as https://frankaemika.github.io/docs/control_parameters.html could be well meet during the simulation.

You mean the actuators defined originally in the mujoco_menagerie? I tried just now, it doesn't work neither. And it seems lack some torque limits on joint torques.

Thanks for your time and help, Yuval. May I just make a small summary of some questions I am confused here so that you can give me a quick check?

1. The only thing that will affect the input torque would be the ctrlrange, right? Without the ctrl range, the joint torque would be the torque you send to the actuator, otherwise it will be clamped to the control range.
2. In real scenarios, I guess friction also plays an important role, so would there be something I can modify or add about joint friction to help me plan the joint motion as I expect?
3. If I need to use the self.data.qfrc_inverse.copy()[0:self.joint_num] to calculate the gravity compensation torque to help the robot maintain the configuratuon, I need to make sure the joint velocities and joint accelerations are all zeros.
4. If everything works fine, it definitely possible to realize a classic joint impedance controller or admittance controller in mujoco, I mean, to realize some expected tasks such as
   a) Follow a defined trajectory in cartesian space and stop at the end position.
   b) Move when some external force is applied to the end-effector and stop quickly and smoothly when the external force is removed.
   Because I have already realized some tasks in simulation, however, because I use this simulation tool not for a long time, I am a bit worried whether I miss some important settings in my model files or other aspects.

Btw, I also referred to some other developers' codes in panda robot simulation, however, most of them simply use methods like mocap, position-type actuators instead of motor-type actuators because we have different focus in simulation.

Anyway, thanks very much for your time and help.

Wishing you all the best.