Merge branch 'feature/grasp_by_side' into 'master'

New feature grasping from side + tests

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

CMakeLists.txt

@@ -70,4 +70,20 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti
 
 add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
 
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+
+if(CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp)
+#    add_executable(tests_grasping test/test.cpp)
+    target_link_libraries(tests_grasping ${catkin_LIBRARIES})
+    add_dependencies(tests_grasping panda_grasping_generate_messages_cpp)
+    add_rostest_gtest(tests_side_grasping test/side_grasping.test test/grasp_from_side_test.cpp)
+    target_link_libraries(tests_side_grasping ${catkin_LIBRARIES} GraspUtil)
+    add_dependencies(tests_side_grasping panda_grasping_generate_messages_cpp)
+endif()
 

include/grasp_util.h

@@ -61,8 +61,51 @@ public:
      */
     geometry_msgs::Pose
     getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
-                         geometry_msgs::Quaternion gripper_orientation,
-                         double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+                         geometry_msgs::Quaternion gripper_orientation, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+    /**
+     * Generate a pick-pose for grasping an object from its back.
+     * @param object_pose_msg the object pose
+     * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object
+     * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation
+     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
+     * @return the pick pose
+     */
+    geometry_msgs::Pose
+    getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+    /**
+     *
+     * Generate a pick-pose for grasping an object from its front.
+     * @param object_pose_msg the object pose
+     * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object
+     * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation
+     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
+     * @return the pick pose
+     */
+    geometry_msgs::Pose
+    getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+    /**
+     * Generate a pick-pose for grasping an object from left.
+     * @param object_pose_msg the object pose
+     * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object
+     * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation
+     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
+     * @return the pick pose
+     */
+    geometry_msgs::Pose
+    getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
+
+    /**
+     * Generate a pick-pose for grasping an object from right.
+     * @param object_pose_msg the object pose
+     * @param gripper_orientation_msg the target gripper orientation -> use a yaw of -M_PI_4 for an orientation orthogonal to the object
+     * @param object_orientation the objects orientation -> use (0,0,0,1) for axisymmetric orientation
+     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
+     * @return the pick pose
+     */
+    geometry_msgs::Pose
+    getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 
     /**
      * Util method to pick an object from above.
@@ -79,17 +122,46 @@ public:
                        std::string object_to_pick_id, bool plan_only = false);
 
     /**
-     * Util method to place an object from above.
+     * Pick an object from the side based on pick-pose.
      * @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 pick_pose pick pose for the approach
+     * @param object_to_pick geometric description of the object
+     * @param open_amount intial opening amount of the gripper
      *  @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
+     * @param object_to_pick_id ID of the object to place
+     * @param plan_only
+     * @return true on success
+     */
+    bool pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+                            shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction,
+                            double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false);
+
+    /**
+     * Place an object from above.
+     * @param move_group an initialized MoveGroupInterface instance
+     * @param place_pose a pre place pose
+     * @param open_amount intial opening amount of the gripper
+     * @param supporting_surface_id ID of surface where object is place on (e.g. the table)
+     * @param object_to_pick_id ID of the object to place
+     * @param plan_only
+     * @return true on success
      */
     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, bool plan_only = false);
 
+    /**
+     * Place an object from side.
+     * @param move_group an initialized MoveGroupInterface instance
+     * @param place_pose a pre-place pose
+     * @param open_amount intial opening amount of the gripper
+     * @param supporting_surface_id ID of surface where object is place on (e.g. the table)
+     * @param object_to_pick_id ID of the object to place
+     * @param plan_only
+     * @return true on success
+     */
+    bool placeFromSide(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, bool plan_only = false);
+
     /**
      * Homes the gripper (in case it was abused before and is in an inconsistent state)
      * This method is blocking with a maximum runtime of ~30s
@@ -97,6 +169,22 @@ public:
      */
     static bool homeGripper();
 
+private:
+
+    /**
+     *
+     * @param object_to_pick_pose
+     * @param gripper_orientation
+     * @param approach_orientation
+     * @param y_distance_diff
+     * @param x_distance_diff
+     * @param object_orientation
+     * @param reach
+     * @return
+     */
+    geometry_msgs::Pose
+    getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation,
+                        tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 };
 
 #endif //PANDA_GRASPING_GRASP_UTIL_H

