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
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/container.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.
*
@ -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.
*/
class Pick : public SerialContainer {
solvers::CartesianPathPtr cartesian_solver_;
solvers::PipelinePlannerPtr pipeline_solver_;
Stage* grasp_stage_ = nullptr;
Stage* approach_stage_ = nullptr;
Stage* lift_stage_ = nullptr;
@ -70,6 +80,9 @@ public:
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,
double min_distance, double max_distance);
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_parent_group", "JMG of eef's parent");
auto cartesian = std::make_shared<solvers::CartesianPath>();
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
pipeline_solver_ = std::make_shared<solvers::PipelinePlanner>();
pipeline_solver_->setTimeout(8.0);
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();
p.property("group").configureInitFrom(Stage::PARENT, "eef_group");
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();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
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();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
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));
{
auto lift = std::make_unique<MoveRelative>("lift object", cartesian);
auto lift = std::make_unique<MoveRelative>("lift object", cartesian_solver_);
PropertyMap& p = lift->properties();
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
p.set("marker_ns", std::string("lift"));