Up-to-date workflow for adding external forces to object post-finalized for trajectory optimization

Question

I am struggling to add an external wrench to a gripper. In my case, I have a floating hand trying to push an object. I would like a spatial wrench applied to the gripper's CoM as part of my decision variable.

Referring to this link, it looks like things have moved on. One of the instantiation VectorExternallyAppliedSpatialForced() is deprecated.

I tried to look for the doc. ExternallyAppliedSpatialForceMultiplexer() seems to be a close one. Then I tried similarly of using a System to output a force:

force_object.body_index = plant.GetBodyIndices(hand_model_index).pop()
force_object.F_Bq_W = SpatialForce(tau=np.zeros(3), f=np.array([0., 0., 10.]))

# a way to put force object into the AbstractValue???

forces = ExternallyAppliedSpatialForceMultiplexer(1)
value = AbstractValue.Make(forces)
force_system = builder.AddSystem(ConstantValueSource(value))
builder.Connect(force_system.get_output_port(0), plant.get_applied_spatial_force_input_port())

Is there an update workflow for this purpose? I am using AutoDiff and having the input wrench as decision variables at all time points. What is the best way to set/update the wrench for trajectory optimization.

Cheers!

Answer 1

If you just want to put a constant external force into the multibody plant input port, you can do e.g.

force = ExternallyAppliedSpatialForce()
force.body_index = plant.GetBodyByName(f"box_0_0_1").index()
force.F_Bq_W = SpatialForce(tau=[0, 0, 0], f=[0, 0, 0])
forces = builder.AddSystem(ConstantValueSource(Value([force])))
builder.Connect(forces.get_output_port(), plant.get_applied_spatial_force_input_port())

If you do want to put an ExternallyAppliedSpatialForceMultiplexer in the middle (it's not needed for this application, but perhaps it helps in a more complicated case), that would look like e.g.:

force = ExternallyAppliedSpatialForce()
force.body_index = plant.GetBodyByName(f"box_0_0_1").index()
force.F_Bq_W = SpatialForce(tau=[0, 0, 0], f=[0, 0, 0])
forces = builder.AddSystem(ConstantValueSource(Value([force])))
                                 
forces_mux = builder.AddSystem(ExternallyAppliedSpatialForceMultiplexer(1))
builder.Connect(forces.get_output_port(), forces_mux.get_input_port(0))
builder.Connect(forces_mux.get_output_port(), plant.get_applied_spatial_force_input_port())