I have a templated Leaf System that works with direct collocation. I have a state variable I would like to find the automatic derivative of in the function DoCalcTimeDerivatives. Is this possible to do this, and how?
For state, I have defined it as
MyState = namedview(
"MyState", ["theta", "theta_dot", "phi", "phi_dot"]
)
The Template System looks like:
@TemplateSystem.define("MyPlant_")
def MyPlant_(T):
class Impl(LeafSystem_[T]):
...
and for DoCalcDerivatives method inside, I have:
def DoCalcTimeDerivatives(self, context, derivatives):
s = MyState(
context.get_mutable_continuous_state_vector().CopyToVector()
)
...
control_effort = self.EvalVectorInput(context, 0)[0]
sdot = MyState(deepcopy(s))
time = context.get_time()
autodiffTime = AutoDiffXd(time, [1])
theta_dot_autodiff = s.theta_dot(autodiffTime)
theta_ddot = theta_dot_autodiff.derivatives()
sdot.theta_dot = theta_ddot + control_effort
This errors of course when I try to use this plant in a program performing Direct Collocation:
RuntimeError: Exception while evaluating SNOPT costs and constraints: 'TypeError: __init__(): incompatible constructor arguments. The following argument types are supported:
1. pydrake.autodiffutils.AutoDiffXd(value: float)
2. pydrake.autodiffutils.AutoDiffXd(value: float, derivatives: numpy.ndarray[numpy.float64[m, 1]])
3. pydrake.autodiffutils.AutoDiffXd(value: float, size: int, offset: int)
Invoked with: <AutoDiffXd 0.0 nderiv=0>, [1]
I was wondering if this idea is something possible to do in Drake, and if so, how to properly implement it?
Thanks!
I tried to perform AutoDifferentiation using time as the input and f(x) as an element of state, which I believe is a symbolic Drake function evaluated at runtime. (I may misunderstand what I am actually doing.)
The program errored indicating I was using incorrect construct arguments for AutoDiffXd, likely the input for time.