🐛 LQR not working as expected

[jc-bao]

Description

Run

cd quadjax/envs
python quad3d_free

The LQR performs as follows:

output.mp4

LQR configuration over here

Possible issue

The scale of the action (i.e. u) does not match.

Current u: thrust (0-0.8), torque(along body frame x,y,z axis) (max value is [9e-3, 9e-3, 2e-3]).

❓ Is this possibly caused by the u scale? Should we rescale u inside the dynamics?

[jc-bao]

After compensate gravity

• before thrust_normed = ((u[0]) / env_params.max_thrust) * 2.0 - 1.0
• after thrust_normed = ((u[0] + env_params.m * env_params.g) / env_params.max_thrust) * 2.0 - 1.0

Performance:

output.mp4

[GuanqiHe]

It seems that the linearized dynamics is not correct. Here is some test I did:

I change the body frame velocity to world frame for easy verification. I observed that the structure of A matrix is:

1.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0200,0.0000,0.0000,0.0000,0.0000,0.0000
0.0000,1.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0200,0.0000,0.0000,0.0000,0.0000
0.0000,0.0000,1.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0200,0.0000,0.0000,0.0000
0.0000,0.0000,0.0000,1.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0100,0.0000,0.0000
0.0000,0.0000,0.0000,0.0000,1.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0100,0.0000
0.0000,0.0000,0.0000,0.0000,0.0000,1.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0100
0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,1.0000,0.0000,0.0000,0.0000,0.0000,0.0000
0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,1.0000,0.0000,0.0000,0.0000,0.0000
0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,1.0000,0.0000,0.0000,0.0000
0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,1.0000,0.0000,0.0000
0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,1.0000,0.0000
0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,1.0000

A is a pure integral chain for position/attitude and velocity and indicates that translation and rotation are decoupled for this model at origin, which is impossible for the underactuated quadrotor system. So I think the linearization is not correct.

[jc-bao]

Got it. Thanks for checking it out.

As for comparison, here is a matrix when I run julia version code:

A from julia

1.0,0.0,0.0,0.0,0.0,0.0,0.02,0.0,0.0,0.0,0.0,0.0
0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.02,0.0,0.0,0.0,0.0
0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.02,0.0,0.0,0.0
0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.01,0.0,0.0
0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.01,0.0
0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.01
0.0,0.0,0.0,0.0,0.3924,0.0,1.0,0.0,0.0,0.0,0.0,0.0
0.0,0.0,0.0,-0.3924,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0

A from our code:

1.000,0.000,0.000,0.000,0.004,0.000,0.020,0.000,0.000,0.000,0.000,0.000
0.000,1.000,0.000,-0.004,0.000,0.000,0.000,0.020,0.000,-0.000,0.000,0.000
0.000,0.000,1.000,0.000,0.000,0.000,0.000,0.000,0.020,0.000,0.000,0.000
0.000,0.000,0.000,1.000,0.000,0.000,0.000,0.000,0.000,0.010,0.000,0.000
0.000,0.000,0.000,0.000,1.000,0.000,0.000,0.000,0.000,0.000,0.010,0.000
0.000,0.000,0.000,0.000,0.000,1.000,0.000,0.000,0.000,0.000,0.000,0.010
0.000,0.000,0.000,0.000,0.392,0.000,1.000,0.000,0.000,0.000,0.002,0.000
0.000,0.000,0.000,-0.392,0.000,0.000,0.000,1.000,0.000,-0.002,0.000,0.000
0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,1.000,0.000,0.000,0.000
0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,1.000,0.000,0.000
0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,1.000,0.000
0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,1.000

[jc-bao]

I see the reason behind. I am using RK-4 for iteration so the attitude actually will entangled with the position. Using forward euler should resolve the issue. Once unify the iteration method of different method, the linearized dynamics should be the same.

[jc-bao]

By comparing K matrix, we might identify our problem as a wrong LQR solver issue:

[jc-bao]

The two control matrix does not has a large difference. While loading the K from the python script to julia, it works fine. But the same K does not work for python:

julia.mp4

python.mp4

Next step: test the input-ouput of the controller from two script to verify if the controller is correct.

[jc-bao]

Julia x->u

([0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 1.0, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], 
[-0.21955000000000008, 0.013699999999999997, -0.1315, -0.41490000000000005])

Python x->u

x:  [0.1 0.2 0.3 0.  0.  0.  0.4 0.5 0.6 0.7 0.8 0.9]
u:  [-1.0878552   0.01377017 -0.13981518 -0.48597685]

[jc-bao]

Problem resolved

for env.step: all action should be [-1,1] (normalized)
for dynamics: all action should be the real value (represented with dataclass)
for controllers: all action should be normalized.

Our issue mainly lies in dynamics we design a dynamics with normalized aciton input. But the input action is not normalized.
Now, all things related to dynamics should be real value for convension.

[jc-bao]

meshcat_1694195028117.tar.mp4