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/alternatives_planner.h b/core/include/moveit/task_constructor/solvers/alternatives_planner.h new file mode 100644 index 000000000..f66754d35 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/alternatives_planner.h @@ -0,0 +1,90 @@ +/********************************************************************* + * 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/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/fallback_planner.h b/core/include/moveit/task_constructor/solvers/fallback_planner.h new file mode 100644 index 000000000..318804a1d --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/fallback_planner.h @@ -0,0 +1,105 @@ +/********************************************************************* + * 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, Sebastian Jahr + Desc: Meta planner, running multiple MTC planners in sequence +*/ + +#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. + * + * 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 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 FallbackPlanner : 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 stopping criterion function for parallel planning + * \param [in] stopping_criterion_callback New stopping criterion function that will be used + */ + void setStoppingCriterionFunction(const StoppingCriterionFunction& stopping_criterion_callback) { + stopping_criterion_function_ = stopping_criterion_callback; + } + +protected: + StoppingCriterionFunction stopping_criterion_function_ = + [](const robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& /*path_constraints*/) { + if (result != nullptr) { + return true; + } + return false; + }; +}; +} // 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..d9385d926 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -18,6 +18,8 @@ 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/fallback_planner.h + ${PROJECT_INCLUDE}/solvers/alternatives_planner.h container.cpp cost_terms.cpp @@ -32,8 +34,10 @@ 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/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 new file mode 100644 index 000000000..0ead26e68 --- /dev/null +++ b/core/src/solvers/alternatives_planner.cpp @@ -0,0 +1,200 @@ +/********************************************************************* + * 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 +#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 AlternativesPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& planner : *this) + planner->init(robot_model); +} + +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; + + // 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)); + } + + // 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 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(); + } + } + + // 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)); +} +} // namespace solvers +} // namespace task_constructor +} // namespace moveit 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/fallback_planner.cpp b/core/src/solvers/fallback_planner.cpp new file mode 100644 index 000000000..7739cd587 --- /dev/null +++ b/core/src/solvers/fallback_planner.cpp @@ -0,0 +1,105 @@ +/********************************************************************* + * 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 + +namespace moveit { +namespace task_constructor { +namespace solvers { + +void FallbackPlanner::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& planner : *this) { + if (remaining_time < 0) { + return false; // timeout} + if (result) { + result->clear(); + } + planner->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/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 7b3186a64..322d48152 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -105,7 +105,8 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr 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(); + 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/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