expose solvers

This commit is contained in:
Robert Haschke 2018-02-25 22:07:47 +01:00
parent ea6cc4b6bf
commit 1c61fb3b85
2 changed files with 22 additions and 9 deletions

View File

@ -36,10 +36,18 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/TwistStamped.h>
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(CartesianPath)
MOVEIT_CLASS_FORWARD(PipelinePlanner)
}
namespace stages {
/** Pick wraps a complete pipeline to pick up an object with a given end effector. /** Pick wraps a complete pipeline to pick up an object with a given end effector.
* *
@ -54,6 +62,8 @@ namespace moveit { namespace task_constructor { namespace stages {
* the end effector's Cartesian pose needs to be provided by an external grasp stage. * the end effector's Cartesian pose needs to be provided by an external grasp stage.
*/ */
class Pick : public SerialContainer { class Pick : public SerialContainer {
solvers::CartesianPathPtr cartesian_solver_;
solvers::PipelinePlannerPtr pipeline_solver_;
Stage* grasp_stage_ = nullptr; Stage* grasp_stage_ = nullptr;
Stage* approach_stage_ = nullptr; Stage* approach_stage_ = nullptr;
Stage* lift_stage_ = nullptr; Stage* lift_stage_ = nullptr;
@ -70,6 +80,9 @@ public:
properties().set<std::string>("object", object); properties().set<std::string>("object", object);
} }
solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; }
solvers::PipelinePlannerPtr pipelineSolver() { return pipeline_solver_; }
void setApproachMotion(const geometry_msgs::TwistStamped& motion, void setApproachMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance); double min_distance, double max_distance);
void setLiftMotion(const geometry_msgs::TwistStamped& motion, void setLiftMotion(const geometry_msgs::TwistStamped& motion,

View File

@ -23,13 +23,13 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
p.declare<std::string>("eef_group", "JMG of eef"); p.declare<std::string>("eef_group", "JMG of eef");
p.declare<std::string>("eef_parent_group", "JMG of eef's parent"); p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
auto cartesian = std::make_shared<solvers::CartesianPath>(); cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
auto pipeline = std::make_shared<solvers::PipelinePlanner>(); pipeline_solver_ = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0); pipeline_solver_->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault"); pipeline_solver_->setPlannerId("RRTConnectkConfigDefault");
{ {
auto move = std::make_unique<MoveTo>("open gripper", pipeline); auto move = std::make_unique<MoveTo>("open gripper", pipeline_solver_);
PropertyMap& p = move->properties(); PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); p.property("group").configureInitFrom(Stage::PARENT, "eef_group");
move->setGoal("open"); // TODO: retrieve from grasp stage move->setGoal("open"); // TODO: retrieve from grasp stage
@ -37,14 +37,14 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
} }
{ {
auto move = std::make_unique<Connect>("move to object", pipeline); auto move = std::make_unique<Connect>("move to object", pipeline_solver_);
PropertyMap& p = move->properties(); PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
insert(std::move(move)); insert(std::move(move));
} }
{ {
auto approach = std::make_unique<MoveRelative>("approach object", cartesian); auto approach = std::make_unique<MoveRelative>("approach object", cartesian_solver_);
PropertyMap& p = approach->properties(); PropertyMap& p = approach->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.set("marker_ns", std::string("approach")); p.set("marker_ns", std::string("approach"));
@ -57,7 +57,7 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
insert(std::move(grasp_stage)); insert(std::move(grasp_stage));
{ {
auto lift = std::make_unique<MoveRelative>("lift object", cartesian); auto lift = std::make_unique<MoveRelative>("lift object", cartesian_solver_);
PropertyMap& p = lift->properties(); PropertyMap& p = lift->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.set("marker_ns", std::string("lift")); p.set("marker_ns", std::string("lift"));