Add toleranced Cartesian waypoints to solver

[marrts]

This is to support tolerances on Cartesian waypoints so that you can set constraints that don't have to match perfectly

[Levi-Armstrong]

In order for this to not have stability issues this cannot use the the jacobian utility to provide the jacobain because the error function is not continuous. When within the tolerance this will produce an all zero jacobian which is not correct so this will need to have its own jacobian method. This need to more or less use the error function without tolerance to calculate the jacobian.

[marrts]

In order for this to not have stability issues this cannot use the the jacobian utility to provide the jacobain because the error function is not continuous. When within the tolerance this will produce an all zero jacobian which is not correct so this will need to have its own jacobian method. This need to more or less use the error function without tolerance to calculate the jacobian.

This is why in my tesseract_planning PR I made it so that we can simultaneously have both costs and constraints, so the cost will continue to drive it to zero while the constraint will keep it inside the bounds. However, if there's a better solution I'd definitely be open to making the changes for that.

[Levi-Armstrong]

I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function signature should be the same as calcTransformError -> VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

[marrts]

I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function have the signature of calcTransformError -> VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

Does this have anything to do with the Jacobian issue, or is the purpose of this to save on computation in the cases where there is no tolerance applied?

[Levi-Armstrong]

Does this have anything to do with the Jacobian issue, or is the purpose of this to save on computation in the cases where there is no tolerance applied?

No, just computation and address the issue @marip8 pointed out.

[marrts]

I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function signature should be the same as calcTransformError -> VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

I added an error function option, but I'm not sure what we want to put the default values of the tolerances at now. The error function defaults to tesseract_common::calcTransformError if none is given.

[marrts]

but I'm not sure what we want to put the default values of the tolerances at now.

Just realized I don't actually use the tolerances anywhere any more. Should the tolerances just be completely dropped from these structs/classes and put the burden on a higher level function calling them to pass in an error function that captures the tolerances to calculate the error?

[Levi-Armstrong]

Sorry for the confusion. I would not pass the error function into the constructor. I would keep the tolerance as input to the constructor and have the constructor evaluate the tolerance to determine which function to assign to the internal member variable.

[marrts]

This has been working for me now and I think it addresses everything. It has been very useful in increasing motion planning success.

[marrts]

The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of calcTransformErrorJac. Fundamentally the "numerical" and "analytical" solution are doing the same thing, except "analytical" now uses calcTransformErrorJac and numerical still uses calcTransformError as a basis for the calculation.

Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56

[Levi-Armstrong]

The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of calcTransformErrorJac. Fundamentally the "numerical" and "analytical" solution are doing the same thing, except "analytical" now uses calcTransformErrorJac and numerical still uses calcTransformError as a basis for the calculation.

Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56

@marrts I will take a look at this and push a fix.

[rjoomen]

Could you implement this for trajopt_ifopt as well?

[marrts]

The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of calcTransformErrorJac. Fundamentally the "numerical" and "analytical" solution are doing the same thing, except "analytical" now uses calcTransformErrorJac and numerical still uses calcTransformError as a basis for the calculation. Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56

@marrts I will take a look at this and push a fix.

This test is failing because the rotational error set to [-pi,pi] for the numerical and [0,2pi] for the analytical. The test produces these error vectors for the initial error.

err_numerical:  -1.19071 -0.0193624  -0.417557  -0.910973  -0.810396   -2.52978
err_analytical:  -1.19071 -0.0193624  -0.417557    1.12722    1.00277    3.13031

The Jacobian results are:

Error:   Numerical:
  0.0193634  -0.189402   0.199863  0.0145331  0.0711069 -0.0978927          0
 -0.189711    0.37213   0.287726 -0.0535873   0.130371   0.122383          0
         0   0.168799 -0.0644466   -0.33596   0.101585 -0.0885404          0
  0.628394  -0.259181  -0.148265    0.46934   -1.30136   0.384817   0.316577
 -0.256931    1.32699   0.362308   -1.27522  -0.361558   -0.60032   -1.19339
  0.856022   0.134472   -1.03897  -0.181689   0.104203    -0.8853   0.628396
Error:   Analytical:
  0.0193634  -0.189402   0.199863  0.0145331  0.0711069 -0.0978927          0
 -0.189711    0.37213   0.287726 -0.0535873   0.130371   0.122383          0
         0   0.168799 -0.0644466   -0.33596   0.101585 -0.0885404          0
 -0.123759   0.625531  -0.454852   -0.85613    1.29629   -1.09011  -0.156293
  0.899549   -1.37083   -1.01615    1.33296   0.168067   0.196658    1.68613
  0.756401   0.680107  -0.486995  -0.539909   -1.00088  -0.609487  -0.123762

The first three rows, corresponding to position, match and the last three, corresponding to rotation are different. This is the expected behavior based on the initial error vectors we have.

If you rotate the target frame by 180 degrees around z this goes away and the test passes:

Eigen::Isometry3d target_frame_offset = Eigen::Isometry3d::Identity() * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ());

err_numerical:  -1.19071 -0.0193624  -0.417557  -0.591328   0.664716   0.344705
err_analytical:  -1.19071 -0.0193624  -0.417557  -0.591328   0.664716   0.344705

I see a few options for resolving this:

1. Simply apply my change and the test passes
2. Do two tests, one with my change and one without and on the one without just check the first three rows of the matrix match
3. Remove this test altogether because it actually isn't necessarily expected that they match
4. Change the numerical implementation to somehow switch to an error that always has a positive angle

[marrts]

Could you implement this for trajopt_ifopt as well?

I haven't used trajopt_ifopt yet, and it's not obvious to me what changes would need to be made. Perhaps we get this one merged we make an issue to implement this in trajopt_ifopt?

I do see that there is a bounds_ object in the CartPosConstraint class, does this allow tolerance by default? https://github.com/tesseract-robotics/trajopt/blob/07337759d7c04cf837c9d2a4fef199c47ed84a74/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h#L175-L176

[Levi-Armstrong]

@marrts I would just apply your change to make the unit test pass.