CODEWITHSUNDEEP
drake
Andy Cohen
Andy Cohen

Reputation: 113

How do you set up input ports in custom plants in Drake?

I am currently working to implement a nonlinear system in Drake, so I've been setting up a LeafSystem as my plant. Following this tutorial, I was able to get a passive version of my system functioning properly (no variable control input). But the tutorial doesn't cover defining inputs in these custom systems, and neither do any examples I was able to find. Could someone help me with a few questions I have listed below?

  1. As shown in the code shown below, I simply guessed at the DeclareVectorInputPort function, but I feel like there should be a third callback similar to CopyStateOut. Should I add a callback that just sets the input port as the scalar phi (my desired voltage input)?
  2. When I call CalcTimeDerivatives, how should I access phi? Should it be passed as another argument or should the aforementioned callback be used to set phi as some callable member of DEAContinuousSys?
  3. At the end of CalcTimeDerivatives, I also feel like there should be a more efficient way to set all derivatives (both velocity and acceleration) to a single vector rather than individually assigning each index of derivatives, but I wasn't able to figure it out. Tips?
from pydrake.systems.framework import BasicVector, LeafSystem

class DEAContinuousSys(LeafSystem):
  def __init__(self):
    LeafSystem.__init__(self)

    self.DeclareContinuousState(2) # two state variables: lam, lam_d
    self.DeclareVectorOutputPort('Lam', BasicVector(2), self.CopyStateOut) # two outputs: lam_d, lam_dd
    self.DeclareVectorInputPort('phi', BasicVector(1)) # help: guessed at this one, do I need a third argument?

  # TODO: add input instead of constant phi
  def DoCalcTimeDerivatives(self, context, derivatives): 
    Lam = context.get_continuous_state_vector() # get state
    Lam = Lam.get_value() # cast as regular array
    Lam_d = DEA.dynamics(Lam, None, phi) # derive acceleration (no timestep required) TODO: CONNECT VOLTAGE INPUT
    derivatives.get_mutable_vector().SetAtIndex(0, Lam_d[0]) # set velocity
    derivatives.get_mutable_vector().SetAtIndex(1, Lam_d[1]) # set acceleration

  def CopyStateOut(self, context, output):
    Lam = context.get_continuous_state_vector().CopyToVector()
    output.SetFromVector(Lam)

continuous_sys = DEAContinuousSys()

Upvotes: 0

Views: 881

Answers (2)

Russ Tedrake
Russ Tedrake

Reputation: 5533

You might take a look at the quadrotor2d example from the underactuated notes: https://github.com/RussTedrake/underactuated/blob/master/underactuated/quadrotor2d.py#L44

Upvotes: 1

Sherm
Sherm

Reputation: 678

You're off to a good start! Normally input ports are wired into some other system's output port so there is no need for them to have their own "Calc" method. However, you can also set them to fixed values, using port.FixValue(context, value) (the port object is returned by DeclareVectorInputPort()). Here is a link to the C++ documentation. I'm not certain of the Python equivalent but it should be very similar.

Hopefully someone else can point you to a Python example if the C++ documentation is insufficient.

Upvotes: 1

Related Questions