Lack of a good way to use InverseDynamicsController for floating base robot under certain conditions

[huihuaTRI]

The InverseDynamicsController(IDC) in Drake requires the passed-in multibody plant is fully actuated. See issue 13718 for details. When working with floating base robots (such as mobile robots), the general workaround is to create a plant with the base welded to the ground to get a fully actuated plant (assuming the upper body is fully actuated). Then the user could pass this plant for the controller.

This workaround would perform just fine for floating-base robots if the movement of the robot base only rotates around the gravity vector. For example, a mobile robot only moves on a plane that is parallel to the ground, which the gravity vector is perpendicular to.

Otherwise, the controller would degrade if the root link (i.e. the welded link) of the actual floating-based robot starts to deviate from the gravity vector. This deviation would cause discrepancy and allow gravity to act differently on the controller plant and the actual simulation plant.

Using a 2D inverted pendulum cart as an example. The cart moves along the circle in the xz plane in the cart frame. The pendulum is connected to the cart with a revolute joint that rotates around the body y axis. Note that, the world frame is denoted with capital XYZ. Using the IDC for the pendulum would be overkill, but we use it here for the purpose of problem illustration.

Firstly we have to create a controller plant with the cart welded to the ground. Once the controller plant has been created, the plant will ignore the actual base movement and calculate the control effort only based on the pendulum angle \theta. As a result, the control effort coming out from the IDC in the two different cart positions (as shown in the pic) would be the same. However, the required torques to keep the pendulum at the same angle in the two positions are clearly different (due to that the angle between the pendulum and the gravity is different). Note that, we assume static case for simplicity and not worry about how the cart can hold its position there on the top. In summary, the IDC is not able to do its job when the cart base rotates around an axis that is not parallel to the gravity vector.

This issue is easy to encounter when working with a floating based robot, but hard to debug. The symptom is that the IDC would perform just fine sometimes, but not other times. Currently, there is no clear solution yet. One possibility is to use the joint locking feature (@joemasterjohn to add a link) instead of welding. We then update the locking position of the free base joints in every control update iteration.

Another related meta issue is 12203, which tries to make the IDC more user friendly.

[sherm1]

Thank you @huihuaTRI for an awesome issue description!

[jwnimmer-tri]

