search

google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.
https://mujoco.org
Apache License 2.0
7.82k stars 780 forks source link

Unknown actuator lag #908

Closed le-horizon closed 1 year ago

le-horizon commented 1 year ago

Hi,

I'm an engineer and I'm trying to use MuJoCo for sim to real deployment.

I'm looking for some help with actuator torque lag in simulation.

Here is the relevant part of the model xml:

        <default class="actuator">
          <general dyntype="none"
                   dynprm="0.005"
                   biastype="affine"
                   gainprm="50"
                   biasprm="0 -50 -1.0"
                   ctrlrange="-2.69653 -0.916298" forcerange="-35.55 35.55" />
        </default>

The response of the actuator lags behind the physical actuator. See torque in orange, it is flat and near zero from t = 0 to 8. Any idea what might be causing this lag?

The real physical controller is documented to be a PD controller with Kp=50 and Kd = 1.

Here is a screenshot, illustrating my question.

In the plot, I plotted both the command (target position) and the joint position reading, as well as the torque from joints.qfrc_actuator.

image

I'm using mujoco 2.3.2 on ubuntu 22.04. Controller runs at 50 Hz, simulation runs at 200 Hz.

Thanks, Le

kevinzakka commented 1 year ago

Why is dynprm != 1?

le-horizon commented 1 year ago

Why is dynprm != 1?

Will it matter if dyntype="none"? I tweaked it a bit previously to see its effect, when I set dyntype="filter".

kevinzakka commented 1 year ago

Could you post the full xml? I can investigate.

le-horizon commented 1 year ago

We've tried using position control in the actuator, with similar lagged response.

Here it is the xml, thanks!

