PoseModelStateSpace: Reintroduce Cartesian interpolation

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

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

@@ -145,5 +145,6 @@ 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,6 +54,8 @@ const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel";
 
 PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec)
 {
+  jump_factor_ = 1.5;  // \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);
@@ -131,9 +133,29 @@ 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
 {
-  // interpolate in joint space
+  // we want to interpolate in Cartesian space to avoid rejection of path constraints
+
+  // interpolate in joint space to find a suitable seed for IK
   ModelBasedStateSpace::interpolate(from, to, t, state);
-  computeStateFK(state);
+  double d_joint = ModelBasedStateSpace::distance(from, 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);
+
+  // compute IK for interpolated Cartesian state
+  if (computeStateIK(state))
+  {
+    double d_cart = ModelBasedStateSpace::distance(from, state);
+
+    // reject if Cartesian interpolation yields much larger distance than joint interpolation
+    if (d_cart > jump_factor_ * d_joint)
+      state->as<StateType>()->markInvalid();
+  }
 }
 
 void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)