If I'm understanding this correctly, I think you don't have to wait for joint locking. You could still weld, but then update the FixedOffsetFrame parameter in the Context (#14137) during InverseDynamics<T>::SetMultibodyContext, where you're already copying the positions over. You still have the submodel / plumbing problems from #12203 of knowing what pose to update (in the ID context) from the input port (the actual floating position), but I think MbP at least is not in your way.

[huihuaTRI]

That's new to me. In that case, I need a custom wrapper around the IDC so that I can control every iteration. By "control" I mean updating the IDC's context using FixedOffsetFrame. Is my understanding correct?

[jwnimmer-tri]

https://github.com/RobotLocomotion/drake/blob/bfed6278b5d3fdbb44abf464a4a7eef77b1fe9af/systems/controllers/inverse_dynamics.cc#L80-L91

This is where the positions are copied from the controller's input port to the inner plant's context, anytime the value on get_input_port_estimated_state() changes. You have full mutable access to the inner context (named multibody_plant_context above) here, so can update the parameters.

[huihuaTRI]

As long as we have a separate control plant that is welded, the state of the base will not be seen in the get_input_port_estimated_state(), i.e., the port only communicates the state of the pendulum. That's why you need a wrapper to explicitly update the inner plant's context. The wrapper will take in the state of the base, which will be used to update the fixed frame.

[jwnimmer-tri]

The IDC input is used both for the PID control and for the ID input. For this case, within the IDC, you need to demux the IDC input so that floating joint state only goes into the ID input, and not the PID part. That's what I meant by "You still have the submodel / plumbing problems from #12203." The point holds that MbP has everything you need (I think).

[huihuaTRI]

Had a f2f conversation with Jeremy to clear the discussion. What Jeremy suggests is that the current MBP has the capability to achieve the goal via the SetPoseInBodyFrame(). We don't need to wait for the "fix joint" functionality. However, it's the IDC that lacks the flexibility/capability to allow the user to pass in the base frame pose to update the controller plant context.

It would be a bigger project to update the IDC in Drake to support that. The plan is to have a customized IDC as a bandit to solve the problem. Once we prove it works, we can make it reusable.

Note that, the capability to fix joints would also be a direction to make IDC easier to use. Potentially, we can totally get rid of the requirement of the controller plant by just passing the simulation plant and the information about which joints to fix.

[EricCousineau-TRI]

EDIT: Nevermind. You're asking how to not do fully-acuated control :facepalm: Sorry!

---

The other option is to wire in information to IDC that enables it to wire in "thrusters" for floating bases.

I believe @amcastro-tri had talked about something like this before; also, didn't you talked with @siyuanfeng-tri about this in another issue?

Adding thrusters isn't great, but not that hard? Then you can either hack in PD control around the quaternions, rpy (but suffer gimbal lock / a non-vector space stuff), or just wire in some sort of floating base control, like this: https://github.com/EricCousineau-TRI/repro/blob/900fc6b5e8997404d0110ac9167a051479e79f94/drake_stuff/control_stuff/se3_pd_control_example.py

[huihuaTRI]

Thanks for the info. We did discuss a similar option by providing an extra input port of the IDC to tell the base position. If the IDC detects if this input port has value, it should update the welding base frame pose (a new constructor may be needed to tell which frame to update). Similar idea, but for fully-actuated systems.

[EricCousineau-TRI]

Gotcha, so use the welding to assume the frame is inertial (so some loss of accuracy). I guess when you start masking stuff, you'd also want to feed in the velocity (SetFreeBodyVelocity or whatevs) so ID is a bit more accurate?

(FWIW This is related in spirit to @calderpg-tri's and @avalenzu //planning setup in Anzu regarding x_active and x_passive.)

[huihuaTRI]

Yep, more accuracy would also require the velocity. But I figure for a controller plant with a welded inertial frame, it is something we have to sacrifice. A more favorable approach would be just passing the sim plant (and the corresponding state) to the IDC. The users may provide a list of joints or model instance to control. The IDC should handle everything (via joint locking or model decomposing) inside the IDC.

[amcastro-tri]

The other option is to wire in information to IDC that enables it to wire in "thrusters" for floating bases. I believe @amcastro-tri had talked about something like this before;

I don't recall that.

I feel like we are getting distracted by system:: wiring. What about the math? what exactly would you like to accomplish? Are the dynamics introduced by an accelerating base need to be considered? or can we assume a quasi-static base?

In any case, the math I would lay out would be quite simple. I'd just describe the fully actuated arm in the non-inertial frame of the base. In that frame, lets call it A, the actuated arm would always move on the Ax-Az plane (according to the simplified description at the top). The only thing that'd you need to change here, is to add the properly oriented gravity vector in frame A and, if important, the fictitious forces due the acceleration of the cart.

There is an assumption in this model: the motions of the fully actuated arm do not affect the motion of the base (say the base is way too massive when compared to the arm).

Is this what you'd want? I can see how we could add this option to the IDC. Essentially an option to perform inverse dynamics control "when in an non-inertial frame". You'd need an additional input port with the spatial velocities and accelerations of the (non-inertial) base frame.

[amcastro-tri]

Also, I believe the proposal from @jwnimmer-tri, is what I describe as a "quasi-static base" approximation. Two assumptions: 1) the base is so massive that arm motions do not affect the motions of the base, 2) the motions of the base are so slow that accelerations of the base do not affect the arm's dynamics.

I like the idea of changing the fixed offset frame of the base (this would be equivalent to describing the "rotated" arm in the world frame W).

Similarly, you could change the orientation of the gravity vector (this would be equivalent to describing the arm in frame A above).

Two math options, same effect I believe. Would that math solve your problem?

[huihuaTRI]

Thanks for the discussion @amcastro-tri. You are absolutely right. For my problem (also when I submitted this issue), I was thinking about a "quasi-static base", which is much simpler than a non-inertial frame.

If we want to consider all the dynamics of the non-inertial base, I would assume that given the actual simulation plant and the full state, it would be enough? That being said, from the system:: wiring perspective, we would definitely want this direction.