0

I have a LeafSystem in drake, with dynamics \dot{x} = f(x,u) written in DoCalcTimeDerivatives. The fixed points and trim points of this system are not trivial to find. Therefore, I image one would need to write a nonlinear optimization problem to find the fixed points:

find x, u; s.t. f(x,u)=0

or

find x,u; min f(x,u)^2

I am wondering, how should I take advantage of the dynamics that I have already written in DoCalcTimeDerivatives of the LeafSystem, and write a non-linear optimization to search over x and u to find the fixed points and trim points in drake? Some existing examples in drake would be greatly appreciated!

Hank
  • 17
  • 2

1 Answers1

1

It's simple to write for your case (and only slightly harder to write for the general case... it's on my TODO list).

Assuming your plant supports symbolic, then looking at the trajectory optimization will give you a sense for how you might write the constraint: https://github.com/RobotLocomotion/drake/blob/master/systems/trajectory_optimization/direct_transcription.cc#L212 (the autodiff version is just below):

fwiw, the general case from the old matlab version is here: https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/solvers/FixedPointProgram.m

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • Found a trajectory optimization example using direct collocation in the pendulum folder: [link] (https://github.com/RobotLocomotion/drake/blob/4990a21481508ebca9b1d3d75f87507a869ccbed/examples/pendulum/trajectory_optimization_simulation.cc#L45) – Hank May 02 '18 at 00:40
  • Wondering is there any code using direct transcription instead? I was looking inside the underactuated folder but they seem to be all python files using direct collocation. – Hank May 02 '18 at 00:51
  • Easy enough to add continuous time support to direct transcription, but as you've found I hadn't done that in my first pass. – Russ Tedrake May 03 '18 at 09:54