Search code examples
drakemotion-planning

Clarification on the distinction between AddPathVelocityConstraint and AddVelocityConstraintAtNormalizedTime for KinematicTrajectoryOptimization


I'm slightly confused by the distinction between these two functions. My current understanding is that the PathVelocityConstraint is essentially a constraint on the 1 dimensional path through cartesian space (more precisely, with respect to its parameterizing variable s), while the the VelocityConstraintAtNormalizedTime is applied directly to the joint states (or generalized variables, however you prefer to view it). This would explain the disclaimer in the docs that the former does not provide any guarantees on qdot(t). Does this logic track?

Now, assuming the above logic is correct, and going on a slight tangent, suppose I had successfully solved the associated mathematical program for a kinematic trajectory optimization, and used ReconstructTrajectory to obtain q(t) as a Bspline object. What would be the most straightforward way to obtain rdot(s) for some s in (0,1)? Would I simply apply forward kinematics to the trajectory and and use something like np.diff? Even then, I would have a vector in 3-space when I'm expecting to have some scalar value.

To give some context, I am performing consecutive solves after which I plan to stitch the trajectories together. So I'd like to be able to ensure continuity in both qdot and rdot in between the trajectories, not just one of the two.


Solution

  • The solution is parameterized by a path, r(s), and a time scaling, T.

    You can constrain ṙ(s) -- which is dr/ds evaluated at s -- using AddPathVelocityConstraint which is a convex constraint, but it does not bound the actual trajectory derivative q̇(t), because it does not take into account the time scaling.

    Calling AddVelocityConstraintAtNormalizedTime allows you to properly constraint q̇(t), but it results in a nonconvex constraint.

    This is the reason for having both methods. You should prefer AddPathVelocityConstraint if it suits your needs... but to e.g. constrain that the derivatives match between two different solved trajectories, then you most likely need the nonconvex version.