def openGripper(self):
res, gripper = sim.simxGetObjectHandle(self.clientID,
'P_Grip_straight_motor',
sim.simx_opmode_blocking)
res = sim.simxSetJointTargetPosition(self.clientID, gripper, -10,
sim.simx_opmode_blocking)
sim.simxSetIntegerSignal(self.clientID, 'P_Grip_straight_motor', 1,
sim.simx_opmode_blocking)
def startStopSim():
global PORT, client, cameras
if client is None:
# No client, then Start
PORT = int(request.args.get('port'))
print("waiting")
while portpicker.is_port_free(PORT):
sleep(5 * againIn)
print(".")
client = sim.simxStart('127.0.0.1', PORT, True, True, 5000, 5)
verboseResp(client, "simxStart")
return str(client)
else:
# There is a client, then Stop
stop()
sleep(.5)
response = sim.simxSetIntegerSignal(client, 'doClose', 1,
sim.simx_opmode_oneshot_wait)
verboseResp(response, "simxSetIntegerSignal doClose")
try:
finish()
except:
verboseResp("already finished?", "finish")
reset()
if response == -1:
return "-1"
else:
return "0"
def transfer_genomes(self, timeout, project):
"""
transfer the genome files to the evo-client
"""
ret, ping_time = sim.simxGetPingTime(self.clientID)
checksum = 0
sim.simxClearIntegerSignal(self.clientID, "ClientID_Signal", mode1)
sim.simxSetIntegerSignal(self.clientID, "ClientID_Signal",
self.clientID, mode1)
for i, filename in enumerate(self.genome_filenames):
file_to_transfer = project.path_to_gene_pool + filename
ret = sim.simxTransferFile(self.clientID, file_to_transfer,
filename, timeout,
mode1) # default: mode2
if ret == 0:
checksum += 1
return checksum
def reset_sim(self):
time.sleep(
.5
) # moved sleep to the top to allow other actions to finish before reset
vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup_active', 0,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup#0_active', 0,
vrep.simx_opmode_oneshot)
for x in range(0, 7):
vrep.simxSetJointTargetPosition(self.clientID,
self.right_joint_array[x],
self.right_joint_org_position[x],
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetPosition(self.clientID,
self.left_joint_array[x],
self.left_joint_org_position[x],
vrep.simx_opmode_oneshot_wait)
self.reset_target_object(0.1250, 0.0, 0.9)
time.sleep(1)
# reset the suction grippers
vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup_active', 1,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup#0_active', 1,
vrep.simx_opmode_oneshot)
# pick up the box
# self.step_arms([.608, .45, 0, 0, -1.6, -1.5, 0], [-.605, .45, 0, 0, 1.6, -1.5, 0])
self.step_arms([0, .45, 0, 0, -1.6, -1.5, 0],
[0, .45, 0, 0, 1.6, -1.5, 0])
time.sleep(.5)
self.step_arms([.65, 0, 0, 0, 0, 0, 0], [-.7, 0, 0, 0, 0, 0, 0])
time.sleep(.5)
self.step_arms([0, -.1, 0, 0, 0, 0, 0], [0, -0.1, 0, 0, 0, 0, 0])
error_code, target_angles = vrep.simxGetObjectOrientation(
self.clientID, self.main_target, -1, vrep.simx_opmode_streaming)
for angle in target_angles:
if angle > 1 or angle < -1:
self.reset_sim()
print("SIM reset error, resetting again.")
break
def step(self, action):
if action <=5:
x,y,z = self.moves[action]
self.S = [self.S[0] + x, self.S[1] + y, self.S[2] + z]
if self.S[0]<0 or self.S[1]<0 or self.S[2]<0:
self.S = [max(0, self.S[0]), max(0, self.S[1]), max(0, self.S[2])]
return self.S,-10,False,{}
if self.S[0]>=5 or self.S[1]>=5 or self.S[2]>=10:
self.S = [min(self.S[0], 5 - 1), min(self.S[1], 5 - 1), min(self.S[2], 10 - 1)]
return self.S,-10,False,{}
else:
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target,-1, [0+0.1*self.S[0],0+0.1*self.S[1],0+0.1*self.S[2]],sim.simx_opmode_oneshot) #starting point is (0.3,0.3,0.3)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
# if (sim.simxGetCollisionHandle(self.clientID,))
return self.S,-1,False,{}
elif action==6:
# pdb.set_trace()
gripperAngle = 150
angle_displacement = (gripperAngle/2-45)*math.pi/180
err, joint_6 = sim.simxGetObjectHandle(self.clientID, "Fourbar_joint6", sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'actuateGripperSignal',sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'gripperAngle',sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',1,sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'gripperAngle',gripperAngle,sim.simx_opmode_oneshot)
while True:
current_joint6_angle = sim.simxGetJointPosition(self.clientID, joint_6, sim.simx_opmode_oneshot)
if abs(abs(current_joint6_angle[1])-abs(angle_displacement))<0.05*math.pi/180:
time.sleep(5)
break
oldPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target, self.copter, [0,0,0.5], sim.simx_opmode_oneshot)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
newPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
if np.linalg.norm(np.array(newPos[1])-np.array(oldPos[1]))>0.4 and np.linalg.norm(np.array(newPos)-np.array(oldPos))<0.5:
return self.S,100,True,{}
else:
return self.S,-100,True,{}
def reset(self):
# stop the simulation and restart the simulation
self.S = [0,0,9]
# sim.simxStartSimulation(self.clientID,sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID,self.target,-1,[0,0,1],sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID,self.sphere,-1,[0,0,0.175],sim.simx_opmode_oneshot)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
sim.simxSetIntegerSignal(self.clientID,'gripperAngle',90,sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
angle = sim.simxGetJointPosition(self.clientID,self.joint_6,sim.simx_opmode_oneshot)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02 and abs(angle[1])<0.5*math.pi/180:
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
time.sleep(5)
break
return self.S
def gripperOpen(self, flag):
vrep.simxSetIntegerSignal(self.clientId, 'RG2_open', flag,
vrep.simx_opmode_oneshot_wait)
time.sleep(0.5)
def clean():
vrep.simxSetIntegerSignal(clientID, "call_1", 0, opmode)
vrep.simxSetIntegerSignal(clientID, "call_2", 0, opmode)
vrep.simxSetIntegerSignal(clientID, "call_3", 0, opmode)
vrep.simxSetIntegerSignal(clientID, "call_4", 0, opmode)
vrep.simxSetIntegerSignal(clientID, "call_5", 0, opmode)
vrep.simxSetIntegerSignal(clientID, "call_6", 0, opmode)
def signal_switch(singal):
if singal == 1:
vrep.simxSetIntegerSignal(clientID, "call_1", 1,
opmode) #here might have a error
if singal == 2:
vrep.simxSetIntegerSignal(clientID, "call_2", 1, opmode)
if singal == 3:
vrep.simxSetIntegerSignal(clientID, "call_3", 1, opmode)
if singal == 4:
vrep.simxSetIntegerSignal(clientID, "call_4", 1, opmode)
if singal == 5:
vrep.simxSetIntegerSignal(clientID, "call_5", 1, opmode)
if singal == 6:
vrep.simxSetIntegerSignal(clientID, "call_6", 1, opmode)
if singal == 12:
vrep.simxSetIntegerSignal(clientID, 'call_1', 0,
opmode) #here might have a error
if singal == 11:
vrep.simxSetIntegerSignal(clientID, "call_2", 0, opmode)
if singal == 10:
vrep.simxSetIntegerSignal(clientID, "call_3", 0, opmode)
if singal == 9:
vrep.simxSetIntegerSignal(clientID, "call_4", 0, opmode)
if singal == 8:
vrep.simxSetIntegerSignal(clientID, "call_5", 0, opmode)
if singal == 7:
vrep.simxSetIntegerSignal(clientID, "call_6", 0, opmode)
# d.set_position(np.array([np.cos(time_start/2), np.sin(time_start/2.5), 0.7+0.2*np.cos(time_start/3)]))
# d.set_orientation(np.array([0, 0, np.pi/2*np.cos(time_start/5.0)]))
# d.set_orientation(np.array([time_start/10.0, 0, 0]))
r2.control(d2.get_position(), d2.get_quaternion(),
d2.get_orientation(), d2.get_velocity())
r2.log_time.append(time.time() - simulation_start)
r1.control(d1.get_position(), d1.get_quaternion(),
d1.get_orientation(), d1.get_velocity())
r1.log_time.append(time.time() - simulation_start)
# print(time.time() - time_start)
while time.time() - time_start < 0.05:
time.sleep(0.001)
r1.actuate(np.array([0] * len(r1.motors)))
r2.actuate(np.array([0] * len(r2.motors)))
sim.simxSetIntegerSignal(r2.client_id, "DummyLink", int(1),
sim.simx_opmode_blocking)
except KeyboardInterrupt:
r2.actuate([0 for i in range(len(r2.motors))])
r1.close_connection()
log_p1 = np.array(r1.log_p)
log_R1 = np.array(r1.log_R)
log_rpy1 = np.array(r1.log_rpy)
log_u1 = np.array(r1.log_u)
log_th1 = np.array(r1.log_th)
log_tor1 = np.array(r1.log_tor)
log_time1 = np.array(r1.log_time)
fig1 = plt.figure()
ax11 = fig1.add_subplot(231)
def send_master_status(self, master_status):
"""
announce a new evo master status
"""
sim.simxSetIntegerSignal(self.clientID, 'Evo-Master Status',
master_status, mode1)
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)
self.one_arm_mode = ''
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)
# get the sensor handles for proximity sensing
error_code, self.right_prox_sensor = vrep.simxGetObjectHandle(
self.clientID, 'Baxter_rightArm_proxSensor',
vrep.simx_opmode_oneshot_wait)
error_code, self.left_prox_sensor = vrep.simxGetObjectHandle(
self.clientID, 'Baxter_leftArm_proxSensor',
vrep.simx_opmode_oneshot_wait)
# initialize the sensors
_, self.right_state, _, self.right_object, _ = vrep.simxReadProximitySensor(
self.clientID, self.right_prox_sensor,
vrep.simx_opmode_streaming)
_, self.left_state, _, self.left_object, _ = vrep.simxReadProximitySensor(
self.clientID, self.left_prox_sensor,
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)
error, self.target_collision_table = vrep.simxGetCollisionHandle(
self.clientID, "object_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)
# start streaming the collision state information
# 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)
error_code, self.object_collision_state_table = vrep.simxReadCollision(
self.clientID, self.target_collision_table,
vrep.simx_opmode_streaming)
# enable vacuum cups
vrep.simxSetIntegerSignal(self.clientID, 'BaxterVacuumCup_active',
1, vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID,
'BaxterVacuumCup#0_active', 1,
vrep.simx_opmode_oneshot)
time.sleep(1)
else:
print('Failed connecting to remote API server')
sys.exit('Could not connect')
import sim as vrep
import math
import random
import time
import math
def moving(x,y):
a=0.4
b=0.4
c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5)
s=(a+b+c)/2
area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5)
h=area/(2*c)
deg1_base=math.atan(x/y)
if x<0 and y<0 :
deg_base1=deg1_base+math.pi
deg1_tri=math.asin(h/a)
deg1=deg1_base+deg1_tri
deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
deg3=deg2-deg1
vrep.simxSetJointTargetPosition(clientID,joint01,deg1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint02,-deg2,opmode)
vrep.simxSetJointTargetPosition(clientID,joint03,deg3,opmode)
print ('Start')
vrep.simxFinish(-1)
clientID = vrep.simxStart('192.168.0.10', 19997, True, True, 5000, 5)
if clientID != -1:
print ('Connected to remote API server')
def setIntegerSignal(self, integerString, value, ignoreError = False):
res = sim.simxSetIntegerSignal(self.clientID, integerString, value, sim.simx_opmode_oneshot)
if res!=sim.simx_return_ok and not ignoreError:
print('Failed to set integer signal {}'.format(integerString))
print(res)
def compress():
sim.simxSetIntegerSignal(clientID=clientID, signalName="compress", signalValue=1,
operationMode=sim.simx_opmode_blocking)