Merge branch 'bugfix/orientation' into 'master'

Bugfix/orientation

See merge request ceti/ros-internal/panda_grasping!1

README.md

 # Panda Grasping Package
 
-Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm.
+Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This package uses the old pick-and-place pipeline of MoveIt.
 
 ## How to install
 

src/grasping/environment_util.h

@@ -15,16 +15,45 @@
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/Vector3.h>
 
+/**
+ * Util for the construction of the scene.
+ */
 class EnvironmentUtil {
 
 public:
 
-    void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension);
-
+    /**
+     * Adds a table plate to the planning scene.
+     * @param collision_objects current collision objects in the planning scene
+     * @param x_dimension width of the table
+     * @param y_dimension length of the table
+     */
+    void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension,
+                        double y_dimension);
+
+    /**
+    * Adds a support surface to the planning scene (with ID "bin").
+    * @param collision_objects current collision objects in the planning scene
+    * @param id id of the support surface to create
+    * @param object_x_dimension x dimension object to be picked
+    * @param object_y_dimension y dimension object to be picked
+    * @param object_z_dimension z dimension object to be picked
+     *@param object_to_pick_pose pose of the object to be picked
+    */
     void constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
                           double object_x_dimension, double object_y_dimension, double object_z_dimension,
                           geometry_msgs::Pose &object_to_pick_pose);
 
-    moveit_msgs::CollisionObject constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions);
+    /**
+     * Adds a object to be picked/placed to the planning scene.
+     * @param collision_objects current collision objects in the planning scene
+     * @param id id of the object to create
+     * @param pose pose of the object to be picked
+     * @param dimensions dimensions of the object to be picked
+     */
+    moveit_msgs::CollisionObject
+    constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
+                          geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions);
 };
+
 #endif //PANDA_GRASPING_ENVIRONMENT_UTIL_H

src/grasping/grasp_service.cpp

@@ -13,12 +13,68 @@
 #include "environment_util.h"
 #include "grasp_util.h"
 
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace grasping_state {
+    bool object_picked = false;
+}
+
+/**
+ * tf2 buffer to receive current robot state
+ */
+tf2_ros::Buffer tfBuffer;
+
+
+/**
+ * Move the robot to a pose between different pick/place-actions.
+ * Solves currently the problem of the wrong eef-to-object orientation.
+ * @param group a initialized MoveGroupInterface instance
+ * @return true on success
+ */
+bool movetoInitPose(moveit::planning_interface::MoveGroupInterface &group) {
+
+    geometry_msgs::Pose another_pose;
+    another_pose.orientation.w = 0;
+    another_pose.orientation.x = 0.981;
+    another_pose.orientation.y = 0.196;
+    another_pose.orientation.z = 0;
+
+    another_pose.position.x = 0.26;
+    another_pose.position.y = 0;
+    another_pose.position.z = 0.87;
+    group.setPoseTarget(another_pose);
+
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+
+    group.plan(my_plan);
+    moveit_msgs::MoveItErrorCodes miec = group.move();
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
+    }
+
+    ROS_ERROR_STREAM("Error while moving to initial pose. MoveItErrorCode: " << miec.val);
+    return false;
+}
+
+/**
+ * Picks a specified object
+ * @param req a message containing the object specification
+ * @param res reference to the returned data
+ * @param planning_scene_interface a valid PlanningSceneInterface instance
+ * @param group a initialized MoveGroupInterface instance
+ * @param n a valid nodehandle
+ */
 bool pickObject(panda_grasping::PickObject::Request &req,
                 panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
                 moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
                 moveit::planning_interface::MoveGroupInterface &group) {
 
-    std::cout << "received new pick-request: " << req << std::endl;
+    if (!grasping_state::object_picked) {
+
+        grasping_state::object_picked = true;
+
         EnvironmentUtil env_util;
         GraspUtil grasp_util;
 
@@ -28,51 +84,101 @@ bool pickObject(panda_grasping::PickObject::Request &req,
         env_util.constructPlate(collision_objects, 3.0, 3.0);
 
         ROS_INFO("Adding pick support.");
-    env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
+        env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y,
+                                  req.dimensions.z,
                                   req.pose);
 
         ROS_INFO("Adding object.");
-    moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
+        moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose,
+                                                                         req.dimensions);
 
         ROS_INFO("Applying to scene.");
-    planning_scene_interface.applyCollisionObjects(collision_objects);
+        if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
+            res.success = false;
+            return false;
+        }
+
+        ROS_INFO("Moving to initial pose.");
+        if (!movetoInitPose(group)) {
+            res.success = false;
+            return false;
+        }
+
+        ROS_INFO("Retrieving current eef pose.");
+        geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
+                                                                                    ros::Time(0));
+        tf2::Quaternion orientation_gripper{transformStamped.transform.rotation.x,
+                                            transformStamped.transform.rotation.y,
+                                            transformStamped.transform.rotation.z,
+                                            transformStamped.transform.rotation.w};
 
         ROS_INFO("Computing pre-grasp-pose.");
