add path constraints to planner API

and use it in the Connect class.

The cartesian planner ignores the constraints for now.
This commit is contained in:
v4hn 2018-02-26 14:13:07 +01:00 committed by Robert Haschke
parent b15c5adbd7
commit 8af44cefc3
7 changed files with 39 additions and 18 deletions

View File

@ -56,14 +56,16 @@ public:
const planning_scene::PlanningSceneConstPtr to, const planning_scene::PlanningSceneConstPtr to,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) override; robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr from, bool plan(const planning_scene::PlanningSceneConstPtr from,
const moveit::core::LinkModel &link, const moveit::core::LinkModel &link,
const Eigen::Affine3d& target, const Eigen::Affine3d& target,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) override; robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
}; };
} } } } } }

View File

@ -62,14 +62,16 @@ public:
const planning_scene::PlanningSceneConstPtr to, const planning_scene::PlanningSceneConstPtr to,
const core::JointModelGroup *jmg, const core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) override; robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr from, bool plan(const planning_scene::PlanningSceneConstPtr from,
const moveit::core::LinkModel &link, const moveit::core::LinkModel &link,
const Eigen::Affine3d& target, const Eigen::Affine3d& target,
const core::JointModelGroup *jmg, const core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) override; robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
protected: protected:
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;

View File

@ -39,6 +39,7 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_msgs/Constraints.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
@ -78,7 +79,8 @@ public:
const planning_scene::PlanningSceneConstPtr to, const planning_scene::PlanningSceneConstPtr to,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) = 0; robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target /// plan trajectory from current robot state to Cartesian target
virtual bool plan(const planning_scene::PlanningSceneConstPtr from, virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
@ -86,7 +88,8 @@ public:
const Eigen::Affine3d& target, const Eigen::Affine3d& target,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) = 0; robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
}; };
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg); std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg);

View File

@ -41,12 +41,18 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/Constraints.h>
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
class Connect : public Connecting { class Connect : public Connecting {
public: public:
Connect(std::string name, const solvers::PlannerInterfacePtr &planner); Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
void setPathConstraints(moveit_msgs::Constraints path_constraints){
setProperty("path_constraints", std::move(path_constraints));
}
void init(const moveit::core::RobotModelConstPtr& robot_model); void init(const moveit::core::RobotModelConstPtr& robot_model);
bool compute(const InterfaceState &from, const InterfaceState &to); bool compute(const InterfaceState &from, const InterfaceState &to);

View File

@ -60,7 +60,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
const planning_scene::PlanningSceneConstPtr to, const planning_scene::PlanningSceneConstPtr to,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& /*path_constraints*/)
{ {
const moveit::core::LinkModel* link; const moveit::core::LinkModel* link;
const std::string& link_name = solvers::getEndEffectorLink(jmg); const std::string& link_name = solvers::getEndEffectorLink(jmg);
@ -78,7 +79,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
const Eigen::Affine3d &target, const Eigen::Affine3d &target,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr &result) robot_trajectory::RobotTrajectoryPtr &result,
const moveit_msgs::Constraints& /*path_constraints*/)
{ {
const auto& props = properties(); const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff(); planning_scene::PlanningScenePtr sandbox_scene = from->diff();

View File

@ -85,7 +85,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
const planning_scene::PlanningSceneConstPtr to, const planning_scene::PlanningSceneConstPtr to,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints)
{ {
const auto& props = properties(); const auto& props = properties();
moveit_msgs::MotionPlanRequest req; moveit_msgs::MotionPlanRequest req;
@ -95,6 +96,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
to->getCurrentState(), jmg, to->getCurrentState(), jmg,
props.get<double>("goal_joint_tolerance")); props.get<double>("goal_joint_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(from, req, res)) if(!planner_->generatePlan(from, req, res))
@ -109,8 +111,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
const Eigen::Affine3d& target_eigen, const Eigen::Affine3d& target_eigen,
const moveit::core::JointModelGroup *jmg, const moveit::core::JointModelGroup *jmg,
double timeout, double timeout,
robot_trajectory::RobotTrajectoryPtr& result) robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints)
{ {
const auto& props = properties(); const auto& props = properties();
moveit_msgs::MotionPlanRequest req; moveit_msgs::MotionPlanRequest req;
@ -125,6 +127,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
link.getName(), target, link.getName(), target,
props.get<double>("goal_position_tolerance"), props.get<double>("goal_position_tolerance"),
props.get<double>("goal_orientation_tolerance")); props.get<double>("goal_orientation_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(from, req, res)) if(!planner_->generatePlan(from, req, res))

View File

@ -48,6 +48,8 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
auto& p = properties(); auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout"); p.declare<double>("timeout", 10.0, "planning timeout");
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
} }
void Connect::init(const core::RobotModelConstPtr& robot_model) void Connect::init(const core::RobotModelConstPtr& robot_model)
@ -63,7 +65,8 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group); const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group);
robot_trajectory::RobotTrajectoryPtr trajectory; robot_trajectory::RobotTrajectoryPtr trajectory;
if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory)) if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory,
props.get<moveit_msgs::Constraints>("path_constraints")))
return false; return false;
connect(from, to, trajectory); connect(from, to, trajectory);