src/grasping/environment_util.cpp

@@ -24,10 +24,11 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
     plate.primitive_poses[0].position.x = 0;
     plate.primitive_poses[0].position.y = 0;
     plate.primitive_poses[0].position.z = -0.1;
+    plate.primitive_poses[0].orientation.w = 1;
 
     plate.operation = plate.ADD;
 
-    collision_objects.push_back(plate);
+//    collision_objects.push_back(plate);
 }
 
 void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
@@ -50,6 +51,7 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
     pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x;
     pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y;
     pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02);
+    pick_support.primitive_poses[0].orientation.w = 1; // TODO align to object
 
     pick_support.operation = pick_support.ADD;
 

src/grasping/grasp_service.cpp

@@ -40,8 +40,6 @@ bool pickObject(panda_grasping::PickObject::Request &req,
 
     if (!grasping_state::object_picked) {
 
-        grasping_state::object_picked = true;
-
         EnvironmentUtil env_util;
         GraspUtil grasp_util;
 
@@ -79,6 +77,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
             return false;
         }
 
+        grasping_state::object_picked = true;
         res.success = true;
         return true;
     }

src/grasping/grasp_util.cpp

@@ -31,6 +31,101 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
     posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
+geometry_msgs::Pose
+GraspUtil::getPickFromBack(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  -M_PI_2;
+    approach_pitch = -M_PI_4;
+    approach_yaw =   -M_PI_4;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
+            approach_orientation, 0, -0.1, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromFront(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  M_PI_2;
+    approach_pitch = -M_PI_4 - M_PI_2;
+    approach_yaw =   -M_PI_4;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
+                               approach_orientation, 0, 0.1, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromLeft(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  M_PI_2;
+    approach_pitch = -M_PI_4 - M_PI_2;
+    approach_yaw =   -M_PI_4 - M_PI_2;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
+                               approach_orientation, -0.1, 0, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromRight(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg, double reach){
+
+    double approach_roll, approach_pitch, approach_yaw;
+    approach_roll =  -M_PI_2;
+    approach_pitch = -M_PI_4;
+    approach_yaw =   -M_PI_4 - M_PI_2;
+    tf2::Quaternion approach_orientation;
+    approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+    return getPickFromSidePose(object_pose_msg, dimensions, gripper_orientation_msg,
+                               approach_orientation, 0.1, 0, reach);
+}
+
+geometry_msgs::Pose
+GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, geometry_msgs::Quaternion gripper_orientation_msg,
+                    tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach){
+
+    // object orientation
+    tf2::Quaternion object_orientation;
+    tf2::fromMsg(object_pose_msg.orientation, object_orientation);
+
+    // gripper orientation
+    tf2::Quaternion gripper_orientation;
+    tf2::fromMsg(gripper_orientation_msg, gripper_orientation);
+    tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
+    geometry_msgs::Pose target_pose;
+    target_pose.orientation = tf2::toMsg(orientation);
+    target_pose.position.x = object_pose_msg.position.x + x_distance_diff;
+    target_pose.position.y = object_pose_msg.position.y + y_distance_diff;
+
+    if(x_distance_diff > 0){
+        target_pose.position.x = object_pose_msg.position.x + (0.5 * dimensions.x) + FRANKA_GRIPPER_DISTANCE - reach;
+    }
+
+    if(x_distance_diff < 0){
+        target_pose.position.x = object_pose_msg.position.x - (0.5 * dimensions.x) - FRANKA_GRIPPER_DISTANCE + reach;
+    }
+
+    if(y_distance_diff > 0){
+        target_pose.position.y = object_pose_msg.position.y + (0.5 * dimensions.y) + FRANKA_GRIPPER_DISTANCE - reach;
+    }
+
+    if(y_distance_diff < 0){
+        target_pose.position.y = object_pose_msg.position.y - (0.5 * dimensions.y) - FRANKA_GRIPPER_DISTANCE + reach;
+    }
+
+    target_pose.position.z = object_pose_msg.position.z;
+
+    return target_pose;
+}
+
 geometry_msgs::Pose
 GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions,
                                 geometry_msgs::Quaternion gripper_orientation_msg, double reach) {
@@ -75,6 +170,69 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
     return target_pose;
 }
 
+bool
+GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
+                        shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction,
+                         double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
+
+    std::vector<moveit_msgs::Grasp> grasps;
+    grasps.resize(1);
+    grasps[0].grasp_pose.header.frame_id = "panda_link0";
+    grasps[0].grasp_pose.pose = pick_pose;
+
+    geometry_msgs::Vector3Stamped stamped_approach_dir;
+    stamped_approach_dir.vector = approach_direction;
+
+    // Setup pre-grasp approach
+    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+    grasps[0].pre_grasp_approach.direction = stamped_approach_dir;
+    grasps[0].pre_grasp_approach.min_distance = 0.095;
+    grasps[0].pre_grasp_approach.desired_distance = 0.115;
+
+    // Setup post-grasp retreat
+    grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
+    grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
+    grasps[0].post_grasp_retreat.min_distance = 0.1;
+    grasps[0].post_grasp_retreat.desired_distance = 0.25;
+
+    setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
+
+    if(object_to_pick.type == shape_msgs::SolidPrimitive::CYLINDER){
+
+        if(object_to_pick.dimensions[1] >= 0.08){
+            ROS_ERROR("Could not grasp: Cylinder object to big.");
+            return false;
+        }
+
+        setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[1]);
+    }else{
+        if (object_to_pick.dimensions[0] <= 0.08) {
+            setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[0]);
+        }
+
+        if (object_to_pick.dimensions[0] > 0.08 && object_to_pick.dimensions[1] <= 0.08) {
+            setupClosedGripper(grasps[0].grasp_posture, object_to_pick.dimensions[1]);
+        }
+
+        if (object_to_pick.dimensions[0] > 0.08 && object_to_pick.dimensions[1] > 0.08) {
+            ROS_ERROR("Could not grasp: Object to big (in any dimension).");
+            return false;
+        }
+    }
+
+    move_group.setSupportSurfaceName(supporting_surface_id);
+
+    moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
+    }
+
+    ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
+
+    return false;
+}
+
 bool
 GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
                          geometry_msgs::Vector3 dimensions,
