RobotLocomotion
KinematicTrajectoryOptimization can impose lower bounds on acceleration/jerk as second order cone constraints
When we have a lower bound on the acceleration/jerk, this is actually a convex second order cone constraint. Use acceleration as an example, we have
a = M * control_pts / h^2 >= lb
which is the same as the rotated Lorentz cone constraint
M / lb * control_pts >= h^2
Similar (but more complicated) math applies to jerk lower bounds.
I think if all other constraints are convex (for example we have linear bounds on the position/velocity), then it makes more sense to impose the lower bound on acceleration and jerk as convex second order cone constraints, so that the optimization problem is solved through a convex solver rather than a generic nonlinear solver.
cc @RussTedrake
Does the second order cone constraint lead to worse performance with SNOPT? If so, we probably don't want this getting added unless all other constraint are convex.
Does the second order cone constraint lead to worse performance with SNOPT? If so, we probably don't want this getting added unless all other constraint are convex.
Great point!
The Lorentz cone constraint z0 >= sqrt(z1^2 + ... + zn^2) isn't always a good formulation for SNOPT (because the sqrt on the right hand side is non-differentiable at 0), hence we allow the users to specify EvalType https://github.com/RobotLocomotion/drake/blob/001ef77270e129847593f52266d082ef83c1d5f3/solvers/constraint.h#L355-L365, which makes it behave nice in a gradient-based nonlinear solver.
But what we need here is a rotated Lorentz cone constraint, which doesn't take sqrt on the right hand side, hence it always has a nice gradient. I think SNOPT should be fine.
Ah, that was the nuance I was missing.
(We probably want to ignore or throw if the user gives lb=0, right? That would be a bit weird, but such a constraint is trivial anyways.)
It's a nice point. But I think it's pretty rare that we would have lower bounds on acceleration/jerk w/o the corresponding upper bounds?
