Fix issue with tolerance size

[rjoomen]

With the new toleranced waypoints, the tolerance variable type was introduced as a fixed Matrix (effectively Vector6d), always requiring 6 values. Here and here, however, a resize was still attempted, leading to silent errors, except for debug builds, where Eigen asserted an error.

This PR changes the types to a VectorXd, bringing the behavior in line with the coefs, allowing either 1 or 6 values, and fixing the resize() assert.

[codecov]

Codecov Report

Attention: Patch coverage is 9.09091% with 20 lines in your changes missing coverage. Please review.

Project coverage is 79.87%. Comparing base (09c3f78) to head (03c699d).

Files with missing lines	Patch %	Lines
...ract_motion_planners/trajopt/src/trajopt_utils.cpp	9.09%	20 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #535      +/-   ##
==========================================
- Coverage   79.96%   79.87%   -0.10%     
==========================================
  Files         248      248              
  Lines       16349    16369      +20     
==========================================
  Hits        13074    13074              
- Misses       3275     3295      +20     

Files with missing lines	Coverage Δ
..._motion_planners/trajopt/trajopt_waypoint_config.h	100.00% <ø> (ø)
...ract_motion_planners/trajopt/src/trajopt_utils.cpp	35.37% <9.09%> (-3.39%)	⬇️

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[Levi-Armstrong]

@rjoomen What about just fixing the xml parsing code to check if the token size is 1 then create fixed size vector of 6 and if not 1 it must be of length 6 otherwise throw an exceptions? @marip8 Interested to hear your thoughts on this change?

[marip8]

I think we should probably add an else case and throw an exception if the vector is not size 1 or 6. I think it's valuable to communicate that error rather than silently ignoring whatever incorrect tolerance is provided by the user

[Levi-Armstrong]

@rjoomen What about just fixing the xml parsing code to check if the token size is 1 then create fixed size vector of 6 and if not 1 it must be of length 6 otherwise throw an exception?

@rjoomen Thoughts on the this question?

[rjoomen]

@rjoomen What about just fixing the xml parsing code to check if the token size is 1 then create fixed size vector of 6 and if not 1 it must be of length 6 otherwise throw an exception?

@rjoomen Thoughts on the this question?

An exception if not size 1 or 6 sounds like a good idea. This also has to be implemented for multiple coeffs, I'll look into that. I'm not sure the xml parsing code is the best location for this, though, as currently all handling of (coeffs.size() == 1) (search for it) is already done in trajopt_utils already, and wrong sizes are ignored silently everywhere.

[rjoomen]

Done.

[rjoomen]

This is ready for review/merger, @Levi-Armstrong

[Levi-Armstrong]

Can you make the same change to the trajopt ifopt one also? I

[Levi-Armstrong]

Once the changes are also made to TrajOpt IFOPT this will be ready to merge.

[rjoomen]

Once the changes are also made to TrajOpt IFOPT this will be ready to merge.

I believe this is already correctly handled in Trajopt-Ifopt, see here, for example.

[Levi-Armstrong]

Once the changes are also made to TrajOpt IFOPT this will be ready to merge.

I believe this is already correctly handled in Trajopt-Ifopt, see here, for example.

Sorry, I should have been more specific. I think we should also update the TrajOptIfoptCartesianWaypointConfig to leverage Eigen::VectorXd to align with your changes to the TrajOpt changes you made.