def applyActionAbsolute(self,action):
self.desiredWheelRotSpeed=self.absSpeed[action[0]]
self.desiredSteeringAngle=self.absAngle[action[1]]
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.ml,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.mr,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
steeringAngleLeft=math.atan(self.l/(-self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
steeringAngleRight=math.atan(self.l/(self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sl,steeringAngleLeft,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sr,steeringAngleRight,vrep.simx_opmode_oneshot)
def applyActionIncremental(self,action):
if action[0]!=0:
self.desiredWheelRotSpeed=self.desiredWheelRotSpeed+action[0]*self.wheelRotSpeedDx
if self.desiredWheelRotSpeed>10*self.wheelRotSpeedDx:
self.desiredWheelRotSpeed = 10*self.wheelRotSpeedDx
elif self.desiredWheelRotSpeed<-10*self.wheelRotSpeedDx:
self.desiredWheelRotSpeed = -10*self.wheelRotSpeedDx
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.ml,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetVelocity(self.clientID,self.mr,self.desiredWheelRotSpeed,vrep.simx_opmode_oneshot)
if action[1]!=0:
self.desiredSteeringAngle=self.desiredSteeringAngle+action[1]*self.steeringAngleDx
if self.desiredSteeringAngle>10*self.steeringAngleDx:
self.desiredSteeringAngle = 10*self.steeringAngleDx
elif self.desiredSteeringAngle<-10*self.steeringAngleDx:
self.desiredSteeringAngle = -10*self.steeringAngleDx
steeringAngleLeft=math.atan(self.l/(-self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
steeringAngleRight=math.atan(self.l/(self.d+self.l/math.tan(self.desiredSteeringAngle+0.00001)))
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sl,steeringAngleLeft,vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetJointTargetPosition(self.clientID,self.sr,steeringAngleRight,vrep.simx_opmode_oneshot)
result, current_frame_pos = vrep.simxGetObjectPosition(
clientID, frame_handle, robot_handle, vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get current frame current position')
print('Final position:', current_frame_pos)
# In[5]:
# Set the desired value for each joint variable
result, theta = vrep.simxGetJointPosition(clientID, joint_one_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print('current value of first joint variable: theta = {:f}'.format(theta))
vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta + 0.1,
vrep.simx_opmode_oneshot)
time.sleep(2)
result, theta = vrep.simxGetJointPosition(clientID, joint_one_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print('current value of first joint variable: theta = {:f}'.format(theta))
result, theta = vrep.simxGetJointPosition(clientID, joint_two_handle,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get first joint variable')
print('current value of second joint variable: theta = {:f}'.format(theta))
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, 0.6 + theta,
def JointControlWhileTakingPictures(clientID,motionProxy,i,Body, naoDirectory, visionSensorName):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
time.sleep(1)
#Head
while(vrep.simxGetConnectionId(clientID)!=-1):
#Getting joint's angles from Choregraphe (please check your robot's IP)
commandAngles = motionProxy.getAngles('Body', False)
#Allow the robot to move in VRep using choregraphe's angles
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
#Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
#Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
#Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
#Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
#Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
#Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_oneshot_wait)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (640,480), image_byte_array, "raw", "RGB", 0, 1)
#Save the image to disk
date_string = datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")
naoDirectory = 'D:\_dev\mo416-final-project\Images\Semaforo_verde'
file_path = naoDirectory + '\img_' + date_string + '.png'
print file_path
rotated_im = im.transpose(I.ROTATE_180)
rotated_im.save(file_path,"PNG")
print 'End of simulation'
def JointControl(clientID, motionProxy, postureProxy, i, Body):
# Head
# velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming)
pos = vrep.simxGetObjectPosition(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
angularPos = vrep.simxGetObjectOrientation(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
alphaPosRegister = []
betaPosRegister = []
gamaPosRegister = []
j = 0
li = []
while vrep.simxGetConnectionId(clientID) != -1:
for x in range(0, 150):
commandAngles = motionProxy.getAngles('Body', False)
j+=1
pos = vrep.simxGetObjectPosition(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
angularPos = vrep.simxGetObjectOrientation(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
alphaPosRegister.append(angularPos[0])
betaPosRegister.append(angularPos[1])
gamaPosRegister.append(angularPos[2])
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
# Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
# Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
# Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
# Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
# Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
# Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
motionProxy.stopMove()
postureProxy.stopMove()
# postureProxy.goToPosture("Stand",0.5)
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
break
print 'End of simulation'
print 'Final x pos'
print pos
calculos = {}
calcular(alphaPosRegister,calculos)
desvpadA = calculos['desvio_padrao']
calculos = {}
calcular(betaPosRegister,calculos)
desvpadB = calculos['desvio_padrao']
calculos = {}
calcular(gamaPosRegister,calculos)
desvpadG = calculos['desvio_padrao']
return [(10*pos[0] - (desvpadA + desvpadB + desvpadG)/3), pos[0]]
# simExtRemoteApiStart(19999)
#
# then start simulation, and run this program.
#
# IMPORTANT: for each successful call to simxStart, there
# should be a corresponding call to simxFinish at the end!
import vrep
import time
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
if clientID!=-1:
print('Connected to remote API server')
#res,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait)
#if res==vrep.simx_error_noerror:
# print('Number of objects in the scene: ',len(objs))
#else:
# print('Remote API function call returned with error code: ',res)
res,jh=vrep.simxGetObjectHandle(clientID, "arm_joint_1", vrep.simx_opmode_oneshot_wait)
print("Joint Handle " + str(jh))
res = vrep.simxSetJointTargetPosition(clientID, jh, 0.5, vrep.simx_opmode_oneshot)
print("Result: " + str(res))
time.sleep(1)
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
errorCode,joint_Handle1=vrep.simxGetObjectHandle(clientID,"LBR4p_joint2",vrep.simx_opmode_oneshot_wait) # Handling of Joints errorCode,joint_Handle2=vrep.simxGetObjectHandle(clientID,"LBR4p_joint4",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle3=vrep.simxGetObjectHandle(clientID,"LBR4p_joint6",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle4=vrep.simxGetObjectHandle(clientID,"LBR4p_joint7",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle12=vrep.simxGetObjectHandle(clientID,"LBR4p_joint5",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle11=vrep.simxGetObjectHandle(clientID,"LBR4p_joint1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle5=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle6=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle7=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_0",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle8=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_0",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle9=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_2",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle10=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_2",vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(65),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle2,math.radians(170),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle3,math.radians(90),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle12,math.radians(90),vrep.simx_opmode_oneshot_wait)#gives position to the joint5 vrep.simxSetIntegerSignal(clientID,"closing_signal",1,vrep.simx_opmode_oneshot_wait)# sends signal to child script errorCode,joint=vrep.simxGetJointPosition(clientID,joint_Handle7,vrep.simx_opmode_oneshot_wait)# get the position of the joint7 time.sleep(5) while joint >= 0.0104653835297: errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(5),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle11,math.radians(90),vrep.simx_opmode_oneshot_wait) break;
def set_joint_angles(self, angles):
assert len(angles) == 9
for joint, angle in zip(self.joints, angles):
rc = vrep.simxSetJointTargetPosition(self.client_id, joint, angle,
vrep.simx_opmode_streaming)
def obj_set_position_target(self, handle, angle):
return self.RAPI_rc(
vrep.simxSetJointTargetPosition(self.cID, handle,
-np.deg2rad(angle), self.opM_set))
i = random.uniform(0, 360)
j = random.uniform(0, 90)
fi = i * m.pi / 180
alf = j * m.pi / 180
x = r * m.cos(alf) * m.cos(fi)
y = r * m.cos(alf) * m.sin(fi)
z = 0.1 + r * m.sin(alf)
T = [x, y, z]
with open(FILENAME, "r", newline="") as file:
reader = csv.reader(file)
for row in reader:
coord = list(map(float, row))
results.append(((coord[0] - T[0])**2 + (coord[1] - T[1])**2 +
(coord[2] - T[2])**2)**0.5)
joints.append([coord[3], coord[4], coord[5], coord[6]])
n = results.index(min(results))
q = joints[n]
errorCode = vrep.simxSetJointTargetPosition(clientID, joint_1, q[0],
vrep.simx_opmode_oneshot_wait)
errorCode = vrep.simxSetJointTargetPosition(clientID, joint_2, q[1],
vrep.simx_opmode_oneshot_wait)
errorCode = vrep.simxSetJointTargetPosition(clientID, joint_3, q[2],
vrep.simx_opmode_oneshot_wait)
errorCode = vrep.simxSetJointTargetPosition(clientID, joint_4, q[3],
vrep.simx_opmode_oneshot_wait)
print(results[n], q, time.process_time())
def loop(self):
lastCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000 # 记录当前时间
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
t = 0
while t < np.pi * 2:
currCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000
dt = currCmdTime - lastCmdTime
t = t + dt
vrep.simxPauseCommunication(self.clientID, True)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[0],
np.sin(t),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[1], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[2], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
time.sleep(2)
t = 0
while t < np.pi * 2:
currCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000
dt = currCmdTime - lastCmdTime
t = t + dt
vrep.simxPauseCommunication(self.clientID, True)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[0], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[1],
np.sin(t),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[2], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
def step(self, action): # action 是速度：rad/s
currentPosition = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, currentPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
action = np.clip(action, *self.action_bound)
self.JointPosition = currentPosition + (action * 1 +
0 * self.action) * self.dt
self.action = action
vrep.simxPauseCommunication(self.clientID, True)
for i in range(self.jointNum):
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[i],
self.JointPosition[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
#获取转移状态包括 关节位置，速度，末端距离目标的差
s = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, s[i] = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
_, pos = vrep.simxGetObjectPosition(self.clientID, self.end_handle,
self.goal_handle,
vrep.simx_opmode_oneshot)
del pos[1]
pos = np.array(pos)
s = np.hstack(
(np.exp(s) / sum(np.exp(s)), np.exp(pos) / sum(np.exp(pos))))
linkPos = []
for i in range(self.jointNum):
_, linkpos = vrep.simxGetObjectPosition(
self.clientID, self.link_handle[i], self.goal_handle,
vrep.simx_opmode_oneshot) # link1 位置
del linkpos[1]
linkPos += linkpos
linkPos = np.array(linkPos)
s = np.hstack((s, np.exp(linkPos) / sum(np.exp(linkPos))))
_, d = vrep.simxReadDistance(self.clientID, self.end_dis_handle,
vrep.simx_opmode_oneshot)
# Ra = -np.sqrt(np.sum(action**2))
_, do = vrep.simxReadDistance(self.clientID, self.dist_handle,
vrep.simx_opmode_oneshot)
Ro = -(self.d_ref / (self.d_ref + do))**8
if d < self.delta:
Rt = -0.5 * d * d
else:
Rt = -self.delta * (d - 0.5 * self.delta)
r = Rt * 1000 + Ro * 250 #DDPG 取100，Naf取500
danger = False
if do < 0.02:
danger = True
r -= 100
else:
danger = False
if d < 0.05 and self.get_point == False:
self.grab_counter += 1
if self.grab_counter > 80:
r += 100
self.get_point = True
r += 20
else:
self.get_point = False
self.grab_counter = 0
return s, r, danger, self.get_point
def reset(self):
# self.lastCmdTime = vrep.simxGetLastCmdTime(self.clientID) # 记录当前时间 单位：秒
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
vrep.simxGetPingTime(self.clientID)
# self.currCmdTime = vrep.simxGetLastCmdTime(self.clientID)
vrep.simxPauseCommunication(self.clientID, True)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[0], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[1], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[2], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
# self.lastCmdTime = self.currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
self.JointPosition = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, self.JointPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
while abs(self.JointPosition[0]) > 1e-4 or abs(
self.JointPosition[1]) > 1e-4 or abs(
self.JointPosition[2]) > 1e-4:
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
for i in range(self.jointNum):
_, self.JointPosition[i] = vrep.simxGetJointPosition(
self.clientID, self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
# print("resetting...")
# print("ready...")
# 获取转移状态
s = np.zeros(3, dtype=float)
for i in range(self.jointNum):
_, s[i] = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_oneshot)
_, pos = vrep.simxGetObjectPosition(self.clientID, self.end_handle,
self.goal_handle,
vrep.simx_opmode_oneshot)
del pos[1]
pos = np.array(pos)
s = np.hstack(
(np.exp(s) / sum(np.exp(s)), np.exp(pos) / sum(np.exp(pos))))
linkPos = []
for i in range(self.jointNum):
_, linkpos = vrep.simxGetObjectPosition(
self.clientID, self.link_handle[i], self.goal_handle,
vrep.simx_opmode_oneshot) # link1 位置
del linkpos[1]
linkPos += linkpos
linkPos = np.array(linkPos)
s = np.hstack((s, np.exp(linkPos) / sum(np.exp(linkPos))))
self.get_point = False
self.grab_counter = 0
_, self.dis = vrep.simxReadDistance(self.clientID, self.end_dis_handle,
vrep.simx_opmode_oneshot)
self.action = np.zeros(3, dtype=float)
return s
vrep.simx_opmode_blocking)[1])
objHandleRightTheoretical = int(
vrep.simxGetObjectHandle(clientID, 'ReferenceFrame0',
vrep.simx_opmode_blocking)[1])
#zero position
r = 0
rpose = rightArmPose(0, r, r, r, r, r, r, r)
l = 0
lpose = leftArmPose(0, l, l, l, l, l, l, l)
moveObj(rpose, clientID, objHandleRightTheoretical)
moveObj(lpose, clientID, objHandleLeftTheoretical)
for i in range(0, 7):
vrep.simxSetJointTargetPosition(clientID, rightArm[i], math.radians(r),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, leftArm[i], math.radians(l),
vrep.simx_opmode_oneshot)
time.sleep(3)
#first position
r = 0
rpose = rightArmPose(0, -45, r, r, r, r, r, r)
l = 0
lpose = leftArmPose(0, 45, l, l, l, l, l, l)
moveObj(rpose, clientID, objHandleRightTheoretical)
moveObj(lpose, clientID, objHandleLeftTheoretical)
time.sleep(3)
print "----- Evaluation of "+ individual.name +"started -----"
vrep.simxStartSimulation(clientID, opmode)
theTime = time.time()
# Start getting the robot position
pret, robotPos = vrep.simxGetObjectPosition(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
pret4, leftWheelPos = vrep.simxGetObjectPosition(clientID, leftWheelHandle, -1, vrep.simx_opmode_streaming)
pret5, rightWheelPos = vrep.simxGetObjectPosition(clientID, rightWheelHandle, -1, vrep.simx_opmode_streaming)
print "2w1a position: (x = " + str(robotPos[0]) + ", y = " + str(robotPos[1]) + ")"
# Start getting the robot orientation
oret, robotOrient = vrep.simxGetObjectOrientation(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
print "2w1a orientation: (x = " + str(robotOrient[0]) + \
", y = " + str(robotOrient[1]) +\
", z = " + str(robotOrient[2]) + ")"
for gene in individual.genes:
vrep.simxSetJointTargetPosition(clientID, gene.type, math.radians(gene.action), opmode)
pgene = vrep.simxGetJointPosition(clientID, gene.type, opmode)
#time.sleep(0.05)
# Wait in order to let the motors finish their movements
# timer = time.time()
# while True:
# pgene = vrep.simxGetJointPosition(clientID, gene.type, opmode)
# if round(math.degrees(pgene[1]), 0) == round(gene.action, 0):
# break
# else:
# time.sleep(0.01)
# timer += 0.01
# if time.time() >= timer + 3:
while trainCount < train_frames:
#Collision handler
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_buffer)
errorCode, collisionState2=vrep.simxReadCollision(clientID,collision_handle2,vrep.simx_opmode_buffer)
errorCode, collisionState3=vrep.simxReadCollision(clientID,collision_handle3,vrep.simx_opmode_buffer)
if collisionState1 == 1 or collisionState2==1 or collisionState3==1:
if maxroute < driveCount and trainCount > observe:
maxroute = driveCount
# Log the car's distance at this T.
data_collect.append([trainCount, maxroute])
driveCount = 0
reward = PUNISH
errorCode=vrep.simxSetJointTargetVelocity(clientID,motor_handle,fallback_speed, vrep.simx_opmode_streaming)
errorCode=vrep.simxSetJointTargetPosition(clientID,steer_handle, fallback_angle, vrep.simx_opmode_streaming)
time.sleep(fallback_sec)
errorCode=vrep.simxSetJointTargetVelocity(clientID,motor_handle,target_speed, vrep.simx_opmode_streaming)
errorCode=vrep.simxSetJointTargetPosition(clientID,steer_handle, 0, vrep.simx_opmode_streaming)
trainCount += 1
if trainCount > observe:
driveCount += 1
# Choose an action.
if random.random() < epsilon or trainCount < observe:
action = np.random.randint(0, ACT_OUTPUT) # random
else:
# Get Q values for each action.
qval = model.predict(state, batch_size=1)
action = (np.argmax(qval)) # best
def move_robot (clientID, theta, robo, robot_handles,ref_object_handles):
(T_1in0, R_1in0, end_eulerAngles, end_position) = forward_kinematics(theta,robo)
#robot_handles = get_robot_handles(clientID, robo)
# Get "handle" to WorldReferenceFrame
#result, WorldReferenceFrame_handle = vrep.simxGetObjectHandle(clientID, 'T_1in0_ReferenceFrame',
# vrep.simx_opmode_blocking)
#if result != vrep.simx_return_ok:
# raise Exception('could not get object handle for world reference frame')
# Get "handle" to T_1in0_ReferenceFrame
#result, T_1in0_ReferenceFrame_handle = vrep.simxGetObjectHandle(clientID, 'T_1in0_ReferenceFrame',
# vrep.simx_opmode_blocking)
#if result != vrep.simx_return_ok:
# raise Exception('could not get object handle for T_1in0 reference frame')
# Get "handle" to UR3_0_ReferenceFrame
#result, UR3_0_ReferenceFrame_handle = vrep.simxGetObjectHandle(clientID, 'UR3_0_ReferenceFrame',
# vrep.simx_opmode_blocking)
#if result != vrep.simx_return_ok:
# raise Exception('could not get object handle for UR3_0 reference frame')
# Wait two seconds
#time.sleep(2)
if robo == 0:
# Set T_1in0_ReferenceFrame position and orientation
vrep.simxSetObjectPosition(clientID, ref_object_handles[1], ref_object_handles[0], end_position,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, ref_object_handles[1], ref_object_handles[0], end_eulerAngles,
vrep.simx_opmode_oneshot)
elif robo == 1:
# Set T_1in0_ReferenceFrame position and orientation
vrep.simxSetObjectPosition(clientID, ref_object_handles[1], ref_object_handles[2], end_position,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, ref_object_handles[1], ref_object_handles[2],
end_eulerAngles,
vrep.simx_opmode_oneshot)
else:
raise Exception('Problem with robot number')
#vrep.simxPauseCommunication(clientID, True)
#targetVelocity = 50
# Position all the joint angles according to user input
# Set the desired value of the first joint variable
#vrep.simxSetJointTargetVelocity(clientID, robot_handles[0], targetVelocity, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, robot_handles[0], theta[0], vrep.simx_opmode_streaming)
#time.sleep(2) # Wait two seconds
# Set the desired value of the second joint variable
#vrep.simxSetJointTargetVelocity(clientID, robot_handles[1], targetVelocity, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, robot_handles[1], theta[1], vrep.simx_opmode_streaming)
#time.sleep(2) # Wait two seconds
# Set the desired value of the third joint variable
#vrep.simxSetJointTargetVelocity(clientID, robot_handles[2], targetVelocity, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, robot_handles[2], theta[2], vrep.simx_opmode_streaming)
#time.sleep(2) # Wait two seconds
# Set the desired value of the fourth joint variable
#vrep.simxSetJointTargetVelocity(clientID, robot_handles[3], targetVelocity, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, robot_handles[3], theta[3], vrep.simx_opmode_streaming)
#time.sleep(2) # Wait two seconds
# Set the desired value of the fifth joint variable
#vrep.simxSetJointTargetVelocity(clientID, robot_handles[4], targetVelocity, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, robot_handles[4], theta[4], vrep.simx_opmode_streaming)
#time.sleep(2) # Wait two seconds
# Set the desired value of the sixth joint variable
#vrep.simxSetJointTargetVelocity(clientID, robot_handles[5], targetVelocity, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, robot_handles[5], theta[5], vrep.simx_opmode_streaming)
time.sleep(2) # Wait two seconds
def set_joint_position(self, handle, angle):
"""
Set a simulated joint identified by a [handle] to a specific [angle]
"""
vrep.simxSetJointTargetPosition(self.client_id, handle, angle,
vrep.simx_opmode_oneshot)
theta_goal5 = theta5 + (np.pi / 2)
theta_goal6 = theta6 + (np.pi / 2)
M = forwardKinematics.forwardKinematicsUr3(
np.array([
theta_goal1, theta_goal2, theta_goal3, theta_goal4, theta_goal5,
theta_goal6
]))
objHand, result = vrep.simxGetObjectHandle(clientID, 'frame',
vrep.simx_opmode_blocking)
result = vrep.simxSetObjectPosition(clientID, objHand, -1,
(M[0, 3], M[1, 3], M[2, 3]),
vrep.simx_opmode_blocking)
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_one_handle,
theta1 + (np.pi / 2), vrep.simx_opmode_oneshot)
time.sleep(2)
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_two_handle,
theta2 + -(np.pi / 2),
vrep.simx_opmode_oneshot)
time.sleep(2)
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_three_handle,
theta3 + (np.pi / 2), vrep.simx_opmode_oneshot)
time.sleep(2)
# Set the desired value of the first joint variable
vrep.simxSetJointTargetPosition(clientID, joint_four_handle,
theta4 + (np.pi / 2), vrep.simx_opmode_oneshot)
time.sleep(2)
# Set the desired value of the first joint variable
def setJointObjectPosition(self, obj, rotation):
return vrep.simxSetJointTargetPosition(self.clientID, obj, math.radians(rotation), self.opmode)
jointhandles.append(
vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)[1])
i = i + 1
errorcode, quad = vrep.simxGetObjectHandle(clientID, 'quad_body',
vrep.simx_opmode_blocking)
k = 0
while k <= 10:
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, jointhandles[i],
cur_genome[i + 16],
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, jointhandles[i],
cur_genome[i],
vrep.simx_opmode_streaming)
time.sleep(1)
for i in range(4, 8):
vrep.simxSetJointTargetVelocity(clientID, jointhandles[i],
cur_genome[i + 16],
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, jointhandles[i],
cur_genome[i],
vrep.simx_opmode_streaming)
time.sleep(1)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(clientID, jointhandles[i],
cur_genome[i + 24],
vrep.simx_opmode_streaming)
def JointControl2(clientID,i,Body, commandAngles):
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
#Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[6],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[7],vrep.simx_opmode_streaming)
#Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[13],vrep.simx_opmode_streaming)
#Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[18],vrep.simx_opmode_streaming)
#Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[19],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[23],vrep.simx_opmode_streaming)
#Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
#Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[27],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[27],vrep.simx_opmode_streaming)
def invLeftArm(xl, yl, zl):
#start at zero position
r = 0
rpose = rightArmPose(0, r, r, r, r, r, r, r)
l = 0
lpose = leftArmPose(0, l, l, l, l, l, l, l)
moveObj(rpose, clientID, objHandleRightTheoretical)
moveObj(lpose, clientID, objHandleLeftTheoretical)
for i in range(0, 7):
vrep.simxSetJointTargetPosition(clientID, rightArm[i], math.radians(r),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, leftArm[i], math.radians(l),
vrep.simx_opmode_oneshot)
leftTip = int(
vrep.simxGetObjectHandle(clientID, 'Baxter_leftArm_tip',
vrep.simx_opmode_blocking)[1])
T_2in0 = np.array([[1, 0, 0, xl], [0, 1, 0, yl], [0, 0, 1, zl],
[0.00000000, 0.00000000, 0.00000000, 1.00000000]])
moveObj(T_2in0, clientID, objHandleLeftTheoretical)
LeftLimits = [(math.radians(-168), math.radians(-168 + 336)),
(math.radians(-96.5), math.radians(-96.5 + 191)),
(math.radians(-121), math.radians(-121 + 179)),
(math.radians(-173), math.radians(-173 + 346)),
(math.radians(0), math.radians(0 + 148)),
(math.radians(-173.3), math.radians(-173.3 + 346.5)),
(math.radians(-88), math.radians(-88 + 206)),
(math.radians(-173.3), math.radians(-173.3 + 346.5))]
SL = leftArmS()
thetaL = np.zeros((SL.shape[1], 1))
thetadot = np.array([100])
TL_1in0 = ML
J = Jmaker(thetaL, SL)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
V = nl.inv(admaker(TL_1in0)) @ V0
while (True):
failCount = 0
count = 0
while nl.norm(V) >= .01 and nl.norm(thetadot) >= .0001:
thetadot = td(J, V0, thetaL)
thetaL = thetaL + thetadot * 1
TL_1in0 = TtoM(thetaL, SL, ML)
J = Jmaker(thetaL, SL)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
V = nl.inv(admaker(TL_1in0)) @ V0
count = count + 1
if (count == 60):
print(
"Norm of V could not merge - Using new starting position and trying again"
)
print()
thetaL = np.array([[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)]])
thetadot = np.array([100])
TL_1in0 = TtoM(thetaL, SL, ML)
J = Jmaker(thetaL, SL)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
V = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
count = 0
#checking for how many times this happens. So we can detect if a position is unreachable
failCount = failCount + 1
if (failCount >= 7):
print("Position Unreachable")
print()
return
# print(repr(thetaL))
#checking if the angles are within limit of their respective joints
if (check(thetaL, LeftLimits)):
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID, rotJoint, thetaL[0],
vrep.simx_opmode_oneshot)
for i in range(0, 7):
vrep.simxSetJointTargetPosition(clientID, leftArm[i],
thetaL[i + 1],
vrep.simx_opmode_oneshot)
time.sleep(.5)
#leftPose = leftArmPose(thetaL[0], thetaL[1], thetaL[2],thetaL[3],thetaL[4],thetaL[5],thetaL[6],thetaL[7],)
time.sleep(.5)
leftEndTip = vrep.simxGetObjectPosition(
clientID, leftTip, -1, vrep.simx_opmode_blocking)[1]
if (nl.norm(T_2in0[:3, 3] - np.array(leftEndTip)) > 0.01):
#checking final pose, in case any obsticles got in the way (even though joint angles were okay.)
print(
"Hmmmm, the end of the baxter arm is not where it should be. Collision detected"
)
print("Difference: " +
str(nl.norm(T_2in0[:3, 3] - np.array(leftEndTip))))
print()
thetaL = np.array([[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)]])
thetadot = np.array([100])
TL_1in0 = TtoM(thetaL, SL, ML)
J = Jmaker(thetaL, SL)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
V = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
continue
print("BINGO! Position Achieved!")
print()
time.sleep(3)
break
else:
print(
"Came up with set of thetas where some of them where out of range, trying again with different starting position"
)
print()
#try different starting orientation to get different end angles that will hopefully meet the angle limits of the joints
thetaL = np.array([[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)],
[random.uniform(-3.14, 3.14)]])
thetadot = np.array([100])
TL_1in0 = TtoM(thetaL, SL, ML)
J = Jmaker(thetaL, SL)
V0 = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
V = dvskew(sl.logm(T_2in0.dot(nl.inv(TL_1in0))))
assert client_id != -1, "Failed connecting to remote API server"
e = vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
# print ping time
sec, msec = vrep.simxGetPingTime(client_id)
print "Ping time: %f" % (sec + msec / 1000.0)
# Handles of body and wheels
e, body = vrep.simxGetObjectHandle(client_id, "body", vrep.simx_opmode_oneshot_wait)
e, joint_0 = vrep.simxGetObjectHandle(client_id, "joint_0", vrep.simx_opmode_oneshot_wait)
e, joint_1 = vrep.simxGetObjectHandle(client_id, "joint_1", vrep.simx_opmode_oneshot_wait)
# Set the joint angles to the default position (0, 0)
e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0, vrep.simx_opmode_streaming)
e = vrep.simxSetJointTargetPosition(client_id, joint_1, 0, vrep.simx_opmode_streaming)
# this line is necessary to enable position recording
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_streaming)
# Wait until the robot is settled to the default position
time.sleep(0.3)
# enable synchronous mode
vrep.simxSynchronous(client_id, True)
position_history = []
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
joint_pos_history = []
def simple():
# obtem os handlers. Um Handler eh um numero que identifica um componente, como, por exemplo, uma junta
res, nao = vrep.simxGetObjectHandle(clientID, "NAO", vrep.simx_opmode_blocking)
res, shL = vrep.simxGetObjectHandle(clientID, "LShoulderPitch3", vrep.simx_opmode_blocking)
res, shR = vrep.simxGetObjectHandle(clientID, "RShoulderPitch3", vrep.simx_opmode_blocking)
res, kneeR = vrep.simxGetObjectHandle(clientID, "RKneePitch3", vrep.simx_opmode_blocking)
res, kneeL = vrep.simxGetObjectHandle(clientID, "LKneePitch3", vrep.simx_opmode_blocking)
res, hipPitchL = vrep.simxGetObjectHandle(clientID, "LHipPitch3", vrep.simx_opmode_blocking)
res, hipPitchR = vrep.simxGetObjectHandle(clientID, "RHipPitch3", vrep.simx_opmode_blocking)
res, hipYawPitchL = vrep.simxGetObjectHandle(clientID, "LHipYawPitch3", vrep.simx_opmode_blocking)
res, hipYawPitchR = vrep.simxGetObjectHandle(clientID, "RHipYawPitch3", vrep.simx_opmode_blocking)
res, anklePitchL = vrep.simxGetObjectHandle(clientID, "LAnklePitch3", vrep.simx_opmode_blocking)
res, anklePitchR = vrep.simxGetObjectHandle(clientID, "RAnklePitch3", vrep.simx_opmode_blocking)
while True:
# Envia comandos para as juntas
vrep.simxSetJointTargetPosition(clientID, kneeR, 1.2,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, anklePitchR, 0.4,vrep.simx_opmode_oneshot)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID, hipYawPitchL, -0.3,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, hipPitchR, -0.1,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, hipYawPitchR, -0.3,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, kneeR, -0.1,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, anklePitchR, 0,vrep.simx_opmode_oneshot)
print vrep.simxGetObjectPosition(clientID, nao, -1, vrep.simx_opmode_blocking)
reset_simulation(clientID)
def JointControl(clientID,motionProxy,i,Body):
#Head
while(vrep.simxGetConnectionId(clientID)!=-1):
#Getting joint's angles from Choregraphe (please check your robot's IP)
commandAngles = motionProxy.getAngles('Body', False)
#Allow the robot to move in VRep using choregraphe's angles
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
#Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
#Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
#Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
#Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
#Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
#Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
print 'End of simulation'
def JointControl(clientID,motionProxy,i,Body,sample_time):
#Head
while(vrep.simxGetConnectionId(clientID)!=-1):
start = time.time()
#Getting joint's angles from Choregraphe (please check your robot's IP)
commandAngles = motionProxy.getAngles('Body', False)
#Allow the robot to move in VRep using choregraphe's angles
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
#Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
#Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
#Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
#Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
#Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
#Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
end = time.time()
dt = end - start
if dt < sample_time:
time.sleep(sample_time - dt)
else:
print "Can't keep a period (%gs>%gs)" % (dt,sample_time)
print 'End of simulation'
# Get the robot position before the movement sequence
pret, lOriginPos = vrep.simxGetObjectPosition(clientID, leftWheelHandle, -1, vrep.simx_opmode_streaming)
pret, rOriginPos = vrep.simxGetObjectPosition(clientID, rightWheelHandle, -1, vrep.simx_opmode_streaming)
msg = "2w1a origin position: leftWheel = " + str(lOriginPos) +\
", rightWheel = " + str(rOriginPos) + ")"
print msg
logging.info(msg)
# The control functions use Radians to determine the target position.
# Here, we use maths.radians to convert degrees into radians.
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxSetJointTargetPosition
msg = "Motors target positions: " + str(ashoulder) + " " + str(aelbow) + " " + str(awrist)
print msg
logging.info(msg)
vrep.simxSetJointTargetPosition(clientID, wristHandle, math.radians(awrist), opmode)
vrep.simxSetJointTargetPosition(clientID, elbowHandle, math.radians(aelbow), opmode)
vrep.simxSetJointTargetPosition(clientID, shoulderHandle, math.radians(ashoulder), opmode)
# Wait in order to let the motors finish their movements
# Tip: there must be a more efficient way to do it...
time.sleep(3)
# Get the motors effective positions after the movement sequence
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetJointPosition
pwrist = vrep.simxGetJointPosition(clientID, wristHandle, opmode)
pelbow = vrep.simxGetJointPosition(clientID, elbowHandle, opmode)
pshoulder = vrep.simxGetJointPosition(clientID, shoulderHandle, opmode)
msg = "Motors reached positions: " + str(pshoulder) + " " + str(pelbow) + " " + str(pwrist)
#print msg
c = rg + by
sal = 1./3 * (saliency.N(intensty) + saliency.N(c) + saliency.N(gabor))
sal = cv2.resize(sal, dsize=(img.shape[1],img.shape[0]))
#sal = (sal + magno)/2
sal = sal.astype(np.uint8)
# finding the most salient point
#max_sal_arg = np.unravel_index(np.argmax(sal), sal.shape)
#print(max_sal_arg)
cv2.imshow('static sal', sal)
cv2.waitKey(2)
# move joints
vrep.simxSetJointTargetPosition(clientID, joint_z,\
get_x_joint_pos(np.pi/4, 256, max_sal_arg[0]),\
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, joint_x,\
get_y_joint_pos(np.pi/4, 256, max_sal_arg[1]),\
vrep.simx_opmode_oneshot)
# segmentation
segm = ml.stochastic_reg_grow(img, (105, 110), 15, 15, 6,\
30, 20)
cv2.imshow('segm', segm)
cv2.waitKey(10)
vrep.simxFinish(clientID)
