def left_collision_state(self): error_code, self.left_arm_collision_state_target = vrep.simxReadCollision( self.clientID, self.left_arm_collision_target, vrep.simx_opmode_buffer) error_code, self.left_arm_collision_state_table = vrep.simxReadCollision( self.clientID, self.left_arm_collision_table, vrep.simx_opmode_buffer) if self.left_arm_collision_state_target or self.left_arm_collision_state_table: return True else: return False
def checkCollision(self, handle, ignoreError = False, initialize = False): if initialize: sim.simxReadCollision(self.clientID, handle, sim.simx_opmode_streaming) res, collision = sim.simxReadCollision(self.clientID, handle, sim.simx_opmode_buffer) if res!=sim.simx_return_ok and not ignoreError: print('Failed to check collision') print(res) return collision
def getCollisionState(self): errorCode, self.right_arm_collision_state_target = vrep.simxReadCollision( self.clientID, self.right_arm_collision_target, vrep.simx_opmode_buffer) errorCode, self.right_arm_collision_state_table = vrep.simxReadCollision( self.clientID, self.right_arm_collision_table, vrep.simx_opmode_buffer) if (self.right_arm_collision_state_target or self.right_arm_collision_state_table): return True else: return False
def collision(): errorCode, car_collision_handle = sim.simxGetCollisionHandle( clientID, 'Collision', sim.simx_opmode_oneshot_wait) number_collisions = 0 if errorCode != sim.simx_return_ok: print('Error: ', errorCode) while (sim.simxGetConnectionId(clientID) != -1): returnCode, collisionState = sim.simxReadCollision( clientID, car_collision_handle, sim.simx_opmode_streaming) if (collisionState != 0): number_collisions += 1 print("Collision {0} occurred".format(str(number_collisions)))
def check_collision(): collision_reading = np.zeros(40) is_collision = 0 for i in range(40): collision_reading[i] = sim.simxReadCollision(clientID, collision_handle_list[i], sim.simx_opmode_buffer)[1] if collision_reading[i] == 1: is_collision = 1 if is_collision == 1: print('Collision detected!') return 1 else: return 0
sim.simx_opmode_oneshot) sim.simxSetObjectIntParameter(clientID, armjoint5_handle, 2000, 1, sim.simx_opmode_oneshot) sim.simxSetObjectIntParameter(clientID, armjoint5_handle, 2001, 1, sim.simx_opmode_oneshot) sim.simxSetObjectIntParameter(clientID, armjoint6_handle, 2000, 1, sim.simx_opmode_oneshot) sim.simxSetObjectIntParameter(clientID, armjoint6_handle, 2001, 1, sim.simx_opmode_oneshot) # get the collision handles collision_handle_list = [] for i in range(40): err_code, collision_handle = sim.simxGetCollisionHandle( clientID, "Collision" + str(i), sim.simx_opmode_blocking) sim.simxReadCollision(clientID, collision_handle, sim.simx_opmode_streaming) collision_handle_list.append(collision_handle) # You do not need to modify the code above # function to control the movement of the arm, the input are the angles of joint1, joint2, joint3, joint4, joint5, joint6. The unit are in degrees def move_arm(armpose): armpose_convert = [] for i in range(6): armpose_convert.append(round(armpose[i] / 180 * math.pi, 3)) sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetPosition(clientID, armjoint1_handle, armpose_convert[0], sim.simx_opmode_oneshot) sim.simxSetJointTargetPosition(clientID, armjoint2_handle, armpose_convert[1], sim.simx_opmode_oneshot)
def __init__(self): # Close any open connections vrep.simxFinish(-1) # Create Var for client connection self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.clientID != -1: print('Connected to remote API server') self.joint_array = [] self.joint_org_position = [] error_code, self.input_cam = vrep.simxGetObjectHandle( self.clientID, 'input_camera', vrep.simx_opmode_oneshot_wait) error_code, self.hand = vrep.simxGetObjectHandle( self.clientID, 'Baxter_rightArm_camera', vrep.simx_opmode_oneshot_wait) error_code, self.target = vrep.simxGetObjectHandle( self.clientID, 'right_target', vrep.simx_opmode_oneshot_wait) error_code, self.video_cam = vrep.simxGetObjectHandle( self.clientID, 'video_camera', vrep.simx_opmode_oneshot_wait) error_code, self.main_target = vrep.simxGetObjectHandle( self.clientID, 'target', vrep.simx_opmode_oneshot_wait) error, self.right_arm_collision_target = vrep.simxGetCollisionHandle( self.clientID, "right_arm_collision_target#", vrep.simx_opmode_blocking) error, self.right_arm_collision_table = vrep.simxGetCollisionHandle( self.clientID, "right_arm_collision_table#", vrep.simx_opmode_blocking) # Used to translate action to joint array position self.joint_switch = { 0: 0, 1: 0, 2: 1, 3: 1, 4: 2, 5: 2, 6: 3, 7: 3, 8: 4, 9: 4, 10: 5, 11: 5, 12: 6, 13: 6 } for x in range(1, 8): error_code, joint = vrep.simxGetObjectHandle( self.clientID, 'Baxter_rightArm_joint' + str(x), vrep.simx_opmode_oneshot_wait) self.joint_array.append(joint) for x in range(0, 7): vrep.simxGetJointPosition(self.clientID, self.joint_array[x], vrep.simx_opmode_streaming) for x in range(0, 7): error_code, temp_pos = vrep.simxGetJointPosition( self.clientID, self.joint_array[x], vrep.simx_opmode_buffer) self.joint_org_position.append(temp_pos) error_code, self.xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.hand, -1, vrep.simx_opmode_streaming) error_code, self.xyz_target = vrep.simxGetObjectPosition( self.clientID, self.target, -1, vrep.simx_opmode_streaming) error_code, self.xyz_main_target = vrep.simxGetObjectPosition( self.clientID, self.main_target, -1, vrep.simx_opmode_streaming) vrep.simxGetVisionSensorImage(self.clientID, self.input_cam, 0, vrep.simx_opmode_streaming) vrep.simxGetVisionSensorImage(self.clientID, self.video_cam, 0, vrep.simx_opmode_streaming) error_code, self.right_arm_collision_state_target = vrep.simxReadCollision( self.clientID, self.right_arm_collision_target, vrep.simx_opmode_streaming) error_code, self.right_arm_collision_state_table = vrep.simxReadCollision( self.clientID, self.right_arm_collision_table, vrep.simx_opmode_streaming) time.sleep(1) else: print('Failed connecting to remote API server') sys.exit('Could not connect')
def __init__(self): # Close any open connections vrep.simxFinish(-1) # Create Var for client connection self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.clientID != -1: print('Connected to remote API server') self.right_joint_array = [] self.left_joint_array = [] self.right_joint_org_position = [] self.left_joint_org_position = [] # input videos and camera error_code, self.input_cam = vrep.simxGetObjectHandle( self.clientID, 'input_camera', vrep.simx_opmode_oneshot_wait) error_code, self.video_cam = vrep.simxGetObjectHandle( self.clientID, 'video_camera', vrep.simx_opmode_oneshot_wait) # main target # get the coordinates for start (0, 0, 0)/ reward these are not the same as the reset coordinates. error_code, self.main_target = vrep.simxGetObjectHandle( self.clientID, 'target', vrep.simx_opmode_oneshot_wait) # get the euler angles of the main target, used to reset the object error_code, self.main_target_angles = vrep.simxGetObjectOrientation( self.clientID, self.main_target, -1, vrep.simx_opmode_streaming) # right arm error_code, self.right_hand = vrep.simxGetObjectHandle( self.clientID, 'Baxter_rightArm_camera', vrep.simx_opmode_oneshot_wait) error_code, self.right_target = vrep.simxGetObjectHandle( self.clientID, 'right_target', vrep.simx_opmode_oneshot_wait) error, self.right_arm_collision_target = vrep.simxGetCollisionHandle( self.clientID, "right_collision_target", vrep.simx_opmode_blocking) error, self.right_arm_collision_table = vrep.simxGetCollisionHandle( self.clientID, "right_collision_table", vrep.simx_opmode_blocking) # left arm error_code, self.left_hand = vrep.simxGetObjectHandle( self.clientID, 'Baxter_leftArm_camera', vrep.simx_opmode_oneshot_wait) error_code, self.left_target = vrep.simxGetObjectHandle( self.clientID, 'left_target', vrep.simx_opmode_oneshot_wait) error, self.left_arm_collision_target = vrep.simxGetCollisionHandle( self.clientID, "left_collision_target", vrep.simx_opmode_blocking) error, self.left_arm_collision_table = vrep.simxGetCollisionHandle( self.clientID, "left_collision_table", vrep.simx_opmode_blocking) # Used to translate action to joint array position self.joint_switch = { 0: 0, 1: 0, 2: 1, 3: 1, 4: 2, 5: 2, 6: 3, 7: 3, 8: 4, 9: 4, 10: 5, 11: 5, 12: 6, 13: 6 } for x in range(1, 8): # right arm error_code, right_joint = vrep.simxGetObjectHandle( self.clientID, 'Baxter_rightArm_joint' + str(x), vrep.simx_opmode_oneshot_wait) self.right_joint_array.append(right_joint) # left arm error_code, left_joint = vrep.simxGetObjectHandle( self.clientID, 'Baxter_leftArm_joint' + str(x), vrep.simx_opmode_oneshot_wait) self.left_joint_array.append(left_joint) for x in range(0, 7): vrep.simxGetJointPosition(self.clientID, self.right_joint_array[x], vrep.simx_opmode_streaming) vrep.simxGetJointPosition(self.clientID, self.left_joint_array[x], vrep.simx_opmode_streaming) for x in range(0, 7): # right arm error_code, right_temp_pos = vrep.simxGetJointPosition( self.clientID, self.right_joint_array[x], vrep.simx_opmode_buffer) self.right_joint_org_position.append(right_temp_pos) # left arm error_code, left_temp_pos = vrep.simxGetJointPosition( self.clientID, self.left_joint_array[x], vrep.simx_opmode_buffer) self.left_joint_org_position.append(left_temp_pos) # right hand error_code, self.right_xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.right_hand, -1, vrep.simx_opmode_streaming) error_code, self.right_xyz_target = vrep.simxGetObjectPosition( self.clientID, self.right_target, -1, vrep.simx_opmode_streaming) # left hand error_code, self.left_xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.left_hand, -1, vrep.simx_opmode_streaming) error_code, self.left_xyz_target = vrep.simxGetObjectPosition( self.clientID, self.left_target, -1, vrep.simx_opmode_streaming) # main target for single point goal error_code, self.xyz_main_target = vrep.simxGetObjectPosition( self.clientID, self.main_target, -1, vrep.simx_opmode_streaming) vrep.simxGetVisionSensorImage(self.clientID, self.input_cam, 0, vrep.simx_opmode_streaming) vrep.simxGetVisionSensorImage(self.clientID, self.video_cam, 0, vrep.simx_opmode_streaming) # right hand error_code, self.right_arm_collision_state_target = vrep.simxReadCollision( self.clientID, self.right_arm_collision_target, vrep.simx_opmode_streaming) error_code, self.right_arm_collision_state_table = vrep.simxReadCollision( self.clientID, self.right_arm_collision_table, vrep.simx_opmode_streaming) # left hand error_code, self.left_arm_collision_state_target = vrep.simxReadCollision( self.clientID, self.left_arm_collision_target, vrep.simx_opmode_streaming) error_code, self.left_arm_collision_state_table = vrep.simxReadCollision( self.clientID, self.left_arm_collision_table, vrep.simx_opmode_streaming) time.sleep(1) else: print('Failed connecting to remote API server') sys.exit('Could not connect')
def object_collision_state(self): error_code, self.object_collision_state_table = vrep.simxReadCollision( self.clientID, self.target_collision_table, vrep.simx_opmode_buffer) return self.object_collision_state_table
if errorCode != sim.simx_return_ok: print('Error: ', errorCode) time.sleep(2) sim.simxAddStatusbarMessage(clientID, 'Connected and running', sim.simx_opmode_oneshot) vel = 10 print("Forward") sim.simxSetJointTargetVelocity(clientID, left_motor_handle, -vel, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, right_motor_handle, -vel, sim.simx_opmode_streaming) #12 second time frame to read if a collision occurred t = time.time() while time.time()-t < 12: returnCode, collisionState = sim.simxReadCollision(clientID, car_collision_handle, sim.simx_opmode_streaming) if(collisionState != 0): print("Collision occurred") time.sleep(1) sys.exit() print(collisionState) if(collisionState != 0): sys.exit("Collision occurred") print("Left") sim.simxSetJointTargetVelocity(clientID, left_motor_handle, vel, sim.simx_opmode_streaming) time.sleep(10) sim.simxSetJointTargetVelocity(clientID, left_motor_handle, 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, right_motor_handle, 0, sim.simx_opmode_streaming)