CODEWITHSUNDEEP
pythondrake
mason
mason

Reputation: 31

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?

Upvotes: 2

Views: 221

Answers (1)

wrangelvid
wrangelvid

Reputation: 21

You are really close! Multibodyplant expects a list of ExternallyAppliedSpatialForce which seems like what you are trying to output. Here, your DeclareAbstractOutputPort's type should look something like this:

lambda: AbstractValue.Make([ExternallyAppliedSpatialForce(), ExternallyAppliedSpatialForce()]

That will take care of the size of your output port. Then you can use output.set_value([propeller_force, wing_force])since you are dealing with AbstractValue's

You can connect plant.get_body_poses_output_port() to as many wings and propellers as you want :)

This is my implementation for an N wing system. Note, I had to make my leafsystem compatible with Autodiff types for trajectory optimization.

@TemplateSystem.define("SpatialForceConcatinator_")
def SpatialForceConcatinator_(T):
    class Impl(LeafSystem_[T]):
        def _construct(self, N_inputs, converter = None):
            LeafSystem_[T].__init__(self, converter)
            self.N_inputs = N_inputs
            self.Input_ports = [self.DeclareAbstractInputPort(f"Spatial_Force_{i}",
                                AbstractValue.Make([ExternallyAppliedSpatialForce_[T]()]))
                                for i in range(N_inputs)]
        
            self.DeclareAbstractOutputPort("Spatial_Forces",
                                           lambda: AbstractValue.Make(                                             
                                           [ExternallyAppliedSpatialForce_[T]()
                                              for i in range(N_inputs)]),
                                           self.Concatenate)

        def Concatenate(self, context, output):
            out = []
            for port in self.Input_ports:
                out += port.Eval(context)
            output.set_value(out)
        
        def _construct_copy(self, other, converter=None,):
            Impl._construct(self, other.N_inputs, converter=converter)
    
    return Impl

# Default instantations
SpatialForceConcatinator = SpatialForceConcatinator_[None]

Upvotes: 2

Related Questions