[Bug] Singularities in control allocation's pseudo inverse

[Donecle]

Describe the bug

While trying some new quad frame geometry parameters today, we had a bad crash at takeoff (2 front motors full throttle and 2 back motors idle).

To Reproduce

In simulation,

1. choose any quad frame, e.g. iris
2. set CA_ROTOR* parameters as in the flight log below
3. print the result of the pseudo-inverse

Expected behavior

The geometry in the log is quite common and should result in a correct mixing matrix.

Screenshot / Media

No response

Flight Log

https://logs.px4.io/plot_app?log=4679a30d-9709-4950-b57f-9b47603b4fa3

Software Version

main, on August 23rd, 2023

Flight controller

px4_sitl

Vehicle type

Multicopter

How are the different components wired up (including port information)

Default

Additional context

No response

[bresch]

@Donecle Could you please show the params and resulting control allocation matrix directly here (copy or screenshot)?

[Donecle]

Sure.

x CA_ROTOR0_AX [123,242] : -0.0612
x CA_ROTOR0_AY [124,243] : 0.0000
x CA_ROTOR0_AZ [125,244] : -1.0000
x + CA_ROTOR0_PX [128,247] : 0.2410
x CA_ROTOR0_PY [129,248] : 0.2380
x CA_ROTOR1_PX [152,274] : -0.2240
x CA_ROTOR1_PY [153,275] : -0.2390
x + CA_ROTOR2_PX [160,283] : 0.2410
x + CA_ROTOR2_PY [161,284] : 0.2380
x CA_ROTOR3_PX [168,292] : -0.2240
x CA_ROTOR3_PY [169,293] : -0.2390

EFFECTIVENESS:

0|-1.52425 1.55350 -1.54700 1.55350
1| 1.56357 -1.45600 1.56650 -1.45600
2| 0.41893 0.32500 -0.32500 -0.32500
3|-0.39723 0 0 0
4| 0 0 0 0
5|-6.48785 -6.50000 -6.50000 -6.50000

MIX:

0|-4.6e+02 -5.3e+02 5.2e+02 -4.6e+01 0 -1.8e+04
1|-4.4e+02 -5.2e+02 5.2e+02 -4.2e+01 0 -1.8e+04
2|-4.0e+02 -4.8e+02 5.2e+02 -3.6e+01 0 -1.8e+04
3|-4.1e+02 -4.9e+02 5.2e+02 -4.5e+01 0 -1.8e+04

[bresch]

If I understand correctly, you have 2 pairs of coaxial rotors, which is quite weird. Maybe check if it fails at the pseudo-inverse level or if it's due to the normalization.

[Donecle]

Apologies, copy paste issues for the parameters, here are the correct ones:
x CA_ROTOR0_AX [123,242] : -0.0612
x CA_ROTOR0_AY [124,243] : 0.0000
x CA_ROTOR0_AZ [125,244] : -1.0000

x CA_ROTOR0_PX [128,247] : 0.2410
x CA_ROTOR0_PY [129,248] : 0.2380
x CA_ROTOR1_PX [152,274] : -0.2240
x CA_ROTOR1_PY [153,275] : -0.2390
x CA_ROTOR2_PX [160,283] : 0.2410
x CA_ROTOR2_PY [161,284] : -0.2380
x CA_ROTOR3_PX [168,292] : -0.2240
x CA_ROTOR3_PY [169,293] : 0.2390

Thanks for your help!

[Donecle]

Nevermind, this is still not enough to reproduce, I'll give you the correct set later today.

[bresch]

@Donecle Maybe you could add your case in an unit test to debug it more easily if you can reproduce it: https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

[Donecle]

Mmh I've never done this but I could try. Here is a set of param to change in startup script to have a failure:

param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.241
param set-default CA_ROTOR0_PY 0.238
param set-default CA_ROTOR0_AX -0.0616628
param set-default CA_ROTOR0_AY 0.0616628
param set-default CA_ROTOR0_AZ -0.996195
param set-default CA_ROTOR0_CT 36.0
param set-default CA_ROTOR0_KM 0.001632
param set-default CA_ROTOR1_PX -0.224
param set-default CA_ROTOR1_PY -0.238
param set-default CA_ROTOR1_AX 0.0616628
param set-default CA_ROTOR1_AY -0.0616628
param set-default CA_ROTOR1_AZ -0.996195
param set-default CA_ROTOR1_CT 36.0
param set-default CA_ROTOR1_KM 0.001632
param set-default CA_ROTOR2_PX 0.241
param set-default CA_ROTOR2_PY -0.238
param set-default CA_ROTOR2_AX -0.0616628
param set-default CA_ROTOR2_AY -0.0616628
param set-default CA_ROTOR2_AZ -0.996195
param set-default CA_ROTOR2_CT 36.0
param set-default CA_ROTOR2_KM -0.001632
param set-default CA_ROTOR3_PX -0.224
param set-default CA_ROTOR3_PY 0.238
param set-default CA_ROTOR3_AX 0.0616628
param set-default CA_ROTOR3_AY 0.0616628
param set-default CA_ROTOR3_AZ -0.996195
param set-default CA_ROTOR3_CT 36.0
param set-default CA_ROTOR3_KM -0.001632

EFFECTIVENESS:
| 0 | 1 | 2 | 3
0|-8.53174 8.53174 8.53174 -8.53174
1| 8.63933 -8.02966 8.63933 -8.02966
2| 1.12184 1.08410 -1.12184 -1.08410
3|-2.21985 2.21985 -2.21985 2.21985
4| 2.21985 -2.21985 -2.21985 2.21985
5|-3.6e+01 -3.6e+01 -3.6e+01 -3.6e+01
MIX:
| 0 | 1 | 2 | 3 | 4 | 5
0|-0.07477 -6.8e+03 0.22655 -1.2e+04 -0.37924 -5.3e+06
1|-0.02525 -7.1e+03 0.22655 -1.3e+04 -0.41406 -5.3e+06
2|-0.02083 -6.8e+03 -0.22678 -1.2e+04 -0.39331 -5.3e+06
3|-0.08107 -7.1e+03 -0.22678 -1.3e+04 -0.39950 -5.3e+06

[Donecle]

Here's a flight log on this setup : https://logs.px4.io/plot_app?log=86e11219-3fcc-46c5-8068-f377ecfd2419

[Donecle]

Looks like the fullRankCholesky function founds a rank of 5 instead of 4 on this param set.

[Donecle]

If I force 10 times higher value here : https://github.com/PX4/PX4-Autopilot/blob/main/src/lib/matrix/matrix/PseudoInverse.hpp#L83, it founds a rank of 4 as it should. This looks like a numerical issue.