pythondrake

`KinematicTrajectoryOptimization` fails with `DurationCost` and any `AccelerationBounds` (even infinite)

KinematicTrajectoryOptimization seems to fail arbitrarily on some inputs when using a DurationCost and AccelerationBounds, even when setting the AccelerationBounds far outside the actual acceleration of the trajectory (or even setting them to infinity).

Here is a simple example: solving fails as written but passes when either the duration cost or (infinite) AccelerationBounds are removed:

import numpy as np
from pydrake.planning import KinematicTrajectoryOptimization
from pydrake.solvers import Solve

trajopt = KinematicTrajectoryOptimization(num_positions=6, num_control_points=10)
prog = trajopt.get_mutable_prog()

positions = np.array([np.ones(6) * 0.9, 
                      np.ones(6) * 0.8, 
                      np.ones(6) * 0.5, 
                      np.zeros(6)])

for i,joint in enumerate(positions):
    trajopt.AddPathPositionConstraint(
        joint, joint , i / (len(positions) - 1)
    )

trajopt.AddDurationCost(0.5)
trajopt.AddAccelerationBounds(-np.inf * np.ones(6), np.inf * np.ones(6))

result = Solve(prog)
if not result.is_success():
    raise RuntimeError("Drake trajectory failed")

My best guess is that the AccelerationBounds trigger a different solver that is really bad at handling DurationCosts, but this example seems like it should be so trivial to solve that I have trouble believing that.

What might be happening here?

Solution

  • Your suspicion is correct. Try adding the following lines after you obtain your result:

    print(result.get_solver_id().name())
traj = trajopt.ReconstructTrajectory(result)
print(traj.start_time(), traj.end_time())

    If you don't have your acceleration constraint, then you have a convex optimization problem (for me, that means Gurobi). If you do add those constraints, then you get a nonconvex optimization (for me, that means SNOPT).

    But notice that, in either case, the optimal solution of the problem is a trajectory that is zero duration... this makes the problem numerically bad, which makes SNOPT fail (I expect that the Hessian is indefinite).

    If you add a few more constraints -- like an initial and final value constraint -- to get a nondegenerate solution, then I think you'll be back in business.