-    geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, 0.05/*req.eef_distance*/, req.dimensions);
+        geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, orientation_gripper);
 
         ROS_INFO("Picking up ...");
         // 0.08 = 8cm = maximum gripper opening
-    grasp_util.pickFromAbove(group, pick_pose, 0.00, 0.08, "pick_support", "pick_object");
+        if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object")) {
+            res.success = false;
+            return false;
+        }
 
         res.success = true;
         return true;
     }
 
-bool placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n,
+    ROS_ERROR("Error: Cannot pick object. An other object is already picked.");
+    res.success = false;
+    return false;
+}
+
+/**
+ * Places a specified object
+ * @param req a message containing the objects target pose specification
+ * @param res reference to the returned data
+ * @param planning_scene_interface a valid PlanningSceneInterface instance
+ * @param group a initialized MoveGroupInterface instance
+ * @param n a valid nodehandle
+ */
+bool
+placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n,
             moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
             moveit::planning_interface::MoveGroupInterface &group) {
 
-    std::cout << "received new place-request: " << req << std::endl;
+    ROS_INFO_STREAM("received new place-request: " << req);
 
+    if (grasping_state::object_picked) {
         EnvironmentUtil env_util;
         GraspUtil grasp_util;
 
         std::vector<moveit_msgs::CollisionObject> collision_objects;
 
         ROS_INFO("Adding place support.");
-    env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
+        env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y,
+                                  req.dimensions.z,
                                   req.pose);
 
         ROS_INFO("Applying to scene.");
-    planning_scene_interface.applyCollisionObjects(collision_objects);
+        if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
+            res.success = false;
+            return false;
+        }
 
         ROS_INFO("Placing object.");
-    grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object");
+        if(!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object")){
+            res.success = false;
+            return false;
+        }
 
+        grasping_state::object_picked = false;
         res.success = true;
         return true;
     }
 
+    ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper.");
+    res.success = false;
+    return false;
+}
+
 
 int main(int argc, char **argv) {
 
@@ -81,6 +187,8 @@ int main(int argc, char **argv) {
     ros::AsyncSpinner spinner(2);
     spinner.start();
 
+    tf2_ros::TransformListener tfListener(tfBuffer);
+
     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
     moveit::planning_interface::MoveGroupInterface group("panda_arm");
 

src/grasping/grasp_util.cpp

@@ -9,6 +9,7 @@
 #include <actionlib/client/simple_client_goal_state.h>
 
 // Franka
+// if this should be required, the dependency to franka_gripper must also be declared
 #include <franka_gripper/MoveAction.h>
 
 void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
@@ -39,18 +40,32 @@ double inline correctHeightError(double height){
     return (-0.5 * height) + 0.1;
 }
 
-geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, double distance_to_eef, geometry_msgs::Vector3 dimensions) {
+geometry_msgs::Pose
+GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
+                                tf2::Quaternion orientation_gripper) {
 
     geometry_msgs::Pose target_pose;
 
-    tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
     tf2::Quaternion orientation_object(object_to_pick_pose.orientation.x, object_to_pick_pose.orientation.y,
                                        object_to_pick_pose.orientation.z, object_to_pick_pose.orientation.w);
 
-    tf2::Quaternion orientation = orientation_gripper * orientation_object;
-    orientation.setZ(orientation.getZ() * -1);
-    orientation.setY(orientation.getY() * -1);
-    orientation.setX(orientation.getX() * -1);
+    tf2::Matrix3x3 m_gripper(orientation_gripper);
+    double eef_roll, eef_pitch, eef_yaw;
+    m_gripper.getRPY(eef_roll, eef_pitch, eef_yaw);
+
+    tf2::Matrix3x3 o_gripper(orientation_object);
+    double o_roll, o_pitch, o_yaw;
+    o_gripper.getRPY(o_roll, o_pitch, o_yaw);
+
+    tf2::Quaternion orientation;
+
+    if (dimensions.x <= 0.08) {
+        orientation.setRPY(M_PI, 0, (o_yaw - (2 * eef_yaw)));
+    } else if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
+        orientation.setRPY(M_PI, 0, (o_yaw - (2 * eef_yaw) + M_PI_2));
+    } else {
+        orientation.setRPY(M_PI, 0, 0);
+    }
 
     target_pose.orientation = tf2::toMsg(orientation);
     target_pose.position.x = object_to_pick_pose.position.x;
@@ -59,14 +74,15 @@ geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_
     // works only with straight placed objects currently
     double baseZ = object_to_pick_pose.position.z + dimensions.z;
     target_pose.position.z = baseZ + correctHeightError(dimensions.z);
-    std::cout << target_pose.position.z << std::endl;
 
     return target_pose;
 }
 
