🐛 LQR not working as expected
jc-bao opened this issue · comments
Description
Run
cd quadjax/envs
python quad3d_freeThe 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?
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
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.
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
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.
By comparing K matrix, we might identify our problem as a wrong LQR solver issue:
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.
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]
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.