From b79ec830712d5928a1f48130cfa516d8dc9b51c2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 Feb 2023 20:44:28 +0100 Subject: [PATCH 1/8] Merge PR #429: MultiPlanner MultiPlanner --- .../task_constructor/solvers/cartesian_path.h | 6 +- .../task_constructor/solvers/multi_planner.h | 77 +++++++++++++++ .../solvers/planner_interface.h | 9 ++ core/src/CMakeLists.txt | 4 +- core/src/solvers/cartesian_path.cpp | 2 +- core/src/solvers/joint_interpolation.cpp | 8 +- core/src/solvers/multi_planner.cpp | 98 +++++++++++++++++++ core/src/solvers/pipeline_planner.cpp | 2 +- core/src/solvers/planner_interface.cpp | 1 + demo/src/pick_place_task.cpp | 4 +- 10 files changed, 200 insertions(+), 11 deletions(-) create mode 100644 core/include/moveit/task_constructor/solvers/multi_planner.h create mode 100644 core/src/solvers/multi_planner.cpp diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 41473e368..5ebe02260 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -56,8 +56,10 @@ class CartesianPath : public PlannerInterface void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } - void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); } - void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); } + [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off + void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } + [[deprecated("Replace with setMaxAccelerationScaling")]] // clang-format off + void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } void init(const moveit::core::RobotModelConstPtr& robot_model) override; diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h new file mode 100644 index 000000000..2487dadff --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -0,0 +1,77 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: meta planner, running multiple planners in parallel or sequence +*/ + +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +MOVEIT_CLASS_FORWARD(MultiPlanner); + +/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution. + * + * This is useful to sequence different planning strategies of increasing complexity, + * e.g. Cartesian or joint-space interpolation first, then OMPL, ... + * This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each + * individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before + * switching to the next child, which possibly applies a different planning strategy. + */ +class MultiPlanner : public PlannerInterface, public std::vector +{ +public: + using PlannerList = std::vector; + using PlannerList::PlannerList; // inherit all std::vector constructors + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; +}; +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 93ef7772e..760d1155e 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene); namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory); } +namespace trajectory_processing { +MOVEIT_CLASS_FORWARD(TimeParameterization); +} namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(LinkModel); @@ -75,6 +78,12 @@ class PlannerInterface const PropertyMap& properties() const { return properties_; } void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); } + void setTimeout(double timeout) { properties_.set("timeout", timeout); } + void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } + void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); } + void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) { + properties_.set("time_parameterization", tp); + } virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index f844f89e4..bab666876 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} SHARED ${PROJECT_INCLUDE}/solvers/cartesian_path.h ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h + ${PROJECT_INCLUDE}/solvers/multi_planner.h container.cpp cost_terms.cpp @@ -32,8 +33,9 @@ add_library(${PROJECT_NAME} SHARED solvers/planner_interface.cpp solvers/cartesian_path.cpp - solvers/pipeline_planner.cpp solvers/joint_interpolation.cpp + solvers/pipeline_planner.cpp + solvers/multi_planner.cpp ) ament_target_dependencies(${PROJECT_NAME} moveit_core diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 02be9a754..07eadef7f 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -75,7 +75,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, // reach pose of forward kinematics return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, - timeout, result, path_constraints); + std::min(timeout, properties().get("timeout")), result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 7b3186a64..72923ec0d 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -104,8 +104,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { - const auto start_time = std::chrono::steady_clock::now(); + const moveit_msgs::Constraints& path_constraints) { + timeout = std::min(timeout, properties().get("timeout")); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); auto to{ from->diff() }; @@ -127,8 +128,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } to->getCurrentStateNonConst().update(); - timeout = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); - if (timeout <= 0.0) + if (std::chrono::steady_clock::now() >= deadline) return false; return plan(from, to, jmg, timeout, result, path_constraints); diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp new file mode 100644 index 000000000..7603f7640 --- /dev/null +++ b/core/src/solvers/multi_planner.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& p : *this) + p->init(robot_model); +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 47bafa8eb..bd469022e 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -156,7 +156,7 @@ void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const Prope const moveit::core::JointModelGroup* jmg, double timeout) { req.group_name = jmg->getName(); req.planner_id = p.get("planner"); - req.allowed_planning_time = timeout; + req.allowed_planning_time = std::min(timeout, p.get("timeout")); req.start_state.is_diff = true; // we don't specify an extra start state req.num_planning_attempts = p.get("num_planning_attempts"); diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 617fb476a..a58b110da 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -47,6 +47,7 @@ namespace solvers { PlannerInterface::PlannerInterface() { auto& p = properties(); + p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); p.declare("time_parameterization", std::make_shared()); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index a52f80759..be989b939 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -117,8 +117,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t // Cartesian planner auto cartesian_planner = std::make_shared(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setMaxVelocityScalingFactor(1.0); + cartesian_planner->setMaxAccelerationScalingFactor(1.0); cartesian_planner->setStepSize(.01); // Set task properties From bf58108e5dfc76b8f147f0a9e38a412c13fb8dcd Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 3 Apr 2023 08:51:29 +0200 Subject: [PATCH 2/8] Port to ROS 2 --- core/include/moveit/task_constructor/solvers/multi_planner.h | 4 ++-- core/src/solvers/joint_interpolation.cpp | 2 +- core/src/solvers/multi_planner.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index 2487dadff..2ddb47f51 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -65,12 +65,12 @@ class MultiPlanner : public PlannerInterface, public std::vector("timeout")); const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp index 7603f7640..cfb90ab5b 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/multi_planner.cpp @@ -52,7 +52,7 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); @@ -75,7 +75,7 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); From dd7aa11bbda3c3077a579ca7819753ce0190db7e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 3 Apr 2023 08:57:43 +0200 Subject: [PATCH 3/8] Rename multi-planner to fallback planner --- .../{multi_planner.h => fallback_planner.h} | 8 +++---- core/src/CMakeLists.txt | 4 ++-- ...multi_planner.cpp => fallback_planner.cpp} | 22 +++++++++---------- 3 files changed, 17 insertions(+), 17 deletions(-) rename core/include/moveit/task_constructor/solvers/{multi_planner.h => fallback_planner.h} (91%) rename core/src/solvers/{multi_planner.cpp => fallback_planner.cpp} (77%) diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/fallback_planner.h similarity index 91% rename from core/include/moveit/task_constructor/solvers/multi_planner.h rename to core/include/moveit/task_constructor/solvers/fallback_planner.h index 2ddb47f51..0876365ac 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/fallback_planner.h @@ -45,17 +45,17 @@ namespace moveit { namespace task_constructor { namespace solvers { -MOVEIT_CLASS_FORWARD(MultiPlanner); +MOVEIT_CLASS_FORWARD(FallbackPlanner); /** A meta planner that runs multiple alternative planners in sequence and returns the first found solution. * * This is useful to sequence different planning strategies of increasing complexity, * e.g. Cartesian or joint-space interpolation first, then OMPL, ... - * This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each - * individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before + * This is (slightly) different from the Fallbacks container, as the FallbackPlanner directly applies its planners to + * each individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before * switching to the next child, which possibly applies a different planning strategy. */ -class MultiPlanner : public PlannerInterface, public std::vector +class FallbackPlanner : public PlannerInterface, public std::vector { public: using PlannerList = std::vector; diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index bab666876..3ace4d4de 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -18,7 +18,7 @@ add_library(${PROJECT_NAME} SHARED ${PROJECT_INCLUDE}/solvers/cartesian_path.h ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h - ${PROJECT_INCLUDE}/solvers/multi_planner.h + ${PROJECT_INCLUDE}/solvers/fallback_planner.h container.cpp cost_terms.cpp @@ -35,7 +35,7 @@ add_library(${PROJECT_NAME} SHARED solvers/cartesian_path.cpp solvers/joint_interpolation.cpp solvers/pipeline_planner.cpp - solvers/multi_planner.cpp + solvers/fallback_planner.cpp ) ament_target_dependencies(${PROJECT_NAME} moveit_core diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/fallback_planner.cpp similarity index 77% rename from core/src/solvers/multi_planner.cpp rename to core/src/solvers/fallback_planner.cpp index cfb90ab5b..6f300ee81 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/fallback_planner.cpp @@ -36,7 +36,7 @@ Desc: generate and validate a straight-line Cartesian path */ -#include +#include #include #include @@ -44,15 +44,15 @@ namespace moveit { namespace task_constructor { namespace solvers { -void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { +void FallbackPlanner::init(const core::RobotModelConstPtr& robot_model) { for (const auto& p : *this) p->init(robot_model); } -bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { +bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); @@ -71,11 +71,11 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, return false; } -bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { +bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); From b75338f56b5cfe9fe612ffdc1336637f42e86a22 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 3 Apr 2023 09:32:03 +0200 Subject: [PATCH 4/8] Add stopping criterion function --- .../solvers/fallback_planner.h | 28 +++++ core/src/solvers/alternatives_planner.cpp | 105 ++++++++++++++++++ core/src/solvers/fallback_planner.cpp | 31 ++++-- 3 files changed, 152 insertions(+), 12 deletions(-) create mode 100644 core/src/solvers/alternatives_planner.cpp diff --git a/core/include/moveit/task_constructor/solvers/fallback_planner.h b/core/include/moveit/task_constructor/solvers/fallback_planner.h index 0876365ac..1afde45a0 100644 --- a/core/include/moveit/task_constructor/solvers/fallback_planner.h +++ b/core/include/moveit/task_constructor/solvers/fallback_planner.h @@ -39,12 +39,23 @@ #pragma once #include +#include #include namespace moveit { namespace task_constructor { namespace solvers { +/** \brief A stopping criterion which is evaluated after running a planner. + * \param [in] result Most recent result of the last planner invoked + * \param [in] path_constraints Set of path constraints passed to the solver + * \return If it returns true, the corresponding trajectory is considered the solution, otherwise false is returned and + * the next fallback planner is called. + */ +typedef std::function + StoppingCriterionFunction; + MOVEIT_CLASS_FORWARD(FallbackPlanner); /** A meta planner that runs multiple alternative planners in sequence and returns the first found solution. @@ -71,6 +82,23 @@ class FallbackPlanner : public PlannerInterface, public std::vector +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +void FallbackPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& p : *this) + p->init(robot_model); +} + +bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) { + return false; // timeout} + if (result) { + result->clear(); + } + p->plan(from, to, jmg, remaining_time, result, path_constraints); + + if (stopping_criterion_function_(result, path_constraints)) { + return true; + } + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; + } +} + +bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints); + + if (stopping_criterion_function_(result, path_constraints)) { + return true; + } + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/solvers/fallback_planner.cpp b/core/src/solvers/fallback_planner.cpp index 6f300ee81..215431e6a 100644 --- a/core/src/solvers/fallback_planner.cpp +++ b/core/src/solvers/fallback_planner.cpp @@ -37,7 +37,6 @@ */ #include -#include #include namespace moveit { @@ -57,18 +56,23 @@ bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, auto start_time = std::chrono::steady_clock::now(); for (const auto& p : *this) { - if (remaining_time < 0) - return false; // timeout - if (result) - result->clear(); - if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) - return true; + if (remaining_time < 0) { + return false; // timeout} + if (result) { + result->clear(); + } + p->plan(from, to, jmg, remaining_time, result, path_constraints); - auto now = std::chrono::steady_clock::now(); - remaining_time -= std::chrono::duration(now - start_time).count(); - start_time = now; + if (stopping_criterion_function_(result, path_constraints)) { + return true; + } + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; } - return false; } bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, @@ -84,8 +88,11 @@ bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co return false; // timeout if (result) result->clear(); - if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) + p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints); + + if (stopping_criterion_function_(result, path_constraints)) { return true; + } auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); From e53a063d1ec6f0a4568734a107f55844c6d429ff Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 3 Apr 2023 10:54:29 +0200 Subject: [PATCH 5/8] Add alternatives planner --- .../solvers/alternatives_planner.h | 88 +++++++++ core/src/CMakeLists.txt | 2 + core/src/solvers/alternatives_planner.cpp | 183 +++++++++++++----- 3 files changed, 229 insertions(+), 44 deletions(-) create mode 100644 core/include/moveit/task_constructor/solvers/alternatives_planner.h diff --git a/core/include/moveit/task_constructor/solvers/alternatives_planner.h b/core/include/moveit/task_constructor/solvers/alternatives_planner.h new file mode 100644 index 000000000..78d2b13d0 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/alternatives_planner.h @@ -0,0 +1,88 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Sebastian Jahr, Robert Haschke + Desc: Meta planner, running multiple planners in parallel +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +MOVEIT_CLASS_FORWARD(AlternativesPlanner); + +/** A meta planner that runs multiple alternative planners in parallel and returns the best solution. + * + * This is useful to try different planning strategies of increasing complexity, + * e.g. Cartesian or joint-space interpolation and OMPL, and choose the most suitable solution out of the ones produced. + */ +class AlternativesPlanner : public PlannerInterface, public std::vector +{ +public: + using PlannerList = std::vector; + using PlannerList::PlannerList; // inherit all std::vector constructors + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + /** \brief Set solution selection function for parallel planning + * \param [in] solution_selection_function New solution selection that will be used + */ + void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function){ + solution_selection_function_ = solution_selection_function; + }; + +protected: + moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_ = &moveit::planning_pipeline_interfaces::getShortestSolution; +}; +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 3ace4d4de..d9385d926 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(${PROJECT_NAME} SHARED ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h ${PROJECT_INCLUDE}/solvers/fallback_planner.h + ${PROJECT_INCLUDE}/solvers/alternatives_planner.h container.cpp cost_terms.cpp @@ -36,6 +37,7 @@ add_library(${PROJECT_NAME} SHARED solvers/joint_interpolation.cpp solvers/pipeline_planner.cpp solvers/fallback_planner.cpp + solvers/alternatives_planner.cpp ) ament_target_dependencies(${PROJECT_NAME} moveit_core diff --git a/core/src/solvers/alternatives_planner.cpp b/core/src/solvers/alternatives_planner.cpp index 215431e6a..0ead26e68 100644 --- a/core/src/solvers/alternatives_planner.cpp +++ b/core/src/solvers/alternatives_planner.cpp @@ -36,69 +36,164 @@ Desc: generate and validate a straight-line Cartesian path */ -#include +#include +#include #include +#include +namespace { +const auto LOGGER = rclcpp::get_logger("AlternativesPlanner"); +using clock = std::chrono::high_resolution_clock; +} // namespace namespace moveit { namespace task_constructor { namespace solvers { -void FallbackPlanner::init(const core::RobotModelConstPtr& robot_model) { - for (const auto& p : *this) - p->init(robot_model); +void AlternativesPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& planner : *this) + planner->init(robot_model); } -bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { - double remaining_time = std::min(timeout, properties().get("timeout")); - auto start_time = std::chrono::steady_clock::now(); - - for (const auto& p : *this) { - if (remaining_time < 0) { - return false; // timeout} - if (result) { - result->clear(); - } - p->plan(from, to, jmg, remaining_time, result, path_constraints); +bool AlternativesPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + moveit::planning_pipeline_interfaces::PlanResponsesContainer plan_responses_container{ this->size() }; + std::vector planning_threads; + planning_threads.reserve(this->size()); + + // Print a warning if more parallel planning problems than available concurrent threads are defined. If + // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work + auto const hardware_concurrency = std::thread::hardware_concurrency(); + if (planning_threads.size() > hardware_concurrency && hardware_concurrency != 0) { + RCLCPP_WARN(LOGGER, + "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " + "hardware ('%d')", + planning_threads.size(), hardware_concurrency); + } + + // Start one planning thread for each available planner + for (const auto& planner : *this) { + auto planning_thread = std::thread([&]() { + // Create trajectory to store planning result in + robot_trajectory::RobotTrajectoryPtr trajectory; - if (stopping_criterion_function_(result, path_constraints)) { - return true; + // Create motion plan response for future evaluation + auto plan_solution = ::planning_interface::MotionPlanResponse(); + moveit::core::robotStateToRobotStateMsg(from->getCurrentState(), plan_solution.start_state); + + // Run planner + auto const t1 = clock::now(); + bool success = planner->plan(from, to, jmg, timeout, result, path_constraints); + plan_solution.planning_time = std::chrono::duration(clock::now() - t1).count(); + + if (success) { + plan_solution.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + plan_solution.trajectory = trajectory; + } else { + plan_solution.error_code = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } + plan_responses_container.pushBack(plan_solution); + }); + + planning_threads.push_back(std::move(planning_thread)); + } - auto now = std::chrono::steady_clock::now(); - remaining_time -= std::chrono::duration(now - start_time).count(); - start_time = now; + // Wait for threads to finish + for (auto& planning_thread : planning_threads) { + if (planning_thread.joinable()) { + planning_thread.join(); } + } + + // Select solution + if (!solution_selection_function_) { + RCLCPP_ERROR(LOGGER, "No solution selection function defined! Cannot choose the best solution so this planner " + "returns failure."); return false; } + + std::vector<::planning_interface::MotionPlanResponse> solutions; + solutions.reserve(1); + solutions.push_back(solution_selection_function_(plan_responses_container.getSolutions())); + + if (solutions.empty()) { + return false; + } + result = solutions.at(1).trajectory; + return bool(solutions.at(1)); } -bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { - double remaining_time = std::min(timeout, properties().get("timeout")); - auto start_time = std::chrono::steady_clock::now(); - - for (const auto& p : *this) { - if (remaining_time < 0) - return false; // timeout - if (result) - result->clear(); - p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints); - - if (stopping_criterion_function_(result, path_constraints)) { - return true; +bool AlternativesPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + moveit::planning_pipeline_interfaces::PlanResponsesContainer plan_responses_container{ this->size() }; + std::vector planning_threads; + planning_threads.reserve(this->size()); + + // Print a warning if more parallel planning problems than available concurrent threads are defined. If + // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work + auto const hardware_concurrency = std::thread::hardware_concurrency(); + if (planning_threads.size() > hardware_concurrency && hardware_concurrency != 0) { + RCLCPP_WARN(LOGGER, + "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " + "hardware ('%d')", + planning_threads.size(), hardware_concurrency); + } + + // Start one planning thread for each available planner + for (const auto& planner : *this) { + auto planning_thread = std::thread([&]() { + // Create trajectory to store planning result in + robot_trajectory::RobotTrajectoryPtr trajectory; + + // Create motion plan response for future evaluation + auto plan_solution = ::planning_interface::MotionPlanResponse(); + moveit::core::robotStateToRobotStateMsg(from->getCurrentState(), plan_solution.start_state); + + // Run planner + auto const t1 = clock::now(); + bool success = planner->plan(from, link, offset, target, jmg, timeout, trajectory, path_constraints); + plan_solution.planning_time = std::chrono::duration(clock::now() - t1).count(); + + if (success) { + plan_solution.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + plan_solution.trajectory = trajectory; + } else { + plan_solution.error_code = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + } + plan_responses_container.pushBack(plan_solution); + }); + + planning_threads.push_back(std::move(planning_thread)); + } + + // Wait for threads to finish + for (auto& planning_thread : planning_threads) { + if (planning_thread.joinable()) { + planning_thread.join(); } + } - auto now = std::chrono::steady_clock::now(); - remaining_time -= std::chrono::duration(now - start_time).count(); - start_time = now; + // Select solution + if (!solution_selection_function_) { + RCLCPP_ERROR(LOGGER, "No solution selection function defined! Cannot choose the best solution so this planner " + "returns failure."); + return false; + } + + std::vector<::planning_interface::MotionPlanResponse> solutions; + solutions.reserve(1); + solutions.push_back(solution_selection_function_(plan_responses_container.getSolutions())); + + if (solutions.empty()) { + return false; } - return false; + result = solutions.at(1).trajectory; + return bool(solutions.at(1)); } } // namespace solvers } // namespace task_constructor From 06129ea2e12a671bd4351c48d6d6c60f75703538 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 3 Apr 2023 10:54:45 +0200 Subject: [PATCH 6/8] Minor variable renaming --- .../moveit/task_constructor/solvers/fallback_planner.h | 4 ++-- core/src/solvers/fallback_planner.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/fallback_planner.h b/core/include/moveit/task_constructor/solvers/fallback_planner.h index 1afde45a0..4d0342c04 100644 --- a/core/include/moveit/task_constructor/solvers/fallback_planner.h +++ b/core/include/moveit/task_constructor/solvers/fallback_planner.h @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Robert Haschke - Desc: meta planner, running multiple planners in parallel or sequence +/* Authors: Robert Haschke, Sebastian Jahr + Desc: Meta planner, running multiple MTC planners in sequence */ #pragma once diff --git a/core/src/solvers/fallback_planner.cpp b/core/src/solvers/fallback_planner.cpp index 215431e6a..7739cd587 100644 --- a/core/src/solvers/fallback_planner.cpp +++ b/core/src/solvers/fallback_planner.cpp @@ -44,8 +44,8 @@ namespace task_constructor { namespace solvers { void FallbackPlanner::init(const core::RobotModelConstPtr& robot_model) { - for (const auto& p : *this) - p->init(robot_model); + for (const auto& planner : *this) + planner->init(robot_model); } bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, @@ -55,13 +55,13 @@ bool FallbackPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); - for (const auto& p : *this) { + for (const auto& planner : *this) { if (remaining_time < 0) { return false; // timeout} if (result) { result->clear(); } - p->plan(from, to, jmg, remaining_time, result, path_constraints); + planner->plan(from, to, jmg, remaining_time, result, path_constraints); if (stopping_criterion_function_(result, path_constraints)) { return true; From 2fdce5537263328b0a792825a34a58ee964ae3e7 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 3 Apr 2023 11:02:58 +0200 Subject: [PATCH 7/8] Format --- .../task_constructor/solvers/alternatives_planner.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/alternatives_planner.h b/core/include/moveit/task_constructor/solvers/alternatives_planner.h index 78d2b13d0..f66754d35 100644 --- a/core/include/moveit/task_constructor/solvers/alternatives_planner.h +++ b/core/include/moveit/task_constructor/solvers/alternatives_planner.h @@ -75,13 +75,15 @@ class AlternativesPlanner : public PlannerInterface, public std::vector Date: Tue, 4 Apr 2023 18:09:32 +0200 Subject: [PATCH 8/8] Check for nullptr and add missing headers --- core/include/moveit/task_constructor/solvers.h | 2 ++ core/include/moveit/task_constructor/solvers/fallback_planner.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/solvers.h b/core/include/moveit/task_constructor/solvers.h index 472fc1523..a31122495 100644 --- a/core/include/moveit/task_constructor/solvers.h +++ b/core/include/moveit/task_constructor/solvers.h @@ -41,3 +41,5 @@ #include "solvers/cartesian_path.h" #include "solvers/joint_interpolation.h" #include "solvers/pipeline_planner.h" +#include "solvers/fallback_planner.h" +#include "solvers/alternatives_planner.h" diff --git a/core/include/moveit/task_constructor/solvers/fallback_planner.h b/core/include/moveit/task_constructor/solvers/fallback_planner.h index 4d0342c04..318804a1d 100644 --- a/core/include/moveit/task_constructor/solvers/fallback_planner.h +++ b/core/include/moveit/task_constructor/solvers/fallback_planner.h @@ -94,7 +94,7 @@ class FallbackPlanner : public PlannerInterface, public std::vector