utils for picking and construction of support-surfaces

CMakeLists.txt

@@ -48,7 +48,7 @@ catkin_package(CATKIN_DEPENDS
 # Specify additional locations of header files Your package locations should be listed before other locations
 include_directories(${catkin_INCLUDE_DIRS})
 
-add_executable(grasp_service src/grasping/grasp_service.cpp)
+add_executable(grasp_service src/grasping/grasp_service.cpp src/grasping/grasp_util.cpp src/grasping/environment_util.cpp)
 
 add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
 

src/grasping/environment_util.cpp

+//
+// Created by sebastian on 19.05.20.
+//
+#include <ros/ros.h>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+void constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects){
+
+    moveit_msgs::CollisionObject plate;
+
+    plate.header.frame_id = "panda_link0";
+    plate.id = "plate";
+
+    plate.primitives.resize(1);
+    plate.primitives[0].type = collision_objects[1].primitives[0].BOX;
+    plate.primitives[0].dimensions.resize(3);
+    plate.primitives[0].dimensions[0] = 3.0;
+    plate.primitives[0].dimensions[1] = 3.0;
+    plate.primitives[0].dimensions[2] = 0.1;
+
+    plate.primitive_poses.resize(1);
+    plate.primitive_poses[0].position.x = 0;
+    plate.primitive_poses[0].position.y = 0;
+    plate.primitive_poses[0].position.z = -0.1;
+
+    plate.operation = plate.ADD;
+
+    collision_objects.push_back(plate);
+}
+
+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 pick_support;
+
+    pick_support.header.frame_id = "panda_link0";
+    pick_support.id = id;
+
+    pick_support.primitives.resize(1);
+    pick_support.primitives[0].type = collision_objects[1].primitives[0].BOX;
+    pick_support.primitives[0].dimensions.resize(3);
+    pick_support.primitives[0].dimensions[0] = object_x_dimension;
+    pick_support.primitives[0].dimensions[1] = object_y_dimension;
+    pick_support.primitives[0].dimensions[2] = 0.04;
+
+    pick_support.primitive_poses.resize(1);
+    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.operation = pick_support.ADD;
+
+    collision_objects.push_back(pick_support);
+}
\ No newline at end of file

src/grasping/grasp_service.cpp

 //
 // Created by sebastian on 13.05.20.
 //
-
 #include "ros/ros.h"
 #include <geometry_msgs/Pose.h>
 

src/grasping/grasp_util.cpp

+//
+// Created by sebastian on 19.05.20.
+//
+#include <ros/ros.h>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+void setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
+    posture.joint_names.resize(2);
+    posture.joint_names[0] = "panda_finger_joint1";
+    posture.joint_names[1] = "panda_finger_joint2";
+
+    posture.points.resize(1);
+    posture.points[0].positions.resize(2);
+    posture.points[0].positions[0] = amount / 2;
+    posture.points[0].positions[1] = amount / 2;
+    posture.points[0].time_from_start = ros::Duration(0.5);
+}
+
+void setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
+    posture.joint_names.resize(2);
+    posture.joint_names[0] = "panda_finger_joint1";
+    posture.joint_names[1] = "panda_finger_joint2";
+
+    posture.points.resize(1);
+    posture.points[0].positions.resize(2);
+    posture.points[0].positions[0] = amount / 2;
+    posture.points[0].positions[1] = amount / 2;
+    posture.points[0].time_from_start = ros::Duration(0.5);
+}
+
+geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose& object_to_pick_pose, double distance_to_eef){
+
+    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);
+
+    target_pose.orientation  = tf2::toMsg(orientation);
+    target_pose.position.x = object_to_pick_pose.position.x;
+    target_pose.position.y = object_to_pick_pose.position.y;
+    target_pose.position.z = object_to_pick_pose.position.z + distance_to_eef;
+
+    return target_pose;
+}
+
+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)
+{
+    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;
+
+    // Setup pre-grasp approach
+    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
+    grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
+    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;
+
+    // Setup the opening / closing - mechanism of the gripper
+    setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
+    setupClosedGripper(grasps[0].grasp_posture, close_amount);
+
+    move_group.setSupportSurfaceName(supporting_surface_id);
+    move_group.pick(object_to_pick_id, grasps);
+}
\ No newline at end of file

srv/PickObject.srv

 geometry_msgs/Pose pose
+float64 x_dimension
+float64 y_dimension
+float64 z_dimension
 ---
 bool success
\ No newline at end of file