CODEWITHSUNDEEP
drake
Shubham Vyas
Shubham Vyas

Reputation: 165

Best method to add generalized forces to Linearized (or LQR) MultiBodyPlant?

I'm trying to add generalized forces on a free-floating system that has been linearized from a MultiBodyPlant. For a fixed base, the generalized forces basically correspond to the joint torques but for a free-floating case, it includes the base torques/forces as well. However, these are not included in the actuation port of the MultiBodyPlant system and hence while providing the actuation port for linearization (or LQR), the base wrenches are not included in the actuation matrix B post-linearization. For my work, I'd like to include the base torques/forces as well in the linearization. For this, I can think of 2 approaches in drake but I'm not sure which would be a better/correct decision architecturally. I plan to use this later for making a TVLQR controller to stabilize trajectories.

Idea 1: Change B matrix in LQR after linearization: Manually edit B matrix after using LinearQuadraticRegulator(system, context, Q, R, plant_actuation_input_pot())

Idea 2: Create a LeafeSystem class (similar to this) with output port connecting to get_applied_generalized_force_input_port() and input port exposed outside diagram (using ExportOutput()) and then linearized this system as a whole (containing the CustomLeafSystem connected to MultiBodyPlant). I assume doing this will add the base actuation to the B matrix of the linearized system.

Upvotes: 3

Views: 158

Answers (1)

Alejandro
Alejandro

Reputation: 1099

Actually these two ideas is what I would've done myself. For your case it'd seem that Idea 1 is simpler to implement? Matrix B maps actuation indexes to generalized velocity indexes. Therefore you have to make sure that you place the necessary "ones" in the right places of B. You can find the free floating body indexes with Body::floating_velocities_start() which returns the start index within the full state x (therefore you'd need to subtract MultibodyPlant::num_positions() for indexing within B). For floating bodies generalized velocities are angular velocity w_WB and translational velocity v_WB, in that order. Therefore the generalized forces corresponds to the torque t_Bo_W and f_Bo_W applied at B about its origin Bo and expressed in the world frame.

Upvotes: 2

Related Questions