diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index a99b3e8c806533a16d0b11b6a5ede667d211d6d1..8c649ed65f675068f4c74bc4eccb4ce30f96ab51 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/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_filter.so 79
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index 73b94dd7c5066c90b3c9ad8baa14adcda17b8395..836711d39b12de45d9d0e96800c31359984789fc 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -39,7 +39,8 @@
"${workspaceFolder}/mtc/src",
"${workspaceFolder}/yaml_to_mtc/include",
"${workspaceFolder}/moveit_task_constructor/core/include",
- "${workspaceFolder}/moveit_task_constructor/"
+ "${workspaceFolder}/moveit_task_constructor/",
+ "${workspaceFolder}/object_creator/include"
],
"name": "ROS",
"cStandard": "c17",
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index 387d9b26cc207ff3be503e835dc81c4365cdd2de..60e789438cf403620374056ec374f7263179fc5a 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -8,7 +8,7 @@
class Moveit_robot : public Robot{
protected:
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{
public:
Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
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;
- 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();
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::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::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::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::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>("group", name_n.str()));
+ map_.insert(std::make_pair<std::string, std::string>("eef", hand_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::string>("ik_frame", ik_frame_n.str()));
+ map_.insert(std::make_pair<std::string, std::string>("hand", hand_n.str()));
}
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_;};
};
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 1d53c0e7b6c34c1d40820fdfbe34a4ad95728eb9..75bf7caca2fc030bb358f60948340953e512bc5f 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -1,8 +1,21 @@
#include "impl/moveit_mediator.h"
#include "impl/wing_moveit_decorator.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){
@@ -61,7 +74,7 @@ void Moveit_mediator::mediate(){
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_);
int max_planning_solutions = 1; // default one solution
this->nh_.getParam("max_planning_solutions", max_planning_solutions);
@@ -76,6 +89,7 @@ void Moveit_mediator::mediate(){
std::cerr << "planning failed with exception" << std::endl << ex << task;
}
ros::waitForShutdown();
+ */
};
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){
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));
// 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_surface2 = "nichts";
@@ -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;
}
+ 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("surface1 %s", support_surface1.c_str());
@@ -202,17 +207,734 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
task[4]["stages"] = b;
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));
+ }
+
+ /****************************************************
+ * *
+ * 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));
+ }
+
+ /******************************************************
+ * *
+ * 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_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 (){
- for (Abstract_robot* robot : robots_){
- Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
-
- 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;
- properties_[mr->map()["ik_frame"].first] = mr->map()["ik_frame"].second;
- properties_[mr->map()["hand"].first] = mr->map()["hand"].second;
- }
+
}