Discrepancies between MATLAB and MuJoCo for second-order discrete controllers

[wjnorton]

Intro

Hi!

I am a graduate student at MIT, I use MuJoCo for my research on robotics.

My setup

I'm using MuJoCo 4.0 in python 3.15.2, on Ubuntu 22.04.5 LTS

My question

I've been using MuJoCo to simulate a DC motor, and comparing the step response of various closed loop position controllers for it to the responses for the equivalent models in MATLAB. The responses match nearly identically for first order controllers, but whenever a second order controller is used, the step response in MuJoCo differs significantly from the MATLAB model, looking significantly more damped. Do you have any idea why this would be, and what I can change in my MuJoCo xml or script to better model these systems?

Minimal model and/or code that explain my question

I'm modelling a DC motor with the parameters:

Parameter	Value
J, rotor inertia	3.22e-6 kg*m^2
b, motor viscous friction	3.51e-6 Nms
K, motor constant	0.0274 V/rad/sec
R, winding resistance	4 ohms
L, winding inductance	2.75e-6 H

This matches the parameters used in this MATLAB tutorial

In MATLAB, this motor is represented by the discrete transfer function:

0.001038 z + 0.001021
  ----------------------
  z^2 - 1.943 z + 0.9425

In MuJoCo, I'm using the following xml for the motor:

DC motor XML

<mujoco>
    <option timestep="0.001" integrator="Euler"/>
    <worldbody>
	<body pos="0 0 0">
	    <joint name="motor_joint" pos="0 0 0" axis="0 1 0" frictionloss="3.51e-06"/>
	    <inertial pos="0.0 0.0 0.0" mass="1.00e-12" diaginertia="1.00e-12 1.00e-12 1.00e-12"/>
        </body>
    </worldbody>
    
    <actuator>
	<dcmotor name="motor" joint="motor_joint" resistance="4" inductance="0.00000275"
	         motorconst="0.0274" armature="0.00000322"/>
    </actuator>
    
    <sensor>
        <jointpos name="motor_angle" joint="motor_joint" />
    </sensor>
</mujoco>

My code for implementing discrete controllers is as follows, where the only change between controllers is the order constant and the difference equation used to update d.ctrl[0]

Code:

DC motor python simulation

    
    import time
    import math

    import numpy as np

    import mujoco
    import mujoco.viewer

    from collections import deque
 
    m = mujoco.MjModel.from_xml_path('dc_motor_sim.xml')
    d = mujoco.MjData(m)

    es = deque()
    us = deque()
    order = 4
    Ref = 1

    for i in range(order+1):
        es.append(0)
        us.append(0)

    with mujoco.viewer.launch_passive(m, d) as viewer:        
         while viewer.is_running() and d.time < 0.5:
             step_start = time.time()
             mujoco.mj_step(m,d)
        
            
             p_angle = d.sensor('motor_angle').data.astype(float)[0]
             e = Ref - p_angle 
             u = us[-1] + 0.5306*e - 0.5285*es[-1]
             d.ctrl[0] = u
             viewer.sync()
             print(p_angle)
             es.append(e)
             us.append(u)
            
             time_until_next_step = m.opt.timestep - (time.time() - step_start)
            
             if time_until_next_step > 0:
                 time.sleep(time_until_next_step)

The above code's difference equation implements the discrete controller:

0.5306z - 0.5285
----------------
z - 1

The MuJoCo and MATLAB step responses for the controller match closely, as seen below. This is the case for all first order controllers:

But when a second order controller is implemented, such as:

u = us[-1] + 13.75*e -26.98*es[-1] + 13.23*es[-2]

equivalent to:

13.75z^2 - 29.98z + 13.23
------------------------------------
z - 1

The MuJoCo response is markedly different from the MATLAB response, appearing much more damped (see below)

Do you have any intuition as to why this behaviour occurs?

Confirmations

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

[tw4]

Hi @wjnorton,

I ran your model with MuJoCo 4.0.0 and compared it against an exact ZOH discretization of the same motor (K/(s((Js+b)(Ls+R)+K²)), Ts = 1 ms, scipy cont2discrete) in closed loop with your two difference equations. MuJoCo matches it for both controllers:

	exact discrete model	MuJoCo, your XML + loop
1st order: peak	1.170 @ 170 ms	1.171 @ 166 ms
2nd order: y(50 ms)	0.639	0.646
2nd order: y(100 ms)	0.835	0.840
2nd order: y(500 ms)	1.000	0.999

So the slow, overdamped response is what this plant does with u[k] = u[k-1] + 13.75 e[k] - 26.98 e[k-1] + 13.23 e[k-2]. The difference has to be on the MATLAB side, i.e. the MATLAB controller isn't the same as the code. Two things stand out:

• the TF you wrote has -29.98, the code has -26.98. With -29.98 the loop is unstable, both in the discrete model and in MuJoCo. With -26.98 the numerator is ≈ 0 at z = 1, so it nearly cancels the integrator.
• (13.75z² - 26.98z + 13.23)/(z - 1) is improper. The difference equation is (13.75z² - 26.98z + 13.23)/(z² - z), so that's what to enter in MATLAB to compare like with like.

Also, while I was at it, two things in the MuJoCo setup (neither explains the gap, both are tiny here):

• frictionloss is dry (Coulomb) friction ("Friction loss due to dry friction" in the XML reference). The viscous b of the tutorial is damping="3.51e-06".
• after mj_step, sensordata belongs to the state at the start of that step (mj_step = mj_forward then the integrator), so your controller sees the angle one step late. The docs call it "outdated by one time step". To match a discrete design exactly:

mujoco.mj_step1(m, d)          # positions, velocities, sensors for the current state
e = Ref - d.sensor('motor_angle').data[0]
d.ctrl[0] = ...                # your difference equation
mujoco.mj_step2(m, d)          # integrate with that control

With 1 ms steps and this plant, both changes only move the third decimal: with damping and mj_step1/mj_step2 I get 0.640 at 50 ms and 0.833 at 100 ms for the 2nd-order controller.

Hope that helps!

[wjnorton]

Thank you kindly @tw4 for your quick response!

I apologize for the typos: the TF was indeed supposed to have -26.98 instead of -29.98, and the denominator should have had the proper form as you have written it.

Do you know where in my process the discrepancy between the MATLAB and MuJoCo models is introduced? I have been tuning the controller in MATLAB first, and then transferring to MuJoCo via the below process (note this has different gains, as I was unable to find the file with the original gains, but is the same sort of controller (Backwards Euler PID):

1. Tune controller in MATLAB

For this comparison, I get the controller:

C_pid_be = 

             Ts*z           z-1 
  Kp + Ki * ------ + Kd * ------
              z-1          Ts*z 

  with Kp = 0.539, Ki = 1.75, Kd = 0.00672, Ts = 0.001

>> tf(C_pid_be)

ans = 

  7.257 z^2 - 13.97 z + 6.717
  ---------------------------
            z^2 - z

2. Get closed-loop step response for plant mentioned above:

>> dP_motor

dP_motor = 

  0.001038 z + 0.001021
  ----------------------
  z^2 - 1.943 z + 0.9425
 
Sample time: 0.001 seconds

>> sys_cl_be = feedback(C_pid_be * dP_motor,1);
>> [x,t] = step(sys_cl_be, .5);
>> stairs(t,x) 
>> writematrix(x,'backwards_euler.csv');

3. implement the controller in MuJoco:

This is done for C_pid_be using the difference equation:

u = us[-1] + 7.257*e -13.97*es[-1] + 6.717*es[-2]

When I compare the graphs of the two responses, they do not match (in this case, there is LESS damping on the MuJoCo response):

Can you see in this process where the error is introduced?

[tw4]

No worries! Found it. It's step 3: the coefficients you copied are the ones MATLAB prints, which are rounded to 4 significant digits. That matters a lot here, because the integral part of the controller is the tiny difference between those numbers.

For a backward-Euler PID over z² - z, the exact numerator is

(Kp + Ki*Ts + Kd/Ts) z²  -  (Kp + 2*Kd/Ts) z  +  Kd/Ts
= 7.26075 z² - 13.979 z + 6.72

The coefficients sum to Ki*Ts = 0.00175. That's the integral gain. The printed 7.257 - 13.97 + 6.717 sums to 0.004, so the controller you ran in MuJoCo has more than twice the integral gain, and Kp/Kd are a bit off as well.

I ran both through the exact discretized motor and through MuJoCo 4.0 with your XML:

	peak	y(100 ms)	y(200 ms)
exact coeffs, discrete model	1.116 @ 225 ms	0.953	1.113
exact coeffs, MuJoCo	1.114 @ 224 ms	0.960	1.112
printed coeffs, discrete model	1.217 @ 179 ms	1.054	1.211
printed coeffs, MuJoCo	1.216 @ 177 ms	1.061	1.208

So MuJoCo agrees with MATLAB once both run the same controller. With the rounded numbers you get exactly the "less damping" you see.

To get the full-precision values out of MATLAB:

[num, den] = tfdata(C_pid_be, 'v');
fprintf('%.10g\n', num)

or compute them from Kp, Ki, Kd and Ts in Python with the formulas above. The same goes for dP_motor: its displayed coefficients are rounded too, and with those 4 digits the plant's integrator pole lands at ~1.0005 instead of 1. So never rebuild a model from the printed numbers either.

[wjnorton]

Thank you kindly!