Correctly handle cartesian waypoints in SimplePlannerFixedSizeAssignPlanProfile

[rjoomen]

Previously, when the only second position was cartesian, the SimplePlannerFixedSizeAssignPlanProfile would take the joint position of the first position as seed, or, if both were cartesian, it would take the environment state as seed, leading to strange planning results. With this PR the 'seed' of the second cartesian position is used if available, or the IK of the second position is calculated using either the first position or the environment state as seed.

[rjoomen]

If you agree with this PR I'll fix the tests.

[Levi-Armstrong]

I will review today

[Levi-Armstrong]

I do not believe this one is intended to run inverse kinematics.

[Levi-Armstrong]

I do believe it should be updated to leverage the seed but should not run inverse kinematics. If I remember correctly this was intended to just used the current state.

[rjoomen]

I do believe it should be updated to leverage the seed but should not run inverse kinematics. If I remember correctly this was intended to just used the current state.

OK, but shouldn't it be called SimplePlannerFixedSizeAssignNoIKPlanProfile instead of SimplePlannerFixedSizeAssignPlanProfile, for clarity and to match SimplePlannerLVSNoIKPlanProfile?

[Levi-Armstrong]

I do believe it should be updated to leverage the seed but should not run inverse kinematics. If I remember correctly this was intended to just used the current state.

OK, but shouldn't it be called SimplePlannerFixedSizeAssignNoIKPlanProfile instead of SimplePlannerFixedSizeAssignPlanProfile, for clarity and to match SimplePlannerLVSNoIKPlanProfile?

I am good with changing the name to make it more clear.

[rjoomen]

I've done more work, including the NoIK renaming, and added it all to #448. Closing this PR.