Constraint motion planning, variational dynamic programming