@@ -125,6 +283,40 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
     ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val);
     return false;
 }
+bool
+GraspUtil::placeFromSide(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, bool plan_only){
+
+    std::vector<moveit_msgs::PlaceLocation> place_location;
+    place_location.resize(1);
+
+    place_location[0].place_pose.header.frame_id = "panda_link0";
+    place_location[0].place_pose.pose.orientation.w = 1;
+    place_location[0].place_pose.pose.position = place_pose.position;
+    place_location[0].place_pose.pose.position.z = place_location[0].place_pose.pose.position.z;
+
+    place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
+    place_location[0].pre_place_approach.direction.vector.z = -1.0;
+    place_location[0].pre_place_approach.min_distance = 0.095;
+    place_location[0].pre_place_approach.desired_distance = 0.115;
+
+    place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
+    place_location[0].post_place_retreat.direction.vector.z = 1.0;
+    place_location[0].post_place_retreat.min_distance = 0.1;
+    place_location[0].post_place_retreat.desired_distance = 0.25;
+
+    setupOpenGripper(place_location[0].post_place_posture, open_amount);
+    move_group.setSupportSurfaceName(supporting_surface_id);
+
+    moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
+
+    if (miec.val == miec.SUCCESS) {
+        return true;
+    }
+
+    ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
+    return false;
+}
 
 bool
 GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,

test/grasp_from_side_test.cpp

