[WIP] Add pick&place MoveGroup capability

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})

capabilities/capabilities_plugin_description.xml

@@ -4,4 +4,10 @@
       Action server to execute solutions generated through the MoveIt Task Constructor.
     </description>
   </class>
+
+  <class name="move_group/PlanPickPlaceCapability" type="move_group::PlanPickPlaceCapability" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Action server to plan full pick and place motions using the MoveIt Task Constructor.
+    </description>
+  </class>
 </library>

capabilities/package.xml

@@ -18,6 +18,7 @@
 	<depend>pluginlib</depend>
 	<depend>std_msgs</depend>
 	<depend>moveit_task_constructor_msgs</depend>
+	<depend>moveit_task_constructor_core</depend>
 
 	<export>
 		<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>

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 <moveit/move_group/capability_names.h>
+#include <moveit/robot_state/conversions.h>
+
+
+namespace move_group {
+
+PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {}
+
+void PlanPickPlaceCapability::initialize() {
+	// configure the action server
+	as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>(
+	    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<PickPlaceTask>("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/class_loader.hpp>
+CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability)

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 <moveit/move_group/move_group_capability.h>
+#include <actionlib/server/simple_action_server.h>
+
+#include <moveit_task_constructor_msgs/PlanPickPlaceAction.h>
+#include <moveit/task_constructor/tasks/pick_place_task.h>
+
+#include <memory>
+
+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<actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>> as_;
+
+  std::unique_ptr<PickPlaceTask> pick_place_task_;
+};
+
+}  // namespace move_group

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 <ros/ros.h>
+
+// MoveIt
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+// MTC
+#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 <actionlib/client/simple_action_client.h>
+#include <actionlib/server/simple_action_server.h>
+
+#include <eigen_conversions/eigen_msg.h>
+
+#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<std::string> 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<double> 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

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}

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})