3

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.

Shubham Vyas
  • 163
  • 10

1 Answers1

2

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.

Alejandro
  • 1,064
  • 1
  • 8
  • 22
  • 1
    FWIW Here is an example PD controller on SE(3), which is just drawing from Greg Izatt's example: [`se3_pd_control_example.py`](https://github.com/EricCousineau-TRI/repro/blob/3d297d22117941d773a954547fc47f673987a111/drake_stuff/control_stuff/se3_pd_control_example.py) This is more-or-less what you and Alejandro discussed; you'd just abstract it such that you augment `u` and `y` / `x` with a flattened version of `[q; X_WB; v; V_WB]` and `[u; F_B_W]` and then encapsulate that logic in a Diagram and reflect that in `B`, as you outlined in your OP. – Eric Cousineau Dec 09 '20 at 22:51
  • @Alejandro : Indeed. Thanks for the `Body::floating_velocities_start()` function. It's a useful utility. The last step would then be to connecr the LQRController output to `get_applied_generalized_force_input_port()` instead of `get_actuation_input_port()`. @Eric Thanks for sharing the example. `FunctionSystem()` looks like a really useful tool for creating systems quickly in PyDrake. I don't see it in the drake Python/C++ documentation (yet?). I see the code for `FunctionSystem()` in the repo and I'll try to use it. – Shubham Vyas Dec 10 '20 at 16:30
  • Welcome! And it's still a bit of a messy prototype that I'm using in an internal project, but I'll see if I can migrate it soon! – Eric Cousineau Dec 11 '20 at 20:40
  • Thoughts on using the Propeller system for the free-floating base to add forces/torques? – Shubham Vyas Jan 11 '21 at 18:30