Fix constrained-based planning / PoseModelStateSpace #2910
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
* 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.
sjahr
approved these changes
Jul 19, 2024
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
This is a forward port of moveit/moveit#3615
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 #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.
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.