diff --git a/ws/src/cartesian_movement_services/CMakeLists.txt b/ws/src/cartesian_movement_services/CMakeLists.txt index bc9ad26..15dfa6f 100644 --- a/ws/src/cartesian_movement_services/CMakeLists.txt +++ b/ws/src/cartesian_movement_services/CMakeLists.txt @@ -65,6 +65,7 @@ add_service_files( PickAndPour.srv Pick.srv Place.srv + Pour.srv ) ## Generate actions in the 'action' folder diff --git a/ws/src/cartesian_movement_services/arm_scripts/pouring_srv.py b/ws/src/cartesian_movement_services/arm_scripts/pouring_srv.py index f9519e8..387cec9 100755 --- a/ws/src/cartesian_movement_services/arm_scripts/pouring_srv.py +++ b/ws/src/cartesian_movement_services/arm_scripts/pouring_srv.py @@ -342,6 +342,25 @@ def pick_and_pour(object_x,object_y,object_z,pouring_point_x,pouring_point_y,pou move_grab_and_place(object_x,object_y,object_z,initial_pose) print('After returning the object to its original position') +#The robot executes a pick with the actual end effector orientation and pours the container +def pour_left_to_right(object_x,object_y,object_z,pouring_point_x,pouring_point_y,pouring_point_z,object_height,bowl_height,bowl_radius): + get_position = rospy.ServiceProxy('/xarm/get_position_rpy', GetFloat32List) + actual_position = list(get_position().datas) + initial_pose = copy.deepcopy(actual_position) + + #Pouring point in Z axis is assumed to be the table's height, though it can be changed for other tasks/scenarios + print('Entering take and pour') + take_and_pour(pouring_point_x,pouring_point_y,pouring_point_z,object_height,bowl_height,bowl_radius,initial_pose) + + #Return to the initial position + print('Returning to initial position') + move_by_coordinates_reverse(initial_pose[0],initial_pose[1],initial_pose[2],initial_pose) + + #Return the object to its origintal position from the current point (must change to move the robot to its default cartesian pose before putting the object into its original pose) + print('Returning the object to its original position') + move_grab_and_place(object_x,object_y,object_z,initial_pose) + print('After returning the object to its original position') + #The robot executes a pick with the actual end effector orientation and pours the container def pick_and_pour_left_to_right(object_x,object_y,object_z,pouring_point_x,pouring_point_y,pouring_point_z,object_height,bowl_height,bowl_radius): get_position = rospy.ServiceProxy('/xarm/get_position_rpy', GetFloat32List) @@ -388,10 +407,31 @@ def pick_and_pour_right_to_left(object_x,object_y,object_z,pouring_point_x,pouri move_grab_and_place(object_x,object_y,object_z,initial_pose) print('After returning the object to its original position') +#The robot executes a pick with the actual end effector orientation and pours the container +def pour_right_to_left(object_x,object_y,object_z,pouring_point_x,pouring_point_y,pouring_point_z,object_height,bowl_height,bowl_radius): + get_position = rospy.ServiceProxy('/xarm/get_position_rpy', GetFloat32List) + actual_position = list(get_position().datas) + initial_pose = copy.deepcopy(actual_position) + + #Pouring point in Z axis is assumed to be the table's height, though it can be changed for other tasks/scenarios + print('Entering take and pour') + take_and_pour_right_to_left(pouring_point_x,pouring_point_y,pouring_point_z,object_height,bowl_height,bowl_radius,initial_pose) + + #Return to the initial position + print('Returning to initial position') + move_by_coordinates_reverse(initial_pose[0],initial_pose[1],initial_pose[2],initial_pose) + + #Return the object to its origintal position from the current point (must change to move the robot to its default cartesian pose before putting the object into its original pose) + print('Returning the object to its original position') + move_grab_and_place(object_x,object_y,object_z,initial_pose) + print('After returning the object to its original position') + #Goes to the vision pose for horizontal places -def stand_up_and_see_horizontal(actual_pose): +def stand_up_and_see_horizontal(): return_to_default_pose_horizontal() - xarm_move_to_point(76.2,-260,883,actual_pose) + get_position = rospy.ServiceProxy('/xarm/get_position_rpy', GetFloat32List) + actual_pose = list(get_position().datas) + xarm_move_to_point(0,-35.3,787.6,actual_pose) #Goes to default pose from vision pose def get_down_and_wait_horizontal(actual_pose): @@ -557,8 +597,9 @@ def cartesian_movement_callback(): # move_grab_and_take(220,-450,420+20,pose_with_changed_end_effector_) # move_grab_and_place(-220,-450,420+20,initial_pose) #adjust_end_effector_yaw(yaw_angle) + # return_to_default_pose_vertical() + # xarm_move_end_effector(1.4741963622250402, 1.5230381068417789, 3.111714519382122,actual_pose) return_to_default_pose_vertical() - xarm_move_end_effector(1.4741963622250402, 1.5230381068417789, 3.111714519382122,actual_pose) #pick_and_pour(0,-320,350,-220,-320,260,container_height,bowl_height,bowl_radius,initial_pose) #pick_and_pour(220,-320,350,-220,-320,260,cereal_height,bowl_height,bowl_radius,initial_pose) # move_grab_and_take(220,-320,350,actual_pose) diff --git a/ws/src/cartesian_movement_services/scripts/cartesian_client.py b/ws/src/cartesian_movement_services/scripts/cartesian_client.py index 20f6d8c..1045a45 100755 --- a/ws/src/cartesian_movement_services/scripts/cartesian_client.py +++ b/ws/src/cartesian_movement_services/scripts/cartesian_client.py @@ -40,6 +40,13 @@ def place_client(destination_pose,is_vertical,tip_pick): print(resp.success) return resp.success +def pour_client(destination_pose,container_height,bowl_radius,bowl_height,left_to_right,tip_pick): + rospy.wait_for_service('/cartesian_movement_services/Pour') + pour_ = rospy.ServiceProxy('/cartesian_movement_services/Pour',Pour) + resp = pour_(object_pose,destination_pose,bowl_height,bowl_radius,container_height,left_to_right,tip_pick) + print(resp.success) + return resp.success + if __name__ == "__main__": client = 3 if client == 0: diff --git a/ws/src/cartesian_movement_services/scripts/cartesian_server.py b/ws/src/cartesian_movement_services/scripts/cartesian_server.py index 5e1c708..ac874c7 100755 --- a/ws/src/cartesian_movement_services/scripts/cartesian_server.py +++ b/ws/src/cartesian_movement_services/scripts/cartesian_server.py @@ -78,7 +78,7 @@ def pick_and_place_server(): s = rospy.Service('/cartesian_movement_services/PickAndPlace',PickAndPlace,handle_pick_and_place) print('Ready to execute Pick and Place') -#Pouring server +#Pick and pour server def handle_pick_and_pour(req): module.return_to_default_pose_horizontal() print('Executing pick and pour') @@ -144,6 +144,7 @@ def handle_pick(req): #The Y axis must be increased by 135mm to grasp the object in the middle of the gripper intsead of the end of it grasping_Y_axis = req.object_pose[1] + 135 module.horizontal_pick(req.object_pose[0],grasping_Y_axis,req.object_pose[2]) + module.stand_up_and_see_horizontal() # set_state(0) # set_mode(4) # time.sleep(2.0) @@ -195,7 +196,7 @@ def handle_place(req): grasping_Y_axis = req.destination_pose[1] + 135 module.horizontal_place(req.destination_pose[0],grasping_Y_axis,req.destination_pose[2]) set_mode_moveit() - return PickResponse(True) + return PlaceResponse(True) except: print('Pick and place failed') set_mode_moveit() @@ -205,6 +206,41 @@ def place_server(): s = rospy.Service('/cartesian_movement_services/Place',Place,handle_place) print('Ready to execute Place') +#POur server +#Pick and pour server +def handle_pour(req): + module.return_to_default_pose_horizontal() + print('Executing pick and pour') + print(req) + # pick_and_pour(0,-330,380,10,-330,380,21,85,70) + #pick_and_pour(req.object_pose[0],req.object_pose[1]+175,req.object_pose[2],req.pouring_point[0],req.pouring_point[1],req.pouring_point[2],req.object_height,req.bowl_height,req.bowl_radius) + try: + if(req.left_to_right == True): + if(req.tip_pick == True): + print('Entered tip pick') + module.pour_left_to_right(req.pouring_point[0],req.pouring_point[1]+175,req.pouring_point[2],req.object_height,req.bowl_height,req.bowl_radius) + return PourResponse(True) + else: + print('Entered no tip pick') + module.pour_left_to_right(req.pouring_point[0],req.pouring_point[1]+135,req.pouring_point[2],req.object_height,req.bowl_height,req.bowl_radius) + return PourResponse(True) + else: + if(req.tip_pick == True): + print('Entered tip pick') + module.pour_right_to_left(req.pouring_point[0],req.pouring_point[1]+175,req.pouring_point[2],req.object_height,req.bowl_height,req.bowl_radius) + return PourResponse(True) + else: + print('Entered no tip pick') + module.pour_right_to_left(req.pouring_point[0],req.pouring_point[1]+135,req.pouring_point[2],req.object_height,req.bowl_height,req.bowl_radius) + return PourResponse(True) + except: + print('Pick and pour failed') + return PourResponse(False) + +def pour_server(): + s = rospy.Service('/cartesian_movement_services/PickAndPour',PickAndPour,handle_pick_and_pour) + print('Ready to execute Pick and Pour') + def init_servers(): move_end_effector_server() pick_and_place_server() diff --git a/ws/src/cartesian_movement_services/srv/Pour.srv b/ws/src/cartesian_movement_services/srv/Pour.srv new file mode 100644 index 0000000..673504d --- /dev/null +++ b/ws/src/cartesian_movement_services/srv/Pour.srv @@ -0,0 +1,8 @@ +float32[] pouring_point +float32 bowl_height +float32 bowl_radius +float32 object_height +bool left_to_right +bool tip_pick +--- +bool success \ No newline at end of file diff --git a/ws/src/object_detector_2d/scripts/Detection2D.py b/ws/src/object_detector_2d/scripts/Detection2D.py index 6f23a63..636c6d6 100755 --- a/ws/src/object_detector_2d/scripts/Detection2D.py +++ b/ws/src/object_detector_2d/scripts/Detection2D.py @@ -48,7 +48,7 @@ "CAMERA_FRAME": "xtion_rgb_optical_frame", "USE_YOLO8": False, "YOLO_MODEL_PATH": str(pathlib.Path(__file__).parent) + "/../models/yolov5s.pt", - "FLIP_IMAGE": True, + "FLIP_IMAGE": False, } class CamaraProcessing: diff --git a/ws/src/object_detector_3d/action/DetectObjects3D.action b/ws/src/object_detector_3d/action/DetectObjects3D.action index ee701f5..431f389 100644 --- a/ws/src/object_detector_3d/action/DetectObjects3D.action +++ b/ws/src/object_detector_3d/action/DetectObjects3D.action @@ -15,6 +15,7 @@ float64 y_plane float64 z_plane float64 width_plane float64 height_plane +sensor_msgs/PointCloud2 object_point_cloud --- #feedback # -1 => Error, 0..1...2 => ObjectsFoundSoFar diff --git a/ws/src/object_detector_3d/src/Detection3D.cpp b/ws/src/object_detector_3d/src/Detection3D.cpp index bd3e3b6..4b3ae40 100644 --- a/ws/src/object_detector_3d/src/Detection3D.cpp +++ b/ws/src/object_detector_3d/src/Detection3D.cpp @@ -182,6 +182,7 @@ class Detect3D result_.z_plane = 0; result_.width_plane = 0; result_.height_plane = 0; + result_.object_point_cloud = sensor_msgs::PointCloud2(); pose_pub_msg_.poses.clear(); gpd_msg_.cloud_sources = gpd_ros::CloudSources(); @@ -900,6 +901,13 @@ class Detect3D ObjectParams selectedObject = objects[selectedId]; ROS_INFO_STREAM("Selected Object " << selectedObject.file_id); + // set point_cloud to return + sensor_msgs::PointCloud2 tmp; + pcl::toROSMsg(selectedObject.cluster_original, tmp); + tmp.header.frame_id = input.header.frame_id; + tmp.header.stamp = input.header.stamp; + result_.object_point_cloud = tmp; + if (!ignore_moveit_) { ROS_INFO_STREAM("STARTED - Building Grasping Info"); addGraspInfo(input, selectedObject); diff --git a/ws/src/pick_and_place/config/params_DASHGO.yaml b/ws/src/pick_and_place/config/params_DASHGO.yaml index e47dd88..4a2dd62 100644 --- a/ws/src/pick_and_place/config/params_DASHGO.yaml +++ b/ws/src/pick_and_place/config/params_DASHGO.yaml @@ -2,15 +2,19 @@ MANIPULATION_ENABLE: true PICK_GROUPS: [arm] #, whole_body, whole_body_rotational ARM_GROUP: "arm" ARM_JOINTS: [joint1, joint2, joint3, joint4, joint5, joint6] -ROBOT_MAX_RANGE_XY: 0.510 +ROBOT_MAX_RANGE_XY: 0.515 +ROBOT_MAX_HORIZONTAL_DIMENSION: 0.1 # Maximum x,y dimension of an object to allow horizontal grasping +ROBOT_HORIZONTAL_PICK_MIN_Z: 0.1 # ARM_GRASP: [-1.57, 0.0, 0.0, -1.57, -1.57] ARM_GRASP: [-1.57, -0.785, -0.785, 0, 1.57, -2.3562] ARM_INIT: [-1.57, -1.39626, -0.174533, 0.0, 1.57, -0.7854] ARM_HOME: [0.0, 0.0, 0.0, -1.5708, 1.5708, 0.7854] ARM_PREGRASP: [-1.57, 0, -3.14, 0, 1.8326, 0.7854] ARM_CARTESIAN_PREGRASP_VERTICAL: [-1.5707963705062866, -0.6108652353286743, -2.181661605834961, 0.0, 2.7925267219543457, -2.356194496154785] -ARM_CARTESIAN_PREGRASP_HORIZONTAL: [-1.570796251296997, -0.4812801480293274, -2.0814273357391357, -8.003445373105933e-08, 0.9919110536575317, 0.7853982448577881] +ARM_CARTESIAN_PREGRASP_HORIZONTAL: [-1.5707963705062866, -1.2217304706573486, -1.1344640254974365, 0.0, 0.7853981852531433, 0.7853981852531433] ARM_CARTESIAN_POSTGRASP_VERTICAL: [-1.5708004236221313, -0.31413206458091736, -2.22076416015625, 5.999552740831859e-06, 2.5305960178375244, -2.3507955074310303] +ARM_CARTESIAN_POSTGRASP_HORIZONTAL: [-1.5707963705062866, -1.2217304706573486, -1.1344640254974365, 0.0, 0.7853981852531433, 0.7853981852531433] + #ARM_PREGRASP: [-1.57, -0.6108, -0.4363, 0, -0.1745, 0.7854] HEAD_ENABLE: false HEAD_GROUP: "head" diff --git a/ws/src/pick_and_place/scripts/cartesianManipulationServer.py b/ws/src/pick_and_place/scripts/cartesianManipulationServer.py index 1101e4f..7eddf07 100755 --- a/ws/src/pick_and_place/scripts/cartesianManipulationServer.py +++ b/ws/src/pick_and_place/scripts/cartesianManipulationServer.py @@ -131,8 +131,11 @@ def moveARM(joints): # a default self.pick_height = 0.3 self.object_picked = False + self.picked_vertical = False self.ROBOT_MAX_RANGE_XY = rospy.get_param("ROBOT_MAX_RANGE_XY", 0.5) + self.ROBOT_MAX_HORIZONTAL_DIMENSION = rospy.get_param("ROBOT_MAX_HORIZONTAL_DIMENSION", 0.1) + self.ROBOT_HORIZONTAL_PICK_MIN_Z = rospy.get_param("ROBOT_HORIZONTAL_PICK_MIN_Z", 0.1) self.ARM_TRANSFORM = "BaseBrazo" self.BASE_TRANSFORM = "Base" @@ -142,6 +145,8 @@ def moveARM(joints): self.ARM_HOME = rospy.get_param("ARM_HOME", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.ARM_CARTESIAN_PREGRASP_VERTICAL = rospy.get_param("ARM_CARTESIAN_PREGRASP_VERTICAL", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.ARM_CARTESIAN_POSTGRASP_VERTICAL = rospy.get_param("ARM_CARTESIAN_POSTGRASP_VERTICAL", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.ARM_CARTESIAN_PREGRASP_HORIZONTAL = rospy.get_param("ARM_CARTESIAN_PREGRASP_HORIZONTAL", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.ARM_CARTESIAN_POSTGRASP_HORIZONTAL = rospy.get_param("ARM_CARTESIAN_POSTGRASP_HORIZONTAL", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) rospy.loginfo("---------------------------------\nLOADED ALL ON MANIPULATION SERVER\n---------------------------------") @@ -177,7 +182,10 @@ def moveARM(self, joints, speed, enable_octomap = True): def initARM(self): # Move to a position to look at the objects - self.moveARM(self.ARM_CARTESIAN_PREGRASP_VERTICAL, 0.15, True) + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15, True) + + def moveArmForVision(self): + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15, True) def graspARM(self): ARM_GRASP = rospy.get_param("ARM_GRASP", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) @@ -194,15 +202,18 @@ def execute_cb(self, goal): # Check if arm is in PREGRASP position, if not, move to it if ARM_ENABLE: current_joints = self.arm_group.get_current_joint_values() - if current_joints != self.ARM_CARTESIAN_PREGRASP_VERTICAL and not self.object_picked: - self.moveARM(self.ARM_CARTESIAN_PREGRASP_VERTICAL, 0.15) + if current_joints != self.ARM_CARTESIAN_PREGRASP_HORIZONTAL and not self.object_picked: + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15) # Get Objects: rospy.loginfo("Getting objects/position") self.target_label = "" found = False if target == -5: #Place action - self.moveARM(self.ARM_CARTESIAN_POSTGRASP_VERTICAL, 0.15) + if self.picked_vertical: + self.moveARM(self.ARM_CARTESIAN_POSTGRASP_VERTICAL, 0.15) + else: + self.moveARM(self.ARM_CARTESIAN_POSTGRASP_HORIZONTAL, 0.15) found = self.get_place_position() if found: @@ -217,10 +228,7 @@ def execute_cb(self, goal): return rospy.loginfo("Robot Placed " + self.target_label + " down") self.toggle_octomap(True) - ## Move Up - if MANIPULATION_ENABLE: - self.graspARM() - self._as.set_succeeded(manipulationServResult(result = True)) + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15) return else: rospy.loginfo("Place Failed") @@ -243,39 +251,49 @@ def execute_cb(self, goal): rospy.loginfo("Waiting for user input") in_ = handleIntInput("(0) Quit, (1) Retry Same PC", range=(0, 1)) - grasping_points = self.get_grasping_points() - if grasping_points is None: - rospy.loginfo("Grasping Points Not Found") - self._as.set_succeeded(manipulationServResult(result = False)) - return + result = 0 - if not MANIPULATION_ENABLE or not ARM_ENABLE: - self._as.set_succeeded(manipulationServResult(result = False)) - return + if self.is_horizontal_possible(self.object_pose): + rospy.loginfo("[INFO] Horizontal Pick Possible") + result = self.pick_horizontal(self.object_pose, "current", allow_contact_with_ = []) + + else: + rospy.loginfo("[INFO] Horizontal Pick Not Possible") + grasping_points = self.get_grasping_points() + if grasping_points is None: + rospy.loginfo("Grasping Points Not Found") + self._as.set_succeeded(manipulationServResult(result = False)) + return + + if not MANIPULATION_ENABLE or not ARM_ENABLE: + self._as.set_succeeded(manipulationServResult(result = False)) + return - # Move to Object - self.toggle_octomap(True) - #self.scan(0.2) - rospy.sleep(1) - self.toggle_octomap(False) - self.grasp_config_list.publish(grasping_points) - rospy.loginfo("Robot Picking " + self.target_label + " up") - result = self.pick(self.object_pose, "current", allow_contact_with_ = [], grasping_points = grasping_points) + # Move to Object + self.toggle_octomap(True) + #self.scan(0.2) + rospy.sleep(1) + self.toggle_octomap(False) + self.grasp_config_list.publish(grasping_points) + rospy.loginfo("Robot Picking " + self.target_label + " up") + result = self.pick_vertical(self.object_pose, "current", allow_contact_with_ = [], grasping_points = grasping_points) + if result != 1: self.toggle_octomap(True) rospy.loginfo("Pick Failed") self._as.set_succeeded(manipulationServResult(result = False)) return + rospy.loginfo("Robot Picked " + self.target_label + " up") self.toggle_octomap(True) def scan(self, speed=0.2): - octo_joints = self.ARM_CARTESIAN_PREGRASP_VERTICAL.copy() + octo_joints = self.ARM_CARTESIAN_PREGRASP_HORIZONTAL.copy() octo_joints[5] -= 1 self.moveARM(octo_joints, speed, False) octo_joints[5] += 1*2 self.moveARM(octo_joints, speed, False) - self.moveARM(self.ARM_CARTESIAN_PREGRASP_VERTICAL, speed, False) + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, speed, False) def get_grasping_points(self): def add_default_grasp(grasp_configs): @@ -310,24 +328,40 @@ def add_default_grasp(grasp_configs): time.sleep(0.5) return None + + def is_horizontal_possible(self, obj_pose): + if self.get_object_max_dimension() > self.ROBOT_MAX_HORIZONTAL_DIMENSION: + return False + return True + + def get_object_max_dimension(self): + # Object should not exceed a maximum dimension in x and y axis to allow horizontal pick + object_cloud = self.object_point_cloud + point_cloud_array = [] + for p in pc2.read_points(object_cloud, field_names = ("x", "y", "z"), skip_nans=True): + # append point xyz to array + if not np.isnan(p[0]) and not np.isnan(p[1]): + point_cloud_array.append([p[0], p[1], p[2]]) + point_cloud_array = np.array(point_cloud_array) - - def pick(self, obj_pose, obj_name, allow_contact_with_ = [], grasping_points = []): - class PickScope: - error_code = 0 - allow_contact_with = allow_contact_with_ - object_pose = obj_pose - object_name = obj_name - result_received = False + # get max and min in x and y, use magnitude of the difference + max_x = np.max(point_cloud_array[:,0]) + min_x = np.min(point_cloud_array[:,0]) + max_y = np.max(point_cloud_array[:,1]) + min_y = np.min(point_cloud_array[:,1]) + max_z = np.max(point_cloud_array[:,2]) + min_z = np.min(point_cloud_array[:,2]) + + print(f"Max_x: {max_x}, Min_x: {min_x}, Max_y: {max_y}, Min_y: {min_y}, Max_z: {max_z}, Min_z: {min_z}") + + length_x = max_x - min_x + length_y = max_y - min_y - def pick_feedback(feedback_msg): - pass + rospy.loginfo(f"[INFO] Object has max dimension in x: {max_x - min_x} and y: {max_y - min_y}") - def pick_callback(state, result): - PickScope.error_code = result.error_code - PickScope.result_received = True - rospy.loginfo("Pick Received") - rospy.loginfo(PickScope.error_code) + return max(max_x - min_x, max_y - min_y) + + def pick_vertical(self, obj_pose, obj_name, allow_contact_with_ = [], grasping_points = []): rospy.loginfo("Pick Action") @@ -335,6 +369,8 @@ def pick_callback(state, result): rospy.loginfo("No Grasping Points Found") return 0 + on_range = False + for grasp in grasping_points.grasps: # move arm directly self.arm_group.set_max_velocity_scaling_factor(0.15) @@ -346,7 +382,7 @@ def pick_callback(state, result): print("GOT POSE: ", pose) # if pose in z obtained from GPD is higher than the highest point in the object mesh, take the highest point in the object mesh - object_cloud = self.object_cloud.cloud_sources.cloud + object_cloud = self.object_point_cloud #print(point_cloud) point_cloud_array = [] for p in pc2.read_points(object_cloud, field_names = ("x", "y", "z"), skip_nans=True): @@ -453,7 +489,7 @@ def pick_callback(state, result): if (math.sqrt(object_pose[0]**2 + object_pose[1]**2) > self.ROBOT_MAX_RANGE_XY *1000): rospy.loginfo("[WARN] Object out of reach, skipping grasp") continue - + on_range = True resp = self.cartesian_pick_server(object_pose, is_vertical, tip_pick) print(resp.success) @@ -461,18 +497,72 @@ def pick_callback(state, result): if resp.success: rospy.loginfo("Pick Success") self.object_picked = True + self.picked_vertical = True + self.moveARM(self.ARM_CARTESIAN_POSTGRASP_VERTICAL, 0.15) break else: rospy.loginfo("Pick Failed") - self.moveARM(self.ARM_CARTESIAN_PREGRASP_VERTICAL, 0.15) + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15) + + if not self.object_picked: + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15) + return 1 + + def pick_horizontal(self, obj_pose, obj_name, allow_contact_with_ = []): + + object_cloud = self.object_point_cloud + + point_cloud_array = [] + for p in pc2.read_points(object_cloud, field_names = ("x", "y", "z"), skip_nans=True): + # append point xyz to array + if not np.isnan(p[0]) and not np.isnan(p[1]): + point_cloud_array.append([p[0], p[1], p[2]]) + point_cloud_array = np.array(point_cloud_array) + + # Get the center in x + x_center = np.mean(point_cloud_array[:,0]) + + # get closest in y, y are negative in front of the robot + y_min = np.max(point_cloud_array[:,1]) + + # pick at the middle or at minimum ROBOT_HORIZONTAL_PICK_MIN_Z + z = np.mean(point_cloud_array[:,2]) + plane_height = self.plane_height + + rospy.loginfo(f"[INFO] Object mean height is {z}, minimum placing height is {plane_height+self.ROBOT_HORIZONTAL_PICK_MIN_Z}") + z_pick = max(z, plane_height+self.ROBOT_HORIZONTAL_PICK_MIN_Z) + self.pick_height = self.plane_height - z_pick + + rospy.loginfo(f"[INFO] Found object picking position at x: {x_center}, y: {y_min}, z: {z}") + + # Transform to arm frame + tf_base_to_arm = self.tf_listener.lookupTransform(self.BASE_TRANSFORM, self.ARM_TRANSFORM, rospy.Time(0)) + x_center -= tf_base_to_arm[0][0] + y_min -= tf_base_to_arm[0][1] + z_pick -= tf_base_to_arm[0][2] + + rospy.loginfo(f"[INFO] Sending to cartesian server x: {x_center}, y: {y_min}, z: {z_pick}") + + + is_vertical = False + tip_pick = False + resp = self.cartesian_pick_server([x_center*1000, y_min*1000, z_pick*1000, 0, 0, 0], is_vertical, tip_pick) + + if resp.success: + rospy.loginfo("Pick Success") + self.object_picked = True + self.picked_vertical = False + self.moveARM(self.ARM_CARTESIAN_POSTGRASP_HORIZONTAL, 0.15) - # RETURN TO CARTESIAN PREGRASP (already done inside cartesian_pick_server) - # Ensure that the arm is in the postrasp position - self.moveARM(self.ARM_CARTESIAN_POSTGRASP_VERTICAL, 0.15) + else: + rospy.loginfo("Pick Failed") + self.moveARM(self.ARM_CARTESIAN_PREGRASP_HORIZONTAL, 0.15) return 1 + + def place(self, obj_pose, obj_name, allow_contact_with_ = []): rospy.loginfo("Place Action") @@ -482,16 +572,20 @@ def place(self, obj_pose, obj_name, allow_contact_with_ = []): print(f"Will place at pick height: {self.pick_height}") obj_pose.pose.position.z = self.plane_height + self.pick_height + if not self.picked_vertical: + rospy.loginfo(f"[INFO] Going for Horizontal Pick, pick height is {self.pick_height}, minimum is {self.plane_height+self.ROBOT_HORIZONTAL_PICK_MIN_Z}") + obj_pose.pose.position.z = max(obj_pose.pose.position.z, self.plane_height + self.ROBOT_HORIZONTAL_PICK_MIN_Z) + # transform to arm frame tf_base_to_arm = self.tf_listener.lookupTransform(self.BASE_TRANSFORM, self.ARM_TRANSFORM, rospy.Time(0)) obj_pose.pose.position.x -= tf_base_to_arm[0][0] obj_pose.pose.position.y -= tf_base_to_arm[0][1] obj_pose.pose.position.z -= tf_base_to_arm[0][2] + is_vertical = self.picked_vertical + tip_pick = False object_pose = [obj_pose.pose.position.x * 1000, obj_pose.pose.position.y * 1000, obj_pose.pose.position.z * 1000, 0, 0, 0] - is_vertical = True - tip_pick = False print("Executing cartesian place") print(f"Sending object position x: {object_pose[0]}, y: {object_pose[1]}, z: {object_pose[2]}") @@ -520,6 +614,7 @@ class GetObjectsScope: z_plane = 0.0 width_plane = 0.0 height_plane = 0.0 + object_point_cloud = [] result_received = False def get_objects_feedback(feedback_msg): @@ -539,6 +634,7 @@ def get_result_callback(state, result): GetObjectsScope.z_plane = result.z_plane GetObjectsScope.width_plane = result.width_plane GetObjectsScope.height_plane = result.height_plane + GetObjectsScope.object_point_cloud = result.object_point_cloud GetObjectsScope.result_received = True if target == -1: # Biggest Object @@ -592,6 +688,7 @@ def get_result_callback(state, result): self.object_pose = GetObjectsScope.object_pose self.object_cloud = GetObjectsScope.object_cloud + self.object_point_cloud = GetObjectsScope.object_point_cloud self.object_cloud_indexed = GetObjectsScope.object_cloud_indexed self.plane_height = GetObjectsScope.height_plane