SimplePlanner interpretation of cartesian waypoint seed

[rjoomen]

In the simple planner, the seed of a cartesian waypoint is used directly. This way, it seems to me a cartesian waypoint with seed waypoint effectively functions as a joint waypoint: the simple planner interpolates towards the seed, instead of the cartesian waypoint.

I was expecting the seed to be used as seed for the IK, like so:

j2 = getClosestJointSolution(base, base_cwp.getSeed().position);

Could you explain the reasoning behind this? And how can I set an actual seed for a cartesian waypoint that does produce a correctly interpolated path?

[Levi-Armstrong]

The simple planner in most cases is to generate a seed trajectory which is then passed to the planners. If the cartesian waypoint already has one it is assumed this is the seed you want it too use and it will avoid running IK. Though I do see your use case and I think there are two option, the first create a new set of profiles to have this behavior or add an option to the existing profiles to change how seeds are handled if provided for cartesian waypoints. I would vote for the latter option.

[Levi-Armstrong]

I see your point, but currently the profiles technically don't make this decision.

The simple planner LVS profile should be interpolating based on linear or freespace type and then the seed assigned should use the provided seed state if one exists and if not it will run IK.

  if (base_cwp.hasSeed())
    j2 = base_cwp.getSeed().position;
  else
    j2 = getClosestJointSolution(base, j1);

Why wouldn't you just replace a Cartesian waypoint with a joint waypoint from the beginning if the intent is to replace it with a joint seed position?

The cartesian information need to be preserved for planners like trajopt which can represent cartesian waypoints within the planner and the seed associated would be used to initialize the solver state.

[marip8]

The function you linked is for interpolation, but it doesn't truly interpolate between the two input waypoints. It copies a seed instruction (which is not necessarily a valid IK solution for the given Cartesian waypoint) and interpolates between a "fake" joint waypoint and the target joint waypoint. My point is that this function shouldn't be making this decision, the profile should be

The cartesian information need to be preserved for planners like trajopt which can represent cartesian waypoints within the planner and the seed associated would be used to initialize the solver state.

Yes, but why not just solve IK? I guess it's more flexible technically, but at the end of the day won't you still be using IK at some level to generate this seed?

If you're going to use the seed in this way, I think it should be explicitly differentiated from an IK seed. Maybe it should be called instruction_seed or joint_state_approximation with a docstring that describes its intended purpose

[Levi-Armstrong]

Yes, but why not just solve IK? I guess it's more flexible technically, but at the end of the day won't you still be using IK at some level to generate this seed?

This breaks chaining planners together because the seed would get over written within the planner. Also IK can be expensive so I wanted to avoid unnecessary calls throughout the pipeline. This way if a seed exists it is used and no IK is ran.

[Levi-Armstrong]

The function you linked is for interpolation, but it doesn't truly interpolate between the two input waypoints. It copies a seed instruction (which is not necessarily a valid IK solution for the given Cartesian waypoint) and interpolates between a "fake" joint waypoint and the target joint waypoint. My point is that this function shouldn't be making this decision, the profile should be

The functions were explicitly written for the simple planner profiles and are separated functions to make the code more readable and easier to test. So the profiles are making the decision. As I suggested we could updated the existing profiles to allow this different behavior which would then result in the functions being updated to support the option.

[Levi-Armstrong]

If you're going to use the seed in this way, I think it should be explicitly differentiated from an IK seed. Maybe it should be called instruction_seed or joint_state_approximation with a docstring that describes its intended purpose

The way the seed is used is not dictated by the waypoint but by the profiles, but renaming the method to getJointStateApprox() and setJointStateApprox() may cause less confusion.

[rjoomen]

Upon giving this a bit more thought I realized that probably when setting a seed I might as well do a full IK and use that as the seed. But I'll keep the idea of adding this as an option in mind.

[Levi-Armstrong]

Upon giving this a bit more thought I realized that probably when setting a seed I might as well do a full IK and use that as the seed. But I'll keep the idea of adding this as an option in mind.

This is typically what I do because a cartesian waypoint may have multiple solution and I want to be explicit and make it use a specific one.