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)
