Add MultiPlanner solvers

a planner that tries multiple planners in sequence
This commit is contained in:
Robert Haschke 2023-02-17 12:17:33 +01:00
parent 3d3236575d
commit 052a56a333
3 changed files with 175 additions and 1 deletions

View File

@ -0,0 +1,74 @@
/*********************************************************************
* 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, ...
*/
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

View File

@ -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}

View File

@ -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 = 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 = 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