Do not add joint waypoint to fixed indices if not constrained

[rjoomen]

Cartesian waypoints are never added as fixed, but joint and state waypoints almost always.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 85.63%. Comparing base (8cd38f5) to head (cca499e).

❗ Current head cca499e differs from pull request most recent head 5cf4ff3

Please upload reports for the commit 5cf4ff3 to get more accurate results.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #427      +/-   ##
==========================================
- Coverage   85.73%   85.63%   -0.10%     
==========================================
  Files         233      221      -12     
  Lines       15347    14677     -670     
==========================================
- Hits        13157    12569     -588     
+ Misses       2190     2108      -82     

see 203 files with indirect coverage changes

[Levi-Armstrong]

I would recommend seeing if we can leverage the trajopt profile to indicate of a waypoint should be treated as fixed. In case when you have extra degrees of freedom these changes will not be ideal.

[rjoomen]

Sure, I was just trying to fix the todo. How exactly do you suggest I do that? And does that only apply to the Cartesian waypoints or also to the other types?

Also, the joint and state waypoints still have todos saying they should not be fixed if they are of term type cost, but I wasn't sure how to obtain the correct profile information.

[Levi-Armstrong]

I would probably update the TrajOpt Plan Profile to return a bool to indicate if it should be treated as fixed. If we think there may be other things that we want to return we could create a struct which gets returned allowing us to expand it in the future if needed.

[rjoomen]

How about this?

(I am unsure about how to handle the state waypoint type, btw, should that be the same way as joint?)

[marrts]

Does this change mean that if we have a fully constrained Cartesian waypoint and a seed joint state for that waypoint the output will always be equivalent to the seed? It looks like trajopt just skips the fixed steps and doesn't add them to the problem at all. This would be problematic for applications where you have redundant axes that you want TrajOpt to smooth across waypoints, but you still have strict Cartesian requirements

[rjoomen]

Does this change mean that if we have a fully constrained Cartesian waypoint and a seed joint state for that waypoint the output will always be equivalent to the seed? It looks like trajopt just skips the fixed steps and doesn't add them to the problem at all. This would be problematic for applications where you have redundant axes that you want TrajOpt to smooth across waypoints, but you still have strict Cartesian requirements

That is a good point. @Levi-Armstrong should cartesian waypoints maybe never be fixed (which effectively is the current situation)? Should I remove that part and only leave the logic pertaining the joint waypoints?

[rjoomen]

I went ahead and removed the fixed cartesian waypoint logic. Now this PR only fixes this and this todo.

Now how should state waypoints be handled?

[Levi-Armstrong]

I went ahead and removed the fixed cartesian waypoint logic. Now this PR only fixes this and this todo.

Now how should state waypoints be handled?

State waypoints should be treated as fixed joint state.

[rjoomen]

State waypoints should be treated as fixed joint state.

But the todo suggests to not make them fixed if they are cost instead of constraint.

[marrts]

State waypoints should be treated as fixed joint state.

So are state waypoints assumed to be always fixed in regards to their position? They aren't fixed in velocity, acceleration, or time stamp because time parameterization tasks expect state waypoints as inputs. So to me it seems arbitrary that you'd expect TrajOpt to treat state waypoints position values as fixed. I know we also have joint waypoints that could or couldn't be fixed, but I feel like there's probably a scenario that a previous planner produces a state waypoint that you want TrajOpt to treat as only a cost.

I also think the fact that for a given profile we have to set all types of waypoints to either a cost or constraint makes this more complicated. That's part of the motivation behind my other PR, #403, where I changed the TrajOptDefaultPlanProfile to have different configs for each waypoint type. I could add another one for state waypoints too, giving control over setting it to a cost (not fixed) vs constraint (fixed).

tesseract_planning/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h

Line 43 in d2fa313

struct CartesianWaypointConfig

tesseract_planning/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h

Line 71 in d2fa313

struct JointWaypointConfig

tesseract_planning/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h

Lines 55 to 58 in d2fa313

	CartesianWaypointConfig cartesian_cost_config;
	CartesianWaypointConfig cartesian_constraint_config;
	JointWaypointConfig joint_cost_config;
	JointWaypointConfig joint_constraint_config;