mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
pick: reusable stage for picking up an object
This commit is contained in:
parent
d731e943f1
commit
bff6cc569c
@ -105,7 +105,7 @@ int main(int argc, char** argv){
|
||||
{
|
||||
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setNamedPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
|
||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||
|
||||
@ -1,11 +1,8 @@
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/gripper.h>
|
||||
#include <moveit/task_constructor/stages/move.h>
|
||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||
#include <moveit/task_constructor/stages/pick.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/CollisionObject.h>
|
||||
@ -44,78 +41,31 @@ int main(int argc, char** argv){
|
||||
Task t;
|
||||
|
||||
Stage* initial_stage = nullptr;
|
||||
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||
initial_stage = initial.get();
|
||||
t.add(std::move(initial));
|
||||
|
||||
{
|
||||
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||
initial_stage = initial.get();
|
||||
t.add(std::move(initial));
|
||||
}
|
||||
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
||||
grasp_generator->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||
grasp_generator->setAngleDelta(.2);
|
||||
grasp_generator->setPreGraspPose("open");
|
||||
grasp_generator->setGraspPose("closed");
|
||||
grasp_generator->setMonitoredStage(initial_stage);
|
||||
|
||||
{
|
||||
auto move= std::make_unique<stages::Gripper>("open gripper");
|
||||
move->setEndEffector("left_gripper");
|
||||
move->setTo("open");
|
||||
t.add(std::move(move));
|
||||
}
|
||||
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
||||
pick->setProperty("eef", std::string("left_gripper"));
|
||||
pick->setProperty("object", std::string("object"));
|
||||
geometry_msgs::TwistStamped approach;
|
||||
approach.header.frame_id = "l_gripper_tool_frame";
|
||||
approach.twist.linear.x = 1.0;
|
||||
pick->setApproachMotion(approach, 0.03, 0.1);
|
||||
|
||||
{
|
||||
auto move= std::make_unique<stages::Move>("move to pre-grasp");
|
||||
move->setGroup("left_arm");
|
||||
move->setPlannerId("RRTConnectkConfigDefault");
|
||||
move->setTimeout(8.0);
|
||||
t.add(std::move(move));
|
||||
}
|
||||
geometry_msgs::TwistStamped lift;
|
||||
lift.header.frame_id = "base_link";
|
||||
lift.twist.linear.z = 1.0;
|
||||
pick->setLiftMotion(lift, 0.03, 0.05);
|
||||
|
||||
{
|
||||
auto move= std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||
move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1));
|
||||
move->setGroup("left_arm");
|
||||
move->setLink("l_gripper_tool_frame");
|
||||
move->setMinMaxDistance(.03, 0.1);
|
||||
move->setCartesianStepSize(0.02);
|
||||
|
||||
geometry_msgs::PointStamped target;
|
||||
target.header.frame_id= "object";
|
||||
move->towards(target);
|
||||
t.add(std::move(move));
|
||||
}
|
||||
|
||||
{
|
||||
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
gengrasp->setEndEffector("left_gripper");
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||
gengrasp->setAngleDelta(.2);
|
||||
gengrasp->setMonitoredStage(initial_stage);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
ik->setEndEffector("left_gripper");
|
||||
t.add(std::move(ik));
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<stages::Gripper>("grasp");
|
||||
move->setEndEffector("left_gripper");
|
||||
move->setAttachLink("l_gripper_tool_frame");
|
||||
move->setTo("closed");
|
||||
move->graspObject("object");
|
||||
t.add(std::move(move));
|
||||
}
|
||||
|
||||
{
|
||||
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||
move->setGroup("left_arm");
|
||||
move->setLink("l_gripper_tool_frame");
|
||||
move->setMinMaxDistance(0.03, 0.05);
|
||||
move->setCartesianStepSize(0.01);
|
||||
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
direction.header.frame_id= "base_link";
|
||||
direction.vector.z= 1.0;
|
||||
move->along(direction);
|
||||
t.add(std::move(move));
|
||||
}
|
||||
t.add(std::move(pick));
|
||||
|
||||
try {
|
||||
t.plan();
|
||||
|
||||
@ -84,7 +84,7 @@ int main(int argc, char** argv){
|
||||
{
|
||||
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
||||
gengrasp->setGripperGraspPose("open");
|
||||
gengrasp->setNamedPose("open");
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||
gengrasp->setAngleDelta(-.2);
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
@ -52,7 +53,7 @@ public:
|
||||
bool compute() override;
|
||||
|
||||
void setEndEffector(const std::string &eef);
|
||||
void setGripperGraspPose(const std::string &pose_name);
|
||||
void setNamedPose(const std::string &pose_name);
|
||||
void setObject(const std::string &object);
|
||||
|
||||
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform);
|
||||
|
||||
@ -60,7 +60,7 @@ namespace moveit { namespace task_constructor { namespace stages {
|
||||
class ModifyPlanningScene : public PropagatingEitherWay {
|
||||
public:
|
||||
typedef std::vector<std::string> Names;
|
||||
typedef std::function<void(planning_scene::PlanningScenePtr scene, PropertyMap& properties)> ApplyCallback;
|
||||
typedef std::function<void(const planning_scene::PlanningScenePtr& scene, const PropertyMap& properties)> ApplyCallback;
|
||||
ModifyPlanningScene(const std::string& name = "modify planning scene");
|
||||
|
||||
bool computeForward(const InterfaceState& from) override;
|
||||
|
||||
79
core/include/moveit/task_constructor/stages/pick.h
Normal file
79
core/include/moveit/task_constructor/stages/pick.h
Normal file
@ -0,0 +1,79 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Bielefeld University
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Bielefeld University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Robert Haschke */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
/** Pick wraps a complete pipeline to pick up an object with a given end effector.
|
||||
*
|
||||
* Picking consist of the following sub stages:
|
||||
* - reaching to the object + "pre-grasp" end effector posture
|
||||
* - linearly approaching the object along an approach direction/twist
|
||||
* - "grasp" end effector posture
|
||||
* - attach object
|
||||
* - lift along along a given direction/twist
|
||||
*
|
||||
* The end effector postures corresponding to pre-grasp and grasp as well as
|
||||
* the end effector's Cartesian pose needs to be provided by an external grasp stage.
|
||||
*/
|
||||
class Pick : public SerialContainer {
|
||||
Stage* grasp_stage_ = nullptr;
|
||||
Stage* approach_stage_ = nullptr;
|
||||
Stage* lift_stage_ = nullptr;
|
||||
|
||||
public:
|
||||
Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick");
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
void setEndEffector(const std::string& eef) {
|
||||
properties().set<std::string>("eef", eef);
|
||||
}
|
||||
void setObject(const std::string& object) {
|
||||
properties().set<std::string>("object", object);
|
||||
}
|
||||
|
||||
void setApproachMotion(const geometry_msgs::TwistStamped& motion,
|
||||
double min_distance, double max_distance);
|
||||
void setLiftMotion(const geometry_msgs::TwistStamped& motion,
|
||||
double min_distance, double max_distance);
|
||||
};
|
||||
|
||||
} } }
|
||||
101
core/include/moveit/task_constructor/stages/simple_grasp.h
Normal file
101
core/include/moveit/task_constructor/stages/simple_grasp.h
Normal file
@ -0,0 +1,101 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Bielefeld University
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Bielefeld University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Robert Haschke */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } }
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class GenerateGraspPose;
|
||||
|
||||
/** Simple Grasp Stage
|
||||
*
|
||||
* Given a named pre-grasp and grasp posture the stage generates
|
||||
* fully-qualified pre-grasp and grasp robot states, connected
|
||||
* by a grasping trajectory of the end-effector.
|
||||
*/
|
||||
class SimpleGrasp : public SerialContainer {
|
||||
moveit::core::RobotModelConstPtr model_;
|
||||
GenerateGraspPose* grasp_generator_ = nullptr;
|
||||
|
||||
public:
|
||||
SimpleGrasp(const std::string& name = "grasp");
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
void setMonitoredStage(Stage* monitored);
|
||||
|
||||
void setEndEffector(const std::string& eef) {
|
||||
properties().set<std::string>("eef", eef);
|
||||
}
|
||||
void setObject(const std::string& object) {
|
||||
properties().set<std::string>("object", object);
|
||||
}
|
||||
|
||||
void setPreGraspPose(const std::string& pregrasp) {
|
||||
properties().set<std::string>("pregrasp", pregrasp);
|
||||
}
|
||||
void setGraspPose(const std::string& grasp) {
|
||||
properties().set<std::string>("grasp", grasp);
|
||||
}
|
||||
|
||||
void setToolToGraspTF(const geometry_msgs::TransformStamped &transform) {
|
||||
properties().set("tool_to_grasp_tf", transform);
|
||||
}
|
||||
void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = "");
|
||||
template <typename T>
|
||||
void setToolToGraspTF(const T& t, const std::string& link = "") {
|
||||
Eigen::Affine3d transform; transform = t;
|
||||
setToolToGraspTF(transform, link);
|
||||
}
|
||||
|
||||
void setAngleDelta(double angle_delta) {
|
||||
properties().set("angle_delta", angle_delta);
|
||||
}
|
||||
|
||||
void setMaxIKSolutions(uint32_t max_ik_solutions) {
|
||||
properties().set("max_ik_solutions", max_ik_solutions);
|
||||
}
|
||||
void setTimeout(double timeout) {
|
||||
properties().set("timeout", timeout);
|
||||
}
|
||||
};
|
||||
|
||||
} } }
|
||||
@ -11,6 +11,9 @@ add_library(${PROJECT_NAME}_stages
|
||||
${PROJECT_INCLUDE}/stages/move_to.h
|
||||
${PROJECT_INCLUDE}/stages/move_relative.h
|
||||
|
||||
${PROJECT_INCLUDE}/stages/simple_grasp.h
|
||||
${PROJECT_INCLUDE}/stages/pick.h
|
||||
|
||||
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
|
||||
${PROJECT_INCLUDE}/stages/gripper.h
|
||||
${PROJECT_INCLUDE}/stages/move.h
|
||||
@ -27,6 +30,9 @@ add_library(${PROJECT_NAME}_stages
|
||||
move_to.cpp
|
||||
move_relative.cpp
|
||||
|
||||
simple_grasp.cpp
|
||||
pick.cpp
|
||||
|
||||
cartesian_position_motion.cpp
|
||||
gripper.cpp
|
||||
move.cpp
|
||||
|
||||
@ -50,8 +50,8 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name)
|
||||
: MonitoringGenerator(name)
|
||||
{
|
||||
auto& p = properties();
|
||||
p.declare<std::string>("eef", "name of end-effector group");
|
||||
p.declare<std::string>("eef_named_pose");
|
||||
p.declare<std::string>("eef", "name of end-effector");
|
||||
p.declare<std::string>("pregrasp", "name of end-effector's pregrasp pose");
|
||||
p.declare<std::string>("object");
|
||||
p.declare<geometry_msgs::TransformStamped>("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame");
|
||||
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
||||
@ -61,8 +61,8 @@ void GenerateGraspPose::setEndEffector(const std::string &eef){
|
||||
setProperty("eef", eef);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setGripperGraspPose(const std::string &pose_name){
|
||||
setProperty("eef_named_pose", pose_name);
|
||||
void GenerateGraspPose::setNamedPose(const std::string &pose_name){
|
||||
setProperty("pregrasp", pose_name);
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setObject(const std::string &object){
|
||||
@ -111,11 +111,13 @@ bool GenerateGraspPose::compute(){
|
||||
const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef);
|
||||
|
||||
robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst();
|
||||
const std::string& eef_named_pose = props.get<std::string>("eef_named_pose");
|
||||
robot_state.setToDefaultValues(jmg , eef_named_pose);
|
||||
const std::string& joint_pose = props.get<std::string>("pregrasp");
|
||||
if(!joint_pose.empty()){
|
||||
robot_state.setToDefaultValues(jmg , joint_pose);
|
||||
}
|
||||
|
||||
geometry_msgs::TransformStamped tool2grasp_msg = props.get<geometry_msgs::TransformStamped>("tool_to_grasp_tf");
|
||||
const std::string &link_name = jmg ->getEndEffectorParentGroup().second;
|
||||
const std::string &link_name = jmg->getEndEffectorParentGroup().second;
|
||||
if (tool2grasp_msg.header.frame_id.empty()) // if no frame_id is given
|
||||
tool2grasp_msg.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame
|
||||
Eigen::Affine3d to_grasp;
|
||||
|
||||
118
core/src/stages/pick.cpp
Normal file
118
core/src/stages/pick.cpp
Normal file
@ -0,0 +1,118 @@
|
||||
#include <moveit/task_constructor/stages/pick.h>
|
||||
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/stages/move_relative.h>
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
|
||||
: SerialContainer(name)
|
||||
{
|
||||
PropertyMap& p = properties();
|
||||
p.declare<std::string>("eef", "end effector name");
|
||||
p.declare<std::string>("object", "name of object to grasp");
|
||||
// internal properties (cannot be marked as such yet)
|
||||
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");
|
||||
|
||||
{
|
||||
auto move = std::make_unique<MoveTo>("open gripper", pipeline);
|
||||
PropertyMap& p = move->properties();
|
||||
p.property("group").configureInitFrom(Stage::PARENT, "eef_group");
|
||||
move->setGoal("open"); // TODO: retrieve from grasp stage
|
||||
insert(std::move(move));
|
||||
}
|
||||
|
||||
{
|
||||
auto move = std::make_unique<Connect>("move to object", pipeline);
|
||||
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);
|
||||
PropertyMap& p = approach->properties();
|
||||
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
|
||||
p.set("marker_ns", std::string("approach"));
|
||||
approach_stage_ = approach.get();
|
||||
insert(std::move(approach));
|
||||
}
|
||||
|
||||
grasp_stage_ = grasp_stage.get();
|
||||
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
|
||||
insert(std::move(grasp_stage));
|
||||
|
||||
{
|
||||
auto attach = std::make_unique<ModifyPlanningScene>("attach object");
|
||||
PropertyMap& p = attach->properties();
|
||||
p.declare<std::string>("eef");
|
||||
p.declare<std::string>("object");
|
||||
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
||||
attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||
const std::string& eef = p.get<std::string>("eef");
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
obj.object.operation = moveit_msgs::CollisionObject::ADD;
|
||||
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
||||
obj.object.id = p.get<std::string>("object");
|
||||
scene->processAttachedCollisionObjectMsg(obj);
|
||||
});
|
||||
insert(std::move(attach));
|
||||
}
|
||||
|
||||
{
|
||||
auto lift = std::make_unique<MoveRelative>("lift object", cartesian);
|
||||
PropertyMap& p = lift->properties();
|
||||
p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group");
|
||||
p.set("marker_ns", std::string("lift"));
|
||||
lift_stage_ = lift.get();
|
||||
insert(std::move(lift));
|
||||
}
|
||||
}
|
||||
|
||||
void Pick::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
// inherit properties from parent
|
||||
PropertyMap* p = &properties();
|
||||
p->performInitFrom(Stage::PARENT, parent()->properties());
|
||||
|
||||
// init internal properties
|
||||
const std::string &eef = p->get<std::string>("eef");
|
||||
const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef);
|
||||
p->set<std::string>("eef_group", jmg->getName());
|
||||
p->set<std::string>("eef_parent_group", jmg->getEndEffectorParentGroup().first);
|
||||
|
||||
// propagate my properties to children (and do standard init)
|
||||
SerialContainer::init(robot_model);
|
||||
}
|
||||
|
||||
void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
|
||||
{
|
||||
auto& p = approach_stage_->properties();
|
||||
p.set("twist", motion);
|
||||
p.set("min_distance", min_distance);
|
||||
p.set("max_distance", max_distance);
|
||||
}
|
||||
|
||||
void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
|
||||
{
|
||||
auto& p = lift_stage_->properties();
|
||||
p.set("twist", motion);
|
||||
p.set("min_distance", min_distance);
|
||||
p.set("max_distance", max_distance);
|
||||
}
|
||||
|
||||
} } }
|
||||
120
core/src/stages/simple_grasp.cpp
Normal file
120
core/src/stages/simple_grasp.cpp
Normal file
@ -0,0 +1,120 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Bielefeld University
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Bielefeld University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Robert Haschke */
|
||||
|
||||
#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/modify_planning_scene.h>
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
SimpleGrasp::SimpleGrasp(const std::string& name)
|
||||
: SerialContainer(name)
|
||||
{
|
||||
{
|
||||
auto gengrasp = std::make_unique<GenerateGraspPose>("generate grasp pose");
|
||||
grasp_generator_ = gengrasp.get();
|
||||
|
||||
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", "tool_to_grasp_tf" };
|
||||
ik->exposePropertiesOfChild(0, grasp_prop_names);
|
||||
insert(std::move(ik));
|
||||
|
||||
exposePropertiesOfChild(-1, grasp_prop_names);
|
||||
exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout" });
|
||||
}
|
||||
{
|
||||
auto allow_touch = std::make_unique<ModifyPlanningScene>("enable object collision");
|
||||
PropertyMap& p = allow_touch->properties();
|
||||
p.declare<std::string>("eef");
|
||||
p.declare<std::string>("object");
|
||||
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
||||
|
||||
allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||
collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
|
||||
const std::string& eef = p.get<std::string>("eef");
|
||||
const std::string& object = p.get<std::string>("object");
|
||||
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
|
||||
->getLinkModelNamesWithCollisionGeometry(), true);
|
||||
});
|
||||
insert(std::move(allow_touch));
|
||||
}
|
||||
{
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||
pipeline->setTimeout(8.0);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
|
||||
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
|
||||
move->restrictDirection(MoveTo::FORWARD);
|
||||
PropertyMap& p = move->properties();
|
||||
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
||||
const std::string& eef = parent_map.get<std::string>("eef");
|
||||
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
|
||||
return boost::any(jmg->getName());
|
||||
});
|
||||
insert(std::move(move));
|
||||
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
|
||||
}
|
||||
}
|
||||
|
||||
void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
model_ = robot_model;
|
||||
SerialContainer::init(robot_model);
|
||||
}
|
||||
|
||||
void SimpleGrasp::setMonitoredStage(Stage* monitored)
|
||||
{
|
||||
grasp_generator_->setMonitoredStage(monitored);
|
||||
}
|
||||
|
||||
void SimpleGrasp::setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link) {
|
||||
geometry_msgs::TransformStamped stamped;
|
||||
stamped.header.frame_id = link;
|
||||
stamped.child_frame_id = "grasp_frame";
|
||||
tf::transformEigenToMsg(transform, stamped.transform);
|
||||
setToolToGraspTF(stamped);
|
||||
}
|
||||
|
||||
} } }
|
||||
Loading…
Reference in New Issue
Block a user