Commits

Anonymous committed 94ca0d5

Added pr2_template_based_grasping stack

Comments (0)

Files changed (23)

pr2/pr2_template_grasping/CMakeLists.txt

+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro.  This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake.  Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake.  Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly.  CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild_make_distribution(0.1.0)

pr2/pr2_template_grasping/Makefile

+include $(shell rospack find mk)/cmake_stack.mk

pr2/pr2_template_grasping/pr2_template_based_grasping/.gitignore

+src/pr2_template_based_grasping

pr2/pr2_template_grasping/pr2_template_based_grasping/CMakeLists.txt

+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+rosbuild_add_library(${PROJECT_NAME} 
+	src/arm_movement_handler.cpp
+	src/head_movement_handler.cpp
+	src/grasp_planning_server.cpp)
+
+rosbuild_add_executable(template_grasp_planning_server
+  src/template_grasp_planning_server.cpp
+)
+target_link_libraries(template_grasp_planning_server ${PROJECT_NAME})
+
+rosbuild_add_executable(template_grasp_feedback_ui
+  src/template_grasp_feedback_ui.cpp
+)
+target_link_libraries(template_grasp_feedback_ui ${PROJECT_NAME})
+
+rosbuild_add_executable(template_grasp_execution_ui
+	src/template_grasp_execution_ui.cpp
+)
+target_link_libraries(template_grasp_execution_ui ${PROJECT_NAME})
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

pr2/pr2_template_grasping/pr2_template_based_grasping/Makefile

+include $(shell rospack find mk)/cmake.mk

pr2/pr2_template_grasping/pr2_template_based_grasping/include/pr2_template_based_grasping/arm_movement_handler.h

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         arm_movment_handler.h
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#ifndef ARM_MOVEMENT_HANDLER_H_
+#define ARM_MOVEMENT_HANDLER_H_
+
+#include <string>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <sensor_msgs/PointCloud.h>
+#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
+#include <arm_navigation_msgs/MoveArmAction.h>
+#include <actionlib/client/simple_action_client.h>
+
+namespace pr2_template_based_grasping
+{
+
+struct ArmMovementResult
+{
+public:
+  ArmMovementResult();
+
+  bool attempt_valid_;
+  bool object_moved_;
+  bool grasp_successfull_;
+};
+
+class ArmMovementHandler
+{
+public:
+
+  ArmMovementHandler(ros::NodeHandle& nh, const std::string& frame_id);
+  ~ArmMovementHandler();
+
+  bool goToZeroPose();
+  bool isObjectInGripper();
+  void transformToWrist(const geometry_msgs::PoseStamped& input_pose,
+      geometry_msgs::PoseStamped& output_pose) const;
+  void getApproachPose(const geometry_msgs::PoseStamped& gripper_pose,
+      geometry_msgs::PoseStamped& aprch_pose) const;
+
+private:
+
+  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm_;
+  actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> gripper_client_;
+  ros::Publisher coll_obj_pub_;
+  geometry_msgs::PoseStamped zero_pose_;
+  std::string frame_id_, gripper_frame_;
+
+  void createGripperPoseGoalMsg(const geometry_msgs::PoseStamped& gripper_pose,
+      arm_navigation_msgs::MoveArmGoal& goal) const;
+  bool executeMovement(arm_navigation_msgs::MoveArmGoal& goal);
+  bool openGripper();
+  bool closeGripper();
+};
+
+} //namespace
+#endif /* ARM_MOVEMENT_HANDLER_H_ */

pr2/pr2_template_grasping/pr2_template_based_grasping/include/pr2_template_based_grasping/grasp_planning_server.h

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         grasp_planning_server.h
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#ifndef GRASP_PLANNING_SERVER_H_
+#define GRASP_PLANNING_SERVER_H_
+
+#include <map>
+#include <string>
+
+#include <actionlib/server/simple_action_server.h>
+#include <grasp_template_planning/visualization.h>
+#include <grasp_template_planning/planning_pipeline.h>
+#include <object_manipulation_msgs/GraspPlanning.h>
+#include <object_manipulation_msgs/GraspPlanningAction.h>
+#include <object_manipulation_msgs/GraspPlanningActionGoal.h>
+#include <grasp_template_planning/template_matching.h>
+#include <grasp_template_planning/object_detection_listener.h>
+#include <pr2_template_based_grasping/PlanningVisualization.h>
+#include <pr2_template_based_grasping/PlanningFeedback.h>
+#include <pr2_template_based_grasping/PlanningSummary.h>
+
+namespace pr2_template_based_grasping
+{
+
+class GraspPlanningServer : private grasp_template_planning::GraspPlanningParams
+{
+public:
+  GraspPlanningServer(ros::NodeHandle& nh, const std::string& demo_path,
+      const std::string& lib_path, const std::string& failure_path);
+
+  bool plan(object_manipulation_msgs::GraspPlanning::Request &req,
+            object_manipulation_msgs::GraspPlanning::Response &res);
+  bool giveFeedback(PlanningFeedback::Request& req, PlanningFeedback::Response& res);
+  bool visualize(PlanningVisualization::Request& req, PlanningVisualization::Response& res);
+  bool getLog(PlanningSummary::Request& req, PlanningSummary::Response& res);
+
+private:
+
+  boost::mutex mutex_;
+  ros::NodeHandle& nh_;
+
+  grasp_template_planning::PlanningPipeline planning_pipe_;
+  grasp_template_planning::Visualization visualizer_;
+  boost::shared_ptr<grasp_template_planning::TemplateMatching> grasp_pool_;
+  std::map<std::string, unsigned int> grasp_keys_;
+  PlanningFeedback::Request grasp_feedback_;
+  ros::Publisher attempt_pub_;
+  geometry_msgs::Pose table_frame_;
+  sensor_msgs::PointCloud target_cloud_;
+  grasp_template_planning::ObjectDetectionListener object_detection_;
+  ros::ServiceServer planning_service_;
+  ros::ServiceServer vis_service_;
+  ros::ServiceServer planning_feedback_service_;
+  ros::ServiceServer planning_summary_service_;
+  static const unsigned int PC_NUM_GRASP_OUTPUT = 100;
+
+  void convertGrasps(const grasp_template_planning::TemplateMatching& pool,
+                     std::vector<object_manipulation_msgs::Grasp>& goals);
+  bool updateGraspLibrary();
+  int getGraspResultIndex(unsigned int pool_index) const;
+};
+
+} //namespace
+#endif /* GRASP_PLANNING_SERVER_H_ */

pr2/pr2_template_grasping/pr2_template_based_grasping/include/pr2_template_based_grasping/head_movement_handler.h

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         head_movment_handler.h
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#ifndef HEAD_MOVEMENT_HANDLER_H_
+#define HEAD_MOVEMENT_HANDLER_H_
+
+#include <ros/ros.h>
+#include <actionlib/client/simple_action_client.h>
+#include <pr2_controllers_msgs/PointHeadAction.h>
+
+namespace pr2_template_based_grasping
+{
+typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
+
+class HeadMovementHandler
+{
+public:
+  HeadMovementHandler();
+  ~HeadMovementHandler();
+
+  //! Points the camera frame at a point in a given frame
+  void lookAt(std::string frame_id, double x, double y, double z);
+
+private:
+  PointHeadClient* point_head_client_;
+};
+
+} //namespace
+#endif /* HEAD_MOVEMENT_HANDLER_H_ */

pr2/pr2_template_grasping/pr2_template_based_grasping/launch/template_grasp_execution_ui.launch

+<launch>
+  <node name="template_grasp_execution_ui" pkg="pr2_template_based_grasping" 
+    type="template_grasp_execution_ui" respawn="false" output="screen">
+  </node>  
+</launch>

pr2/pr2_template_grasping/pr2_template_based_grasping/launch/template_grasp_feedback_ui.launch

+<launch>
+  <arg name="log_dir" value="$(env HOME)/.ros/" />
+	
+  <node name="template_grasp_feedback_ui" pkg="pr2_template_based_grasping" 
+    type="template_grasp_feedback_ui" respawn="false"
+    args="$(arg log_dir)" output="screen">
+  </node>  
+</launch>

pr2/pr2_template_grasping/pr2_template_based_grasping/launch/template_grasp_planning_server.launch

+<launch>
+	<arg name="grasp_library_file" value="$(find grasp_template_planning)/data/grasp_library.bag" /> 
+	<arg name="grasp_demonstrations_path" value="$(find grasp_template_planning)/data/grasp_demonstrations_data/" />
+	<arg name="library_negatives" value="$(find grasp_template_planning)/data/library_negatives/" />
+	
+  <node name="template_grasp_planning_server" pkg="pr2_template_based_grasping" 
+    type="template_grasp_planning_server" respawn="false"
+    args="$(arg grasp_demonstrations_path) $(arg grasp_library_file) $(arg library_negatives)" output="screen">
+    <rosparam file="$(find grasp_template_planning)/config/template_config_pr2.yaml" command="load"/>
+  </node>  
+</launch>

pr2/pr2_template_grasping/pr2_template_based_grasping/mainpage.dox

+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b pr2_template_based_grasping is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

pr2/pr2_template_grasping/pr2_template_based_grasping/manifest.xml

+<package>
+  <description brief="Implementation of grasp_template_planning on the PR2">
+
+     pr2_template_based_grasping
+
+  </description>
+  <author>Alexander Herzog (USC)</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/template_based_grasp_planning</url>
+
+  <depend package="roscpp"/>
+  <!-- depend package="rosrt"/ -->
+  
+  <depend package="grasp_template"/>
+  <depend package="grasp_template_planning"/>
+  
+  <depend package="pcl"/>
+  
+  <depend package="tabletop_object_detector"/>
+  <depend package="visualization_msgs" />
+  
+  <depend package="actionlib"/>
+  <depend package="pr2_controllers_msgs"/>
+  <depend package="pr2_arm_navigation_tutorials"/>
+  <depend package="tabletop_collision_map_processing"/>
+  <depend package="object_manipulation_msgs"/>
+    
+  <depend package="arm_navigation_msgs"/>
+</package>
+
+

