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 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 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
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)
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 def get_camera_data(self): # Get color image from simulation sim_ret, resolution, raw_image = vrep.simxGetVisionSensorImage( self.sim_client, self.cam_handle, 0, vrep.simx_opmode_blocking) color_img = np.asarray(raw_image)
def setJointTargetVelocity(sim_client, joint, velocity): return vrep.simxSetJointTargetVelocity(sim_client, joint, velocity, VREP_BLOCKING)