Sinusoidal initial trajectory guess for DirectCollocation in Drake

Question

I am trying to provide the solver with an initial guess that is a sinusoidal trajectory. I was hoping that there would be a way to specify the initial guess for the individual knot points, because then it would have been quite simple to provide the solver with an initial trajectory guess for a trajectory of any shape.

I have only found examples using the dircol.SetInitialTrajectory() function, together with a PiecewisePolynomial with FirstOrderHold, i.e. a linear initial guess. Is there a way to provide i.e. a sinuosidal initial trajectory?

Thanks!

Answer 1

SetInitialTrajectory takes an arbitrary PiecewisePolynomial as input, and will only be evaluated at the sample times of the initial guess. So you should be able to pass in a PiecewisePolynomial that matches your sinusoid perfectly.

Alternatively, you can use the SetInitialGuess interface from MathematicalProgram, and simply iterate through the dircol.state(i) variables to set them (allow you to define the mapping to time yourself).