pr2/pr2_template_grasping/pr2_template_based_grasping/src/arm_movement_handler.cpp

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         arm_movment_handler.cpp
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#include <ros/ros.h>
+#include <Eigen/Eigen>
+#include <arm_navigation_msgs/SimplePoseConstraint.h>
+#include <arm_navigation_msgs/utils.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <pr2_template_based_grasping/arm_movement_handler.h>
+
+using namespace std;
+using namespace Eigen;
+using namespace geometry_msgs;
+
+namespace pr2_template_based_grasping
+{
+
+/* MovementResult */
+
+ArmMovementResult::ArmMovementResult() :
+  attempt_valid_(false), object_moved_(false), grasp_successfull_(false)
+{
+}
+
+/* MovementHandler */
+
+ArmMovementHandler::ArmMovementHandler(ros::NodeHandle& nh, const string& frame_id) :
+  move_arm_("/move_right_arm", true), gripper_client_("r_gripper_controller/gripper_action", true)
+{
+  frame_id_ = frame_id;
+  gripper_frame_ = "r_gripper_led_frame";
+
+  zero_pose_.header.frame_id = frame_id_;
+  zero_pose_.header.stamp = ros::Time::now();
+
+  tf::TransformListener listener;
+  tf::StampedTransform transform;
+  try
+  {
+    if (!listener.waitForTransform(frame_id_, gripper_frame_, ros::Time(0), ros::Duration(3.0)))
+    {
+      ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: Waiting for transform timed out.");
+    }
+    listener.lookupTransform(frame_id_, gripper_frame_, ros::Time(0), transform);
+    zero_pose_.pose.position.x = transform.getOrigin().x();
+    zero_pose_.pose.position.y = transform.getOrigin().y();
+    zero_pose_.pose.position.z = transform.getOrigin().z();
+    zero_pose_.pose.orientation.x = transform.getRotation().x();
+    zero_pose_.pose.orientation.y = transform.getRotation().y();
+    zero_pose_.pose.orientation.z = transform.getRotation().z();
+    zero_pose_.pose.orientation.w = transform.getRotation().w();
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: %s", ex.what());
+  }
+
+  while (!move_arm_.waitForServer(ros::Duration(2.0)))
+  {
+    ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: Waiting for Move Arm Service...");
+  }
+
+  ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: Connected to server");
+}
+
+ArmMovementHandler::~ArmMovementHandler()
+{
+}
+
+bool getTransform(tf::StampedTransform& transform, const string& from, const string& to)
+{
+  tf::TransformListener listener;
+  bool result = false;
+  try
+  {
+    if (!listener.waitForTransform(from, to, ros::Time(0), ros::Duration(3.0)))
+    {
+      ROS_DEBUG_STREAM("pr2_template_based_grasping::ArmMovementHandler: Wait for transform timed out! "
+          "Tried to transform from " << from << " to " << to);
+    }
+    else
+    {
+      listener.lookupTransform(from, to, ros::Time(0), transform);
+      result = true;
+    }
+
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: %s", ex.what());
+  }
+
+  return result;
+}
+
+void ArmMovementHandler::transformToWrist(const PoseStamped& input_pose, PoseStamped& output_pose) const
+{
+  tf::StampedTransform led_to_wrist;
+  getTransform(led_to_wrist, gripper_frame_, "r_wrist_roll_link");
+
+  tf::Transform led_to_base;
+  tf::poseMsgToTF(input_pose.pose, led_to_base);
+  tf::poseTFToMsg(led_to_base * led_to_wrist, output_pose.pose);
+}
+
+void ArmMovementHandler::createGripperPoseGoalMsg(const PoseStamped& gripper_pose, arm_navigation_msgs::MoveArmGoal& goal) const
+{
+  goal.motion_plan_request.group_name = "right_arm";
+  goal.motion_plan_request.num_planning_attempts = 1;
+  goal.motion_plan_request.planner_id = string("");
+  goal.planner_service_name = string("ompl_planning/plan_kinematic_path");
+  goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
+
+  arm_navigation_msgs::SimplePoseConstraint desired_pose;
+  PoseStamped wrist_pose;
+  transformToWrist(gripper_pose, wrist_pose);
+  desired_pose.header.frame_id = gripper_pose.header.frame_id;
+  desired_pose.link_name = "r_wrist_roll_link";
+  desired_pose.pose = wrist_pose.pose;
+
+  desired_pose.absolute_position_tolerance.x = 0.02;
+  desired_pose.absolute_position_tolerance.y = 0.02;
+  desired_pose.absolute_position_tolerance.z = 0.02;
+
+  desired_pose.absolute_roll_tolerance = 0.04;
+  desired_pose.absolute_pitch_tolerance = 0.04;
+  desired_pose.absolute_yaw_tolerance = 0.04;
+
+  arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose, goal);
+}
+
+bool ArmMovementHandler::executeMovement(arm_navigation_msgs::MoveArmGoal& goal)
+{
+  bool finished_within_time = false;
+  move_arm_.sendGoal(goal);
+  finished_within_time = move_arm_.waitForResult(ros::Duration(200.0));
+  if (!finished_within_time)
+  {
+    move_arm_.cancelGoal();
+    ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: "
+        "Timed out trying to achieve goal");
+  }
+  else
+  {
+    actionlib::SimpleClientGoalState state = move_arm_.getState();
+    bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
+    if (success)
+    {
+      ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: Action finished: %s",
+          state.toString().c_str());
+
+      return true;
+    }
+    else
+    {
+      ROS_DEBUG("pr2_template_based_grasping::ArmMovementHandler: Action failed: %s",
+          state.toString().c_str());
+    }
+  }
+
+  return false;
+}
+
+void ArmMovementHandler::getApproachPose(const PoseStamped& gripper_pose, PoseStamped& aprch_pose) const
+{
+  aprch_pose = gripper_pose;
+  Vector3d backstep(-0.02, 0, 0);
+  Quaterniond orientation;
+  orientation.w() = aprch_pose.pose.orientation.w;
+  orientation.x() = aprch_pose.pose.orientation.x;
+  orientation.y() = aprch_pose.pose.orientation.y;
+  orientation.z() = aprch_pose.pose.orientation.z;
+
+  backstep = orientation * backstep;
+
+  aprch_pose.pose.position.x += backstep.x();
+  aprch_pose.pose.position.y += backstep.y();
+  aprch_pose.pose.position.z += backstep.z();
+}
+
+bool ArmMovementHandler::goToZeroPose()
+{
+  bool succ = false;
+  succ = openGripper();
+
+  arm_navigation_msgs::MoveArmGoal goal_pose;
+  createGripperPoseGoalMsg(zero_pose_, goal_pose);
+  succ = succ && executeMovement(goal_pose);
+
+  return succ;
+}
+
+bool ArmMovementHandler::openGripper()
+{
+  pr2_controllers_msgs::Pr2GripperCommandGoal open;
+  open.command.position = 0.08;
+  open.command.max_effort = -1.0; // Do not limit effort (negative)
+
+  gripper_client_.sendGoal(open);
+  gripper_client_.waitForResult();
+
+  return (gripper_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
+}
+
+bool ArmMovementHandler::closeGripper()
+{
+  pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
+  squeeze.command.position = 0.0;
+  squeeze.command.max_effort = 50.0; // Close gently
+
+  gripper_client_.sendGoal(squeeze);
+  gripper_client_.waitForResult();
+
+  return (gripper_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
+}
+
+bool ArmMovementHandler::isObjectInGripper()
+{
+
+  pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
+  squeeze.command.position = 0.0;
+  squeeze.command.max_effort = 100.0;
+
+  gripper_client_.sendGoal(squeeze);
+  gripper_client_.waitForResult();
+
+  return (gripper_client_.getResult()->position > 0.002);
+
+}
+
+} //namespace

pr2/pr2_template_grasping/pr2_template_based_grasping/src/grasp_planning_server.cpp

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         grasp_planning_server.cpp
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#include <vector>
+#include <string>
+#include <boost/bind.hpp>
+
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <grasp_template_planning/object_detection_listener.h>
+#include <pr2_template_based_grasping/grasp_planning_server.h>
+
+using namespace std;
+using namespace Eigen;
+using namespace grasp_template_planning;
+using namespace grasp_template;
+using namespace geometry_msgs;
+
+namespace pr2_template_based_grasping
+{
+
+GraspPlanningServer::GraspPlanningServer(ros::NodeHandle& nh, const string& demo_path,
+    const string& lib_path, const string& failure_path) :
+  nh_(nh), planning_pipe_(demo_path, lib_path, failure_path), visualizer_(false),
+      attempt_pub_(nh.advertise<PoseStamped> ("grasp_attempt_viz", 5))
+{
+  object_detection_.connectToObjectDetector(nh_);
+  visualizer_.initialize(nh_);
+  visualizer_.startPublishing();
+
+  planning_service_ = nh_.advertiseService("pr2_template_grasp_planner", &GraspPlanningServer::plan, this);
+  ROS_INFO("Template grasp planner is up");
+  vis_service_
+      = nh_.advertiseService("pr2_template_grasp_planner_visualization", &GraspPlanningServer::visualize, this);
+  ROS_INFO("Template grasp planner visualization service is up.");
+  planning_feedback_service_ = nh_.advertiseService("pr2_template_grasp_planner_feedback",
+                                                    &GraspPlanningServer::giveFeedback, this);
+  ROS_INFO("Template grasp planner feedback service is up.");
+  planning_summary_service_ = nh_.advertiseService("pr2_template_grasp_planner_logging", &GraspPlanningServer::getLog,
+                                                   this);
+  ROS_INFO("Template grasp planner logging service is up.");
+}
+
+bool GraspPlanningServer::plan(object_manipulation_msgs::GraspPlanning::Request &req,
+    object_manipulation_msgs::GraspPlanning::Response &res)
+{
+  ros::Time t_start = ros::Time::now();
+  boost::mutex::scoped_lock lock(mutex_);
+
+  ObjectDetectionListener object_detection;
+  object_detection.connectToObjectDetector(nh_);
+  if (!object_detection.fetchClusterFromObjectDetector())
+  {
+    ROS_ERROR("Grasp planner could not obtain table pose.");
+  }
+  table_frame_ = object_detection.getTableFrame().pose;
+
+  if (!object_detection_.fetchClusterFromObjectDetector())
+  {
+    target_cloud_ = req.target.cluster;
+  }
+  else
+  {
+    target_cloud_ = object_detection_.getCluster();
+  }
+  Pose table = table_frame_;
+  planning_pipe_.initialize(target_cloud_, table);
+
+  ros::Time t_init = ros::Time::now();
+  ros::Duration init_duration = t_init - t_start;
+
+  planning_pipe_.planGrasps(grasp_pool_);
+
+  ros::Time t_extract = ros::Time::now();
+  ros::Duration extract_duration = t_extract - t_init;
+
+  ROS_DEBUG_STREAM("pr2_template_based_grasping::GraspPlanningServer: Number of generated Grasps: "
+      << grasp_pool_->size() << endl);
+
+  ros::Time t_shift = ros::Time::now();
+  ros::Duration shift_duration = t_shift - t_extract;
+
+  convertGrasps(*grasp_pool_, res.grasps);
+
+  ros::Time t_convert = ros::Time::now();
+  ros::Duration convert_duration = t_convert - t_shift;
+
+  ros::Duration call_duration = ros::Time::now() - t_start;
+
+  ROS_DEBUG_STREAM("pr2_template_based_grasping::GraspPlanningServer: Initializing took: " << init_duration);
+  ROS_DEBUG_STREAM("Converting took: " << convert_duration);
+  ROS_DEBUG_STREAM("Overall planning took: " << call_duration);
+
+  return true;
+}
+
+void GraspPlanningServer::convertGrasps(const TemplateMatching& pool,
+    vector<object_manipulation_msgs::Grasp>& grasps)
+{
+  tf::StampedTransform led_to_wrist;
+  getTransform(led_to_wrist, frameGripper(), "r_wrist_roll_link");
+
+  grasp_keys_.clear();
+  const unsigned int num_grasps = min(PC_NUM_GRASP_OUTPUT, static_cast<unsigned int> (pool.size()));
+  for (unsigned int i = 0; i < num_grasps; i++)
+  {
+
+    Pose led_pose = pool.getGrasp(i).gripper_pose.pose;
+    Pose wrist_pose;
+
+    tf::Transform led_to_base;
+    tf::poseMsgToTF(led_pose, led_to_base);
+    tf::poseTFToMsg(led_to_base * led_to_wrist, wrist_pose);
+
+    grasps.push_back(object_manipulation_msgs::Grasp());
+    grasps.back().grasp_pose = wrist_pose;
+
+    sensor_msgs::JointState pre_g_posture;
+    pre_g_posture.header.stamp = ros::Time::now();
+    pre_g_posture.header.frame_id = "/base_link";
+    pre_g_posture.name.push_back("r_gripper_l_finger_joint");
+    pre_g_posture.name.push_back("r_gripper_r_finger_joint");
+    pre_g_posture.name.push_back("r_gripper_r_finger_tip_joint");
+    pre_g_posture.name.push_back("r_gripper_l_finger_tip_joint");
+    pre_g_posture.position.push_back(10);
+    pre_g_posture.position.push_back(10);
+    pre_g_posture.position.push_back(10);
+    pre_g_posture.position.push_back(10);
+    pre_g_posture.effort.push_back(100);
+    pre_g_posture.effort.push_back(100);
+    pre_g_posture.effort.push_back(100);
+    pre_g_posture.effort.push_back(100);
+    grasps.back().pre_grasp_posture = pre_g_posture;
+
+    sensor_msgs::JointState g_posture = pre_g_posture;
+    g_posture.header.stamp = ros::Time::now();
+    g_posture.position[0] = 0;
+    g_posture.position[1] = 0;
+    g_posture.position[2] = 0;
+    g_posture.position[3] = 0;
+    grasps.back().grasp_posture = g_posture;
+
+    grasps.back().success_probability = 1 - static_cast<double> (i) / num_grasps;
+
+    grasps.back().desired_approach_distance = 0.1;
+    grasps.back().min_approach_distance = 0.05;
+
+    string mp_key;
+    {
+      stringstream ss;
+      string tmp;
+      ss << grasps.back().pre_grasp_posture.header.stamp;
+      ss >> mp_key;
+      ss.clear();
+      ss << grasps.back().grasp_posture.header.stamp;
+      ss >> tmp;
+      mp_key.append(tmp);
+    }
+    grasp_keys_.insert(make_pair<string, unsigned int> (mp_key, i));
+  }
+}
+
+bool GraspPlanningServer::updateGraspLibrary()
+{
+  const object_manipulation_msgs::Grasp& attempt = grasp_feedback_.feedback.grasp;
+  string mp_key;
+  {
+    stringstream ss;
+    string tmp;
+    ss << attempt.pre_grasp_posture.header.stamp;
+    ss >> mp_key;
+    ss.clear();
+    ss << attempt.grasp_posture.header.stamp;
+    ss >> tmp;
+    mp_key.append(tmp);
+  }
+  unsigned int pool_key = grasp_keys_.find(mp_key)->second;
+  if (abs(grasp_feedback_.success) < 0.00001)
+  {
+    //fail
+    return planning_pipe_.addFailure(grasp_pool_->getLib(pool_key), grasp_pool_->getGrasp(pool_key));
+
+  }
+  else
+  {
+    //success
+    tf::StampedTransform wrist_to_led;
+    getTransform(wrist_to_led, "r_wrist_roll_link", frameGripper());
+
+    Pose led_pose;
+    Pose wrist_pose = grasp_feedback_.feedback.grasp.grasp_pose;
+
+    tf::Transform wrist_to_base;
+    tf::poseMsgToTF(wrist_pose, wrist_to_base);
+    tf::poseTFToMsg(wrist_to_base * wrist_to_led, led_pose);
+
+    return planning_pipe_.addSuccess(led_pose, grasp_pool_->getLib(pool_key), *grasp_pool_);
+  }
+}
+
+bool GraspPlanningServer::giveFeedback(PlanningFeedback::Request& req, PlanningFeedback::Response& res)
+{
+  boost::mutex::scoped_lock lock(mutex_);
+
+  bool upgrade_lib_result = false;
+  switch (req.action)
+  {
+    case PlanningFeedback::Request::DONT_UPGRADE_LIB:
+      grasp_feedback_ = req;
+      upgrade_lib_result = true;
+      break;
+    case PlanningFeedback::Request::UPDATE_LIB:
+      grasp_feedback_ = req;
+      upgrade_lib_result = updateGraspLibrary();
+      break;
+    case PlanningFeedback::Request::CHANGE_SUCCESS_AND_DO_UPGRADE:
+      grasp_feedback_.success = req.success;
+      upgrade_lib_result = updateGraspLibrary();
+      break;
+    default:
+      upgrade_lib_result = false;
+      break;
+  }
+
+  unsigned int vis_id = 0;
+  while(vis_id < grasp_feedback_.feedback.attempted_grasp_results.size()
+      && grasp_feedback_.feedback.attempted_grasp_results[vis_id].result_code
+      != object_manipulation_msgs::ManipulationResult::SUCCESS)
+  {
+    vis_id++;
+  }
+
+  if(vis_id < grasp_feedback_.feedback.attempted_grasp_results.size())
+  {
+    PlanningVisualization::Request vis_req;
+    vis_req.index = vis_id;
+    PlanningVisualization::Response dummy_response;
+    lock.unlock(); // might be not such a good idea!
+    visualize(vis_req, dummy_response);
+  }
+  else
+  {
+    ROS_INFO("In the evaluation of grasps none was successful.");
+  }
+
+  return upgrade_lib_result;
+}
+
+bool GraspPlanningServer::visualize(PlanningVisualization::Request& req,
+    PlanningVisualization::Response& res)
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  const unsigned int r = req.index % grasp_pool_->size();
+  visualizer_.resetData(planning_pipe_, grasp_pool_->getLib(r), grasp_pool_->getGrasp(r));
+
+  const int result_index = getGraspResultIndex(r);
+  if (result_index >= 0)
+  {
+    const object_manipulation_msgs::GraspResult& attempt_results =
+        grasp_feedback_.feedback.attempted_grasp_results[result_index];
+    const object_manipulation_msgs::Grasp& attempt = grasp_feedback_.feedback.attempted_grasps[result_index];
+    {
+      PoseStamped ps;
+      ps.header = attempt.grasp_posture.header;
+      ps.pose = attempt.grasp_pose;
+      attempt_pub_.publish(ps);
+    }
+    ROS_INFO_STREAM("Visualizing grasp with rank " << r << " and result code " << attempt_results.result_code);
+  }
+  else
+  {
+    ROS_INFO_STREAM("Visualizing grasp with rank " << r << ". The grasp has not been evaluated.");
+  }
+
+  return true;
+}
+
+int GraspPlanningServer::getGraspResultIndex(unsigned int pool_index) const
+{
+  string mp_key = "-1";
+  for (map<string, unsigned int>::const_iterator it = grasp_keys_.begin();
+      it != grasp_keys_.end(); it++)
+  {
+    if (it->second == pool_index)
+    {
+      mp_key = it->first;
+      break;
+    }
+  }
+
+  if (mp_key == "-1")
+  {
+    return -1;
+  }
+
+  for (unsigned int i = 0; i < grasp_feedback_.feedback.attempted_grasps.size(); i++)
+  {
+    const object_manipulation_msgs::Grasp& attempt = grasp_feedback_.feedback.attempted_grasps[i];
+    string key;
+    {
+      stringstream ss;
+      string tmp;
+      ss << attempt.pre_grasp_posture.header.stamp;
+      ss >> key;
+      ss.clear();
+      ss << attempt.grasp_posture.header.stamp;
+      ss >> tmp;
+      key.append(tmp);
+    }
+
+    if (mp_key.compare(key) == 0)
+    {
+      return i;
+    }
+  }
+
+  return -1;
+}
+
+bool GraspPlanningServer::getLog(PlanningSummary::Request& req, PlanningSummary::Response& res)
+{
+  res.score_labels.clear();
+  res.score_labels.push_back("a = m(c, l))");
+  res.score_labels.push_back("b = m(c, f))");
+  res.score_labels.push_back("c = m(f, l))");
+  res.score_labels.push_back(grasp_pool_->getScoreFormula());
+  for (unsigned int i = 0; i < grasp_pool_->size() && i < PC_NUM_GRASP_OUTPUT; i++)
+  {
+    res.grasp_hypotheses.push_back(grasp_pool_->getGrasp(i));
+
+    res.score_values.push_back(grasp_pool_->getLibScore(i));
+    res.score_values.push_back(grasp_pool_->getFailScore(i));
+    res.score_values.push_back(grasp_pool_->getLibQuality(i));
+    res.score_values.push_back(grasp_pool_->getHypothesisScore(i));
+  }
+
+  res.table_frame = table_frame_;
+  res.target_object = target_cloud_;
+  res.attempted_grasp = grasp_feedback_.feedback.grasp;
+  res.success = grasp_feedback_.success;
+
+  return true;
+}
+
+}

