Fix constrained-based planning / PoseModelStateSpace (#2910)

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

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h

@@ -145,6 +145,5 @@ class PoseModelStateSpace : public ModelBasedStateSpace
   };
 
   std::vector<PoseComponent> poses_;
-  double jump_factor_;
 };
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp

@@ -54,8 +54,6 @@ const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel";
 
 PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec)
 {
-  jump_factor_ = 3;  // \todo make this a param
-
   if (spec.joint_model_group_->getGroupKinematics().first)
   {
     poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first);
@@ -82,10 +80,7 @@ PoseModelStateSpace::~PoseModelStateSpace() = default;
 
 double PoseModelStateSpace::distance(const ompl::base::State* state1, const ompl::base::State* state2) const
 {
-  double total = 0;
-  for (std::size_t i = 0; i < poses_.size(); ++i)
-    total += poses_[i].state_space_->distance(state1->as<StateType>()->poses[i], state2->as<StateType>()->poses[i]);
-  return total;
+  return ModelBasedStateSpace::distance(state1, state2);
 }
 
 double PoseModelStateSpace::getMaximumExtent() const
@@ -136,42 +131,9 @@ void PoseModelStateSpace::sanityChecks() const
 void PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t,
                                       ompl::base::State* state) const
 {
-  // we want to interpolate in Cartesian space; we do not have a guarantee that from and to
-  // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states)
-
   // interpolate in joint space
   ModelBasedStateSpace::interpolate(from, to, t, state);
-
-  // interpolate SE3 components
-  for (std::size_t i = 0; i < poses_.size(); ++i)
-  {
-    poses_[i].state_space_->interpolate(from->as<StateType>()->poses[i], to->as<StateType>()->poses[i], t,
-                                        state->as<StateType>()->poses[i]);
-  }
-
-  // the call above may reset all flags for state; but we know the pose we want flag should be set
-  state->as<StateType>()->setPoseComputed(true);
-
-  /*
-  std::cout << "*********** interpolate\n";
-  printState(from, std::cout);
-  printState(to, std::cout);
-  printState(state, std::cout);
-  std::cout << "\n\n";
-  */
-
-  // after interpolation we cannot be sure about the joint values (we use them as seed only)
-  // so we recompute IK if needed
-  if (computeStateIK(state))
-  {
-    double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to);
-    double d_from = ModelBasedStateSpace::distance(from, state);
-    double d_to = ModelBasedStateSpace::distance(state, to);
-
-    // if the joint value jumped too much
-    if (d_from + d_to > std::max(0.2, dj))  // \todo make 0.2 a param
-      state->as<StateType>()->markInvalid();
-  }
+  computeStateFK(state);
 }
 
 void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)