Commits on Jul 15, 2024

1. Fix constrained-based planning / PoseModelStateSpace …

   * 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.

   

   rhaschke committed Jul 15, 2024
   Configuration menu

   • View commit details
   • Copy full SHA for f3dd1b0
   • Browse repository at this point

   Copy the full SHA

   f3dd1b0 View commit details

   Browse the repository at this point in the history