pr2/pr2_template_grasping/pr2_template_based_grasping/src/head_movement_handler.cpp

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         head_movment_handler.cpp
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#include <pr2_template_based_grasping/head_movement_handler.h>
+
+using namespace std;
+
+namespace pr2_template_based_grasping
+{
+
+HeadMovementHandler::HeadMovementHandler()
+{
+  //Initialize the client for the Action interface to the head controller
+  point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
+
+  //wait for head controller action server to come up
+  while (!point_head_client_->waitForServer(ros::Duration(5.0)))
+  {
+    ROS_DEBUG("pr2_template_based_grasping::HeadMovementHandler: Waiting for the point_head_action server to come up");
+  }
+}
+
+HeadMovementHandler::~HeadMovementHandler()
+{
+  delete point_head_client_;
+}
+
+void HeadMovementHandler::lookAt(string frame_id, double x, double y, double z)
+{
+  //the goal message we will be sending
+  pr2_controllers_msgs::PointHeadGoal goal;
+
+  //the target point, expressed in the requested frame
+  geometry_msgs::PointStamped point;
+  point.header.frame_id = frame_id;
+  point.point.x = x;
+  point.point.y = y;
+  point.point.z = z;
+  goal.target = point;
+
+  //we are pointing the camera frame
+  //(pointing_axis defaults to X-axis)
+  goal.pointing_frame = "high_def_frame";
+
+  //take at least 0.5 seconds to get there
+  goal.min_duration = ros::Duration(0.5);
+
+  //and go no faster than 1 rad/s
+  goal.max_velocity = 1.0;
+
+  //send the goal
+  point_head_client_->cancelGoalsAtAndBeforeTime(ros::Time::now());
+  point_head_client_->sendGoal(goal);
+
+  //wait for it to get there (abort after 2 secs to prevent getting stuck)
+  point_head_client_->waitForResult(ros::Duration(2));
+}
+} //namespace