`

<option cone="elliptic" impratio="100"/>
<size njmax="500" nconmax="100" />
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>

<default>
    <geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>

    <motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
    <camera fovy="60"/>
    <joint damping="0.01" armature="0.01" frictionloss="0.2" />

    <!-- The ctrlrange below are based on menagerie defaults and are just
         for reference. This will be overridden by the UnitreeGo1 class as
         a smaller range is more desired for the policy training purpose.
    -->
    <default class="actuator_on_hip">
      <general dyntype="none"
               dynprm="0.005"
               biastype="affine"
               gainprm="50"
               biasprm="0 -50 -1.0"
               ctrlrange="-0.802851 0.802851"
               forcerange="-23.7 23.7" />
    </default>
    <default class="actuator_on_thigh">
      <general dyntype="none"
               dynprm="0.005"
               biastype="affine"
               gainprm="50"
               biasprm="0 -50 -1.0"
               ctrlrange="-1.0472 4.18879" forcerange="-23.7 23.7" />
    </default>
    <default class="actuator_on_calf">
      <general dyntype="none"
               dynprm="0.005"
               biastype="affine"
               gainprm="50"
               biasprm="0 -50 -1.0"
               ctrlrange="-2.69653 -0.916298" forcerange="-35.55 35.55" />
    </default>
    <default class="foot">
      <!-- the 3rd value of solimp is roughly proportional to the deformation
      of the foot when the robot stands on the ground. With this value (0.01)
      the deformation is about 0.44 cm -->
      <geom type="sphere" size="0.02" pos="0 0 -0.213" priority="1" solimp="0.015 1 0.01"
          condim="6" friction="0.8 0.02 0.01" rgba="0 0 0 1"/>
    </default>
</default>
<asset>
    <mesh name="trunk" file="trunk.stl" />
    <mesh name="hip" file="hip.stl" />
    <mesh name="thigh_mirror" file="thigh_mirror.stl" />
    <mesh name="calf" file="calf.stl" />
    <mesh name="thigh" file="thigh.stl" />
</asset>

<asset>
    <texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
    <texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
    <material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
</asset>

<worldbody>
    <!-- <camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/> -->
    <body name="trunk" pos="0 0 0.325">
        <inertial pos="0.0116053 0.00442221 0.000106692" quat="0.0111438 0.707126 -0.00935374 0.706938" mass="1.801" diaginertia="0.0447997 0.0366257 0.0162187" />
        <freejoint/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
        <geom size="0.13 0.04675 0.057" type="box" rgba="0.913725 0.913725 0.847059 1" />
        <geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
        <geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" rgba="0.8 0 0 0" />
        <geom size="0.13 0.08675 0.045" type="box" rgba="1 0 0 0"/>
        <geom size="0.05 0.06675 0.057" type="box" pos="0.18 0 0" rgba="1 0 0 0"/>
        <geom size="0.03 0.06675 0.057" type="box" pos="0.26 0 0.01" rgba="1 0 0 0"/>
        <site name="imu" pos="0 0 0"/>

        <body name="FR_hip" pos="0.1881 -0.04675 0">
            <inertial pos="-0.00406411 -0.0193463 4.50733e-06" quat="0.467526 0.531662 -0.466259 0.530431" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
            <joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
            <geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
            <geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
            <geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />

            <body name="FR_thigh" pos="0 -0.08 0">
                <inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
                <joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
                <geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
                <body name="FR_calf" pos="0 0 -0.213">
                    <inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
                    <joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
                    <geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
                    <geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
                    <!-- pos should be omitted. However, Object.get_local_position() does not handle
                    the default value from class. So we need to set it here in order to pass some unittests -->
                    <geom class="foot" pos="0 0 -0.213" />
                </body>
            </body>
        </body>
        <body name="FL_hip" pos="0.1881 0.04675 0">
            <inertial pos="-0.00406411 0.0193463 4.50733e-06" quat="0.531662 0.467526 -0.530431 0.466259" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
            <joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
            <geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
            <geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />

            <body name="FL_thigh" pos="0 0.08 0">
                <inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
                <joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
                <geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
                <body name="FL_calf" pos="0 0 -0.213">
                    <inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
                    <joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
                    <geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
                    <geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
                    <geom class="foot" pos="0 0 -0.213" />
                </body>
            </body>
        </body>
        <body name="RR_hip" pos="-0.1881 -0.04675 0">
            <inertial pos="0.00406411 -0.0193463 4.50733e-06" quat="0.530431 0.466259 -0.531662 0.467526" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
            <joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
            <geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
            <geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
            <geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
            <geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />

            <body name="RR_thigh" pos="0 -0.08 0">
                <inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
                <joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
                <geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
                <body name="RR_calf" pos="0 0 -0.213">
                    <inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
                    <joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
                    <geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
                    <geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
                    <geom class="foot" pos="0 0 -0.213" />
                </body>
            </body>
        </body>
        <body name="RL_hip" pos="-0.1881 0.04675 0">
            <inertial pos="0.00406411 0.0193463 4.50733e-06" quat="0.466259 0.530431 -0.467526 0.531662" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
            <joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851"  />
            <geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
            <geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
            <geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
            <geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />

            <body name="RL_thigh" pos="0 0.08 0">
                <inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
                <joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
                <geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
                <body name="RL_calf" pos="0 0 -0.213">
                    <inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
                    <joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
                    <geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
                    <geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
                    <geom class="foot" pos="0 0 -0.213" />
                </body>
            </body>
        </body>
    </body>
</worldbody>
<actuator>
    <general class="actuator_on_hip" joint="FR_hip_joint" />
    <general class="actuator_on_thigh" joint="FR_thigh_joint" />
    <general class="actuator_on_calf" joint="FR_calf_joint" />

    <general class="actuator_on_hip" joint="FL_hip_joint" />
    <general class="actuator_on_thigh" joint="FL_thigh_joint" />
    <general class="actuator_on_calf" joint="FL_calf_joint" />

    <general class="actuator_on_hip" joint="RR_hip_joint" />
    <general class="actuator_on_thigh" joint="RR_thigh_joint" />
    <general class="actuator_on_calf" joint="RR_calf_joint" />

    <general class="actuator_on_hip" joint="RL_hip_joint" />
    <general class="actuator_on_thigh" joint="RL_thigh_joint" />
    <general class="actuator_on_calf" joint="RL_calf_joint" />
</actuator>

<sensor>

    <jointpos name="FR_hip_pos"     joint="FR_hip_joint"/>
    <jointpos name="FR_thigh_pos"   joint="FR_thigh_joint"/>
    <jointpos name="FR_calf_pos"    joint="FR_calf_joint"/>
    <jointpos name="FL_hip_pos"     joint="FL_hip_joint"/>
    <jointpos name="FL_thigh_pos"   joint="FL_thigh_joint"/>
    <jointpos name="FL_calf_pos"    joint="FL_calf_joint"/>
    <jointpos name="RR_hip_pos"     joint="RR_hip_joint"/>
    <jointpos name="RR_thigh_pos"   joint="RR_thigh_joint"/>
    <jointpos name="RR_calf_pos"    joint="RR_calf_joint" />
    <jointpos name="RL_hip_pos"     joint="RL_hip_joint"/>
    <jointpos name="RL_thigh_pos"   joint="RL_thigh_joint"/>
    <jointpos name="RL_calf_pos"    joint="RL_calf_joint"/>

    <jointvel name="FR_hip_vel"     joint="FR_hip_joint"/>
    <jointvel name="FR_thigh_vel"   joint="FR_thigh_joint"/>
    <jointvel name="FR_calf_vel"    joint="FR_calf_joint"/>
    <jointvel name="FL_hip_vel"     joint="FL_hip_joint"/>
    <jointvel name="FL_thigh_vel"   joint="FL_thigh_joint"/>
    <jointvel name="FL_calf_vel"    joint="FL_calf_joint"/>
    <jointvel name="RR_hip_vel"     joint="RR_hip_joint"/>
    <jointvel name="RR_thigh_vel"   joint="RR_thigh_joint"/>
    <jointvel name="RR_calf_vel"    joint="RR_calf_joint" />
    <jointvel name="RL_hip_vel"     joint="RL_hip_joint"/>
    <jointvel name="RL_thigh_vel"   joint="RL_thigh_joint"/>
    <jointvel name="RL_calf_vel"    joint="RL_calf_joint"/>

    <accelerometer name="Body_Acc" site="imu"/>

    <gyro name="Body_Gyro" site="imu"/>

    <framepos name="Body_Pos" objtype="site" objname="imu"/>

    <framequat name="Body_Quat" objtype="site" objname="imu"/>

</sensor>

`

