MBP input port for applied forces

[amcastro-tri]

Description

Users have been requesting ways to apply external forces to bodies of an MBP model. With our systems:: framework, that means we need an input port. This issue is to open the discussion on what exactly that input port should be/look like.

Users have expressed their need to:

• Apply (spatial) forces on arbitrary bodies.
• Ability to specify the point of application.
• Ability to specify the "expressed-in" frame (typical uses W, world, and B, body).

We could have an API like so:

MultibodyPlant<double> plant;
// Load model from SDF, add elements programmatically....
const auto& on_body = plant.GetBodyByName("body_to_apply_force_on");
plant.RegisterAppliedForce(on_body);
plant.Finalize();

// Connect to system applying the external force
builder.Connect(forcing_system.force_port(), plant.get_external_forces_port());

I was thinking the most general port for external forces would be of type std::vector<SpatialForce<T>> with one entry per call to RegisterAppliedForce(). The port would then expect forces at body frame origin Bo, expressed in world W. Whoever applies the force has the job of shifting and re-expressing as needed (SpatialForce allows you to Shift() and multiply by a rotation matrix to re-express, so it should be easy for users to do this).

What do people think abut this? any other ways to do it?

cc'ing @rcory, @RussTedrake

[sherm1]

I think the case of applying a force to a point will be the most common need, and doing the shift to body origin might be awkward from outside MBPlant since it is Context dependent. I would like to see the interface support that common case conveniently, allowing the user to provide a force vector in W and apply it to a point of a body with the point given in B.

One way to handle that would be to accept a SpatialForce and an application point so that torques could be added also if needed (likely rare). Setting the point to (0,0,0)_B recovers exactly the semantics you proposed above.

[rcory]

I agree with @sherm1. Providing the user a way to specify a force vector and a point of application would be much nicer. Having to worry about computing the shift on the user side doesn't really provide much utility, and seems like it could be more prone to errors.

[amcastro-tri]

so the API you propose would look something more like:

plant.RegisterAppliedForce(on_body, p_BQ);

to register an entry in the input port of applied forces (of type std::vector<Vector3<T>>, with one entry per call to RegisterAppliedForce()), where each entry only contains a force (3D vector) f_Q_W.

Is that right?

[RussTedrake]

I think that there are (at least) two important use cases.

1. Apply a spatial force to a particular body. This could be used to, e.g., simulate a push disturbance or add some new type of force element (like an unconventional spring, or actuator). Another good example from before is adding aerodynamic forces to a lifting surface.

Your proposed API almost suits for that, but I would think to tweak it slightly as

port_id = RegisterAppliedSpatialForce(on_body, frame_on_body)

note the few suggested changes here. "Force"=>"SpatialForce" or "Wrench". "p_BQ" to a 6dof transform. And I would definitely think you'd want a distinct input port for each of the registered forces, instead of concatenating them into a single vector?

but there is another important use case that has come up...

2. for, e.g., implementing a new contact model outside of MBP, the system will not know apriori which body the model should be applying forces, too. So I would think that the input port in this case would be declared more generically, and the input data type would be not just a spatial force, but a spatial force + body (+ frame_on_body?).

[sherm1]

Russ, can you clarify why you want a frame rather than a point? Are you thinking you would want the applied SpatialForce F expressed in that frame? I haven't seen that use case much -- usually it is much easier to express F in W and apply it at a point on the body. Since the bodies are rigid the torque component of F has the same effect for any frame fixed to the body.

A fully-general external force input would be F(body,point,wrench).

[amcastro-tri]

I’m also confused by the frame_on_body part.

But let me also throw in there another option we talked with @RussTedrake just recently. The case where the input port also supplies the body on which the force is applied. That is, the body is not provided at a “registration time” but it could change dynamically trough the output of some other system (I think this is the “contact” application @RussTedrake is thinking about, correct me if I’m wrong). In that case the MBP input port type would be some sort of “pair” or struct holding not only the applied forces (and presumably the applied point?) but also the body (most likely BodyIndex).

We could probably provide both services. Starting with the fixed body at registration for immediate use.

[RussTedrake]

