From efc6869411b97adbfa64d79029880739472aeb42 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 31 Oct 2024 12:08:42 +0100 Subject: [PATCH] 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... --- .../work_space/pose_model_state_space.h | 1 + .../work_space/pose_model_state_space.cpp | 26 +++++++++++++++++-- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 6f7837ae3a5..4ebe272e8d3 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/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 poses_; + double jump_factor_; }; } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 12d95371abb..5e4552ab4a3 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/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()->poses[i], to->as()->poses[i], t, + state->as()->poses[i]); + + // the call above may reset all flags for state; but we know the pose we want flag should be set + state->as()->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()->markInvalid(); + } } void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)