On the Controlabillity of Linearized Free-Floating Systems

Question

I have been trying to get a linearized version of a MultiBodyPlant without gravity to experiment with LQR for systems without gravity. However, the linearization leads to some interesting phenomena. A simplified example can be found in this google colab notebook.

When I linearize and check the rank of the controllability matrix for a single free-floating rigid body, I get a rank of 6. This is expected as I use the get_applied_generalized_force_input_port() as the input port, hence making sure that all possible forces can be applied to the system. The system of a single rigid body has 6 degrees of freedom (DoF) and the rank of the controllability matrix is 6, hence it is controllable.

However, when I use Drake's in-built function IsControllable() to check the controllability, it results False meaning that it thinks the system is not controllable. In the source of the IsControllable() function, the rank of the matrix is checked against the number of rows in the A matrix. I think that this might be causing an issue as the linearization involves the use of quaternions during the AutoDiff (thus adding one more row to the A matrix due to 4 numbers being used for quaternions to represent the state). The linearization process does not know about the unit-quaternion constraint, and hence the A matrix for a system using quaternions will have 1 more row than the DoF of the system.

I wonder if this is the correct intuition for the controllability mismatch?

And could this cause issues as other functions within Drake that maybe use the IsControllable() function for verifying controllability?

Answer 1

I think IsControllable() is doing the right thing. If you have a single body with a floating base, then you have 13 state variables (7 positions, 6 velocities). If you were to simply linearize the equations, then you are right that the resulting linear system would not know about the unit quaternion constraint. Asking for controllability of this system would be asking you to drive the system to the origin (quaternion => 0 ~= unit quaternion). Since your dynamics model cannot achieve that, even in the linearization, I expect your system is not controllable in that linearization.

You could replace the quaternion floating base with a roll-pitch-yaw floating base. We have some API that will make that easier coming in https://github.com/RobotLocomotion/drake/issues/14949 . But in the mean time, you can add the three translations and a BallRpyJoint.

The alternative to look into the literature on control in SE(3) directly using quaternions. There are elegant results there, but linear analysis won’t help.