Merge PR moveit#429: MultiPlanner

MultiPlanner

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;
 

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 <moveit/task_constructor/solvers/planner_interface.h>
+#include <vector>
+
+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<solvers::PlannerInterfacePtr>
+{
+public:
+	using PlannerList = std::vector<solvers::PlannerInterfacePtr>;
+	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

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;
 

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

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<double>("timeout")), result, path_constraints);
 }
 
 bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,

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<double>("timeout"));
+	const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
 
 	auto to{ from->diff() };
 
@@ -127,8 +128,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 	}
 	to->getCurrentStateNonConst().update();
 
-	timeout = std::chrono::duration<double>(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);

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 <moveit/task_constructor/solvers/multi_planner.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <chrono>
+
+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<double>("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<double>(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<double>("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<double>(now - start_time).count();
+		start_time = now;
+	}
+	return false;
+}
+}  // namespace solvers
+}  // namespace task_constructor
+}  // namespace moveit

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<std::string>("planner");
-	req.allowed_planning_time = timeout;
+	req.allowed_planning_time = std::min(timeout, p.get<double>("timeout"));
 	req.start_state.is_diff = true;  // we don't specify an extra start state
 
 	req.num_planning_attempts = p.get<uint>("num_planning_attempts");

core/src/solvers/planner_interface.cpp

@@ -47,6 +47,7 @@ namespace solvers {
 
 PlannerInterface::PlannerInterface() {
 	auto& p = properties();
+	p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
 	p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
 	p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
 	p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());

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<solvers::CartesianPath>();
-	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