Moveit: ComputeCartesianPath related improvements

[mlautman]

A summary of thoughts from a call with @rhaschke and @davetcoleman on ComputeCartesianPath functionality:

• [ ] computeCartesianPath() should be moved out of RobotState
• [ ] OMPL provides continuous collision checking through FCL. We could optionally enable this for users to ensure no collisions occur between waypoints
• [ ] Use the max_eef step parameter to prune paths that have large movements in Cartesian space between waypoints
• [ ] Use the jump_threshold to compare each joint to itself rather than combining all joints into a single average movement parameter
• [ ] There was an old open source project that implemented a backtracking feature that would identify path forks as it generated the Cartesian path so that on failure it could backtrack to most recent fork rather than fail. We could incorporate a similar method to make computeCartesianPath more robust
• [ ] computeDescartesPath
• [ ] API change to replace jump_threshold that have better theoretical backing.

[BryceStevenWilley]

For some previous discussion on 2, there's #29.

[mlautman]

To follow up on point 4, I implemented this and found that it rejected a lot of trajectories that could be considered valid. Outputting the joint value deltas for each index in an example trajectory of a rejected path shows that the jump at ix: 4 for the 4th joint is actually a very small movement but the algorithm sees it as a jump because it only looks at ratios not absolute joint space jumps. I would see this as evidence for the argument for an absolute jump threshold #773

dist matrix:
ix: 0, 0.074, 0.234, 0.109, 0.417, 0.000, 0.274, 0.036
ix: 1, 0.106, 0.131, 0.131, 0.209, 0.000, 0.154, 0.039
ix: 2, 0.133, 0.092, 0.156, 0.149, 0.000, 0.114, 0.040
ix: 3, 0.154, 0.059, 0.178, 0.114, 0.000, 0.087, 0.040
ix: 4, 0.165, 0.028, 0.192, 0.088, 0.014, 0.066, 0.038

[v4hn]

Some comments:

1. This is probably the most commonly used method in actual demos all over MoveIt.
   So at least we should provide a one-line c++-function-only replacement if we ever remove it there.
   Additionally, but with more overhead, It has been requested before to put this functionality into a separate Cartesian PlannerManager plugin to support other pipeline features (fix start state, adapt time parameterization, ...).
   If we want to do this, we should also have an official PlannerManagerManager to support multiple planner plugins at the same time.

2. continuous collision checking only gets you so far because FCL does not support motions of articulated chains (yet?). As far as I understand it, we would have to discretize the articulated motion into primitive movement types and that somewhat defeats the purpose.

3. I don't understand what this means here. The trajectory eef waypoints are generated with a distance of max_eef length to begin with. So where do you want to prune here?

Yes to 5!

(5a) To support such a feature we should probably also try to interpolate waypoints if absolute joint thresholds (which we don't have in the current API) are too high to detect infeasible IK solutions at some step.

[mlautman]

To elaborate on number 3: We discussed taking the midpoint of the joint trajectories and solving the FK to ensure that the distance from the start and end points are less than the max_eef.

[v4hn]

We discussed taking the midpoint of the joint trajectories and solving the FK to ensure that the distance from the start and end points are less than the max_eef.

That makes sense as a last resort after workspace interpolation failed.
But even then this would not guarantee that the interpolated state lies on the same Cartesian trajectory.
So either we enforce a very high hard-coded threshold there or we would have to introduce another parameter...

[mlautman]

To add to the list from above:

1. computeDescartesPath
2. API change to replace jump_threshold that have better theoretical backing.

[davetcoleman]

@mlautman can you update your original issue description to include the 2 new goals in your above comment? Could you also turn the bullet points into check boxes and check off the ones you have completed, or link to open pull requests if they are in progress?

[felixvd]

I suppose "computeDescartesPath" refers to this?

I agree that computeCartesianPath is expected by almost all users, and it's not doing very well. I just had a case where it fails to plan a short approach path that works fine with regular move(), because of (I believe) an issue in the sampler.

To add to the list:

• The tutorial for computeCartesianPath does not show how to execute the computed trajectory. This isn't trivial because:
• computeCartesianPath has no logical follow-up function. It returns a trajectory, but no MoveGroupInterface function accepts it as an input. Users following the tutorial have to google how a plan is set up and hack together something like this:

moveit::planning_interface::MoveGroupInterface::Plan myplan;
myplan.trajectory_ = trajectory;
group.execute(myplan);

One solution is implementing execute(const RobotTrajectory &trajectory), another would be to follow the example of many robot firmwares and offer a one-line moveLIN() command.

[k-okada]

I agree that computeCartesianPath is expected by almost all users, and it's not doing very well.

Agreed, and from beginners point of view, supporting cartesian path in GUI is needed. see https://github.com/ros-planning/moveit/pull/931

[felixvd]

In line with what you wrote in that PR, I would add considering a name change to something like "computeLinearPath". I know it's technically still ambiguous, but PTP, LIN and CIRC motions are industry parlance. A look at this shows that the terms are used that way in the scripting languages of at least ABB, UR, KUKA, DENSO, Fanuc, HIWIN and Yamaha.* Is there a reason for the current name? If there isn't, I'd be in favor of following the standard of the rest of the robotics world.

*Some use "moveJ" or similar instead of "PTP", but all of them say "linear" instead of "cartesian". Mitsubishi and some others seem to use "straight" instead of "linear".

[gavanderhoorn]

@felixvd wrote:

but PTP, LIN and CIRC motions are industry parlance. [..] I'd be in favor of following the standard of the rest of the robotics world.

Those names may be common in many industrial robot programming languages, but MoveIt is not such a language. Those names are also as they are because most industrial robots are programmed in something akin to an assembly language, where proper names for identifiers are hard because everything has to fit on tiny LCD screens, sometimes even text-only.

In addition, the motion primitives that you mention (ie: PTP, LIN and CIRC) are semantically different from the functionality MoveIt offers (ignoring the fact that MoveIt doesn't have circular trajectory generation).

So if we start using such language, people coming from contexts in which that nomenclature is common will assume that they know what it all means and they will have certain expectations wrt behaviour of the algorithms behind those names.

I'm not saying that computeCartesianPath is the best name, nor am I saying that we can never introduce functions with names similar to industrial robot programming languages. It just feels like renaming computeCartesianPath to LIN or MOVEL may not do as much for the people that know those names, while it probably doesn't matter to those that don't already know them (ie: users without experience with industrial robot programming languages).

[felixvd]

MoveIt itself is not a robot programming language, but the MoveGroupInterface and MoveGroupCommander serve a similar purpose to end users. We would do well to consider their expectations. While I wouldn't copy any names, I do think it is a good idea to connect users with familiar concepts. I think most novice users just have something like "PTP is unpredictable, LIN goes in a line" in their heads.

Regardless, I also think computeLinearPath is a better name than computeCartesianPath in its own right, so we can stash the first paragraph of this answer as an aside.

[pbeeson]

So given that absolute jump_thresholds were added at the low-level, but these are NOT supported via message calls through move_group, if I were to extend computeCartesianPath() to allow absolute prismatic/revolute thresholds, is there any possibility in getting this merged since the first item on this list is to move this out of RobotState?

I just don't want to set off on this to have absolute joint checking if there's a parallel effort going on, or if the sentiment is that this all needs replacing anyways, so no point in making it better as is. IF that's the case, I'll port an existing Cartesian path solver as a plugin.

If this is something that has merit, perhaps one idea of a simple solution that would provide the maximum usefulness for the largest population is just to add an optional bool parameter to computeCartesianPath() that defaults to false (so old behavior is the default), that is simply "avoid_rollouts" (or similar) that ensures that (for revolute joints only) there is never a delta jump of more than PI.

This would be the same as having a absolute_revolute_thresholed=PI and absolute_prismatic_threshold=0 is I were to extend the API like I mentioned at the top of this post.

[pbeeson]

So given that absolute jump_thresholds were added at the low-level, but these are NOT supported via message calls through move_group, if I were to extend computeCartesianPath() to allow absolute prismatic/revolute thresholds, is there any possibility in getting this merged since the first item on this list is to move this out of RobotState?

This was less work than I thought previously, so I did it. I hope people find it worthwhile.
See PR #2181

[s-trinh]

I have stumbled on this IK jumps issue for a very long time before actually understanding what was happening. I will try to describe the issue to help novice user understanding the IK jumps issue.

---

IK jumps issue visualization

In my mind, IK jumps were only big jumps in joint space like elbow up/down configuration or big jumps for redundant joints, while it is not necessary the case.

The following is the visualization of the IK jumps issue. This is for via points mode for the fake controller execution.

Notice how the arm pauses a moment when clearing the tile (and the "jump" in the final arm configuration).

Now, the same trajectory but with interpolate mode for the fake controller execution.

Notice now what is happening when the arm was previously "static" with via points mode and now with interpolate. The arm seems to perform weird movement. This is where the IK jumps is happening. There is a jump in the joint trajectory when solving the Cartesian move, not directly visible with via points mode but clearly visible with interpolate. Because ultimately you need to interpolate the joint trajectory to actually control the real robot, the robot can perform weird movement. Moreover, if collision checking is done only on via points trajectory, collision issues can occur. Also, what about joint limit constraints? Can these be violated during the interpolation?

Another way to see this IK jumps issue is to plot the joint trajectory.

When the Cartesian trajectory is ok, you have the following:

Now, with the faulty trajectory:

You can clearly see the IK jumps issue.

Recall what is done behind computeCartesianPath():

• the linear trajectory between two waypoints is interpolated in the Cartesian space with at most eef_step distance in Cartesian space between interpolated points
• the IK solver is called for each of these interpolated points, while trying to guaranteeing the joint trajectory continuity
• what is happening with the IK jumps issue is that the Cartesian interpolated points are evenly spaced in Cartesian space
• but not in the joint space, recall in the via points mode when the arm seems to be paused: if you look in Cartesian space, the end-effector will correctly pass through the interpolated points in 3D but in the joint space there is now this jump issue

Another example of the issue:

---

Feedback about MoveIt documentation

I am using the MoveGroupInterface move_group.computeCartesianPath(); with jump_threshold=0.

This is totally wrong. My mistake.

I am not a native English speaker, and maybe these feedbacks could be useful to improve the MoveIt documentation:

• in the MoveIt tutorial, there is:

We want the Cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the max step in Cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it. Warning - disabling the jump threshold while operating real hardware can cause large unpredictable motions of redundant joints and could be a safety issue

Maybe it is only me, but I understood these sentences as setting 0 will disable the possibility to have IK jumps, that is of course the opposite. effectively disabling it should be rephrased to understand what is disabled.

• in the computeCartesianPath() documentation:

No more than jump_threshold is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK solutions).

So for me, again setting jump_threshold=0 would have meant that we want 0 number of jumps.

This is probably only me that was wrongly understanding the documentation, but I think the documentation can be improved, especially to specify that jump_threshold is used in this formula: [single joint state change] < jump_threshold * [mean joint state change]

---

Questions

For me the open questions are:

• how to choose the value of jump_threshold, especially when you have prismatic and revolute joints?
• do I need to switch to the RobotState::computeCartesianPath() to be able to set the jt_revolute and jt_prismatic variables for better results?
• is it possible to use the time_from_start variable to (indirectly) detect IK jumps? From the joint trajectory plotting, it can be clearly seen that the interpolated values are not evenly sampled.

---

Related issues for additional information: #773 #798 #821

[gavanderhoorn]

In my mind, IK jumps were only big jumps in joint space like elbow up/down configuration or big jumps for redundant joints

Those are in general called "configuration changes".

[s-trinh]

For what it's worth.

I don't want to use the jump_threshold parameter from the MGI because:

• it does not make the distinction between prismatic and revolute joints that can have different motion ranges
• it uses the mean distance that is not robust for me

For someone with almost no experience with MoveIt codebase, using RobotState::computeCartesianPath() (to use jt_revolute and jt_prismatic) looks too complicated because I think I would have to do all this but in my code.

2181 looks great but will be available only for master.

My solution that seems to work for my use case is to detect jump from time_from_start difference distribution. I have used MAD that seems to work, with one parameter to tune.