Fix constrained-based planning#3615
Conversation
Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameter enforce_joint_model_state_space in PR moveit#541 as a work-around. A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space. However, as the trajectory is random-sampled anyway, this shouldn't be a big issue.
|
Codecov ReportAttention: Patch coverage is
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## master #3615 +/- ##
==========================================
- Coverage 47.16% 47.10% -0.06%
==========================================
Files 603 603
Lines 59196 59215 +19
Branches 7024 6994 -30
==========================================
- Hits 27915 27886 -29
- Misses 30863 30911 +48
Partials 418 418 ☔ View full report in Codecov by Sentry. |
|
I think it would be also sensible to switch distance computation from Cartesian space to joint space: Similar eef poses in Cartesian spaces can be generated by very different joint poses. What actually matters when connecting states is the joint-space distance. Any thoughts, @mamoll and @zkingston? Edit: Turns out that both interpolation and distance measurement in joint-space are required: Without joint-space interpolation, only a few states will be generated, leading to an invalid trajectory. Without joint-space distance measurement, an actually valid path (with multiple states) will be simplified to an invalid one with only the two far-apart start and end states: |
|
In my experience, using the joint space metric is better than an EE-based metric in workspace for the exact reason you listed, and is what I usually use when doing any kind of constrained planning. There are many configurations that are similar in EE pose that are very far apart in joint space, and interpolation is far more likely to fail when they are far apart in joint space. |
|
If you interpolate in joint space, how would the motion still be constrained? I am confused: it sounds like the fix to constrained planning is to remove constraints. If the constrained planning is correctly implemented, then it should be possible to find constrained EE paths (with probabilistic completeness guarantees). Does the IK solver use the correct seed to find nearby solutions? The constrained interpolator should reject motions with big jumps in joint space. |
The interpolated state will be validated (and rejected if not satisfying the constraints).
Interpolating in EE space and computing IK can result in joint-space poses far away from the poses of one or the other interpolation states (or both). Thus we had big joint-space jumps. Interpolation in joint-space will allow to reject those path connections because the interpolated state probably will not satisfy the constraints anymore. |
Configurations may be similar in Cartesian EE pose, but very far apart in joint space. What actually matters when connecting states is the joint-space distance.
|
I see. When I saw the description, I was thinking of the new constrained-based planning in MoveIt2 based on @zkingston's work in OMPL. This is not that. Your changes look fine to me and make the code look a lot cleaner and simpler. I suspect that this will make planning also much faster (since it's not calling IK as often). |
* Fix constrained-based planning Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameter enforce_joint_model_state_space in PR moveit#541 as a work-around. A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space. However, as the trajectory is random-sampled anyway, this shouldn't be a big issue. * PoseModelStateSpace: Use joint-space distance Configurations may be similar in Cartesian EE pose, but very far apart in joint space. What actually matters when connecting states is the joint-space distance.
#3615 made PoseModelStateSpace identical to ModelBasedStateSpace, both using joint-space interpolation and distance measurements. This resulted in planning timeouts for code that previously worked. Obviously, using joint-space interpolation much more often results in the rejection of tight path constraints than Cartesian interpolation did. The problem with potential jumps due to Cartesian interpolation in principle remains, which is a dilemma of the current design: - We do need Cartesian interpolation to reduce the risk of state rejection due to a failing path constraint, which currently cannot be checked during interpolation. - Ideally, we would do joint interpolation and only resort to Cartesian interpolation if the path constraint is violated, i.e. follow a similar approach as in #3618. However, this would require access to the path constraint...
Constrained-based planning enables the
PoseModelStateSpaceby default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameterenforce_joint_model_state_spacein PR #541 as a work-around.A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion:
To avoid this issue, interpolation and distance measurement should be performed in joint space. The only drawback of this change is that trajectories are less linear in Cartesian space (see videos below). However, as the trajectory is random-sampled anyway, this shouldn't be a big issue.