diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 732111cb9..7cb53ee38 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 60139b3bd..67a876ee2 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 e25a036c5..45cefe57c 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} ${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} solvers/planner_interface.cpp solvers/cartesian_path.cpp - solvers/pipeline_planner.cpp solvers/joint_interpolation.cpp + solvers/pipeline_planner.cpp + solvers/multi_planner.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 9852470ff..4e9df5dc3 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -71,7 +71,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 600fd6f02..e9727529d 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -103,7 +103,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::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() }; @@ -125,8 +126,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 66d12d5c1..333759084 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -143,7 +143,7 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa 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 e4232ec3e..5eb72ec8a 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -163,8 +163,8 @@ bool PickPlaceTask::init() { // 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