Skip to content
Snippets Groups Projects
Commit 70d556b1 authored by KingMaZito's avatar KingMaZito
Browse files

yaml2mtc cant handle 2 robs, changed plan, now mtc is fully implemented with 2 robs

parent a15ac815
Branches
No related tags found
No related merge requests found
Pipeline #14952 failed
/home/matteo/ws_panda/devel/./cmake.lock 42 /home/matteo/ws_panda/devel/./cmake.lock 42
/home/matteo/reachability/devel/./cmake.lock 21138 /home/matteo/reachability/devel/./cmake.lock 21925
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
...@@ -39,7 +39,8 @@ ...@@ -39,7 +39,8 @@
"${workspaceFolder}/mtc/src", "${workspaceFolder}/mtc/src",
"${workspaceFolder}/yaml_to_mtc/include", "${workspaceFolder}/yaml_to_mtc/include",
"${workspaceFolder}/moveit_task_constructor/core/include", "${workspaceFolder}/moveit_task_constructor/core/include",
"${workspaceFolder}/moveit_task_constructor/" "${workspaceFolder}/moveit_task_constructor/",
"${workspaceFolder}/object_creator/include"
], ],
"name": "ROS", "name": "ROS",
"cStandard": "c17", "cStandard": "c17",
......
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
class Moveit_robot : public Robot{ class Moveit_robot : public Robot{
protected: protected:
moveit::planning_interface::MoveGroupInterface* mgi_; moveit::planning_interface::MoveGroupInterface* mgi_;
std::map<std::string, std::pair <std::string, std::string>> map_; std::map<std::string, std::string> map_;
...@@ -16,26 +16,21 @@ class Moveit_robot : public Robot{ ...@@ -16,26 +16,21 @@ class Moveit_robot : public Robot{
public: public:
Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){ Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
mgi_ = new moveit::planning_interface::MoveGroupInterface(name); mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
std::stringstream group, eef, hand_grasping_frame, ik_frame, hand, hand_n, ik_frame_n, name_n; std::stringstream hand_n, ik_frame_n, name_n;
name_n << name; name_n << name;
group << "group_" << name;
eef << "eef_" << name;
hand_grasping_frame << "hand_grasping_frame_" << name;
ik_frame << "ik_frame_" << name;
hand << "hand_" << name;
hand_n << "hand_" << name.back(); hand_n << "hand_" << name.back();
ik_frame_n << "panda_" << name.back() << "_link8"; ik_frame_n << "panda_" << name.back() << "_link8";
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("group", std::make_pair<std::string, std::string>(group.str(), name_n.str()))); map_.insert(std::make_pair<std::string, std::string>("group", name_n.str()));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("eef", std::make_pair<std::string, std::string>(eef.str(), hand_n.str()))); map_.insert(std::make_pair<std::string, std::string>("eef", hand_n.str()));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand_grasping_frame", std::make_pair<std::string, std::string>(hand_grasping_frame.str(), ik_frame_n.str()))); map_.insert(std::make_pair<std::string, std::string>("hand_grasping_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("ik_frame", std::make_pair<std::string, std::string>(ik_frame.str(), ik_frame_n.str()))); map_.insert(std::make_pair<std::string, std::string>("ik_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand", std::make_pair<std::string, std::string>(hand.str(), hand_n.str()))); map_.insert(std::make_pair<std::string, std::string>("hand", hand_n.str()));
} }
inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;} inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
inline std::map<std::string, std::pair <std::string, std::string>>& map(){return map_;}; inline std::map<std::string, std::string>& map(){return map_;};
}; };
......
#include "impl/moveit_mediator.h" #include "impl/moveit_mediator.h"
#include "impl/wing_moveit_decorator.h" #include "impl/wing_moveit_decorator.h"
#include "impl/collision_helper.h" #include "impl/collision_helper.h"
#include <yaml_to_mtc.h> #include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <eigen_conversions/eigen_msg.h>
void Moveit_mediator::publish_tables(moveit::planning_interface::PlanningSceneInterface& psi){ void Moveit_mediator::publish_tables(moveit::planning_interface::PlanningSceneInterface& psi){
...@@ -61,7 +74,7 @@ void Moveit_mediator::mediate(){ ...@@ -61,7 +74,7 @@ void Moveit_mediator::mediate(){
rewrite_task_template(robots_[0], psi.getObjects().at("bottle"), target); rewrite_task_template(robots_[0], psi.getObjects().at("bottle"), target);
Yaml_Mtc_Parser parser = Yaml_Mtc_Parser(); /*Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
moveit::task_constructor::Task task = parser.init_task(this->nh_); moveit::task_constructor::Task task = parser.init_task(this->nh_);
int max_planning_solutions = 1; // default one solution int max_planning_solutions = 1; // default one solution
this->nh_.getParam("max_planning_solutions", max_planning_solutions); this->nh_.getParam("max_planning_solutions", max_planning_solutions);
...@@ -76,6 +89,7 @@ void Moveit_mediator::mediate(){ ...@@ -76,6 +89,7 @@ void Moveit_mediator::mediate(){
std::cerr << "planning failed with exception" << std::endl << ex << task; std::cerr << "planning failed with exception" << std::endl << ex << task;
} }
ros::waitForShutdown(); ros::waitForShutdown();
*/
}; };
void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){ void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
...@@ -103,17 +117,6 @@ void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){ ...@@ -103,17 +117,6 @@ void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z)); tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
// unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up // unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up
setup_task();
XmlRpc::XmlRpcValue task;
nh_.getParam("task/stages", task);
std::stringstream ss;
ss << "hand_" << r->name().back();
std::string hand = ss.str();
std::stringstream sss;
sss << "panda_" << r->name().back() << "_link8";
std::string last_link = sss.str();
std::string support_surface1 = "nichts"; std::string support_surface1 = "nichts";
std::string support_surface2 = "nichts"; std::string support_surface2 = "nichts";
...@@ -125,6 +128,8 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll ...@@ -125,6 +128,8 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
if(ar->check_single_object_collision(target, str)) support_surface2= str; if(ar->check_single_object_collision(target, str)) support_surface2= str;
} }
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
/*
ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ()); ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ());
ROS_INFO("surface1 %s", support_surface1.c_str()); ROS_INFO("surface1 %s", support_surface1.c_str());
...@@ -202,17 +207,734 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll ...@@ -202,17 +207,734 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
task[4]["stages"] = b; task[4]["stages"] = b;
nh_.setParam("task/stages", task); nh_.setParam("task/stages", task);
*/
const std::string object = source.id;
// Reset ROS introspection before constructing the new object
// TODO(v4hn): global storage for Introspection services to enable one-liner
moveit::task_constructor::Task task_;
task_.stages()->setName("Pick and Place test");
task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel());
// Sampling planner
auto sampling_planner = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>();
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
// Cartesian planner
auto cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScaling(1.0);
cartesian_planner->setMaxAccelerationScaling(1.0);
cartesian_planner->setStepSize(.01);
// Set task properties
task_.setProperty("group", "panda_arm1");
task_.setProperty("eef", "hand_1");
task_.setProperty("hand", "panda_1_link8");
task_.setProperty("hand_grasping_frame", "panda_1_link8");
task_.setProperty("ik_frame", "hand_1");
/****************************************************
* *
* Current State *
* *
***************************************************/
moveit::task_constructor::Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
{
auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
// Verify that object is not attached
auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
comment = "object with id '" + object + "' is already attached and cannot be picked";
return false;
} }
return true;
});
current_state_ptr = applicability_filter.get();
task_.add(std::move(applicability_filter));
}
void Moveit_mediator::setup_task (){ /****************************************************
for (Abstract_robot* robot : robots_){ * *
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot); * Open Hand *
* *
***************************************************/
{ // Open Hand
auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner);
stage->setGroup("hand_1");
stage->setGoal("open");
task_.add(std::move(stage));
}
/****************************************************
* *
* Move to Pick *
* *
***************************************************/
{ // Move-to pre-grasp
auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
"move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm1", sampling_planner} });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task_.add(std::move(stage));
}
/****************************************************
* *
* Pick Object *
* *
***************************************************/
moveit::task_constructor::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
/****************************************************
---- * Approach Object *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", "panda_1_link8");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.07, 0.2);
// Set hand forward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "panda_1_link8";
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
/****************************************************
---- * Generate Grasp Pose *
***************************************************/
{
// Sample grasp pose
auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose("open");
stage->setObject(object);
stage->setAngleDelta(M_PI / 2);
stage->setMonitoredStage(current_state_ptr); // Hook into current state
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8);
wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(ik, "panda_1_link8");
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
grasp->insert(std::move(wrapper));
}
/****************************************************
---- * Allow Collision (hand object) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task_.getRobotModel()->getJointModelGroup("hand_1")->getLinkModelNamesWithCollisionGeometry(),
true);
grasp->insert(std::move(stage));
}
/****************************************************
---- * Close Hand *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner);
stage->setGroup("hand_1");
stage->setGoal("close");
grasp->insert(std::move(stage));
}
/****************************************************
.... * Attach Object *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, "panda_1_link8");
attach_object_stage = stage.get();
grasp->insert(std::move(stage));
}
/****************************************************
.... * Allow collision (object support) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
stage->allowCollisions({ object }, support_surface1, true);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Lift object *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.1, 0.2);
stage->setIKFrame("panda_1_link8");
stage->properties().set("marker_ns", "lift_object");
// Set upward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Forbid collision (object support) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surface1, false);
grasp->insert(std::move(stage));
}
// Add grasp container to task
task_.add(std::move(grasp));
}
/******************************************************
* *
* Move to Place *
* *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
"move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm1", sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task_.add(std::move(stage));
}
properties_[mr->map()["group"].first] = mr->map()["group"].second; /******************************************************
properties_[mr->map()["eef"].first] = mr->map()["eef"].second; * *
properties_[mr->map()["hand_grasping_frame"].first] = mr->map()["hand_grasping_frame"].second; * Place Object *
properties_[mr->map()["ik_frame"].first] = mr->map()["ik_frame"].second; * *
properties_[mr->map()["hand"].first] = mr->map()["hand"].second; *****************************************************/
{
auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
/****************************************************
.... * Forbid collision (object support) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surface2, true);
place->insert(std::move(stage));
} }
/******************************************************
---- * Lower Object *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner);
stage->properties().set("marker_ns", "lower_object");
stage->properties().set("link", "panda_1_link8");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.03, .13);
// Set downward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
/******************************************************
---- * Generate Place Pose *
*****************************************************/
{
// Generate Place Pose
auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
stage->properties().set("marker_ns", "place_pose");
stage->setObject(object);
// Set target pose
geometry_msgs::PoseStamped p;
p.header.frame_id = "world";
p.pose.position.x = target.getOrigin().getX();
p.pose.position.y = target.getOrigin().getY();
p.pose.position.z = target.getOrigin().getZ();
p.pose.orientation.x = target.getRotation().getX();
p.pose.orientation.y = target.getRotation().getY();
p.pose.orientation.z = target.getRotation().getZ();
p.pose.orientation.w = target.getRotation().getW();
stage->setPose(p);
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setIKFrame(ik, "panda_1_link8");
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
/******************************************************
---- * Open Hand *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner);
stage->setGroup("hand_1");
stage->setGoal("open");
place->insert(std::move(stage));
}
/******************************************************
---- * Forbid collision (hand, object) *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task_.getRobotModel()->getJointModelGroup("hand_1")->getLinkModelNamesWithCollisionGeometry(),
false);
place->insert(std::move(stage));
}
/******************************************************
---- * Detach Object *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
stage->detachObject(object, "panda_1_link8");
place->insert(std::move(stage));
}
/******************************************************
---- * Retreat Motion *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.12, .25);
stage->setIKFrame("panda_1_link8");
stage->properties().set("marker_ns", "retreat");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "panda_1_link8";
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
// Add place container to task
task_.add(std::move(place));
}
/******************************************************
* *
* Move to Home *
* *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setGoal("ready");
stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
task_.add(std::move(stage));
}
// ------------------
Moveit_robot* mr2 = dynamic_cast<Moveit_robot*>(robots_[1]);
moveit::task_constructor::Task task;
task.stages()->setName("Pick and Place test1");
task.loadRobotModel();
task.setRobotModel(mr2->mgi()->getRobotModel());
// Set task properties
task.setProperty("group", "panda_arm2");
task.setProperty("eef", "hand_2");
task.setProperty("hand", "panda_2_link8");
task.setProperty("hand_grasping_frame", "panda_2_link8");
task.setProperty("ik_frame", "hand_2");
/****************************************************
* *
* Current State *
* *
***************************************************/
current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
{
auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
// Verify that object is not attached
auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
comment = "object with id '" + object + "' is already attached and cannot be picked";
return false;
}
return true;
});
current_state_ptr = applicability_filter.get();
task.add(std::move(applicability_filter));
}
/****************************************************
* *
* Open Hand *
* *
***************************************************/
{ // Open Hand
auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner);
stage->setGroup("hand_2");
stage->setGoal("open");
task.add(std::move(stage));
}
/****************************************************
* *
* Move to Pick *
* *
***************************************************/
{ // Move-to pre-grasp
auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
"move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm2", sampling_planner} });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task.add(std::move(stage));
}
/****************************************************
* *
* Pick Object *
* *
***************************************************/
attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
task.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
/****************************************************
---- * Approach Object *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", "panda_2_link8");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.07, 0.2);
// Set hand forward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "panda_2_link8";
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
/****************************************************
---- * Generate Grasp Pose *
***************************************************/
{
// Sample grasp pose
auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose("open");
stage->setObject(object);
stage->setAngleDelta(M_PI / 2);
stage->setMonitoredStage(current_state_ptr); // Hook into current state
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8);
wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(ik, "panda_2_link8");
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
grasp->insert(std::move(wrapper));
}
/****************************************************
---- * Allow Collision (hand object) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task.getRobotModel()->getJointModelGroup("hand_2")->getLinkModelNamesWithCollisionGeometry(),
true);
grasp->insert(std::move(stage));
}
/****************************************************
---- * Close Hand *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner);
stage->setGroup("hand_2");
stage->setGoal("close");
grasp->insert(std::move(stage));
}
/****************************************************
.... * Attach Object *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, "panda_2_link8");
attach_object_stage = stage.get();
grasp->insert(std::move(stage));
}
/****************************************************
.... * Allow collision (object support) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
stage->allowCollisions({ object }, support_surface1, true);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Lift object *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.1, 0.2);
stage->setIKFrame("panda_2_link8");
stage->properties().set("marker_ns", "lift_object");
// Set upward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Forbid collision (object support) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surface1, false);
grasp->insert(std::move(stage));
}
// Add grasp container to task
task.add(std::move(grasp));
}
/******************************************************
* *
* Move to Place *
* *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
"move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm2", sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task.add(std::move(stage));
}
/******************************************************
* *
* Place Object *
* *
*****************************************************/
{
auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
task.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
/****************************************************
.... * Forbid collision (object support) *
***************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surface2, true);
place->insert(std::move(stage));
}
/******************************************************
---- * Lower Object *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner);
stage->properties().set("marker_ns", "lower_object");
stage->properties().set("link", "panda_2_link8");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.03, .13);
// Set downward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
/******************************************************
---- * Generate Place Pose *
*****************************************************/
{
// Generate Place Pose
auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
stage->properties().set("marker_ns", "place_pose");
stage->setObject(object);
// Set target pose
geometry_msgs::PoseStamped p;
p.header.frame_id = "world";
p.pose.position.x = target.getOrigin().getX();
p.pose.position.y = target.getOrigin().getY();
p.pose.position.z = target.getOrigin().getZ();
p.pose.orientation.x = target.getRotation().getX();
p.pose.orientation.y = target.getRotation().getY();
p.pose.orientation.z = target.getRotation().getZ();
p.pose.orientation.w = target.getRotation().getW();
stage->setPose(p);
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setIKFrame(ik, "panda_2_link8");
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
/******************************************************
---- * Open Hand *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner);
stage->setGroup("hand_2");
stage->setGoal("open");
place->insert(std::move(stage));
}
/******************************************************
---- * Forbid collision (hand, object) *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task.getRobotModel()->getJointModelGroup("hand_2")->getLinkModelNamesWithCollisionGeometry(),
false);
place->insert(std::move(stage));
}
/******************************************************
---- * Detach Object *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
stage->detachObject(object, "panda_2_link8");
place->insert(std::move(stage));
}
/******************************************************
---- * Retreat Motion *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.12, .25);
stage->setIKFrame("panda_2_link8");
stage->properties().set("marker_ns", "retreat");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "panda_2_link8";
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
// Add place container to task
task.add(std::move(place));
}
/******************************************************
* *
* Move to Home *
* *
*****************************************************/
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setGoal("ready");
stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
task.add(std::move(stage));
}
int max_planning_solution = 1;
if (task_.plan(max_planning_solution)){
moveit_msgs::MoveItErrorCodes execute_result;
execute_result = task_.execute(*task_.solutions().front());
//task_.introspection().publishSolution(*task_.solutions().front());
}
if (task.plan(max_planning_solution)){
moveit_msgs::MoveItErrorCodes execute_result;
execute_result = task.execute(*task.solutions().front());
//task_.introspection().publishSolution(*task_.solutions().front());
}
}
void Moveit_mediator::setup_task (){
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment