diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt
index df46f9ff2..99a1ab58d 100644
--- a/capabilities/CMakeLists.txt
+++ b/capabilities/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
moveit_core
moveit_ros_move_group
moveit_task_constructor_msgs
+ moveit_task_constructor_core
pluginlib
std_msgs
)
@@ -16,6 +17,7 @@ catkin_package(
LIBRARIES $PROJECT_NAME}
CATKIN_DEPENDS
moveit_task_constructor_msgs
+ moveit_task_constructor_core
std_msgs
)
@@ -25,6 +27,7 @@ include_directories(
add_library(${PROJECT_NAME}
src/execute_task_solution_capability.cpp
+ src/plan_pick_place_capability.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
diff --git a/capabilities/capabilities_plugin_description.xml b/capabilities/capabilities_plugin_description.xml
index eba669c8f..a0ecf7adf 100644
--- a/capabilities/capabilities_plugin_description.xml
+++ b/capabilities/capabilities_plugin_description.xml
@@ -4,4 +4,10 @@
Action server to execute solutions generated through the MoveIt Task Constructor.
+
+
+
+ Action server to plan full pick and place motions using the MoveIt Task Constructor.
+
+
diff --git a/capabilities/package.xml b/capabilities/package.xml
index 11eaa3065..27b5cfb5a 100644
--- a/capabilities/package.xml
+++ b/capabilities/package.xml
@@ -18,6 +18,7 @@
pluginlib
std_msgs
moveit_task_constructor_msgs
+ moveit_task_constructor_core
diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp
new file mode 100644
index 000000000..4ecdced34
--- /dev/null
+++ b/capabilities/src/plan_pick_place_capability.cpp
@@ -0,0 +1,78 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016, Kentaro Wada.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+/* Author: Henning Kayser */
+
+#include "plan_pick_place_capability.h"
+
+#include
+#include
+
+
+namespace move_group {
+
+PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {}
+
+void PlanPickPlaceCapability::initialize() {
+ // configure the action server
+ as_.reset(new actionlib::SimpleActionServer(
+ root_node_handle_, "plan_pick_place",
+ std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false));
+ as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this));
+ as_->start();
+ pick_place_task_ = std::make_unique("pick_place_task");
+}
+
+void PlanPickPlaceCapability::goalCallback(
+ const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) {
+ moveit_task_constructor_msgs::PlanPickPlaceResult result;
+
+ // TODO: fill parameters
+ PickPlaceTask::Parameters parameters;
+
+ // initialize task
+ pick_place_task_->init(parameters);
+ // run plan
+ pick_place_task_->plan();
+ // retrieve and return result
+}
+
+void PlanPickPlaceCapability::preemptCallback() {
+ // TODO(henningkayser): abort planning
+}
+
+} // namespace move_group
+
+#include
+CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability)
diff --git a/capabilities/src/plan_pick_place_capability.h b/capabilities/src/plan_pick_place_capability.h
new file mode 100644
index 000000000..6df190b1d
--- /dev/null
+++ b/capabilities/src/plan_pick_place_capability.h
@@ -0,0 +1,71 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Hamburg 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 Hamburg 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.
+ *********************************************************************/
+
+/*
+ * Capability to plan pick and place motions using the MoveIt Task Constructor.
+ *
+ * Author: Henning Kayser
+ * */
+
+#pragma once
+
+#include
+#include
+
+#include
+#include
+
+#include
+
+namespace move_group {
+
+using moveit::task_constructor::tasks::PickPlaceTask;
+
+class PlanPickPlaceCapability : public MoveGroupCapability
+{
+public:
+ PlanPickPlaceCapability();
+
+ virtual void initialize();
+
+private:
+ void goalCallback(const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal);
+ void preemptCallback();
+
+ std::unique_ptr> as_;
+
+ std::unique_ptr pick_place_task_;
+};
+
+} // namespace move_group
diff --git a/core/include/moveit/task_constructor/tasks/pick_place_task.h b/core/include/moveit/task_constructor/tasks/pick_place_task.h
new file mode 100644
index 000000000..5a50ef380
--- /dev/null
+++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h
@@ -0,0 +1,128 @@
+/*********************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2019 PickNik LLC.
+ * 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 the copyright holder 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 HOLDER 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.
+ *********************************************************************/
+
+/* Author: Henning Kayser, Simon Goldstein
+ Desc: A demo to show MoveIt Task Constructor in action
+*/
+
+// ROS
+#include
+
+// MoveIt
+#include
+#include
+#include
+
+// MTC
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+
+#pragma once
+
+namespace moveit {
+namespace task_constructor {
+namespace tasks {
+using namespace moveit::task_constructor;
+
+class PickPlaceTask
+{
+public:
+ struct Parameters
+ {
+ // planning group properties
+ std::string arm_group_name_;
+ std::string eef_name_;
+ std::string hand_group_name_;
+ std::string hand_frame_;
+
+ // object + surface
+ std::vector support_surfaces_;
+ std::string object_reference_frame_;
+ std::string surface_link_;
+ std::string object_name_;
+ std::string world_frame_;
+ std::string grasp_frame_;
+ std::vector object_dimensions_;
+
+ // Predefined pose targets
+ std::string hand_open_pose_;
+ std::string hand_close_pose_;
+ std::string arm_home_pose_;
+
+ // Pick metrics
+ Eigen::Isometry3d grasp_frame_transform_;
+ double approach_object_min_dist_;
+ double approach_object_max_dist_;
+ double lift_object_min_dist_;
+ double lift_object_max_dist_;
+ double place_object_min_dist_;
+ double place_object_max_dist_;
+ double retreat_object_min_dist_;
+ double retreat_object_max_dist_;
+
+ // Place metrics
+ geometry_msgs::PoseStamped place_pose_;
+ double place_surface_offset_;
+ };
+
+ PickPlaceTask(const std::string& task_name);
+ ~PickPlaceTask() = default;
+
+ void init(const Parameters& parameters);
+
+ bool plan();
+
+private:
+ moveit::task_constructor::TaskPtr task_;
+ const std::string task_name_;
+};
+
+} // namespace tasks
+} // namespace task_constructor
+} // namespace moveit
diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt
index cf8b6ad1f..b9e9823a9 100644
--- a/core/src/CMakeLists.txt
+++ b/core/src/CMakeLists.txt
@@ -41,6 +41,7 @@ target_include_directories(${PROJECT_NAME}
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
add_subdirectory(stages)
+add_subdirectory(tasks)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
diff --git a/core/src/tasks/CMakeLists.txt b/core/src/tasks/CMakeLists.txt
new file mode 100644
index 000000000..ef0d55285
--- /dev/null
+++ b/core/src/tasks/CMakeLists.txt
@@ -0,0 +1,9 @@
+add_library(${PROJECT_NAME}_tasks
+ ${PROJECT_INCLUDE}/tasks/pick_place_task.h
+ pick_place_task.cpp
+)
+target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}_tasks
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp
new file mode 100644
index 000000000..4400ab47d
--- /dev/null
+++ b/core/src/tasks/pick_place_task.cpp
@@ -0,0 +1,401 @@
+/*********************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2019 PickNik LLC.
+ * 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 the copyright holder 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 HOLDER 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.
+ *********************************************************************/
+
+/* Author: Henning Kayser, Simon Goldstein
+ Desc: A demo to show MoveIt Task Constructor in action
+*/
+
+#include
+#include
+
+namespace moveit {
+namespace task_constructor {
+namespace tasks {
+
+constexpr char LOGNAME[] = "pick_place_task";
+PickPlaceTask::PickPlaceTask(const std::string& task_name)
+ : task_name_(task_name) {}
+
+void PickPlaceTask::init(const Parameters& parameters)
+{
+ ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
+ // Reset ROS introspection before constructing the new object
+ // TODO(henningkayser): verify this is a bug, fix if possible
+ task_.reset();
+ task_.reset(new moveit::task_constructor::Task(task_name_));
+ Task& t = *task_;
+ t.loadRobotModel();
+
+ // Sampling planner
+ // TODO(henningkayser): Setup and parameterize alternative planners
+ auto sampling_planner = std::make_shared();
+ sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
+
+ // Cartesian planner
+ auto cartesian_planner = std::make_shared();
+ cartesian_planner->setMaxVelocityScaling(1.0);
+ cartesian_planner->setMaxAccelerationScaling(1.0);
+ cartesian_planner->setStepSize(.01);
+
+ // Set task properties
+ t.setProperty("group", parameters.arm_group_name_);
+ t.setProperty("eef", parameters.eef_name_);
+ t.setProperty("hand", parameters.hand_group_name_);
+ t.setProperty("ik_frame", parameters.hand_frame_);
+
+ /****************************************************
+ * *
+ * Current State *
+ * *
+ ***************************************************/
+ Stage* current_state = nullptr; // Forward current_state on to grasp pose generator
+ {
+ auto _current_state = std::make_unique("current state");
+ _current_state->setTimeout(10);
+
+ // Verify that object is not attachd
+ auto applicability_filter =
+ std::make_unique("applicability test", std::move(_current_state));
+ applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) {
+ s.start()->scene()->printKnownObjects(std::cout);
+ if (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_))
+ {
+ comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked";
+ return false;
+ }
+ return true;
+ });
+
+ current_state = applicability_filter.get();
+ t.add(std::move(applicability_filter));
+ }
+
+ /****************************************************
+ * *
+ * Open Hand *
+ * *
+ ***************************************************/
+ { // Open Hand
+ auto stage = std::make_unique("open hand", sampling_planner);
+ stage->setGroup(parameters.hand_group_name_);
+ stage->setGoal(parameters.hand_open_pose_);
+ t.add(std::move(stage));
+ }
+
+ /****************************************************
+ * *
+ * Move to Pick *
+ * *
+ ***************************************************/
+ { // Move-to pre-grasp
+ auto stage = std::make_unique(
+ "move to pick", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } });
+ stage->setTimeout(5.0);
+ stage->properties().configureInitFrom(Stage::PARENT);
+ t.add(std::move(stage));
+ }
+
+ /****************************************************
+ * *
+ * Pick Object *
+ * *
+ ***************************************************/
+ Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
+ {
+ auto grasp = std::make_unique("pick object");
+ t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
+ grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
+
+ /****************************************************
+ ---- * Approach Object *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("approach object", cartesian_planner);
+ stage->properties().set("marker_ns", "approach_object");
+ stage->properties().set("link", parameters.hand_frame_);
+ stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+ stage->setMinMaxDistance(parameters.approach_object_min_dist_, parameters.approach_object_max_dist_);
+
+ // Set hand forward direction
+ geometry_msgs::Vector3Stamped vec;
+ vec.header.frame_id = parameters.object_name_;
+ vec.vector.z = -1.0;
+ stage->setDirection(vec);
+ grasp->insert(std::move(stage));
+ }
+
+ /****************************************************
+ ---- * Generate Grasp Pose *
+ ***************************************************/
+ {
+ // Sample grasp pose
+ auto stage = std::make_unique("generate grasp pose");
+ stage->properties().configureInitFrom(Stage::PARENT);
+ stage->properties().set("marker_ns", "grasp_pose");
+ stage->setPreGraspPose(parameters.hand_open_pose_);
+ stage->setObject(parameters.object_name_);
+ stage->setAngleDelta(M_PI / 12);
+ stage->setMonitoredStage(current_state); // Hook into current state
+
+ // Compute IK
+ auto wrapper = std::make_unique("grasp pose IK", std::move(stage));
+ wrapper->setMaxIKSolutions(2);
+ wrapper->setMinSolutionDistance(1.0);
+ wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
+ //wrapper->setIgnoreCollisions(true);
+ wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
+ wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
+ grasp->insert(std::move(wrapper));
+ }
+
+ /****************************************************
+ ---- * Allow Collision (hand object) *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("allow collision (hand,object)");
+ stage->allowCollisions(
+ parameters.object_name_,
+ t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
+ true);
+ grasp->insert(std::move(stage));
+ }
+
+ /****************************************************
+ ---- * Close Hand *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("close hand", sampling_planner);
+ stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
+ stage->setGoal(parameters.hand_close_pose_);
+ grasp->insert(std::move(stage));
+ }
+
+ /****************************************************
+ .... * Attach Object *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("attach object");
+ stage->attachObject(parameters.object_name_, parameters.hand_frame_);
+ attach_object_stage = stage.get();
+ grasp->insert(std::move(stage));
+ }
+
+ /****************************************************
+ .... * Allow collision (object support) *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("allow collision (object,support)");
+ stage->allowCollisions({ parameters.object_name_ }, "source_container", true); // TODO(henningkayser): parameterize
+ grasp->insert(std::move(stage));
+ }
+
+ /****************************************************
+ .... * Lift object *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("lift object", cartesian_planner);
+ stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+ stage->setMinMaxDistance(parameters.lift_object_min_dist_, parameters.lift_object_max_dist_);
+ stage->setIKFrame(parameters.hand_frame_);
+ stage->properties().set("marker_ns", "lift_object");
+
+ // Set upward direction
+ geometry_msgs::Vector3Stamped vec;
+ vec.header.frame_id = parameters.world_frame_;
+ vec.vector.z = 1.0;
+ stage->setDirection(vec);
+ grasp->insert(std::move(stage));
+ }
+
+ /****************************************************
+ .... * Forbid collision (object support) *
+ ***************************************************/
+ {
+ auto stage = std::make_unique("forbid collision (object,surface)");
+ stage->allowCollisions({ parameters.object_name_ }, "source_container", false); // TODO(henningkayser): parameterize
+ grasp->insert(std::move(stage));
+ }
+
+ // Add grasp container to task
+ t.add(std::move(grasp));
+ }
+
+ /******************************************************
+ * *
+ * Move to Place *
+ * *
+ *****************************************************/
+ {
+ auto stage = std::make_unique(
+ "move to place", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } });
+ stage->setTimeout(5.0);
+ stage->properties().configureInitFrom(Stage::PARENT);
+ t.add(std::move(stage));
+ }
+
+ /******************************************************
+ * *
+ * Place Object *
+ * *
+ *****************************************************/
+ {
+ auto place = std::make_unique("place object");
+ t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
+ place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" });
+
+ /******************************************************
+ ---- * Lower Object *
+ *****************************************************/
+ {
+ auto stage = std::make_unique("lower object", cartesian_planner);
+ stage->properties().set("marker_ns", "lower_object");
+ stage->properties().set("link", parameters.hand_frame_);
+ stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+ stage->setMinMaxDistance(parameters.place_object_min_dist_, parameters.place_object_max_dist_);
+
+ // Set downward direction
+ geometry_msgs::Vector3Stamped vec;
+ vec.header.frame_id = parameters.world_frame_;
+ vec.vector.z = -1.0;
+ stage->setDirection(vec);
+ place->insert(std::move(stage));
+ }
+
+ /******************************************************
+ ---- * Generate Place Pose *
+ *****************************************************/
+ {
+ // Generate Place Pose
+ auto stage = std::make_unique("generate place pose");
+ stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
+ stage->properties().set("marker_ns", "place_pose");
+ stage->setObject(parameters.object_name_);
+ stage->setPose(parameters.place_pose_);
+ // Hook into attach_object_stage which allows us to use the attached object as IK frame
+ stage->setMonitoredStage(attach_object_stage);
+
+ // Compute IK
+ auto wrapper = std::make_unique("place pose IK", std::move(stage));
+ wrapper->setMaxIKSolutions(2);
+ wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
+ // TODO(henningkayser): Enable collisions
+ wrapper->setIgnoreCollisions(true);
+ wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
+ wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
+ place->insert(std::move(wrapper));
+ }
+
+ /******************************************************
+ ---- * Open Hand *
+ *****************************************************/
+ {
+ auto stage = std::make_unique("open hand", sampling_planner);
+ stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
+ stage->setGoal(parameters.hand_open_pose_);
+ place->insert(std::move(stage));
+ }
+
+ /******************************************************
+ ---- * Forbid collision (hand, object) *
+ *****************************************************/
+ {
+ // TODO(henningkayser): Forbid collision after retreat?
+ auto stage = std::make_unique("forbid collision (hand,object)");
+ stage->allowCollisions(
+ parameters.object_name_,
+ t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
+ false);
+ place->insert(std::move(stage));
+ }
+
+ /******************************************************
+ ---- * Detach Object *
+ *****************************************************/
+ {
+ auto stage = std::make_unique("detach object");
+ stage->detachObject(parameters.object_name_, parameters.hand_frame_);
+ place->insert(std::move(stage));
+ }
+
+ /******************************************************
+ ---- * Retreat Motion *
+ *****************************************************/
+ {
+ // TODO(henningkayser): Do we need this if items are dropped?
+ auto stage = std::make_unique("retreat after place", cartesian_planner);
+ stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+ stage->setMinMaxDistance(parameters.retreat_object_min_dist_, parameters.retreat_object_max_dist_);
+ stage->setIKFrame(parameters.hand_frame_);
+ stage->properties().set("marker_ns", "retreat");
+ geometry_msgs::Vector3Stamped vec;
+ vec.header.frame_id = parameters.hand_frame_;
+ vec.vector.z = -1.0;
+ stage->setDirection(vec);
+ place->insert(std::move(stage));
+ }
+
+ // Add place container to task
+ t.add(std::move(place));
+ }
+
+ /******************************************************
+ * *
+ * Move to Home *
+ * *
+ *****************************************************/
+ {
+ auto stage = std::make_unique("move home", sampling_planner);
+ stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+ stage->setGoal(parameters.arm_home_pose_);
+ stage->restrictDirection(stages::MoveTo::FORWARD);
+ t.add(std::move(stage));
+ }
+}
+
+bool PickPlaceTask::plan() {
+ ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
+ try {
+ task_->plan(10); // TODO: parameterize
+ } catch (InitStageException& e) {
+ ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
+ return false;
+ }
+ if (task_->numSolutions() == 0) {
+ ROS_ERROR_NAMED(LOGNAME, "Planning failed");
+ return false;
+ }
+ return true;
+}
+
+} // namespace tasks
+} // namespace task_constructor
+} // namespace moveit
diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt
index 07ed05951..c49dcd9a6 100644
--- a/msgs/CMakeLists.txt
+++ b/msgs/CMakeLists.txt
@@ -27,6 +27,7 @@ add_service_files(DIRECTORY srv FILES
add_action_files(DIRECTORY action FILES
ExecuteTaskSolution.action
+ PlanPickPlace.action
)
generate_messages(DEPENDENCIES ${MSG_DEPS})
diff --git a/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action
new file mode 100644
index 000000000..5acfdb21a
--- /dev/null
+++ b/msgs/action/PlanPickPlace.action
@@ -0,0 +1,16 @@
+# goal
+string object_id
+string end_effector
+
+moveit_msgs/PlaceLocation place_location
+
+---
+
+# result
+moveit_msgs/MoveItErrorCodes error_code
+Solution solution
+
+---
+
+# feedback
+string feedback