mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
SimpleGrasp(GraspGenerator)
allow any GraspGenerator stage that provides "pregrasp" and "grasp" postures as well as a target_pose for grasping.
This commit is contained in:
parent
2e9932cfbc
commit
92296e020f
@ -1,6 +1,7 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
#include <moveit/task_constructor/stages/pick.h>
|
#include <moveit/task_constructor/stages/pick.h>
|
||||||
#include <moveit/task_constructor/stages/connect.h>
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
@ -55,15 +56,17 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(connect));
|
t.add(std::move(connect));
|
||||||
|
|
||||||
// grasp generator
|
// grasp generator
|
||||||
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
auto grasp_generator = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||||
grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
|
||||||
grasp_generator->setAngleDelta(.2);
|
grasp_generator->setAngleDelta(.2);
|
||||||
grasp_generator->setPreGraspPose("open");
|
|
||||||
grasp_generator->setGraspPose("closed");
|
auto grasp = std::make_unique<stages::SimpleGrasp>(std::move(grasp_generator));
|
||||||
grasp_generator->setMonitoredStage(initial_stage);
|
grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||||
|
grasp->setPreGraspPose("open");
|
||||||
|
grasp->setGraspPose("closed");
|
||||||
|
grasp->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
// pick stage
|
// pick stage
|
||||||
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
auto pick = std::make_unique<stages::Pick>(std::move(grasp));
|
||||||
pick->setProperty("eef", std::string("left_gripper"));
|
pick->setProperty("eef", std::string("left_gripper"));
|
||||||
pick->setProperty("object", std::string("object"));
|
pick->setProperty("object", std::string("object"));
|
||||||
geometry_msgs::TwistStamped approach;
|
geometry_msgs::TwistStamped approach;
|
||||||
|
|||||||
@ -1,6 +1,7 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
#include <moveit/task_constructor/stages/pick.h>
|
#include <moveit/task_constructor/stages/pick.h>
|
||||||
#include <moveit/task_constructor/stages/connect.h>
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
@ -55,15 +56,17 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(connect));
|
t.add(std::move(connect));
|
||||||
|
|
||||||
// grasp generator
|
// grasp generator
|
||||||
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
auto grasp_generator = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||||
grasp_generator->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
|
||||||
grasp_generator->setMaxIKSolutions(8);
|
|
||||||
grasp_generator->setAngleDelta(.2);
|
grasp_generator->setAngleDelta(.2);
|
||||||
grasp_generator->setPreGraspPose("open");
|
|
||||||
grasp_generator->setGraspPose("closed");
|
|
||||||
grasp_generator->setMonitoredStage(initial_stage);
|
|
||||||
|
|
||||||
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
auto grasp = std::make_unique<stages::SimpleGrasp>(std::move(grasp_generator));
|
||||||
|
grasp->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||||
|
grasp->setMaxIKSolutions(8);
|
||||||
|
grasp->setPreGraspPose("open");
|
||||||
|
grasp->setGraspPose("closed");
|
||||||
|
grasp->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
|
auto pick = std::make_unique<stages::Pick>(std::move(grasp));
|
||||||
pick->setProperty("eef", std::string("gripper"));
|
pick->setProperty("eef", std::string("gripper"));
|
||||||
pick->setProperty("object", std::string("object"));
|
pick->setProperty("object", std::string("object"));
|
||||||
geometry_msgs::TwistStamped approach;
|
geometry_msgs::TwistStamped approach;
|
||||||
|
|||||||
@ -48,34 +48,45 @@ class GenerateGraspPose;
|
|||||||
|
|
||||||
/** base class for simple grasping / ungrasping
|
/** base class for simple grasping / ungrasping
|
||||||
*
|
*
|
||||||
* Given a named pre-grasp and grasp posture the stage generates fully-qualified
|
* Given a pre-grasp and grasp posture the stage generates a trajectory
|
||||||
* pre-grasp and grasp robot states, connected by a grasping trajectory of the end-effector.
|
* connecting these two states by a grasping trajectory.
|
||||||
|
* The class takes a generator stage that provides the grasp pose and
|
||||||
|
* optionally the pre-grasp and grasp postures.
|
||||||
*
|
*
|
||||||
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
|
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
|
||||||
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
|
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
|
||||||
*/
|
*/
|
||||||
class SimpleGraspBase : public SerialContainer {
|
class SimpleGraspBase : public SerialContainer {
|
||||||
moveit::core::RobotModelConstPtr model_;
|
moveit::core::RobotModelConstPtr model_;
|
||||||
GenerateGraspPose* grasp_generator_ = nullptr;
|
MonitoringGenerator* generator_ = nullptr;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void setup(std::unique_ptr<MonitoringGenerator>&& generator, bool forward);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
SimpleGraspBase(const std::string& name, bool forward);
|
SimpleGraspBase(const std::string& name);
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
void setMonitoredStage(Stage* monitored);
|
void setMonitoredStage(Stage* monitored);
|
||||||
|
|
||||||
void setEndEffector(const std::string& eef) {
|
void setEndEffector(const std::string& eef) {
|
||||||
properties().set<std::string>("eef", eef);
|
properties().set("eef", eef);
|
||||||
}
|
}
|
||||||
void setObject(const std::string& object) {
|
void setObject(const std::string& object) {
|
||||||
properties().set<std::string>("object", object);
|
properties().set("object", object);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setPreGraspPose(const std::string& pregrasp) {
|
void setPreGraspPose(const std::string& pregrasp) {
|
||||||
properties().set<std::string>("pregrasp", pregrasp);
|
properties().set("pregrasp", pregrasp);
|
||||||
}
|
}
|
||||||
void setGraspPose(const std::string& grasp) {
|
void setGraspPose(const std::string& grasp) {
|
||||||
properties().set<std::string>("grasp", grasp);
|
properties().set("grasp", grasp);
|
||||||
|
}
|
||||||
|
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) {
|
||||||
|
properties().set("pregrasp", pregrasp);
|
||||||
|
}
|
||||||
|
void setGraspPose(const moveit_msgs::RobotState& grasp) {
|
||||||
|
properties().set("grasp", grasp);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setIKFrame(const geometry_msgs::PoseStamped &transform) {
|
void setIKFrame(const geometry_msgs::PoseStamped &transform) {
|
||||||
@ -91,30 +102,23 @@ public:
|
|||||||
setIKFrame(Eigen::Affine3d::Identity(), link);
|
setIKFrame(Eigen::Affine3d::Identity(), link);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setAngleDelta(double angle_delta) {
|
|
||||||
properties().set("angle_delta", angle_delta);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setMaxIKSolutions(uint32_t max_ik_solutions) {
|
void setMaxIKSolutions(uint32_t max_ik_solutions) {
|
||||||
properties().set("max_ik_solutions", max_ik_solutions);
|
properties().set("max_ik_solutions", max_ik_solutions);
|
||||||
}
|
}
|
||||||
void setTimeout(double timeout) {
|
|
||||||
properties().set("timeout", timeout);
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/// specialization of SimpleGraspBase to realize grasping
|
/// specialization of SimpleGraspBase to realize grasping
|
||||||
class SimpleGrasp : public SimpleGraspBase {
|
class SimpleGrasp : public SimpleGraspBase {
|
||||||
public:
|
public:
|
||||||
SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {}
|
SimpleGrasp(std::unique_ptr<MonitoringGenerator>&& generator, const std::string& name = "grasp");
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/// specialization of SimpleGraspBase to realize ungrasping
|
/// specialization of SimpleGraspBase to realize ungrasping
|
||||||
class SimpleUnGrasp : public SimpleGraspBase {
|
class SimpleUnGrasp : public SimpleGraspBase {
|
||||||
public:
|
public:
|
||||||
SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {}
|
SimpleUnGrasp(std::unique_ptr<MonitoringGenerator>&& generator, const std::string& name = "ungrasp");
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -36,7 +36,6 @@
|
|||||||
|
|
||||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
|
||||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
||||||
#include <moveit/task_constructor/stages/move_to.h>
|
#include <moveit/task_constructor/stages/move_to.h>
|
||||||
@ -49,24 +48,29 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
SimpleGraspBase::SimpleGraspBase(const std::string& name)
|
||||||
: SerialContainer(name)
|
: SerialContainer(name)
|
||||||
|
{
|
||||||
|
auto &props = properties();
|
||||||
|
props.declare<boost::any>("pregrasp", "pregrasp posture");
|
||||||
|
props.declare<boost::any>("grasp", "grasp posture");
|
||||||
|
}
|
||||||
|
|
||||||
|
void SimpleGraspBase::setup(std::unique_ptr<MonitoringGenerator>&& generator, bool forward)
|
||||||
{
|
{
|
||||||
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
|
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
|
||||||
{
|
{
|
||||||
auto gengrasp = std::make_unique<GenerateGraspPose>(forward ? "generate grasp pose" : "generate release pose");
|
generator_ = generator.get();
|
||||||
grasp_generator_ = gengrasp.get();
|
auto ik = new ComputeIK("compute ik", std::move(generator));
|
||||||
|
const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object" };
|
||||||
auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp));
|
|
||||||
const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" };
|
|
||||||
ik->exposePropertiesOfChild(0, grasp_prop_names);
|
ik->exposePropertiesOfChild(0, grasp_prop_names);
|
||||||
insert(std::move(ik), insertion_position);
|
insert(std::unique_ptr<ComputeIK>(ik), insertion_position);
|
||||||
|
|
||||||
exposePropertiesOfChild(insertion_position, grasp_prop_names);
|
exposePropertiesOfChild(insertion_position, grasp_prop_names);
|
||||||
exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" });
|
exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" });
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto allow_touch = std::make_unique<ModifyPlanningScene>(forward ? "allow object collision" : "forbid object collision");
|
auto allow_touch = new ModifyPlanningScene(forward ? "allow object collision" : "forbid object collision");
|
||||||
PropertyMap& p = allow_touch->properties();
|
PropertyMap& p = allow_touch->properties();
|
||||||
p.declare<std::string>("eef");
|
p.declare<std::string>("eef");
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
@ -79,25 +83,25 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
|||||||
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
|
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
|
||||||
->getLinkModelNamesWithCollisionGeometry(), true);
|
->getLinkModelNamesWithCollisionGeometry(), true);
|
||||||
});
|
});
|
||||||
insert(std::move(allow_touch), insertion_position);
|
insert(std::unique_ptr<ModifyPlanningScene>(allow_touch), insertion_position);
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||||
pipeline->setTimeout(8.0);
|
pipeline->setTimeout(8.0);
|
||||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||||
|
|
||||||
auto move = std::make_unique<MoveTo>(forward ? "close gripper" : "open gripper", pipeline);
|
auto move = new MoveTo(forward ? "close gripper" : "open gripper", pipeline);
|
||||||
PropertyMap& p = move->properties();
|
PropertyMap& p = move->properties();
|
||||||
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
||||||
const std::string& eef = parent_map.get<std::string>("eef");
|
const std::string& eef = parent_map.get<std::string>("eef");
|
||||||
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
|
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
|
||||||
return boost::any(jmg->getName());
|
return boost::any(jmg->getName());
|
||||||
});
|
});
|
||||||
insert(std::move(move), insertion_position);
|
insert(std::unique_ptr<MoveTo>(move), insertion_position);
|
||||||
exposePropertyOfChildAs(insertion_position, "named_joint_pose", forward ? "grasp" : "pregrasp");
|
exposePropertyOfChildAs(insertion_position, "goal", forward ? "grasp" : "pregrasp");
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto attach = std::make_unique<ModifyPlanningScene>(forward ? "attach object" : "detach object");
|
auto attach = new ModifyPlanningScene(forward ? "attach object" : "detach object");
|
||||||
PropertyMap& p = attach->properties();
|
PropertyMap& p = attach->properties();
|
||||||
p.declare<std::string>("eef");
|
p.declare<std::string>("eef");
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
@ -111,7 +115,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
|||||||
obj.object.id = p.get<std::string>("object");
|
obj.object.id = p.get<std::string>("object");
|
||||||
scene->processAttachedCollisionObjectMsg(obj);
|
scene->processAttachedCollisionObjectMsg(obj);
|
||||||
});
|
});
|
||||||
insert(std::move(attach), insertion_position);
|
insert(std::unique_ptr<ModifyPlanningScene>(attach), insertion_position);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -123,7 +127,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
|
|
||||||
void SimpleGraspBase::setMonitoredStage(Stage* monitored)
|
void SimpleGraspBase::setMonitoredStage(Stage* monitored)
|
||||||
{
|
{
|
||||||
grasp_generator_->setMonitoredStage(monitored);
|
generator_->setMonitoredStage(monitored);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
|
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
|
||||||
@ -133,4 +137,15 @@ void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string&
|
|||||||
setIKFrame(pose_msg);
|
setIKFrame(pose_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SimpleGrasp::SimpleGrasp(std::unique_ptr<MonitoringGenerator>&& generator, const std::string& name)
|
||||||
|
: SimpleGraspBase(name)
|
||||||
|
{
|
||||||
|
setup(std::move(generator), true);
|
||||||
|
}
|
||||||
|
|
||||||
|
SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr<MonitoringGenerator>&& generator, const std::string& name)
|
||||||
|
: SimpleGraspBase(name) {
|
||||||
|
setup(std::move(generator), false);
|
||||||
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user