pr2/pr2_template_grasping/pr2_template_based_grasping/src/template_grasp_execution_ui.cpp

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         template_grasp_execution_ui.cpp
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#include <ros/ros.h>
+#include <actionlib/client/simple_action_client.h>
+#include <tabletop_object_detector/TabletopDetection.h>
+#include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h>
+#include <object_manipulation_msgs/PickupAction.h>
+#include <object_manipulation_msgs/PlaceAction.h>
+#include <pr2_controllers_msgs/PointHeadAction.h>
+#include <pr2_template_based_grasping/arm_movement_handler.h>
+#include <pr2_template_based_grasping/head_movement_handler.h>
+
+//// TEMPLATE GRASPING CODE BEGIN: code for planning service and related messages ////
+#include <pr2_template_based_grasping/PlanningFeedback.h>
+#include <object_manipulation_msgs/GraspPlanning.h>
+//// TEMPLATE GRASPING CODE END: code for planning service and related messages ////
+
+using namespace std;
+using namespace pr2_template_based_grasping;
+
+int main(int argc, char **argv)
+{
+  //initialize the ROS node
+  ros::init(argc, argv, "template_grasp_execution_ui");
+  ros::NodeHandle nh;
+
+  //set service and action names
+  const string OBJECT_DETECTION_SERVICE_NAME = "/object_detection";
+  const string COLLISION_PROCESSING_SERVICE_NAME =
+      "/tabletop_collision_map_processing/tabletop_collision_map_processing";
+  const string PICKUP_ACTION_NAME = "/object_manipulator/object_manipulator_pickup";
+  const string PLACE_ACTION_NAME = "/object_manipulator/object_manipulator_place";
+  const string HEAD_ACTION_NAME = "/head_traj_controller/point_head_action";
+
+  //create service and action clients
+  ros::ServiceClient object_detection_srv;
+  ros::ServiceClient collision_processing_srv;
+  actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> pickup_client(PICKUP_ACTION_NAME, true);
+  actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> place_client(PLACE_ACTION_NAME, true);
+  actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> head_client(HEAD_ACTION_NAME, true);
+  pr2_controllers_msgs::PointHeadGoal head_goal;
+  ArmMovementHandler mov_handl(nh, "/base_link");
+
+//// TEMPLATE GRASPING CODE BEGIN: connect to planning service ////
+  const string GRASP_PLANNING_SERVICE_NAME = "/pr2_template_grasp_planner";
+  const string GRASP_FEEDBACK_SERVICE_NAME = "/pr2_template_grasp_planner_feedback";
+  ros::ServiceClient grasp_planning_client;
+  ros::ServiceClient grasp_planning_feedback_client;
+
+  //wait for grasp planning client
+  while (!ros::service::waitForService(GRASP_PLANNING_SERVICE_NAME, ros::Duration(2.0)) && nh.ok())
+  {
+    ROS_INFO("Waiting for grasp planning service to come up");
+  }
+  if (!nh.ok())
+    exit(0);
+  grasp_planning_client = nh.serviceClient<object_manipulation_msgs::GraspPlanning> (
+      GRASP_PLANNING_SERVICE_NAME, true);
+
+  //wait for grasp planning feedback client
+  while (!ros::service::waitForService(GRASP_FEEDBACK_SERVICE_NAME, ros::Duration(2.0)) && nh.ok())
+  {
+    ROS_INFO("Waiting for grasp planning feedback service to come up");
+  }
+  if (!nh.ok())
+    exit(0);
+  grasp_planning_feedback_client = nh.serviceClient<PlanningFeedback> (
+      GRASP_FEEDBACK_SERVICE_NAME);
+//// TEMPLATE GRASPING CODE END: connect to planning service ////
+
+  //wait for detection client
+  while (!ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, ros::Duration(2.0)) && nh.ok())
+  {
+    ROS_INFO("Waiting for object detection service to come up");
+  }
+  if (!nh.ok())
+    exit(0);
+  object_detection_srv = nh.serviceClient<tabletop_object_detector::TabletopDetection> (
+      OBJECT_DETECTION_SERVICE_NAME, true);
+
+  //wait for collision map processing client
+  while (!ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, ros::Duration(2.0)) && nh.ok())
+  {
+    ROS_INFO("Waiting for collision processing service to come up");
+  }
+  if (!nh.ok())
+    exit(0);
+  collision_processing_srv = nh.serviceClient<tabletop_collision_map_processing::
+      TabletopCollisionMapProcessing> (COLLISION_PROCESSING_SERVICE_NAME, true);
+
+  //wait for pickup client
+  while (!pickup_client.waitForServer(ros::Duration(2.0)) && nh.ok())
+  {
+    ROS_INFO_STREAM("Waiting for action client " << PICKUP_ACTION_NAME);
+  }
+  if (!nh.ok())
+    exit(0);
+
+  //wait for place client
+  while (!place_client.waitForServer(ros::Duration(2.0)) && nh.ok())
+  {
+    ROS_INFO_STREAM("Waiting for action client " << PLACE_ACTION_NAME);
+  }
+  if (!nh.ok())
+    exit(0);
+
+  while (ros::ok())
+  {
+    ROS_INFO_STREAM("Enter 'go' to start the pickup process or" << endl <<
+        "'zero' to move the arm to it's initial position or" << endl <<
+        "'q' to quit.");
+
+    string uinp;
+    cin >> uinp;
+    if (uinp == "q")
+    {
+      break;
+    }
+    else if (uinp == "go")
+    {
+      //call the tabletop detection
+      ROS_INFO("Calling tabletop detector");
+      tabletop_object_detector::TabletopDetection detection_call;
+      detection_call.request.return_clusters = true;
+      detection_call.request.return_models = true;
+      if (!object_detection_srv.call(detection_call))
+      {
+        ROS_ERROR("Tabletop detection service failed");
+        continue;
+      }
+      if (detection_call.response.detection.result != detection_call.response.detection.SUCCESS)
+      {
+        ROS_ERROR("Tabletop detection returned error code %d",
+            detection_call.response.detection.result);
+        continue;
+      }
+      if (detection_call.response.detection.clusters.empty()
+          && detection_call.response.detection.models.empty())
+      {
+        ROS_ERROR("The tabletop detector detected the table, but found no objects");
+        continue;
+      }
+
+      //call collision map processing
+      ROS_INFO("Calling collision map processing");
+      tabletop_collision_map_processing::TabletopCollisionMapProcessing processing_call;
+      processing_call.request.detection_result = detection_call.response.detection;
+      processing_call.request.reset_collision_models = true;
+      processing_call.request.reset_attached_models = true;
+      processing_call.request.desired_frame = "base_link";
+      if (!collision_processing_srv.call(processing_call))
+      {
+        ROS_ERROR("Collision map processing service failed");
+        continue;
+      }
+      if (processing_call.response.graspable_objects.empty())
+      {
+        ROS_ERROR("Collision map processing returned no graspable objects");
+        continue;
+      }
+
+//// TEMPLATE GRASPING CODE BEGIN: request grasps for detected point-cloud cluster ////
+      ROS_INFO("Calling template grasp planner");
+      object_manipulation_msgs::GraspPlanning grasp_planning_call;
+      grasp_planning_call.request.target.cluster = detection_call.response.detection.clusters.at(0);
+      if (!grasp_planning_client.call(grasp_planning_call))
+      {
+        ROS_ERROR("Grasp planning service failed.");
+        continue;
+      }
+//// TEMPLATE GRASPING CODE END: request grasps for detected point-cloud cluster ////
+
+      //call object pickup
+      ROS_INFO("Calling the pickup action");
+      object_manipulation_msgs::PickupGoal pickup_goal;
+
+//// TEMPLATE GRASPING CODE BEGIN: give computed grasps to manipulation pipeline ////
+      pickup_goal.desired_grasps = grasp_planning_call.response.grasps;
+//// TEMPLATE GRASPING CODE END: give computed grasps to manipulation pipeline ////
+
+      pickup_goal.allow_gripper_support_collision = true; //allow gripper to touch table
+      pickup_goal.target = processing_call.response.graspable_objects.at(0);
+      pickup_goal.collision_object_name = processing_call.response.collision_object_names.at(0);
+      pickup_goal.collision_support_surface_name = processing_call.response.collision_support_surface_name;
+      pickup_goal.arm_name = "right_arm";
+      geometry_msgs::Vector3Stamped direction;
+      direction.header.stamp = ros::Time::now();
+      direction.header.frame_id = "base_link";
+      direction.vector.x = 0;
+      direction.vector.y = 0;
+      direction.vector.z = 1;
+      pickup_goal.lift.direction = direction;
+      pickup_goal.lift.desired_distance = 0.1;
+      pickup_goal.lift.min_distance = 0.05;
+      pickup_goal.use_reactive_lift = false;
+      pickup_goal.use_reactive_execution = false;
+      pickup_client.sendGoal(pickup_goal);
+      while (!pickup_client.waitForResult(ros::Duration(10.0)))
+      {
+        ROS_INFO("Waiting for the pickup action...");
+      }
+      object_manipulation_msgs::PickupResult pickup_result = *(pickup_client.getResult());
+      if (pickup_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
+      {
+        ROS_ERROR("The pickup action has failed with result code %d",
+            pickup_result.manipulation_result.value);
+        continue;
+      }
+
+//// TEMPLATE GRASPING CODE BEGIN: after grasp execution return result to grasp service ////
+      PlanningFeedback feedback;
+      feedback.request.action = PlanningFeedback::Request::DONT_UPGRADE_LIB;
+      feedback.request.feedback = pickup_result;
+      ROS_INFO("Calling template grasp planning feedback...");
+      if (!grasp_planning_feedback_client.call(feedback))
+      {
+        ROS_INFO("Feedback service failed");
+      }
+//// TEMPLATE GRASPING CODE END: after grasp execution return result to grasp service ////
+
+      //remember where we picked the object up from
+      geometry_msgs::PoseStamped pickup_location;
+      if (processing_call.response.graspable_objects.at(0).potential_models.size() != 0)
+      {
+        pickup_location = processing_call.response.graspable_objects.at(0).potential_models[0].pose;
+      }
+      else
+      {
+        pickup_location.header = processing_call.response.graspable_objects.at(0).cluster.header;
+        pickup_location.pose.position.x = processing_call.response.graspable_objects.at(0).cluster.points[0].x;
+        pickup_location.pose.position.y = processing_call.response.graspable_objects.at(0).cluster.points[0].y;
+        pickup_location.pose.position.z = processing_call.response.graspable_objects.at(0).cluster.points[0].z;
+        pickup_location.pose.orientation.w = 1;
+      }
+
+      //remember where to look at after execution of place action
+      head_goal.target.point = pickup_location.pose.position;
+
+      //place object
+      geometry_msgs::PoseStamped place_location = pickup_location;
+      place_location.header.stamp = ros::Time::now();
+      place_location.pose.position.x += 0.0;
+      ROS_INFO("Calling the place action");
+      object_manipulation_msgs::PlaceGoal place_goal;
+      place_goal.allow_gripper_support_collision = true;
+      place_goal.place_locations.push_back(place_location);
+      place_goal.collision_object_name = processing_call.response.collision_object_names.at(0);
+      place_goal.collision_support_surface_name = processing_call.response.collision_support_surface_name;
+      place_goal.grasp = pickup_result.grasp;
+      place_goal.arm_name = "right_arm";
+      place_goal.place_padding = 0.02;
+      place_goal.desired_retreat_distance = 0.1;
+      place_goal.min_retreat_distance = 0.05;
+      direction.header.stamp = ros::Time::now();
+      direction.header.frame_id = "base_link";
+      direction.vector.x = 0;
+      direction.vector.y = 0;
+      direction.vector.z = -1;
+      place_goal.approach.direction = direction;
+      place_goal.approach.desired_distance = 0.1;
+      place_goal.approach.min_distance = 0.05;
+      place_goal.use_reactive_place = false;
+      place_client.sendGoal(place_goal);
+      while (!place_client.waitForResult(ros::Duration(10.0)))
+      {
+        ROS_INFO("Waiting for the place action...");
+      }
+      object_manipulation_msgs::PlaceResult place_result = *(place_client.getResult());
+      if (place_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
+      {
+        ROS_ERROR("Place failed with error code %d",
+            place_result.manipulation_result.value);
+        continue;
+      }
+
+      ROS_INFO("Finished pickup and place.");
+    }
+    else if (uinp == "zero")
+    {
+      mov_handl.goToZeroPose();
+      ros::Duration(3.0).sleep();
+      HeadMovementHandler h_mov;
+      h_mov.lookAt("base_link", head_goal.target.point.x, head_goal.target.point.y, head_goal.target.point.z);
+    }
+  }
+  return 0;
+}

pr2/pr2_template_grasping/pr2_template_based_grasping/src/template_grasp_feedback_ui.cpp

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         template_grasp_feedback_ui.cpp
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#include <sstream>
+#include <fstream>
+
+#include <ros/ros.h>
+#include <rosbag/bag.h>
+#include <pr2_template_based_grasping/PlanningFeedback.h>
+#include <pr2_template_based_grasping/PlanningVisualization.h>
+#include <pr2_template_based_grasping/PlanningSummary.h>
+
+using namespace std;
+using namespace pr2_template_based_grasping;
+
+bool plotSummary(ros::ServiceClient& sum_client)
+{
+  PlanningSummary planning_sum;
+  if (!sum_client.call(planning_sum))
+  {
+    return false;
+  }
+
+  unsigned int cols = planning_sum.response.score_labels.size();
+  for (unsigned int i = 0; i < planning_sum.response.score_labels.size(); i++)
+  {
+    cout << planning_sum.response.score_labels[i] << "\t";
+  }
+  cout << endl;
+
+  for (unsigned int i = 0; i < planning_sum.response.score_values.size(); i++)
+  {
+    if (i % cols == 0)
+    {
+      cout << i / cols;
+    }
+    cout << planning_sum.response.score_values[i];
+    cout << "\t";
+    if (i % cols == cols - 1)
+    {
+      cout << endl;
+    }
+  }
+
+  return true;
+}
+
+bool writeSummaryToBag(ros::ServiceClient& sum_client, rosbag::Bag& bag)
+{
+  PlanningSummary planning_sum;
+  if (!sum_client.call(planning_sum))
+  {
+    return false;
+  }
+
+  ROS_INFO("Writing grasp planning log to bag: %s", bag.getFileName().c_str());
+  try
+  {
+    bag.write("grasp_template_planning_log", ros::Time::now(), planning_sum.response);
+
+  }
+  catch (rosbag::BagIOException ex)
+  {
+    ROS_ERROR("Problem when writing log file >%s< : %s.",
+        bag.getFileName().c_str(), ex.what());
+
+    return false;
+  }
+
+  return true;
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "template_grasp_feedback_ui");
+  ros::NodeHandle n;
+
+  ros::ServiceClient vis_client = n.serviceClient<PlanningVisualization> ("pr2_template_grasp_planner_visualization");
+  ros::ServiceClient fb_client = n.serviceClient<PlanningFeedback> ("pr2_template_grasp_planner_feedback");
+  ros::ServiceClient sum_client = n.serviceClient<PlanningSummary> ("pr2_template_grasp_planner_logging");
+
+  string log_filename, bag_filename;
+  log_filename = argv[1];
+  log_filename.append("template_grasping_");
+  stringstream ss;
+  ss << ros::Time::now();
+  string tmp;
+  ss >> tmp;
+  log_filename.append(tmp);
+  bag_filename = log_filename;
+  log_filename.append(".log");
+  bag_filename.append(".bag");
+
+  ofstream log_file;
+  log_file.open(log_filename.c_str(), std::ios::out | std::ios::app);
+  rosbag::Bag log_bag;
+  ROS_INFO_STREAM("Opening new bag for logging grasp planning results: " << bag_filename);
+  try
+  {
+    log_bag.open(bag_filename, rosbag::bagmode::Write);
+
+  }
+  catch (rosbag::BagIOException ex)
+  {
+    ROS_ERROR("Problem when opening bag file >%s< : %s.",
+        bag_filename.c_str(), ex.what());
+  }
+
+  string feedback_input = "i";
+  while (ros::ok() && feedback_input != "q")
+  {
+    ROS_INFO_STREAM("Please, wait until the robot finishes the grasp execution. "
+        "Then label the grasp by entering "
+        "the first character of:" << endl <<
+        "\t (f)ail or" << endl <<
+        "\t (s)uccess or"<< endl << endl <<
+        "\t (p)lot the values of the matching distance or" << endl <<
+        "\t (q)uit" << endl << "followed by enter.");
+    cin >> feedback_input;
+
+    PlanningFeedback fb_service;
+    fb_service.request.action = -1;
+    if (feedback_input == "f")
+    {
+      fb_service.request.action = PlanningFeedback::Request::CHANGE_SUCCESS_AND_DO_UPGRADE;
+      fb_service.request.success = 0.0;
+      log_file << "0" << endl;
+      ROS_INFO_STREAM("Logged FAILED ATTEMPT to " << log_filename);
+
+      // log result
+      if (!writeSummaryToBag(sum_client, log_bag))
+      {
+        ROS_WARN("Failed to write planning log to bag file...");
+      }
+    }
+    else if (feedback_input == "s")
+    {
+      fb_service.request.action = PlanningFeedback::Request::CHANGE_SUCCESS_AND_DO_UPGRADE;
+      fb_service.request.success = 1.0;
+
+      log_file << "1" << endl;
+      ROS_INFO_STREAM("Logged SUCCESSFUL ATTEMPT to " << log_filename);
+
+      // log result
+      if (!writeSummaryToBag(sum_client, log_bag))
+      {
+        ROS_WARN("Failed to write planning log to bag file...");
+      }
+    }
+    else if (feedback_input == "p")
+    {
+      if (!plotSummary(sum_client))
+      {
+        ROS_WARN("Failed to write planning log to bag file...");
+      }
+    }
+
+    if (feedback_input == "f" || feedback_input == "s")
+    {
+      if (!fb_client.call(fb_service))
+      {
+        ROS_ERROR("Feedback service failed. Grasp Library was not updated.");
+      }
+    }
+  }
+
+  log_file.close();
+  log_bag.close();
+
+  return 0;
+}

