mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
expose solvers
This commit is contained in:
parent
ea6cc4b6bf
commit
1c61fb3b85
@ -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,
|
||||||
|
|||||||
@ -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"));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user