yuvaltassa commented 1 year ago

Do your analysis with step response, not these smooth curves.

From what I can tell this is just the delay coming from the 2nd orderness of the physics, but very hard to judge

le-horizon commented 1 year ago

Make sense @yuvaltassa . I'll do step response.

In the mean time, by 2nd orderness of the physics, do you mean the delay between torque generated to position change? That's not what the plots suggest. The torque (orange) curves show a flat region at the first 8 control steps where there is already a difference between commanded position and current position of the joint. It's hard to imagine in such a case, the torque would be delayed by 8 control steps. Unless I misunderstand what you mean by 2nd orderness.

Also, under the same Kp and damping setup, the real robot picks up the change much faster. That means something is different between the two setups, the torque delay seems most suspicious so far. The setup is the quadruped starting from a prone position to stand up using a simple policy (linearly interpolating between current and target positions from t=0 to 100, 2 seconds real time).

yuvaltassa commented 1 year ago

In that case, could it not be that the delay you're seeing is just gravity opposing your commands until they are actually strong enough to make the robot start moving?

Can you retry with gravity disabled?

breakds commented 1 year ago

In that case, could it not be that the delay you're seeing is just gravity opposing your commands until they are actually strong enough to make the robot start moving?

Can you retry with gravity disabled?

Thanks for the prompt response, @yuvaltassa. I might have understood this wrong - I thought qfrc_acutator is the torque generated by the actuator (e.g. a motor) instead of the net torque, is this right? Or is it the net torque (motor generated torque + torque generated by the gravity and others)?

le-horizon commented 1 year ago

Even if I set gainprm and bias to 200 (from 50), the initial lag is still there and similarly about 8 steps long. It's just less smooth. If it is due to gravity, increasing gain should've shortened the lag:

image
yuvaltassa commented 1 year ago

Yes, qfrc_actuator is the torque output from the actuators, but there are all sorts of other torques on the RHS, and some of them (like joint friction) react instantaneously, opposing the actuator torque. So you won't see any motion, even though actuator torque is changing.

I strongly advise that you simplify your model (e.g. 1 leg, attached to the world) and try to understand what is happening in that setting, rather than asking questions of the complicated model.

breakds commented 1 year ago

Thank you for taking the time to explain qfrc_actuator and the factors affecting the torque output. I appreciate your patience and your insightful advice. We will try to do more experiments.

Apologize if our questions seemed to over-complicate the issue or if it seemed like we were bypassing efforts to understand the model in a more basic setting. Our intention was merely to seek some expert guidance as we are not as knowledgeable in control systems.

le-horizon commented 1 year ago

We discovered the reason for the mismatch. We have domain randomization in simulation, we turned off some, but did not turn off center of mass randomization, which causes the responses to be different from real.

Clean step response matches real quite accurately.

The qfrc_actuator being zero initially is still counter intuitive, but does not seem to affect our energy computation in any significant way, given that the speed is zero when torque is zero. There might be small differences in energy calculation, but the majority of the times, it seems to be correct.

Thanks for taking a look and the advices.