diff --git a/CMakeLists.txt b/CMakeLists.txt
index 072f313281ea11cf204a65841314be0d68b27ad0..b3b288fba595ab0d1f95aef76a0cd4502a9f0b03 100644
--- a/CMakeLists.txt
+++ b/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()
diff --git a/include/grasp_util.h b/include/grasp_util.h
index 66bdb7f373f7a92aaa51e3f2490c0ea6aa3eda33..4e778c010060038aae0a2ac6b8825b80237e3ab2 100644
--- a/include/grasp_util.h
+++ b/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 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 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_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
diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index 54766834b5e4fb2a625ddabc44a907940babeea2..6d8af3d85f7d18691629029c1c537c3379662ceb 100644
--- a/src/grasping/environment_util.cpp
+++ b/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;
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..752d8eb1abcbaffe84196cb15220416aab7c2c1e 100644
--- a/src/grasping/grasp_service.cpp
+++ b/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;
}
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index b89d05d4c7a49a20168f8ec3e5da4becd9aab221..89c857a2c3e1f6660b2be39392e56ca63c9981b1 100644
--- a/src/grasping/grasp_util.cpp
+++ b/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,
diff --git a/test/grasp_from_side_test.cpp b/test/grasp_from_side_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aff90ba38fdc76900ae8de0bf645183daa8b3794
--- /dev/null
+++ b/test/grasp_from_side_test.cpp
@@ -0,0 +1,271 @@
+#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();
+}
diff --git a/test/grasping.test b/test/grasping.test
new file mode 100644
index 0000000000000000000000000000000000000000..5765d69f50606ef7e5bc2165852b9177ca2aced9
--- /dev/null
+++ b/test/grasping.test
@@ -0,0 +1,14 @@
+<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
diff --git a/test/plan_visualizer.rviz b/test/plan_visualizer.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..2f631356378c3b3dc72a04abff35b9b9d5f1893f
--- /dev/null
+++ b/test/plan_visualizer.rviz
@@ -0,0 +1,286 @@
+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
diff --git a/test/side_grasping.test b/test/side_grasping.test
new file mode 100644
index 0000000000000000000000000000000000000000..78444dedfb2f922ab9678e701639f327d69812d9
--- /dev/null
+++ b/test/side_grasping.test
@@ -0,0 +1,6 @@
+<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>
diff --git a/test/test.cpp b/test/test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..185ea255f8b1f22361657f0486eb9be61c84435f
--- /dev/null
+++ b/test/test.cpp
@@ -0,0 +1,182 @@
+//
+// 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();
+}