pr2/pr2_template_grasping/pr2_template_based_grasping/src/template_grasp_planning_server.cpp

+/*********************************************************************
+ Computational Learning and Motor Control Lab
+ University of Southern California
+ Prof. Stefan Schaal
+ *********************************************************************
+ \remarks      ...
+
+ \file         template_grasp_planning_server.cpp
+
+ \author       Alexander Herzog
+ \date         April 1, 2012
+
+ *********************************************************************/
+
+#include <ros/ros.h>
+
+#include <pr2_template_based_grasping/grasp_planning_server.h>
+
+using namespace std;
+using namespace pr2_template_based_grasping;
+
+int main(int argc, char **argv)
+{
+  if (argc < 3)
+  {
+    ROS_ERROR_STREAM("You missed some arguments. The correct call is: "
+        << "template_grasp_planning_server [grasp_demonstrations_path] "
+        "[grasp_library_path] [demo_filename] ...");
+    return -1;
+  }
+
+  ros::init(argc, argv, "template_grasp_planning_server");
+  ros::NodeHandle n;
+  GraspPlanningServer planning(n, argv[1], argv[2], argv[3]);
+  ros::spin();
+  return 0;
+}

pr2/pr2_template_grasping/pr2_template_based_grasping/srv/PlanningFeedback.srv

+object_manipulation_msgs/PickupResult feedback
+
+int32 UPDATE_LIB = 1
+
+int32 DONT_UPGRADE_LIB = 2
+
+#ignores feedback from this message. be aware of possible race conditions
+int32 CHANGE_SUCCESS_AND_DO_UPGRADE = 3
+
+int32 action
+float64 success
+---

pr2/pr2_template_grasping/pr2_template_based_grasping/srv/PlanningSummary.srv

+---
+grasp_template_planning/GraspAnalysis[] grasp_hypotheses
+geometry_msgs/Pose table_frame
+sensor_msgs/PointCloud target_object
+object_manipulation_msgs/Grasp attempted_grasp
+string[] score_labels
+float64[] score_values
+float64 success 

pr2/pr2_template_grasping/pr2_template_based_grasping/srv/PlanningVisualization.srv

+int64 index
+---
+

pr2/pr2_template_grasping/stack.xml

+<stack>
+  <description brief="PR2 specific implementation of the template-based grasp selection algorithm">pr2_template_grasping</description>
+  <author>Alexander Herzog</author>
+  <license>BSD</license>  
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/pr2_template_grasping</url>
+  <depend stack="ros" />
+  <depend stack="template_based_grasp_planning" />
+</stack>