Yes -- @amcastro-tri you understood me correctly. @sherm1 -- Good point about the rigid bodies. I'm aok with the point instead of frame.

[sherm1]

Yes, that's what I meant by F(body,point,wrench). If we have a port that accepts triples (body_index_B, p_BQ_B, F_WQ_W) then an outside force can do anything.

[amcastro-tri]

Thanks you @sherm1 and @RussTedrake. So lets see, this would look like:

force_port_id = RegisterAppliedSpatialForce();

creating a new port with a data type to represent this "triplet" type (bodyB_index, p_BQ_B, F_Q_W). did I get that right?

[sherm1]

:+1: and thanks for fixing my notation!

[sherm1]

Let's raise the priority on this -- we have outside users who need it, for example to implement a vacuum model. I believe all the internal pieces are there and we just need to hook up a port, right @amcastro-tri ?

[edrumwri]

A few months ago, I offered to @amcastro to put this into place (I completed a related prototype). Will start work on this now.

[edrumwri]

Following up on the discussion above (pinging @RussTedrake): do we actually want these three values coupled together (precluding AutoDiff'ing through this abstract value port)? @sherm1 and I discussed a few possibilities for passing this data; each candidate seems to possess some undesirable traits. The abstract-valued port seems like the best option if we can neglect the AutoDiff.

[jwnimmer-tri]

I don't yet see why the derivatives either cannot be expressed, or cannot be usefully populated or propagated? If we had Value<AppliedSpatialForceInfo<T>> as the abstract input port type (a copyable class with our three fields as members), then I'd expect that the MultibodyPlant<AD> would use Value<AppliedSpatialForceInfo<AD>> as the port type, which means that all of the computations could use, e.g., Vector3<AD> AppliedSpatialForce<AD>::p_PQ_B() const; as the point of the applied force, etc. etc.? Nothing says that the erased type underling an abstract port needs to remain invariant across transmogrification, does it?

[edrumwri]

Excuse the confusion on our end. We'd both heard an old wive's tale that you can't AutoDiff through an abstract value. Clearly that's not true!

[jwnimmer-tri]

There's a kernel of truth in that, though. When you have abstract state for example, it's very difficult to establish partial derivatives with respect to that state. (You can't do it generically.) In this case it's that we have an input (or output) port that allows us a bit more leeway.

[sherm1]

If your abstract state variable were templatized in the same way, x=Value<MyHiddenNumericalType<T>> or whatever, is there any reason we can't calculate derivatives w.r.t. x when T=AutoDiffScalar?

[sherm1]

Oh, I think transmogrifying the context might be where this fails.

[sherm1]

The method Context<T>::SetTimeStateAndParametersFrom(const Context<double>& source) would fail while copying the abstract state unless it is OK to assign AutoDiff<T> = AutoDiff<double>. We could certainly make that work but I'd be surprised if it does now.

[RussTedrake]

I think that we'll want autodiff, etc, support eventually (so don't want to architect ourselves out of it). but i think we can unlock a lot of important use cases with double-only for the first pass.

(Is that what you needed from me?)

[edrumwri]

Yes- thanks Russ!

---

Evan Drumwright Senior Research Scientist http://positronicslab.github.io Toyota Research Institute Palo Alto, CA

On Mon, Jan 28, 2019 at 5:47 PM Russ Tedrake notifications@github.com wrote:

I think that we'll want autodiff, etc, support eventually (so don't want to architect ourselves out of it). but i think we can unlock a lot of important use cases with double-only for the first pass.

(Is that what you needed from me?)

— You are receiving this because you were assigned. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/9123#issuecomment-458374807, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwz3eDScQAoj4lUq4miWTHPXnfWk5tks5vH6hJgaJpZM4VNfbI .

--

Confidential or protected information may be contained in this email and/or attachment. Unless otherwise marked, all TRI email communications are considered "PROTECTED" and should not be shared or distributed. Thank you.

[amcastro-tri]

Thank you for volunteering to work on this @edrumwri! Let me know if you'd need any help. I got lost on the AbstractValue implementation details but, how do you think the API to users would look like?

[edrumwri]

