def move_to(self, tool_position, tool_orientation): # sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking) sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.02 * move_direction / move_magnitude num_move_steps = int(np.floor(move_magnitude / 0.02)) for step_iter in range(num_move_steps): vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0], UR5_target_position[1] + move_step[1], UR5_target_position[2] + move_step[2]), vrep.simx_opmode_blocking) sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking)
def restart_sim(self): sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_target', vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (-0.5, 0, 0.3), vrep.simx_opmode_blocking) vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(1) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(0.5) sim_ret, self.RG2_tip_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking) sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking) while gripper_position[ 2] > 0.4: # V-REP bug requiring multiple starts and stops to restart vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(1) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(0.5) sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking)
def reposition_objects(self, workspace_limits): # Move gripper out of the way self.move_to([-0.1, 0, 0.3], None) # sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking) # vrep.simxSetObjectPosition(self.sim_client, UR5_target_handle, -1, (-0.5,0,0.3), vrep.simx_opmode_blocking) # time.sleep(1) for object_handle in self.object_handles: # Drop object at random x,y location and random orientation in robot workspace drop_x = (workspace_limits[0][1] - workspace_limits[0][0] - 0.2) * np.random.random_sample() + \ workspace_limits[0][0] + 0.1 drop_y = (workspace_limits[1][1] - workspace_limits[1][0] - 0.2) * np.random.random_sample() + \ workspace_limits[1][0] + 0.1 object_position = [drop_x, drop_y, 0.15] object_orientation = [ 2 * np.pi * np.random.random_sample(), 2 * np.pi * np.random.random_sample(), 2 * np.pi * np.random.random_sample() ] vrep.simxSetObjectPosition(self.sim_client, object_handle, -1, object_position, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(self.sim_client, object_handle, -1, object_orientation, vrep.simx_opmode_blocking) time.sleep(2)
def move_linear(self, tool_position, num_steps=10): sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) num_move_steps = num_steps move_step = move_direction / num_move_steps for step_iter in range(num_move_steps): vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0], UR5_target_position[1] + move_step[1], UR5_target_position[2] + move_step[2]), vrep.simx_opmode_blocking) sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) time.sleep(0.05)
def remove_height_object(self): maxhandle = self.object_handles[0] sim_ret, obj_position = vrep.simxGetObjectPosition( self.clientID, self.object_handles[0], -1, vrep.simx_opmode_blocking) maxposition = obj_position[2] for i in range(1, len(self.object_handles)): sim_ret, obj_position = vrep.simxGetObjectPosition( self.clientID, self.object_handles[i], -1, vrep.simx_opmode_blocking) if obj_position[2] > maxposition: maxposition = obj_position[2] maxhandle = self.object_handles[i] #print(obj_position) vrep.simxSetObjectPosition(self.clientID, maxhandle, -1, (0.5, -0.2, 0.3), vrep.simx_opmode_blocking)
def restart_sim(self): _, self.UR5_target_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_target', vrep.simx_opmode_blocking) _, self.UR5_tip_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1, self.gripper_home_pos, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, self.gripper_home_ori, vrep.simx_opmode_blocking) vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(0.3) _, self.RG2_tip_handle = vrep.simxGetObjectHandle( self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking) _, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking) while gripper_position[2] > self.gripper_home_pos[ 2] + 0.01: # V-REP bug requiring multiple starts and stops to restart vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(1) sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking) self.open_RG2_gripper() self.obj_target_handles = [] if self.is_insert_task: self.hole_handles = [] self.add_hole() self.add_objects() time.sleep(0.8)
def move_relative(self, dp, step=0.02): ''' :param dp: [dx,dy,dz] :param step: step lenght :return: ''' move_direction = np.asarray(dp) move_magnitude = np.linalg.norm(move_direction) move_step = step * move_direction / move_magnitude num_move_steps = int(np.floor(move_magnitude / step)) for step_iter in range(num_move_steps): sim_ret, UR3_target_position = vrep.simxGetObjectPosition( self.clientID, self.UR3_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.clientID, self.UR3_target_handle, -1, (UR3_target_position[0] + move_step[0], UR3_target_position[1] + move_step[1], UR3_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
def push(self, position, heightmap_rotation_angle, workspace_limits): print('Executing: push at (%f, %f, %f)' % (position[0], position[1], position[2])) # Compute tool orientation from heightmap rotation angle tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 # Adjust pushing point to be on tip of finger position[2] = position[2] + 0.015 # Compute pushing direction push_orientation = [1.0, 0.0] push_direction = np.asarray([ push_orientation[0] * np.cos(heightmap_rotation_angle) - push_orientation[1] * np.sin(heightmap_rotation_angle), push_orientation[0] * np.sin(heightmap_rotation_angle) + push_orientation[1] * np.cos(heightmap_rotation_angle) ]) # Move gripper to location above pushing point pushing_point_margin = 0.1 location_above_pushing_point = (position[0], position[1], position[2] + pushing_point_margin) # Compute gripper position and linear movement increments tool_position = location_above_pushing_point sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.05 * move_direction / move_magnitude + 1e-8 num_move_steps = int(np.floor(move_direction[0] / move_step[0])) # Compute gripper orientation and rotation increments sim_ret, gripper_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) rotation_step = 0.3 if ( tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 num_rotation_steps = int( np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) # Simultaneously move and rotate gripper for step_iter in range(max(num_move_steps, num_rotation_steps)): vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) # Ensure gripper is closed self.close_gripper() # Approach pushing point self.move_to(position, None) # Compute target location (push to the right) push_length = 0.1 target_x = min( max(position[0] + push_direction[0] * push_length, workspace_limits[0][0]), workspace_limits[0][1]) target_y = min( max(position[1] + push_direction[1] * push_length, workspace_limits[1][0]), workspace_limits[1][1]) push_length = np.sqrt( np.power(target_x - position[0], 2) + np.power(target_y - position[1], 2)) # Move in pushing direction towards target location self.move_to([target_x, target_y, position[2]], None) # Move gripper to location above grasp target self.move_to([target_x, target_y, location_above_pushing_point[2]], None)
def grasp(self, position, heightmap_rotation_angle, workspace_limits): print('Executing: grasp at (%f, %f, %f)' % (position[0], position[1], position[2])) # Compute tool orientation from heightmap rotation angle tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 # Avoid collision with floor position = np.asarray(position).copy() position[2] = max(position[2] - 0.04, workspace_limits[2][0] + 0.02) # Move gripper to location above grasp target grasp_location_margin = 0.15 # sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking) location_above_grasp_target = (position[0], position[1], position[2] + grasp_location_margin) # Compute gripper position and linear movement increments tool_position = location_above_grasp_target sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.05 * move_direction / move_magnitude + 1e-8 num_move_steps = int(np.floor(move_direction[0] / move_step[0])) # Compute gripper orientation and rotation increments sim_ret, gripper_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) rotation_step = 0.3 if ( tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 num_rotation_steps = int( np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) # Simultaneously move and rotate gripper for step_iter in range(max(num_move_steps, num_rotation_steps)): vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) # Ensure gripper is open self.open_gripper() # Approach grasp target self.move_to(position, None) # Close gripper to grasp target gripper_full_closed = self.close_gripper() # Move gripper to location above grasp target self.move_to(location_above_grasp_target, None) # Check if grasp is successful gripper_full_closed = self.close_gripper() grasp_success = not gripper_full_closed # Move the grasped object elsewhere grasped_object_name = None if grasp_success: object_positions = np.asarray(self.get_obj_positions()) object_positions = object_positions[:, 2] grasped_object_ind = np.argmax(object_positions) grasped_object_handle = self.object_handles[grasped_object_ind] grasped_object_name = self.object_names[grasped_object_ind] del self.object_handles[grasped_object_ind] del self.object_names[grasped_object_ind] vrep.simxSetObjectPosition( self.sim_client, grasped_object_handle, -1, (-1, -1 + 0.05 * float(grasped_object_ind), 0.1), vrep.simx_opmode_blocking) return grasped_object_name
def set_single_obj_position(self, object_handle, goal_pos): _ = vrep.simxSetObjectPosition(self.sim_client, object_handle, -1, goal_pos, vrep.simx_opmode_blocking)
def grasp(self, position, heightmap_rotation_angle, workspace_limits): print('Executing: grasp at (%f, %f, %f)' % (position[0], position[1], position[2])) # Compute tool orientation from heightmap rotation angle tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 # Avoid collision with floor position = np.asarray(position).copy() position[2] = max(position[2] - 0.04, workspace_limits[2][0] + 0.02) # Move gripper to location above grasp target grasp_location_margin = 0.15 # sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking) location_above_grasp_target = (position[0], position[1], position[2] + grasp_location_margin) # Compute gripper position and linear movement increments tool_position = location_above_grasp_target sim_ret, UR5_target_position = vrep.simxGetObjectPosition( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([ tool_position[0] - UR5_target_position[0], tool_position[1] - UR5_target_position[1], tool_position[2] - UR5_target_position[2] ]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.05 * move_direction / move_magnitude num_move_steps = int(np.floor(move_direction[0] / move_step[0])) # Compute gripper orientation and rotation increments sim_ret, gripper_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) rotation_step = 0.3 if ( tool_rotation_angle - gripper_orientation[1] > 0) else -0.3 num_rotation_steps = int( np.floor((tool_rotation_angle - gripper_orientation[1]) / rotation_step)) # Simultaneously move and rotate gripper for step_iter in range(max(num_move_steps, num_rotation_steps)): vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (UR5_target_position[0] + move_step[0] * min(step_iter, num_move_steps), UR5_target_position[1] + move_step[1] * min(step_iter, num_move_steps), UR5_target_position[2] + move_step[2] * min(step_iter, num_move_steps)), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, gripper_orientation[1] + rotation_step * min(step_iter, num_rotation_steps), np.pi / 2), vrep.simx_opmode_blocking) vrep.simxSetObjectPosition( self.sim_client, self.UR5_target_handle, -1, (tool_position[0], tool_position[1], tool_position[2]), vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation( self.sim_client, self.UR5_target_handle, -1, (np.pi / 2, tool_rotation_angle, np.pi / 2), vrep.simx_opmode_blocking) # Ensure gripper is open self.open_gripper() # Approach grasp target self.move_to(position, None) # Get images before grasping color_img, depth_img = self.get_camera_data() depth_img = depth_img * self.cam_depth_scale # Apply depth scale from calibration # Get heightmaps beforew grasping color_heightmap, depth_heightmap = utils.get_heightmap( color_img, depth_img, self.cam_intrinsics, self.cam_pose, workspace_limits, 0.002) # heightmap resolution from args valid_depth_heightmap = depth_heightmap.copy() valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0 # Close gripper to grasp target gripper_full_closed = self.close_gripper() # Move gripper to location above grasp target self.move_to(location_above_grasp_target, None) # Check if grasp is successful gripper_full_closed = self.close_gripper() grasp_success = not gripper_full_closed # Move the grasped object elsewhere if grasp_success: object_positions = np.asarray(self.get_obj_positions()) object_positions = object_positions[:, 2] grasped_object_ind = np.argmax(object_positions) print('grasp obj z position', max(object_positions)) grasped_object_handle = self.object_handles[grasped_object_ind] vrep.simxSetObjectPosition( self.sim_client, grasped_object_handle, -1, (-0.5, 0.5 + 0.05 * float(grasped_object_ind), 0.1), vrep.simx_opmode_blocking) return grasp_success, color_img, depth_img, color_heightmap, valid_depth_heightmap, grasped_object_ind else: return grasp_success, None, None, None, None, None
def setObjectPosition(sim_client, obj_handle, position): return vrep.simxSetObjectPosition(sim_client, obj_handle, -1, position, VREP_BLOCKING)