pydrake: How do I aggregate multiple List[ExternallyAppliedSpatialForce] from distinct systems for a plant?
Question:
I am testing out some simple controllers, and would like to know how to aggregate List[ExternallyAppliedSpatialForce], specifically to provide to MultibodyPlant.get_applied_spatial_force_input_port().
If I try to connect both outputs to it, I get the following error:
RuntimeError: Input port is already wired.
For more context, I am looking at bilateral control, and want to eventually be able to study the effects of delays between the separate controllers. In this case, I am just trying out floating bodies as the "main" and "secondary" driven components.
An rough example of floating body controller:
https://gist.github.com/EricCousineau-TRI/f774b936aa930cf29777ed411815bbce#file-simple_floating_body_controller-py
Answers:
A few ways that I know of (thus far), with some notes:
1. Aggregate the lists of external forces as an intermediate system
This can be done relatively easily. See rough example system here:
https://gist.github.com/EricCousineau-TRI/f774b936aa930cf29777ed411815bbce#file-external_force_aggregator_system-py
2. Couple the control itself
Take in references and generate inputs for both objects in the same system – this would produce one list of forces.
FWIW, I may do this eventually for idealized bilateral control, but doing this may hurt the ability to study delays easily using composite systems (e.g. a ZeroOrderHold).
3. Consider using generalized forces (where its easier to aggregate / add)
This is probably the more common use case when an actual manipulator / robot arm is involved 😉
As an aside, the bilateral control can be studied in simpler settings (e.g. one-DoF) as is done in literature, e.g. [1], Sec. 3.4, p. 47.
[1] J. G. Park, “Improving Teleoperation with Models and Tasks,” PhD Thesis, Stanford University, 2010. [Online]. Available: https://stacks.stanford.edu/file/druid:ph465qm8083/dissertation-augmented.pdf
The following PR has merged and provides ExternallyAppliedSpatialForceMultiplexer:
drake#18171
I am testing out some simple controllers, and would like to know how to aggregate List[ExternallyAppliedSpatialForce], specifically to provide to MultibodyPlant.get_applied_spatial_force_input_port().
If I try to connect both outputs to it, I get the following error:
RuntimeError: Input port is already wired.
For more context, I am looking at bilateral control, and want to eventually be able to study the effects of delays between the separate controllers. In this case, I am just trying out floating bodies as the "main" and "secondary" driven components.
An rough example of floating body controller:
https://gist.github.com/EricCousineau-TRI/f774b936aa930cf29777ed411815bbce#file-simple_floating_body_controller-py
A few ways that I know of (thus far), with some notes:
1. Aggregate the lists of external forces as an intermediate system
This can be done relatively easily. See rough example system here:
https://gist.github.com/EricCousineau-TRI/f774b936aa930cf29777ed411815bbce#file-external_force_aggregator_system-py
2. Couple the control itself
Take in references and generate inputs for both objects in the same system – this would produce one list of forces.
FWIW, I may do this eventually for idealized bilateral control, but doing this may hurt the ability to study delays easily using composite systems (e.g. a ZeroOrderHold).
3. Consider using generalized forces (where its easier to aggregate / add)
This is probably the more common use case when an actual manipulator / robot arm is involved 😉
As an aside, the bilateral control can be studied in simpler settings (e.g. one-DoF) as is done in literature, e.g. [1], Sec. 3.4, p. 47.
[1] J. G. Park, “Improving Teleoperation with Models and Tasks,” PhD Thesis, Stanford University, 2010. [Online]. Available: https://stacks.stanford.edu/file/druid:ph465qm8083/dissertation-augmented.pdf
The following PR has merged and provides ExternallyAppliedSpatialForceMultiplexer:
drake#18171