[Python] Applying Propeller and Wing forces to a MultiBodyPlant in Drake

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?

Continue reading...