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 setup_sim_camera(self, resolution_x=1024., resolution_y=1024.): # Get handle to camera perspectiveAngle = np.deg2rad(54.70) self.cam_intrinsics = np.asarray( [[ resolution_x / (2 * np.tan(perspectiveAngle / 2)), 0, resolution_x / 2 ], [ 0, resolution_y / (2 * np.tan(perspectiveAngle / 2)), resolution_y / 2 ], [0, 0, 1]]) _, self.cam_handle = vrep.simxGetObjectHandle( self.sim_client, 'Vision_sensor_persp_0', vrep.simx_opmode_blocking) # Get camera pose and intrinsics in simulation _, cam_position = vrep.simxGetObjectPosition(self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking) _, cam_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking) cam_trans = np.eye(4, 4) cam_trans[0:3, 3] = np.asarray(cam_position) cam_orientation = [ cam_orientation[0], cam_orientation[1], cam_orientation[2] ] cam_rotm = np.eye(4, 4) cam_rotm[0:3, 0:3] = utils.euler2rotm(cam_orientation) self.cam_pose = np.dot( cam_trans, cam_rotm ) # Compute rigid transformation representating camera pose self.cam_depth_scale = 1
def setup_sim_camera(self): # Get handle to camera sim_ret, self.cam_handle = vrep.simxGetObjectHandle( self.sim_client, 'Vision_sensor_ortho', vrep.simx_opmode_blocking) # Get camera pose and intrinsics in simulation sim_ret, cam_position = vrep.simxGetObjectPosition( self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking) sim_ret, cam_orientation = vrep.simxGetObjectOrientation( self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking) cam_trans = np.eye(4, 4) cam_trans[0:3, 3] = np.asarray(cam_position) cam_orientation = [ -cam_orientation[0], -cam_orientation[1], -cam_orientation[2] ] cam_rotm = np.eye(4, 4) cam_rotm[0:3, 0:3] = np.linalg.inv(utils.euler2rotm(cam_orientation)) self.cam_pose = np.dot( cam_trans, cam_rotm ) # Compute rigid transformation representating camera pose self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) self.cam_depth_scale = 1 # Get background image self.bg_color_img, self.bg_depth_img = self.get_camera_data() self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale
def close_RG2_gripper(self, default_vel=-0.1, motor_force=50): # RG2 gripper function gripper_motor_velocity = default_vel if self.is_insert_task: gripper_motor_force = 200 else: gripper_motor_force = motor_force _, gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) _, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) gripper_fully_closed = False close_gripper_count = 0 while gripper_joint_position > -0.04: # Block until gripper is fully closed _, new_gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, gripper_handle, vrep.simx_opmode_blocking) close_gripper_count += 1 if new_gripper_joint_position < gripper_joint_position: close_gripper_count = 0 gripper_joint_position = new_gripper_joint_position if close_gripper_count > 1: return gripper_fully_closed gripper_fully_closed = True return gripper_fully_closed
def checkgrasp(self): sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.clientID, RG2_gripper_handle, vrep.simx_opmode_blocking) if gripper_joint_position > -0.047: self.remove_height_object() return True else: return False
def close_gripper(self): motorVelocity = -0.5 # m / s motorForce = 200 # N sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.clientID, RG2_gripper_handle, motorForce, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.clientID, RG2_gripper_handle, motorVelocity, vrep.simx_opmode_blocking) time.sleep(0.2)
def open_gripper(self, isAsync=False): gripper_motor_velocity = 0.5 gripper_motor_force = 20 sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) while gripper_joint_position < 0.0536: # Block until gripper is fully open sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
def open_RG2_gripper(self, default_vel=0.5, motor_force=100): # RG2 Gripper gripper_motor_velocity = default_vel gripper_motor_force = motor_force sim_ret, gripper_handle = vrep.simxGetObjectHandle( self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) _, _ = vrep.simxGetJointPosition(self.sim_client, gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, gripper_handle, gripper_motor_velocity, 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 env_prepare(self): sim_ret, self.UR3_target_handle = vrep.simxGetObjectHandle( self.clientID, 'UR3_Target', vrep.simx_opmode_blocking) sim_ret, self.WorkSpace_handle = vrep.simxGetObjectHandle( self.clientID, 'WorkSpace', vrep.simx_opmode_blocking) sim_ret, self.WorkSpace_position = vrep.simxGetObjectPosition( self.clientID, self.WorkSpace_handle, -1, vrep.simx_opmode_blocking) print("WorkSpace_position", self.WorkSpace_position) self.WorkSpace_size = 0.34 # 0.36*0.36 self.workspace_limits = np.array( [[ self.WorkSpace_position[0] - self.WorkSpace_size / 2, self.WorkSpace_position[0] + self.WorkSpace_size / 2 ], [ self.WorkSpace_position[1] - self.WorkSpace_size / 2, self.WorkSpace_position[1] + self.WorkSpace_size / 2 ]]) self.move_abs_ownframe( [self.WorkSpace_size / 2, self.WorkSpace_size / 2, 0.3]) self.setangle_360(0) self.setVisionSensor() self.color_space = np.asarray([ [78.0, 121.0, 167.0], # blue [89.0, 161.0, 79.0], # green [156, 117, 95], # brown [242, 142, 43], # orange [237.0, 201.0, 72.0], # yellow [186, 176, 172], # gray [255.0, 87.0, 89.0], # red [176, 122, 161], # purple [118, 183, 178], # cyan [255, 157, 167] ]) / 255 # pink
def setVisionSensor(self): sim_ret, self.VS_left_handle = vrep.simxGetObjectHandle( self.clientID, 'Vision_sensor_left', vrep.simx_opmode_blocking) ret, self.VS_left_position = vrep.simxGetObjectPosition( self.clientID, self.VS_left_handle, -1, vrep.simx_opmode_blocking) ret, self.VS_left_orientation = vrep.simxGetObjectOrientation( self.clientID, self.VS_left_handle, -1, vrep.simx_opmode_blocking) self.left_cam_intrinsics = np.asarray([[561.82, 0, 320], [0, 561.82, 240], [0, 0, 1]]) # 内部参数 self.leftmat = utils.Creat_posemat(self.VS_left_orientation, self.VS_left_position) sim_ret, self.VS_right_handle = vrep.simxGetObjectHandle( self.clientID, 'Vision_sensor_right', vrep.simx_opmode_blocking) ret, self.VS_right_position = vrep.simxGetObjectPosition( self.clientID, self.VS_right_handle, -1, vrep.simx_opmode_blocking) ret, self.VS_right_orientation = vrep.simxGetObjectOrientation( self.clientID, self.VS_right_handle, -1, vrep.simx_opmode_blocking) self.right_cam_intrinsics = np.asarray([[561.82, 0, 320], [0, 561.82, 240], [0, 0, 1]]) # 内部参数 self.rightmat = utils.Creat_posemat(self.VS_right_orientation, self.VS_right_position)
def close_gripper(self, isAsync=False): gripper_motor_velocity = -0.5 gripper_motor_force = 100 sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) gripper_fully_closed = False while gripper_joint_position > -0.047: # Block until gripper is fully closed sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) print(gripper_joint_position) if new_gripper_joint_position >= gripper_joint_position: return gripper_fully_closed gripper_joint_position = new_gripper_joint_position
def checkState(self): sim_ret, UR3_target_position = vrep.simxGetObjectPosition( self.clientID, self.UR3_target_handle, -1, vrep.simx_opmode_blocking) sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, RG2_gripper_position = vrep.simxGetObjectPosition( self.clientID, RG2_gripper_handle, -1, vrep.simx_opmode_blocking) #print("===================================================================") #print(UR3_target_position) #print(RG2_gripper_position) dist=pow(pow(UR3_target_position[0]-RG2_gripper_position[0],2)+\ pow(UR3_target_position[1] - RG2_gripper_position[1], 2) +\ pow(UR3_target_position[2] - RG2_gripper_position[2], 2),0.5) if dist > 0.1: return True #异常状态 else: return False
import time import random import copy vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') ## define Baxter's joints # define the body joints bodyJoints = np.zeros(3) bodyError = np.zeros(3) vertError, vertJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_verticalJoint', vrep.simx_opmode_blocking) rotError, rotJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_rotationJoint', vrep.simx_opmode_blocking) monitorError, monitorJoint = vrep.simxGetObjectHandle( clientID, 'Baxter_monitorJoint', vrep.simx_opmode_blocking) block0err, block0 = vrep.simxGetObjectHandle(clientID, "cube0", vrep.simx_opmode_blocking) # block2err, block2 = vrep.simxGetObjectHandle(clientID, "cube2", vrep.simx_opmode_blocking) # block1err, block1 = vrep.simxGetObjectHandle(clientID, "cube1", vrep.simx_opmode_blocking) #block3err, block3 = vrep.simxGetObjectHandle(clientID, "cube3", vrep.simx_opmode_blocking) # fireerr, fire = vrep.simxGetObjectHandle(clientID, "fire", vrep.simx_opmode_blocking) # smoke0err, smoke0 = vrep.simxGetObjectHandle(clientID, "smoke#0", vrep.simx_opmode_blocking) # smoke1err, smoke1 = vrep.simxGetObjectHandle(clientID, "smoke#1", vrep.simx_opmode_blocking)
from PIL import Image import detect #Setup is_sim = True workspace_limits = np.asarray([ [-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.4] ]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates) robot = Robot(is_sim, workspace_limits) #Variable Creation heightmap_rotation_angle = 0 #needed for grasp function workspace_center = [-.5, 0, .03] #where the objects will be placed #Handle of UR5_target, needed as a home for the robotic arm sim_ret, UR5_target_handle = vrep.simxGetObjectHandle( robot.sim_client, 'UR5_target', vrep.simx_opmode_blocking) sim_ret, UR5_target_position = vrep.simxGetObjectPosition( robot.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking) #Camera With YOLOv5 robot.setup_sim_camera() rgb, depth = robot.get_camera_data() workspace_picture = Image.fromarray(rgb) workspace_picture.save( "/home/saeid/Downloads/CoppeliaSim_Edu_V4_1_0_Ubuntu18_04/programming/remoteApiBindings/python/python/data/images/workspace.jpg" ) #Detect number of objects objectsPresent = detect.detect(False) #filled with objects detected by cameras objectsPresent = int(objectsPresent) print("Number of Objects detected: ", objectsPresent)
def getObjectHandle(sim_client, obj_name): return vrep.simxGetObjectHandle(sim_client, obj_name, VREP_BLOCKING)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) while gripper_joint_position < 0.0536: # Block until gripper is fully open sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) def close_gripper(self, async=False): gripper_motor_velocity = -0.5 gripper_motor_force = 100 sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) gripper_fully_closed = False while gripper_joint_position > -0.047: # Block until gripper is fully closed sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) print(gripper_joint_position) if new_gripper_joint_position >= gripper_joint_position: return gripper_fully_closed gripper_joint_position = new_gripper_joint_position
class RobotSim(object): def __init__(self, obj_mesh_dir, workspace_limits, is_testing, ip_vrep='127.0.0.1'): self.workspace_limits = workspace_limits # Read files in object mesh directory self.obj_mesh_dir = obj_mesh_dir self.mesh_list = os.listdir(self.obj_mesh_dir) vrep.simxFinish(-1) # Just in case, close all opened connections self.sim_client = vrep.simxStart(ip_vrep, 19997, True, True, 5000, 5) # Connect to V-REP on port 19997 if self.sim_client == -1: print( 'Failed to connect to simulation (V-REP remote API server). Exiting.' ) exit() else: print('Connected to simulation.') self.restart_sim() # Setup virtual camera in simulation self.setup_sim_camera() self.is_testing = is_testing def check_sim(self): # Check if simulation is stable by checking if gripper is within workspace sim_ret, gripper_position = vrep.simxGetObjectPosition( self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking) sim_ok = gripper_position[0] > self.workspace_limits[0][ 0] - 0.1 and gripper_position[0] < self.workspace_limits[0][ 1] + 0.1 and gripper_position[1] > self.workspace_limits[1][ 0] - 0.1 and gripper_position[1] < self.workspace_limits[ 1][1] + 0.1 and gripper_position[ 2] > self.workspace_limits[2][ 0] and gripper_position[ 2] < self.workspace_limits[2][1] if not sim_ok: print('Simulation unstable. Restarting environment.') self.restart_sim() 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) vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking) time.sleep(1) 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) 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) def move_to(self, tool_position): # 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 open_gripper(self, async=False): gripper_motor_velocity = 0.5 gripper_motor_force = 20 sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle( self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking) sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity, vrep.simx_opmode_blocking) while gripper_joint_position < 0.0536: # Block until gripper is fully open sim_ret, gripper_joint_position = vrep.simxGetJointPosition( self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)