-void GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                              double close_amount,
+bool
+GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+                         geometry_msgs::Vector3 dimensions,
                          double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) {
+
     std::vector<moveit_msgs::Grasp> grasps;
     grasps.resize(1);
     grasps[0].grasp_pose.header.frame_id = "panda_link0";
@@ -85,18 +101,36 @@ void GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &mo
     grasps[0].post_grasp_retreat.desired_distance = 0.25;
 
     // Setup the opening / closing - mechanism of the gripper
+
     setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
-    setupClosedGripper(grasps[0].grasp_posture, close_amount);
+
+    if (dimensions.x <= 0.08) {
+        setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
+    }
+
+    if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
+        setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
+    }
+
+    if (dimensions.x > 0.08 && dimensions.y > 0.08) {
+        ROS_ERROR("Could not grasp: Object to big (in any dimension).");
+        return false;
+    }
 
     move_group.setSupportSurfaceName(supporting_surface_id);
-    move_group.pick(object_to_pick_id, grasps);
+    moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps);
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
     }
 
-void GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
-                               double open_amount, std::string supporting_surface_id, std::string object_to_place_id){
+    ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+    return false;
+}
 
-    //tf2::Transform object_pose_transform;
-    //tf2::fromMsg (place_pose, object_pose_transform);
+bool
+GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
+                          double open_amount, std::string supporting_surface_id, std::string object_to_place_id) {
 
     std::vector<moveit_msgs::PlaceLocation> place_location;
     place_location.resize(1);
@@ -119,7 +153,14 @@ void GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &m
 
     setupOpenGripper(place_location[0].post_place_posture, open_amount);
     move_group.setSupportSurfaceName(supporting_surface_id);
-    move_group.place(object_to_place_id, place_location);
+    moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location);
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
+    }
+
+    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
+    return false;
 }
 
 void GraspUtil::resetGripperForNextAction() {

src/grasping/grasp_util.h

@@ -16,20 +16,62 @@
 #include <trajectory_msgs/JointTrajectory.h>
 #include <geometry_msgs/Pose.h>
 
+/**
+ * Util for grasping.
+ */
 class GraspUtil {
 
 public:
 
+    /**
+     * Sets up how far the gripper should be opened for the next action.
+     * @param posture empty posture object defining grasp
+     * @param amount total opening amount (space between "forks")
+     */
     void setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount);
 
+    /**
+    * Sets up how much the gripper should be closed for the next action.
+    * @param posture empty posture object defining grasp
+    * @param amount total close amount (space between "forks")
+    */
     void setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount);
 
-    geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, double distance_to_eef, geometry_msgs::Vector3 dimensions);
+    /**
+     * Compute the "pre-pick"-pose
+     * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
+     * @param dimensions of the object
+     * @param orientation_gripper current gripper orientation
+     * @return the "pre-pick"-pose
+     */
+    geometry_msgs::Pose
+    getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
+                         tf2::Quaternion orientation_gripper);
 
-    void pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                                  double close_amount, double open_amount, std::string supporting_surface_id, std::string object_to_pick_id);
+    /**
+     * Util method to pick an object from above.
+     * @param move_group an initialized MoveGroupInterface instance
+     * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose
+     * @param close_amount total close amount (space between "forks")
+     * @param open_amount total opening amount (space between "forks")
+     * @param supporting_surface_id ID of surface where object is picked from (e.g. the table)
+     * @param object_to_pick_id ID of the object to pick
+     * @return true if successfully picked
+     */
+    bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+                       geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
+                       std::string object_to_pick_id);
 
-    void placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
+    /**
+     * Util method to place an object from above.
+     * @param move_group an initialized MoveGroupInterface instance
+     * @param place_pose target place pose for the object
+     * @param close_amount total close amount (space between "forks")
+     * @param open_amount total opening amount (space between "forks")
+     * @param supporting_surface_id ID of surface where object is placed on (e.g. the table)
+     * @param object_to_place_id ID of the object to place
+     */
+    bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
                         double open_amount, std::string supporting_surface_id, std::string object_to_place_id);
 
     /**
@@ -37,4 +79,5 @@ public:
      */
     void resetGripperForNextAction();
 };
+
 #endif //PANDA_GRASPING_GRASP_UTIL_H

srv/PickObject.srv

 geometry_msgs/Pose pose
 geometry_msgs/Vector3 dimensions
-float64 eef_distance
 ---
 bool success
\ No newline at end of file