I am attempting to connect the spatial_forces outputs of a Propeller
system and a Wing
system to a single MultiBodyPlant
by utilizing a LeafSystem
to combine the outputs from both systems into a single output.
I have attempted to create a system to do this below:
class ForceMux(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("propeller_force",AbstractValue.Make(ExternallyAppliedSpatialForce()))
self.DeclareAbstractInputPort("wing_force",AbstractValue.Make(ExternallyAppliedSpatialForce()) )
self.DeclareAbstractOutputPort("spatial_forces",
ExternallyAppliedSpatialForce(),
self.OutputForces)
def OutputForces(self, context, output):
propeller_force = self.EvalVectorInput(context, 0)[0]
wing_force = self.EvalVectorInput(context, 0)[0]
print(propeller_force)
print(wing_force)
output.SetFromVector(np.concatenate(propeller_force, wing_force))
print(output)
When I try to instantiate this ForceMux
class I get the following error:
TypeError: DeclareAbstractOutputPort(): incompatible function arguments. The following argument types are supported: 1. (self: pydrake.systems.framework.LeafSystem_[float], name: str, alloc: Callable[[], pydrake.common.value.AbstractValue], calc: Callable[[pydrake.systems.framework.Context_[float], pydrake.common.value.AbstractValue], None], prerequisites_of_calc: Set[pydrake.systems.framework.DependencyTicket] = {DependencyTicket(15)}) -> pydrake.systems.framework.OutputPort_[float]
What is the correct way to define the input & output ports of a LeafSystem in order to pass through the spatial_forces outputs of the Propeller & Wing?
Will output.SetFromVector(np.concatenate(propeller_force, wing_force))
generate the correct output to be applied the "applied_spatial_foce" input of a MultiBodyPlant, or do these two vectors need to be combined in a different way?
Also, how do you declare the size of the inputs and outputs when using DeclareAbstractInputPort
and DeclareAbstractOutputPort
?
Finally, both the Propeller and Wing classes have a "body_poses" input port that should be connected the "body_poses" output of a MultiBodyPlant. I don't believe you can connect two input ports to the same output port in Drake, so how are you supposed to get the same body pose information to both systems?