Horizontal Pick and Place

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

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)

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:

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()

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

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:

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

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);

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"