I've been writing a single port to accept a vector of ExternallyAppliedForce, defined as:

  struct ExternallyAppliedForce {
    // The index of the body that the force is to be applied to.
    BodyIndex body_index;

    // A vector from Body B's origin (Bo) to location Q, expressed in
    // B's frame.
    Vector3<T> p_BoQ_B;

    // A spatial force applied to Body B at location Q, expressed in the
    // world frame.
    SpatialForce<T> F_B_W;
  };

with the idea being that you can apply as many spatial forces to as many links (and on as many model instances) as you want. I think this is consistent with the discussions in the issue to-date. But I definitely would appreciate any course corrections sooner rather than later. (Please excuse any inconsistencies with the monogram notation in the above fragment: I have yet to write code using it and am running this by @mitiguy before submitting a PR).

Also, the plant will create a port if the caller requests it (using CreateAppliedSpatialForcePort()). I'd prefer to just automatically create the port, but that would break a lot of existing MBP code.

[jwnimmer-tri]

... but that would break a lot of existing MBP code.

Can you elaborate? (Nobody should be hard-coding port indices.)

[edrumwri]

Sure- if the port isn't connected to a source, then an exception will be thrown when, e.g., simulating. Unless there is a trick I'm unaware of for ignoring the port when it's unconnected (that would be nice in this case)...

[jwnimmer-tri]

Yup, there is. If the EvalInput (family of) method(s) returns nullptr, then the port was not connected. Many Systems already in Drake use the "declare an input port, but it doesn't strictly need to be connected" idiom, by checking for nullptr.

[edrumwri]

Wonderful! I'll do that then.

[amcastro-tri]

@edrumwri, it'd also be nice having the expressed-in frame E in ExternallyAppliedForce so that the spatial force would be F_Bq_E. You'd only need to re-express to W. That's a two step process:

1. Get frame E pose in world. X_WE = frame_E.CalcPoseInWorld(context)
2. Re-express the spatial force. F_Bq_W = X_WE.linear() * F_Bq_E

That'd support the super common case of E being the body frame B, and others. Imagine the case of modeling a quadrotor propeller, a drag force or a suction cap in a gripper (forces are modeled in B).

[edrumwri]

If I'm understanding you correctly, the user would have to specify the frame as well. Wouldn't the net effect, which would require always passing in a frame, be as cumbersome as the user re-expressing the force?

---

Evan Drumwright Senior Research Scientist http://positronicslab.github.io Toyota Research Institute Palo Alto, CA

On Thu, Jan 31, 2019 at 6:52 AM Alejandro Castro notifications@github.com wrote:

@edrumwri https://github.com/edrumwri, it'd also be nice having the expressed-in frame E in ExternallyAppliedForce so that the spatial force would be F_Bq_E. You'd only need to re-express to W. That's a two step process:

1. Get frame E pose in world. X_WE = frame_E.CalcPoseInWorld(context)
2. Re-express the spatial force. F_Bq_W = X_WE.linear() * F_Bq_E

That'd support the super common case of E being the body frame B, and others. Imagine the case of modeling a quadrotor propeller, a drag force or a suction cap in a gripper (forces are modeled in B).

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/9123#issuecomment-459372403, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwzwLDPfGWyTPJMtrT_owoPhq3QNPsks5vIwMcgaJpZM4VNfbI .

--

Confidential or protected information may be contained in this email and/or attachment. Unless otherwise marked, all TRI email communications are considered "PROTECTED" and should not be shared or distributed. Thank you.

[sherm1]

Applying the API design guideline of "minimal sufficient interface" suggests we should just pick a frame for each quantity. We already have great tools for users to re-express from one frame to another, so we don't have to build that mechanism into the input port design.

My 2 cents: I'd prefer to express the force in the World frame. Everyone knows how to express vectors in that frame, and it is particularly convenient when

• applying equal-and-opposite forces to two bodies (just negate the world-frame force)
• working with spatially-oriented fields (gravity, wind)

There are good applications for body-frame forces too so that is a reasonable alternative but I think World is a (mildly) better choice. In any case I'd prefer we just pick one.