🐛 LQR controllability issue

[jc-bao]

The controllability matrix of a linear 3d free quadrotor is 8 < 12.

• How to replicate:

cd quadjax/envs/envs
python quad3d_free.py

• Code link

[jc-bao]

Compare the A, B matrix with the original one.

Compare the linearized dynamic with others' implementation:

# compare two matrix values and highlight the difference
for i in range(A_ours.shape[0]):
    for j in range(A_ours.shape[1]):
        if np.abs(A_ours[i][j] - A_zac[i][j]) > 1e-5:
            print('A_ours[{}][{}] = {}, A_zac[{}][{}] = {}'.format(i, j, A_ours[i][j], i, j, A_zac[i][j]))

Now we get the result:

A_ours[0][3] = -0.024525000000000005, A_zac[0][3] = 0.0
A_ours[0][7] = 0.05, A_zac[0][7] = 0.019999997690320015
A_ours[0][9] = -0.00020437500000000005, A_zac[0][9] = 0.0
A_ours[1][8] = 0.05, A_zac[1][8] = 0.019999997690320015
A_ours[2][9] = 0.025, A_zac[2][9] = 0.009999998845160007
A_ours[3][10] = 0.025, A_zac[3][10] = 0.009999998845160007
A_ours[4][11] = 0.025, A_zac[4][11] = 0.009999998845160007
A_ours[5][4] = 0.981, A_zac[5][4] = 0.0
A_ours[5][10] = 0.012262500000000003, A_zac[5][10] = 0.0
A_ours[6][3] = -0.981, A_zac[6][3] = 0.0
A_ours[6][9] = -0.012262500000000003, A_zac[6][9] = 0.0

--> issue located in wrong dynamic function.

[jc-bao]

check out functions

def qtoQ(q: jnp.ndarray) -> jnp.ndarray:
    '''
    covert a quaternion to a 3x3 rotation matrix
    '''
    T = jnp.diag(jnp.array([-1, -1, -1, 1]))
 ---> H = jnp.vstack((jnp.eye(3), jnp.zeros((1, 3)))) # used to convert a 3d vector to 4d vector
    Lq = L(q)
    return H.T @ T @ Lq @ T @ Lq @ H

In our case quaternion is xyzw while other people tend use wxyz. By fix the matrix, now the problem is resolved.

🔔Be careful with quaternion!