Trajopt enforces joint limits after optimization

[rjoomen]

Trajopt bluntly enforces joint limits after the optimization here. Shouldn't the optimization itself take care of this? And in case this fails or if a user has some other reason to do a simple joint limit enforcement, shouldn't something like the fix_state_bounds_profile. or the kinematic_limits_check_profile be used?

[Levi-Armstrong]

This was added because trajopt is producing solution slightly outside of the limits causing issues down stream. I believe this should be handled by trajopt because I think this is a numerical issue and it just needs to be clamped to avoid issues down stream.

[rjoomen]

A, I guess it's when it optimizes against a joint limit it can be over the limit by the constraint tolerance. Not a numerical issue then, but a feature. Would it be an idea to automatically subtract (or add, for a lower limit) the constraint tolerance from the constraints, so as to ensure the optimizer (and hence the robot) stays within the limits? Then this limit enforcement isn't necessary anymore either.

This also happens for collisions: a collision check after Trajopt has ran sometimes still finds small collisions, but less than the constraint tolerance within the collision margin.

[Levi-Armstrong]

We had went down this path before but ran into other issues. For example, a fixed joint provided that is right at the limit causing the problem to be infeasible when adjusting the bounds to account for the tolerance. We also have the same issue within the convex to convex narrow phase which can also converge slightly within the threshold causing the subsequent checks/planning to fail. This is one reason when chaining planner we use slightly different contact threshold to avoid this issue. I am not sure what the best way to handle this.

[rjoomen]

But shouldn't this fixed joint be not part of the optimization in the first place, as it's fixed?
I think the collision margins can be fairly easily adjusted to account for constraint tolerance by the user (provided he knows about the issue in the first place), but the joint limits are less easily set for only one profile/part of the pipeline. So having the limits corrected automatically by Trajopt would be a nice feature.

[Levi-Armstrong]

But shouldn't this fixed joint be not part of the optimization in the first place, as it's fixed?

I believe in the legacy trajopt they are treated as soft constraints, but I believe in the ifopt version they are treated as a hard constraints.

So having the limits corrected automatically by Trajopt would be a nice feature.

I agree, should the code for enforcing the limits just be moved to trajopt? I would recommend doing what is cleanest.

for (Eigen::Index i = 0; i < traj.rows(); i++)
  {
    assert(tesseract_common::satisfiesLimits<double>(traj.row(i), joint_limits, 1e-4));
    tesseract_common::enforceLimits<double>(traj.row(i), joint_limits);
  }