4

This questions is strongly related to adding-forces-to-body-post-finalize

I would like to be able to apply an external force to simple geometric primitives in pydrake. This is to perform an evaluation of interactions between bodies.

My current implementation:


builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.001))
parser = Parser(plant)
cube_instance = parser.AddModelFromFile('cube.urdf', model_name='cube')

plant.Finalize()

force = builder.AddSystem(ConstantVectorSource(np.zeros(6)))
builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())

diagram = builder.Build()

However when I run it, I get the following error:

builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())
RuntimeError: DiagramBuilder::Connect: Cannot mix vector-valued and abstract-valued ports while connecting output port y0 of System drake/systems/ConstantVectorSource@0000000002db5aa0 to input port applied_spatial_force of System drake/multibody/MultibodyPlant@0000000003118680

I have an inclination that I have to implement a LeafSystem which implements the abstract-value port on the plant.

Update based on Suggestion

Use: AbstractValue and ConstantValueSource

value = AbstractValue.Make([np.zeros(6)])
force = builder.AddSystem(ConstantValueSource(value))

ref_vector_externally_applied_spatial_force = plant.get_applied_spatial_force_input_port()
builder.Connect(force.get_output_port(0), ref_vector_externally_applied_spatial_force)

I got the following error:

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource@0000000002533a30 (type pybind11::object) to
input port applied_spatial_force of System drake/multibody/MultibodyPlant@0000000002667760 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)

Which makes sense the types of the input-output ports should match. The expected type seems to be a vector of ExternallyAppliedSpatialForce.

I then changed the Abstract type as follows:

value = AbstractValue.Make(ExternallyAppliedSpatialForce())

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource@0000000002623980 (type drake::multibody::ExternallyAppliedSpatialForce<double>)
to input port applied_spatial_force of System drake/multibody/MultibodyPlant@00000000027576b0 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)

I am getting closer. However, I was not able to send a vector of ExternallyAppliedSpatialForce. If I try to send it as a list, I get a complaint that it is unable to pickle the object. I did not see in the AbstractValue examples how to create such a vector of objects.

Any additional help would be greatly appreciated.

Solution was to use the type VectorExternallyAppliedSpatialForced

full solution to be posted later.

Guillaume
  • 163
  • 10

2 Answers2

6

No need to implement a LeafSystem — we have a ConstantValueSource that is analogous to ConstantVectorSource, but for Abstract types. https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_constant_value_source.html

And you’re correct that you need the abstract type for this port: https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ab2ad1faa7547d440f008cdddd32d85e8

Some examples of working with the abstract values from python can be found here: https://github.com/RobotLocomotion/drake/blob/d6133a04/bindings/pydrake/systems/test/value_test.py#L84

Eric Cousineau
  • 1,944
  • 14
  • 23
Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • Thanks a lot for your suggestion. I think I am in the right direction now. I am still facing a few issues with the input-output types. I updated the original question to reflect the change. – Guillaume Apr 17 '20 at 13:40
  • ok VectorExternallyAppliedSpatialForced seems to be correct type. – Guillaume Apr 17 '20 at 14:29
  • What helped also was in this commit [commit message](https://github.com/RobotLocomotion/drake/commit/18ecc2447c1e4afe210cc977a378e16af9adcb05). After inspecting: **def test_applied_force_input_ports(self)** – Guillaume Apr 17 '20 at 14:35
  • Thanks for the question. I believe @Russ Tedrake pointed you to all the correct bits. Please mark his reply as "the answer" if it did help you out. Best. – Alejandro Apr 17 '20 at 14:47
6

Here is a working example of applying an external static force to a rigid object:

sim_time_step = 0.001
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, sim_time_step)
object_instance = Parser(plant).AddModelFromFile('box.urdf')
scene_graph.AddRenderer("renderer", MakeRenderEngineVtk(RenderEngineVtkParams()))
ConnectDrakeVisualizer(builder, scene_graph)

plant.Finalize()

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

forces = VectorExternallyAppliedSpatialForced()
forces.append(force_object)

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())

diagram = builder.Build()
simulator = Simulator(diagram)

context = simulator.get_mutable_context()

plant.SetPositions(context, object_instance, [0, 0, 0, 1, 0, 0, 0])

time_ = 0
while True:
    time_ += sim_time_step
    simulator.AdvanceTo(time_)
    time.sleep(sim_time_step)

However, I was not able to change externally applied force in the simulation loop afterwards.

To achieve this I had to make a LeafSystem.

An implementation that allows you to change the force applied to a rigid object over time can be found here

Both static and dynamic examples can be found here.

Guillaume
  • 163
  • 10