From 4a76656ff7921de6f2ca1f56e85f90a4f9b74e13 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 27 Jun 2024 21:00:47 +0200 Subject: [PATCH] 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. --- .../work_space/pose_model_state_space.h | 1 - .../work_space/pose_model_state_space.cpp | 37 +------------------ 2 files changed, 2 insertions(+), 36 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 5adb82bb44..15571ce99f 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 @@ -137,6 +137,5 @@ 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 8f050071ae..e87826810f 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 @@ -50,8 +50,6 @@ const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = " ompl_interface::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); else if (!spec.joint_model_group_->getGroupKinematics().second.empty()) @@ -128,42 +126,11 @@ void ompl_interface::PoseModelStateSpace::sanityChecks() const void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, ompl::base::State* state) const { - // moveit::Profiler::ScopedBlock sblock("interpolate"); - - // 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) + // moveit::Profiler::ScopedBlock sblock("interpolate"); // 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()->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); - - /* - 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()->markInvalid(); - } + computeStateFK(state); } void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,