+#include "ros/ros.h"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <gtest/gtest.h>
+#include <grasp_util.h>
+
+/**
+ * Testing grasping from side.
+ */
+class Grasping : public ::testing::Test {
+
+protected:
+
+    GraspUtil grasp_util;
+
+    geometry_msgs::Pose pick_table_pose;
+    geometry_msgs::Pose place_table_pose;
+    geometry_msgs::Pose obj_pose;
+    geometry_msgs::Pose place_pose;
+
+    tf2::Quaternion gripper_orientation;
+    tf2::Quaternion place_orientation;
+
+    geometry_msgs::Vector3 dimensions;
+
+    void SetUp() override {
+
+        ros::NodeHandle n("panda_grasping");
+
+        gripper_orientation.setRPY(0, 0, -M_PI_4);
+        place_pose.position.z = 0.4 + 0.05; // table height + object height / 2 + buffer
+        place_orientation.setRPY(0, 0, M_PI / 2);
+        dimensions.x = 0.05;
+        dimensions.y = 0.05;
+        dimensions.z = 0.1;
+
+        CleanupScene();
+    }
+
+    static void CleanupScene() {
+        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+
+        for (auto p : planning_scene_interface.getObjects()) {
+            p.second.operation = moveit_msgs::CollisionObject::REMOVE;
+            planning_scene_interface.applyCollisionObject(p.second);
+        }
+        ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);
+        ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);
+    }
+
+    void doTest(geometry_msgs::Pose &pick_pose, geometry_msgs::Pose &pick_table_pose, geometry_msgs::Pose &place_table_pose,
+            geometry_msgs::Pose &obj_pose, geometry_msgs::Pose &place_pose, geometry_msgs::Vector3 &approach_direction){
+
+        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+        moveit::planning_interface::MoveGroupInterface group("panda_arm");
+
+        std::vector<moveit_msgs::CollisionObject> collision_objects;
+        collision_objects.resize(3);
+
+        collision_objects[0].id = "table1";
+        collision_objects[0].header.frame_id = "panda_link0";
+
+        collision_objects[0].primitives.resize(1);
+        collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
+        collision_objects[0].primitives[0].dimensions.resize(3);
+        collision_objects[0].primitives[0].dimensions[0] = 0.2;
+        collision_objects[0].primitives[0].dimensions[1] = 0.2;
+        collision_objects[0].primitives[0].dimensions[2] = 0.4;
+        collision_objects[0].primitive_poses.resize(1);
+        collision_objects[0].primitive_poses[0] = pick_table_pose;
+        collision_objects[0].operation = collision_objects[0].ADD;
+
+        collision_objects[1].id = "table2";
+        collision_objects[1].header.frame_id = "panda_link0";
+
+        collision_objects[1].primitives.resize(1);
+        collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
+        collision_objects[1].primitives[0].dimensions.resize(3);
+        collision_objects[1].primitives[0].dimensions[0] = 0.2;
+        collision_objects[1].primitives[0].dimensions[1] = 0.2;
+        collision_objects[1].primitives[0].dimensions[2] = 0.4;
+        collision_objects[1].primitive_poses.resize(1);
+        collision_objects[1].primitive_poses[0] = place_table_pose;
+        collision_objects[1].operation = collision_objects[1].ADD;
+
+        collision_objects[2].header.frame_id = "panda_link0";
+        collision_objects[2].id = "object";
+
+        collision_objects[2].primitives.resize(1);
+        collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
+        collision_objects[2].primitives[0].dimensions.resize(3);
+        collision_objects[2].primitives[0].dimensions[0] = dimensions.x;
+        collision_objects[2].primitives[0].dimensions[1] = dimensions.y;
+        collision_objects[2].primitives[0].dimensions[2] = dimensions.z;
+        collision_objects[2].primitive_poses.resize(1);
+        collision_objects[2].primitive_poses[0] = obj_pose;
+        collision_objects[2].operation = collision_objects[2].ADD;
+
+        planning_scene_interface.applyCollisionObjects(collision_objects);
+
+        pickAndPlace(pick_pose, planning_scene_interface, group, place_pose, approach_direction);
+    }
+
+    void pickAndPlace(geometry_msgs::Pose &pick_pose, moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
+            moveit::planning_interface::MoveGroupInterface &group, geometry_msgs::Pose &place_pose, geometry_msgs::Vector3 &approach_direction) {
+
+        shape_msgs::SolidPrimitive object;
+        object.type = shape_msgs::SolidPrimitive::CYLINDER;
+        object.dimensions.resize(2);
+        object.dimensions[0] = dimensions.x;
+        object.dimensions[1] = dimensions.y;
+
+        GraspUtil grasp_util;
+
+
+        ros::WallDuration(1.0).sleep();
+
+        ASSERT_TRUE(grasp_util.pickFromSide(group, pick_pose, object, approach_direction,0.06, "table1", "object"));
+
+        ros::WallDuration(1.0).sleep();
+
+        ASSERT_TRUE(grasp_util.placeFromSide(group, place_pose, 0.06, "table2", "object", false));
+    }
+};
+
+/**
+ * test from -x grasp
+*/
+TEST_F(Grasping, SIDE_GRASPING_1) {
+
+    pick_table_pose.position.x = 0.5;
+    pick_table_pose.position.y = -0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = 0;
+    place_table_pose.position.y = 0.5;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = 0.5;
+    obj_pose.position.y = -0.5;
+    obj_pose.position.z = 0.45;
+    obj_pose.orientation.w = 1.0;
+
+
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.x = 1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromBack(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+/**
+ * test from +x grasp
+*/
+TEST_F(Grasping, SIDE_GRASPING_2) {
+
+    pick_table_pose.position.x = -0.5;
+    pick_table_pose.position.y = -0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = 0;
+    place_table_pose.position.y = 0.5;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = -0.5;
+    obj_pose.position.y = -0.5;
+    obj_pose.position.z = 0.45;
+    obj_pose.orientation.w = 1.0;
+
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.x = -1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromFront(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+/**
+ * test from -y grasp
+*/
+TEST_F(Grasping, SIDE_GRASPING_3) {
+
+    pick_table_pose.position.x = 0.5;
+    pick_table_pose.position.y = 0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = -0.5;
+    place_table_pose.position.y = 0;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = 0.5;
+    obj_pose.position.y = 0.5;
+    obj_pose.position.z = 0.45;
+    obj_pose.orientation.w = 1.0;
+
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.y = +1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromLeft(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+/**
+ * test from +y grasp
+ */
+TEST_F(Grasping, SIDE_GRASPING_4) {
+
+    pick_table_pose.position.x = 0.5;
+    pick_table_pose.position.y = -0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = -0.5;
+    place_table_pose.position.y = 0;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = 0.5;
+    obj_pose.position.y = -0.5;
+    obj_pose.position.z = 0.45;
+    obj_pose.orientation.w = 1.0;
+
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.y = -1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromRight(obj_pose, dimensions, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "tests_side_grasping");
+    ros::NodeHandle n("panda_grasping");
+
+    ros::AsyncSpinner spinner(4);
+    spinner.start();
+
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+    ros::Duration(3.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<");
+
+
+    return RUN_ALL_TESTS();
+}

test/grasping.test

+<launch>
+    <include file="$(find panda_simulation)/launch/simulation.launch">
+        <arg name="slow_simulation" value="false" />
+    </include>
+
+    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+          args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen">
+        <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    </node>
+
+    <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" respawn="true"/>
+
+    <test test-name="tests_grasping" pkg="panda_grasping" type="tests_grasping" time-limit="3600.0"/>
+</launch>
\ No newline at end of file

test/plan_visualizer.rviz

+Panels:
+  []
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Goal_Tolerance: 0
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: true
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: true
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_arm
+        Query Goal State: false
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: false
+      Value: true
+      Velocity_Scaling_Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 2.9803097248077393
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.0864897966384888
+        Y: 0.3387787342071533
+        Z: 2.2351798634190345e-7
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.270203560590744
+      Target Frame: panda_link0
+      Value: XYOrbit (rviz)
+      Yaw: 0.42677485942840576
+    Saved: ~
+Window Geometry:
+  Height: 1404
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1278
+  X: 0
+  Y: 18

test/side_grasping.test

+<launch>
+    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+
+    <test test-name="tests_side_grasping" pkg="panda_grasping" type="tests_side_grasping" time-limit="3600.0"/>
+</launch>

test/test.cpp

+//
+// Created by Johannes Mey on 22.11.20.
+//
+
+#include "ros/ros.h"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include "panda_grasping/PickObject.h"
+#include "panda_grasping/PlaceObject.h"
+
+#include <gtest/gtest.h>
+
+class Grasping : public ::testing::Test {
+protected:
+
+    void SetUp() override {
+        ros::NodeHandle n("panda_grasping");
+        pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService");
+        placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService");
+
+        // clean up scene
+        CleanupScene();
+    }
+
+
+    static void CleanupScene() {
+        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+
+        for (auto p : planning_scene_interface.getObjects()) {
+            p.second.operation = moveit_msgs::CollisionObject::REMOVE;
+            planning_scene_interface.applyCollisionObject(p.second);
+        }
+        ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);
+        ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);
+    }
+
+    // void TearDown() override {}
+
+    ros::ServiceClient pickClient;
+    ros::ServiceClient placeClient;
+
+    std::vector<tf2::Vector3> objects{
+            tf2::Vector3(0.01, 0.1, 0.1),
+            tf2::Vector3(0.05, 0.1, 0.1),
+            tf2::Vector3(0.005, 0.1, 0.1)
+    };
+
+    std::vector<tf2::Vector3> points{
+            tf2::Vector3(0.5, 0.3, 0.2),
+            tf2::Vector3(-0.5, -0.3, 0.3),
+            tf2::Vector3(0.5, 0.3, 0.1),
+            tf2::Vector3(0.4, 0.5, 0.4),
+
+    };
+
+    tf2::Vector3 nullLocation{-0.4, 0.5, 0};
+
+    std::vector<double> angles{
+            M_PI_2,
+            1,
+            -M_PI_4
+    };
+
+    void pickAndPlace(tf2::Vector3 size, tf2::Vector3 from, double fromAngle, tf2::Vector3 to, double toAngle) {
+        // wait a little while
+        ros::Duration(0.5).sleep();
+        ROS_INFO_STREAM("starting test");
+        tf2::Quaternion orientation;
+        {
+            orientation.setRPY(0, 0, fromAngle);
+
+            panda_grasping::PickObject pickObjectSrv;
+            pickObjectSrv.request.dimensions = tf2::toMsg(size);
+            tf2::toMsg(from, pickObjectSrv.request.pose.position);
+            pickObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+
+            ROS_INFO_STREAM("starting to pick");
+
+            ASSERT_TRUE(pickClient.exists());
+            ASSERT_TRUE(pickClient.call(pickObjectSrv));
+        }
+        {
+            orientation.setRPY(0, 0, toAngle);
+
+            panda_grasping::PlaceObject placeObjectSrv;
+            placeObjectSrv.request.dimensions = tf2::toMsg(size);
+            tf2::toMsg(to, placeObjectSrv.request.pose.position);
+            placeObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+
+            ROS_INFO_STREAM("starting to place");
+
+            ASSERT_TRUE(placeClient.exists());
+            ASSERT_TRUE(placeClient.call(placeObjectSrv));
+
+
+            ROS_INFO_STREAM("pick and place finished");
+        }
+    }
+};
+
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01) {
+    auto object = objects[0];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_Null1) {
+    auto object = objects[0];
+    pickAndPlace(object, nullLocation, 0, points[1], 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_1_FromTo_01) {
+    auto object = objects[1];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_2_FromTo_01) {
+    auto object = objects[2];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_1Null) {
+    auto object = objects[0];
+    pickAndPlace(object, points[1], 0, nullLocation, 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01_23) {
+    auto object = objects[0];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+    CleanupScene();
+    pickAndPlace(object, points[2], 0, points[3], 0);
+}
+
+TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01_12) {
+    auto object = objects[0];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+    CleanupScene();
+    pickAndPlace(object, points[1], 0, points[2], 0);
+}
+
+TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_00) {
+    auto object = objects[0];
+    auto fromAngle = angles[0];
+    auto toAngle = angles[0];
+
+    pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
+}
+
+TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_01) {
+    auto object = objects[0];
+    auto fromAngle = angles[0];
+    auto toAngle = angles[1];
+
+    pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
+}
+
+TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_12) {
+    auto object = objects[0];
+    auto fromAngle = angles[1];
+    auto toAngle = angles[2];
+
+    pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "grasping_test");
+    ros::NodeHandle n("panda_grasping");
+
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+    ros::Duration(3.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<");
+
+
+    return RUN_ALL_TESTS();
+}