From 8debe68f99e92dc451e8132f1594670c8d8b6573 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 4 Mar 2020 10:05:36 +0100 Subject: [PATCH] fix joint_interpolation - return a trajectory in any case (even if there is no motion needed) - check feasability of goal pose --- core/src/solvers/joint_interpolation.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index f77c2ff1..cf09bc0a 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -64,16 +64,16 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const moveit::core::RobotState& to_state = to->getCurrentState(); for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels()) d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm)); - if (d < 1e-6) - return true; // return empty result trajectory: no motion needed result = std::make_shared(from->getRobotModel(), jmg); // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); + if (from->isStateColliding(from_state, jmg->getName())) + return false; moveit::core::RobotState waypoint(from_state); - double delta = props.get("max_step") / d; + double delta = d < 1e-6 ? 1.0 : props.get("max_step") / d; for (double t = delta; t < 1.0; t += delta) { from_state.interpolate(to_state, t, waypoint); result->addSuffixWayPoint(waypoint, t); @@ -83,7 +83,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } // add goal point - result->addSuffixWayPoint(to->getCurrentState(), 1.0); + result->addSuffixWayPoint(to_state, 1.0); + if (from->isStateColliding(to_state, jmg->getName())) + return false; // add timing, TODO: use a generic method to add timing via plugins trajectory_processing::IterativeParabolicTimeParameterization timing;