From 89f6a6d1ff2d05236cc506f27ccd9cef461c1ef4 Mon Sep 17 00:00:00 2001 From: Nick Greene Date: Sun, 9 Feb 2020 12:51:44 -0500 Subject: [PATCH 1/6] robot.py Added Unstacking complete message --- robot.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/robot.py b/robot.py index 5fa265b6..2281f9a5 100755 --- a/robot.py +++ b/robot.py @@ -618,7 +618,11 @@ def reposition_objects(self, workspace_limits=None): self.place(rand_position, rand_angle) + self.place(rand_position, rand_angle) + self.place_pose_history = [] # clear place position history + print("------- UNSTACKING COMPLETE --------") + else: if self.is_sim: # Move gripper out of the way to the home position From 29e33acd286784253cbd76702474eaa0222baa4a Mon Sep 17 00:00:00 2001 From: Nick Greene Date: Sun, 9 Feb 2020 14:42:06 -0500 Subject: [PATCH 2/6] robot.py organized random magic numbers --- robot.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/robot.py b/robot.py index 2281f9a5..3e52cec3 100755 --- a/robot.py +++ b/robot.py @@ -107,7 +107,7 @@ class Robot(object): def __init__(self, is_sim=True, obj_mesh_dir=None, num_obj=None, workspace_limits=None, tcp_host_ip='192.168.1.155', tcp_port=502, rtc_host_ip=None, rtc_port=None, is_testing=False, test_preset_cases=None, test_preset_file=None, place=False, grasp_color_task=False, - real_gripper_ip='192.168.1.11', calibrate=False, unstack=False): + real_gripper_ip='192.168.1.11', calibrate=False, unstack=False, heightmap_resolution=0.002): ''' real_gripper_ip: None to assume the gripper is connected via the UR5, @@ -127,9 +127,10 @@ def __init__(self, is_sim=True, obj_mesh_dir=None, num_obj=None, workspace_limit # Dimensions of workspace should be 448 mm x 448 mm. That's 224x224 pixels with each pixel being 2mm x2mm. workspace_limits = np.asarray([[0.376, 0.824], [-0.264, 0.184], [-0.07, 0.4]]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates) self.workspace_limits = workspace_limits - self.heightmap_resolution = 0.002 + self.heightmap_resolution = heightmap_resolution self.place_task = place self.unstack = unstack + self.place_pose_history_limit = 6 self.grasp_color_task = grasp_color_task self.sim_home_position = [-0.3, 0.0, 0.45] # old value [-0.3, 0, 0.3] # self.gripper_ee_offset = 0.17 @@ -1551,7 +1552,7 @@ def place(self, position, heightmap_rotation_angle, workspace_limits=None, dista print('Executing: Place at (%f, %f, %f) angle: %f' % place_pose) self.place_pose_history.append(place_pose) - while len(self.place_pose_history) > 4: # only store x most recent place attempts + while len(self.place_pose_history) > self.place_pose_history_limit: # only store x most recent place attempts self.place_pose_history.pop(0) if self.is_sim: From b242b83cdb8910723b427874f27032c9523bcf00 Mon Sep 17 00:00:00 2001 From: Nick Greene Date: Sun, 9 Feb 2020 14:54:15 -0500 Subject: [PATCH 3/6] robot.py main.py put preprocessing in get_camera_data and check z errors --- main.py | 11 ++-------- robot.py | 63 ++++++++++++++++++++++++++++++++++++++++++-------------- 2 files changed, 50 insertions(+), 24 deletions(-) diff --git a/main.py b/main.py index 37a387b4..84040946 100755 --- a/main.py +++ b/main.py @@ -188,7 +188,7 @@ def main(args): # Initialize pick-and-place system (camera and robot) robot = Robot(is_sim, obj_mesh_dir, num_obj, workspace_limits, tcp_host_ip, tcp_port, rtc_host_ip, rtc_port, - is_testing, test_preset_cases, test_preset_file, place, grasp_color_task, unstack=unstack) + is_testing, test_preset_cases, test_preset_file, place, grasp_color_task, unstack=unstack, heightmap_resolution) # Initialize trainer trainer = Trainer(method, push_rewards, future_reward_discount, @@ -1041,14 +1041,7 @@ def detect_changes(prev_primitive_action, depth_heightmap, prev_depth_heightmap, def get_and_save_images(robot, workspace_limits, heightmap_resolution, logger, trainer, filename_poststring='0', save_image=True): # Get latest RGB-D image - color_img, depth_img = robot.get_camera_data() - depth_img = depth_img * robot.cam_depth_scale # Apply depth scale from calibration - # Get heightmap from RGB-D image (by re-projecting 3D point cloud) - color_heightmap, depth_heightmap = utils.get_heightmap(color_img, depth_img, robot.cam_intrinsics, robot.cam_pose, - workspace_limits, heightmap_resolution, background_heightmap=robot.background_heightmap) - # TODO(ahundt) switch to masked array, then only have a regular heightmap - valid_depth_heightmap = depth_heightmap.copy() - valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 + valid_depth_heightmap, color_heightmap, depth_heightmap, _, color_img, depth_img = robot.get_camera_data(return_heightmaps=True) # Save RGB-D images and RGB-D heightmaps if save_image: diff --git a/robot.py b/robot.py index 3e52cec3..15ad5e85 100755 --- a/robot.py +++ b/robot.py @@ -591,25 +591,21 @@ def reposition_objects(self, workspace_limits=None): for pose in place_pose_history: x, y, z, angle = pose - color_img, depth_img = self.get_camera_data() - depth_img = depth_img * self.cam_depth_scale - - color_heightmap, depth_heightmap = utils.get_heightmap(color_img, depth_img, self.cam_intrinsics, self.cam_pose, self.workspace_limits, self.heightmap_resolution) - valid_depth_heightmap = depth_heightmap.copy() - valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 # required otherwise max_z is always NaN - _, max_z, _ = self.check_z_height(valid_depth_heightmap, reward_multiplier=1.0) - - stack_z_height = max_z + valid_depth_heightmap, color_heightmap, depth_heightmap, max_z_height, color_img, depth_img = self.get_camera_data(return_heightmaps=True) if self.is_sim: # otherwise simulated gripper grasps too low offset = 0.01 - if stack_z_height + offset < self.workspace_limits[2][1]: - z = stack_z_height + offset + if max_z_height + offset < self.workspace_limits[2][1]: + z = max_z_height + offset else: + offset = 0.05 # otherwise real gripper grasps too high - z -= 0.05 + + if max_z_height - offset > self.workspace_limits[2][0]: + z = max_z_height - offset + grasp_success, color_success = self.grasp([x, y, z], angle) if grasp_success: @@ -619,8 +615,6 @@ def reposition_objects(self, workspace_limits=None): self.place(rand_position, rand_angle) - self.place(rand_position, rand_angle) - self.place_pose_history = [] # clear place position history print("------- UNSTACKING COMPLETE --------") @@ -645,7 +639,12 @@ def reposition_objects(self, workspace_limits=None): # TODO(ahundt) add real robot support for reposition_objects - def get_camera_data(self): + def get_camera_data(self, workspace_limits=None, heightmap_resolution=None, return_heightmaps=False, go_home=True, z_height_retake_threshold=0.3): + if workspace_limits is None: + workspace_limits = self.workspace_limits + + if heightmap_resolution is None: + heightmap_resolution = self.heightmap_resolution if self.is_sim: sim_ret = None @@ -672,12 +671,46 @@ def get_camera_data(self): depth_img = depth_img * (zFar - zNear) + zNear else: + # prevent camera from taking a picture while the robot is still in the frame. + if go_home: + self.go_home(block_until_home=True) + # Get color and depth image from ROS service color_img, depth_img = self.camera.get_data() depth_img = depth_img.astype(float) / 1000 # unit: mm -> meter # color_img = self.camera.color_data.copy() # depth_img = self.camera.depth_data.copy() + if return_heightmaps: + # this allows the error to print only once, so it doesn't spam the console. + print_error = 0 + + max_z_height = np.inf + while max_z_height > z_height_retake_threshold: + scaled_depth_img = depth_img * self.cam_depth_scale # Apply depth scale from calibration + color_heightmap, depth_heightmap = utils.get_heightmap(color_img, scaled_depth_img, self.cam_intrinsics, self.cam_pose, + workspace_limits, heightmap_resolution, background_heightmap=self.background_heightmap) + # TODO(ahundt) switch to masked array, then only have a regular heightmap + valid_depth_heightmap = depth_heightmap.copy() + valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 + + _, max_z_height, _ = self.check_z_height(valid_depth_heightmap, reward_multiplier=1) + + if max_z_height > z_height_retake_threshold: + if print_error == 3: + print("ERROR: depth_heightmap value too high. max_z_height: ", max_z_height) + + # Get color and depth image from ROS service + color_img, depth_img = self.camera.get_data() + depth_img = depth_img.astype(float) / 1000 # unit: mm -> meter + + print_error += 1 + time.sleep(0.1) + + + return valid_depth_heightmap, color_heightmap, depth_heightmap, max_z_height, color_img, depth_img + + # if not return_depthmaps, return just raw images return color_img, depth_img From 832063746c1ea6e6ce821f4d6e348d263c59cf08 Mon Sep 17 00:00:00 2001 From: Nick Greene Date: Sun, 9 Feb 2020 15:26:05 -0500 Subject: [PATCH 4/6] robot.py check if holding object when unstacking starts --- robot.py | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/robot.py b/robot.py index 15ad5e85..2da4511f 100755 --- a/robot.py +++ b/robot.py @@ -580,13 +580,22 @@ def get_obj_positions_and_orientations(self): return obj_positions, obj_orientations - def reposition_objects(self, workspace_limits=None): + def reposition_objects(self, workspace_limits=None, unstack_drop_height=0.05): # grasp blocks from previously placed positions and place them in a random position. if self.unstack: print("------- UNSTACKING --------") place_pose_history = self.place_pose_history.copy() place_pose_history.reverse() + holding_object = self.close_gripper(): + # if already has an object in the gripper when reposition objects gets called, place that object somewhere random + if holding_object: + _, _, rand_position, rand_orientation = self.generate_random_object_pose() + rand_position[2] = unstack_drop_height # height from which to release blocks (0.05 m per block) + rand_angle = rand_orientation[0] + + self.place(rand_position, rand_angle, save_history=False) + # go to x,y position of previous places and pick up the max_z height from the depthmap (top of the stack) for pose in place_pose_history: x, y, z, angle = pose @@ -610,10 +619,10 @@ def reposition_objects(self, workspace_limits=None): grasp_success, color_success = self.grasp([x, y, z], angle) if grasp_success: _, _, rand_position, rand_orientation = self.generate_random_object_pose() - rand_position[2] = 0.05 # height from which to release blocks (0.05 m per block) + rand_position[2] = unstack_drop_height # height from which to release blocks (0.05 m per block) rand_angle = rand_orientation[0] - self.place(rand_position, rand_angle) + self.place(rand_position, rand_angle, save_history=False) self.place_pose_history = [] # clear place position history print("------- UNSTACKING COMPLETE --------") @@ -1573,7 +1582,7 @@ def block_until_joint_position(self, position, timeout_seconds=7): time.sleep(0.1) - def place(self, position, heightmap_rotation_angle, workspace_limits=None, distance_threshold=0.06, go_home=True): + def place(self, position, heightmap_rotation_angle, workspace_limits=None, distance_threshold=0.06, go_home=True, save_history=True): """ Place an object, currently only tested for blocks. When in sim mode it assumes the current position of the robot and grasped object is higher than any other object. @@ -1584,9 +1593,10 @@ def place(self, position, heightmap_rotation_angle, workspace_limits=None, dista place_pose = (position[0], position[1], position[2], heightmap_rotation_angle) print('Executing: Place at (%f, %f, %f) angle: %f' % place_pose) - self.place_pose_history.append(place_pose) - while len(self.place_pose_history) > self.place_pose_history_limit: # only store x most recent place attempts - self.place_pose_history.pop(0) + if save_history: + self.place_pose_history.append(place_pose) + while len(self.place_pose_history) > self.place_pose_history_limit: # only store x most recent place attempts + self.place_pose_history.pop(0) if self.is_sim: From 9d1b0fbcde1dcc99f37cb6fe81e7cd0199d8ed0f Mon Sep 17 00:00:00 2001 From: Nick Greene Date: Sun, 9 Feb 2020 15:35:37 -0500 Subject: [PATCH 5/6] main.py fixed robot() parameters --- main.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/main.py b/main.py index 84040946..eacf1336 100755 --- a/main.py +++ b/main.py @@ -188,7 +188,8 @@ def main(args): # Initialize pick-and-place system (camera and robot) robot = Robot(is_sim, obj_mesh_dir, num_obj, workspace_limits, tcp_host_ip, tcp_port, rtc_host_ip, rtc_port, - is_testing, test_preset_cases, test_preset_file, place, grasp_color_task, unstack=unstack, heightmap_resolution) + is_testing, test_preset_cases, test_preset_file, + place, grasp_color_task, unstack=unstack, heightmap_resolution=heightmap_resolution) # Initialize trainer trainer = Trainer(method, push_rewards, future_reward_discount, From 8479f124fccf17f36cc100f881d99c50cbc288cb Mon Sep 17 00:00:00 2001 From: Nick Greene Date: Sun, 9 Feb 2020 16:06:57 -0500 Subject: [PATCH 6/6] robot.py fixed gripper check --- robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot.py b/robot.py index 2da4511f..5f5ccdf7 100755 --- a/robot.py +++ b/robot.py @@ -587,7 +587,7 @@ def reposition_objects(self, workspace_limits=None, unstack_drop_height=0.05): place_pose_history = self.place_pose_history.copy() place_pose_history.reverse() - holding_object = self.close_gripper(): + holding_object = not(self.close_gripper()) # if already has an object in the gripper when reposition objects gets called, place that object somewhere random if holding_object: _, _, rand_position, rand_orientation = self.generate_random_object_pose()