Handle solver failures like TrajOpt does

tesseract-robotics / trajopt

Trajectory Optimization Motion Planner for ROS

385 stars 103 forks source link

Handle solver failures like TrajOpt does #369

Closed rjoomen closed 10 months ago

rjoomen commented 10 months ago

Added max_qp_solver_failures parameter and the logic of TrajOpt to handle solver failures
Added updateBounds() call after each box size modification
Moved setVariables() call after a solver failure to solveQPProblem()
Renamed SQPStatus::TIME_LIMIT to SQPStatus::OPT_TIME_LIMIT because of a name collision with OSQP (in tesseract_planning)

Closes #368

rjoomen commented 10 months ago

I'm not entirely sure about removing the setVariables, what do you think, @Levi-Armstrong?

The calls to updateBounds() seems necessary to me, as box size changes do modify the bounds here.

Levi-Armstrong commented 10 months ago

I'm not entirely sure about removing the setVariables, what do you think, @Levi-Armstrong?

The calls to updateBounds() seems necessary to me, as box size changes do modify the bounds here.

Yea, the set variables need to stay because it need to set them back to the previous iterations values.

rjoomen commented 10 months ago

Unstable/jammy fails due to clang-tidy-14. It needs a more extensive clang-tidy config, I think. Ubuntu/jammy succeeds, as no clang-tidy is found.

Levi-Armstrong commented 10 months ago

Unstable/jammy fails due to clang-tidy-14. It needs a more extensive clang-tidy config, I think. Ubuntu/jammy succeeds, as no clang-tidy is found.

Lets try for CI to install the same version of clang-tidy that is used on focal for now until we have time to go through and upgrade to the latest version of clang-tidy.

Levi-Armstrong commented 10 months ago

I compared trajopt_sqp numerical ik output between this branch and master with no differences found. I think your changes are good.

Levi-Armstrong commented 10 months ago

@rjoomen You good with me squash merging this?

rjoomen commented 10 months ago

Go ahead! Thanks for checking