0

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()
Andy Cohen
  • 113
  • 1
  • 8

2 Answers2

1

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.

Sherm
  • 643
  • 4
  • 6
  • Right, I'm planning to connect this to a controller that would be derived from a value iteration algorithm optimization. Hence I need this to be non-constant, so that when the control policy is derived I can hook up the controller's output to the plant's voltage input. My question really centers around: how can I access the value coming in from the input? – Andy Cohen Apr 28 '20 at 22:43
  • 2
    In C++ it is `const Type& value = port.Eval(context)` (see the same link as above). I believe in Python you'll just need `port.Eval(context)`. – Sherm Apr 29 '20 at 03:33
  • Here is the Python API documentation: https://drake.mit.edu/pydrake/pydrake.systems.framework.html#pydrake.systems.framework.InputPort_.InputPort_[float].FixValue, example: https://git.io/JfYzQ – Eric Cousineau Apr 29 '20 at 17:56
1

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

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10