search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.34k stars 1.26k forks source link

DircolTrajectoryOptimization should implement a spline for state trajectory #3787

Closed sammy-tri closed 7 years ago

sammy-tri commented 8 years ago

In MATLAB, DircolTrajectoryOptimization calculates a spline in reconstructStateTrajectory. In the C++ implementation, this is just a TODO in dircol_trajectory_optimization.h.

Lucy-tri commented 8 years ago

The formulation of the direct collocation method can be found in C. R. Hargraves and S. W. Paris. Direct trajectory optimization using nonlinear programming and collocation. J Guidance, 10(4):338-342, July-August 1987. @hongkai-dai has the paper. However, we were planning to use the Matlab DircolTrajectoryOptimization as a guide, unless it just calls some Matlab spline function.

Lucy-tri commented 7 years ago

@siyuanfeng-tri I see you have implemented a Pchip interpolation method in piecewise_polynomial.cc (in PR #4334 in progress). We have been wanting to finish dircol_trajectory_optimization.cc by using a Pchip method in ReconstructStateTrajectory(). If you look in DircolTrajectoryOptimization.m, look at the call to pchipDeriv() (which is in pchipDeriv.m). It's like your Pchip() function, but takes the derivatives as an argument also. I don't want to hold up your PR, but would like to understand.

siyuanfeng-tri commented 7 years ago

i didn't look at it super carefully, but at a glance it seems to be doing exactly what my spline(T, Y, Ydot) is doing in #4334 except the matlab code has some shifting about the velocities if it's length doesn't match Ts. @hongkai-dai can you take a quick look when you have a chance.

hongkai-dai commented 7 years ago

@siyuanfeng-tri I think the pchipDeriv.m in MATLAB is Cubic(T, Y, Ydot) in your PR? The Matlab pchipDeriv.m also allows the time derivative to be dis-continuous at the knot point (namely ydot_minus and ydot_plus can be different).

I do not think in DircolTrajectoryOptimization we want to use discontinuous time derivative. So my understanding is that we should use Cubic(T, Y, Ydot) in DircolTrajectoryOptimization.

siyuanfeng-tri commented 7 years ago

that's what i thought. i'd say we proceed with Cubic(T, Y, Ydot). we can take a closer look when something blows up.

Lucy-tri commented 7 years ago

https://reviewable.io/reviews/RobotLocomotion/drake/4575#-

Lucy-tri commented 7 years ago

PR #4575 is merged, so this is done.