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
|
||||
|
||||
#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,
|
||||
|
||||
@ -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"));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user