def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1,
gearHandle2):
RAD = 180 / math.pi
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1:
count += 1
if count > 20:
break
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1,
vrep.simx_opmode_buffer)
ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2,
vrep.simx_opmode_buffer)
degree1 = round(gear_pos1 * RAD, 2)
degree2 = round(gear_pos2 * RAD, 2)
print(degree1, degree2)
if abs(degree1 - target_degree1) < 0.5 and abs(degree2 -
target_degree2) < 0.5:
break
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, gearHandle1,
target_degree1 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, gearHandle2,
target_degree2 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
def getDroneImage(clientID, height):
position = np.asarray([0, 0, height])
# Get Drone Handle
errorCode, droneHandle = vrep.simxGetObjectHandle(
clientID, 'drone', vrep.simx_opmode_oneshot_wait)
# Set Drone Height (in meters)
vrep.simxSetObjectPosition(clientID, droneHandle, -1, position,
vrep.simx_opmode_oneshot_wait)
# Get Drone Camera Image (First Call)
errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage(
clientID, droneHandle, 0, vrep.simx_opmode_streaming)
count = 0
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in drone camera image (Note operating mode changed to buffer)
errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage(
clientID, droneHandle, 0, vrep.simx_opmode_buffer)
count += 1
# quit after 5 images read (first images may be empty)
if (errorDrone == vrep.simx_return_ok and count >= 1):
droneImage = np.array(droneImage, dtype=np.uint8)
droneImage.resize([resolution[1], resolution[0], 3])
break
return resolution, droneImage
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
#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_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
print 'End of Simulation'
def simulate(self):
plt.xlabel('time')
plt.ylabel('ditance, meters')
while vrep.simxGetConnectionId(self.clientID) != -1:
# получаем данные с сенсоров
(errorCode, sensorState, sensorDetection, detectedObjectHandle,
detectedSurfaceNormalVectorUp) = vrep.simxReadProximitySensor(
self.clientID, self.sonic_sensor, vrep.simx_opmode_streaming)
(errorCode, frontState, frontDetection, detectedObjectHandle,
detectedSurfaceNormalVectorFr) = vrep.simxReadProximitySensor(
self.clientID, self.front_sonic_sensor,
vrep.simx_opmode_streaming)
if (frontState and sensorState):
# есть данные с двух сенсоров
self.calulate(sensorState,
min(sensorDetection[2], frontDetection[2]))
self.dists.append(min(sensorDetection[2], frontDetection[2]))
elif (frontState):
# данные только с переднего сенсора
self.calulate(frontState, frontDetection[2])
self.dists.append(frontDetection[2])
elif (sensorState):
# данные только с бокового сенсора
self.calulate(sensorState, sensorDetection[2])
self.dists.append(sensorDetection[2])
else:
# нет данных
self.calulate(sensorState, self.reqDist + 0.1)
self.dists.append(self.reqDist + 0.1)
self.timeLine.append(time.clock())
# добавляем точку на график в реальном времени
plt.scatter(self.timeLine[-1], self.dists[-1])
plt.pause(0.05)
time.sleep(0.12)
plt.show()
def analysisVideo(vision_sensor_name, client_id, config,
handle_video_coordlist):
res1, visionSensorHandle = vrep.simxGetObjectHandle(
client_id, vision_sensor_name, vrep.simx_opmode_oneshot_wait)
# config["vision_sensor_handle"] = visionSensorHandle
vrep.simxGetVisionSensorImage(client_id, visionSensorHandle, 0,
vrep.simx_opmode_streaming)
time.sleep(1)
while (vrep.simxGetConnectionId(client_id) != -1):
# if visionSensorHandle != config["vision_sensor_handle"]:
# continue
# print "config_handle:", config["vision_sensor_handle"]
# print "handle:", visionSensorHandle
res, resolution, image = vrep.simxGetVisionSensorImage(
client_id, visionSensorHandle, 0, vrep.simx_opmode_buffer)
while (not image):
pass
if len(resolution) == 0:
continue
image_byte_array = array.array('b', image)
_img = I.frombuffer("RGB", (resolution[0], resolution[1]),
image_byte_array, "raw", "RGB", 0, 1)
im = np.array(_img)[::-1, :, ::-1].copy()
coordlist = detect(im) # 检测目标物体坐标
handle_video_coordlist(coordlist, config)
# if config["play_video"]:
cv2.imshow(config["video_label"], im)
if cv2.waitKey(15) & 0xFF == 27: # Press ESC to exit :)
break
print 'End of Analysis'
def compassRobot(clientID):
# print('Getting Robot GPS Location')
# Get Robot GPS Handle
errorCode, gpsHandle = vrep.simxGetObjectHandle(
clientID, 'GPS', vrep.simx_opmode_oneshot_wait)
orientationError, orientation = vrep.simxGetObjectOrientation(
clientID, gpsHandle, -1, vrep.simx_opmode_streaming)
# loop while connected to simulation
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in camera information (Note operating mode changed to buffer)
orientationError, orientation = vrep.simxGetObjectOrientation(
clientID, gpsHandle, -1, vrep.simx_opmode_buffer)
orientation = math.degrees(orientation[2])
if orientation < 0:
orientation += 360
errorCodes = [orientationError]
# Check if have both image and xyz coordinate data available
if (all(errors == vrep.simx_return_ok for errors in errorCodes)):
# print ("Pose OK!!!")
break
elif (all(errors == vrep.simx_return_novalue_flag
for errors in errorCodes)):
# print ("Pose Not Available Yet")
pass
else:
print(errorCodes)
return orientation
def collsion_Read():
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
for i in collision_handle:
check = []
while (vrep.simxGetConnectionId(clientID[0]) != -1):
err, collision = vrep.simxReadCollision(
clientID[0], i, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
print('collision information imported')
del check[:]
check.append(collision)
break
elif err == vrep.simx_return_novalue_flag:
print("no collision information")
else:
print('Error')
print(err)
if (check[0] == 1):
print('Collision detected')
return 1
print('No Collision')
return 0
def getimageinformation(sensor_index):
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
if (sensor_index == 1 or sensor_index == 2):
while (vrep.simxGetConnectionId(clientID[0]) != -1):
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID[0], vision_sensor[sensor_index - 1], 0,
vrep.simx_opmode_buffer)
# Save image to txt file => But it have a problem
if err == vrep.simx_return_ok:
# save image information list
print('vision information imported')
image_byte_array = np.asarray(image).astype('uint8')
result_image = Im.frombuffer(
"RGB", (resolution[0], resolution[1]),
image_byte_array, "raw", "RGB", 0, 1)
return result_image
elif err == vrep.simx_return_novalue_flag:
print("no vision information")
else:
print('Error')
print(err)
else:
print('Wrong index number')
def getEndEffector(): # only for objects
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
Object_Handle = end_effector[0]
while (vrep.simxGetConnectionId(clientID[0]) != -1):
err, Output_Postion = vrep.simxGetObjectPosition(
clientID[0], Object_Handle, UR3_handle[0],
vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
time.sleep(1)
print('Position')
print(Output_Postion)
return Output_Postion
elif err == vrep.simx_return_novalue_flag:
print("no value")
else:
print("not yet")
def getVisionSensor(visionSensorName,clientID,sample_time):
#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_streaming)
time.sleep(0.5)
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID)!=-1):
start = time.time()
#Get the image of the vision sensor
res,resolution,image = vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
# #Set the image for naoqi
# imagepixel = []#*(resolution[0]*resolution[1])
# for i in xrange(resolution[0] * resolution[1]):
# r = i*3
# g = r+1
# b = r+2
# imagepixel.append((image[b], image[g], image[r]))
# img.putdata(imagepixel)
# print image
# print imagepixel
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.rotate(180).tostring())
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'
def gpsRobot(clientID):
# print('Getting Robot GPS Location')
# Get Robot GPS Handle
errorCode, gpsHandle = vrep.simxGetObjectHandle(
clientID, 'GPS', vrep.simx_opmode_oneshot_wait)
# Get GPS location
positionError, position = vrep.simxGetObjectPosition(
clientID, gpsHandle, -1, vrep.simx_opmode_streaming)
# loop while connected to simulation
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in camera information (Note operating mode changed to buffer)
positionError, position = vrep.simxGetObjectPosition(
clientID, gpsHandle, -1, vrep.simx_opmode_buffer)
errorCodes = [positionError]
# Check if have both image and xyz coordinate data available
if (all(errors == vrep.simx_return_ok for errors in errorCodes)):
# print ("Pose OK!!!")
break
elif (all(errors == vrep.simx_return_novalue_flag
for errors in errorCodes)):
# print ("Pose Not Available Yet")
pass
else:
print(errorCodes)
position = np.asarray(position)
# print("position")
# print(position)
position = position[:2]
# print(position)
return position
def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
#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_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
c = 0
while (vrep.simxGetConnectionId(clientID) != -1):
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' + str(c))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
c += 1
print('End of Simulation')
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
#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_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
print 'End of Simulation'
def main():
# Initialize two Vision Sensors. If we don't do this, we can't retrieve images from them.
vrep.simxGetVisionSensorImage(clientID, line_sensor, 0, vrep.simx_opmode_streaming)
vrep.simxGetVisionSensorImage(clientID, obstacle_sensor, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# PID Parameters
# Motor Kp Ki Kd
# 5 0.01 0 0
# 10 0.0001 0.0005
pid = pid_controller(1, 0.0001, 0.0005)
# Set the Power!
motor(10)
# Start the main loop.
while vrep.simxGetConnectionId(clientID) != -1:
raw = get_line_image()
# Crop the line image.
raw = raw[128:192, :]
# Find the desired moment.
ret = get_moment(raw)
if ret is None:
continue
cx, cy, img = ret
# Calculate the angle we want to steer.
angle = math.atan((128 - cx) / cy)
steer(pid(angle))
cv2.imshow("result", img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def streamVisionSensor(visionSensorName, clientID, pause, sample_time):
#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_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
print resolution
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID) != -1):
start = time.time()
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b', image)
im = I.frombuffer("RGB", (resolution[0], resolution[1]),
image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
#Set the image for naoqi
imagepixel = [] #*(resolution[0]*resolution[1])
for i in xrange(resolution[0] * resolution[1]):
r = i * 3
g = r + 1
b = r + 2
imagepixel.append((image[b], image[g], image[r]))
img.putdata(imagepixel)
# print image
# print imagepixel
data = img.tostring()
im = im.rotate(180)
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0],
resolution[1], im.tostring())
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'
def getPosition(Objects_num): # only for objects
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
if (Objects_num < 10 and Objects_num >= 0):
Object_Handle = Objects_handle[Objects_num]
else:
print('wrong object number')
return 0
while (vrep.simxGetConnectionId(clientID[0]) != -1):
err, Output_Postion = vrep.simxGetObjectPosition(
clientID[0], Object_Handle, UR3_handle[0],
vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
time.sleep(1)
print('Position')
print(Output_Postion)
return Output_Postion
elif err == vrep.simx_return_novalue_flag:
print("no value")
else:
print("not yet")
def run_sim_data(subject, clientID): vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legJoints = [0,0,0,0,0,0,0,0,0,0,0,0] #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legPositions = [0,0,0,0,0,0,0,0,0,0,0,0] legJointInv = [1,1,1,-1,-1,1,1,1,1,1,-1,1] hipJointInv = [1,1,1,1,1,1,1,1,1,1,1,1] returnCode01, legJoints[0] = vrep.simxGetObjectHandle(clientID, "rightLegJoint0", vrep.simx_opmode_blocking) returnCode02, legJoints[1] = vrep.simxGetObjectHandle(clientID, "rightLegJoint1", vrep.simx_opmode_blocking) returnCode03, legJoints[2] = vrep.simxGetObjectHandle(clientID, "rightLegJoint2", vrep.simx_opmode_blocking) returnCode04, legJoints[6] = vrep.simxGetObjectHandle(clientID, "rightLegJoint3", vrep.simx_opmode_blocking) returnCode05, legJoints[8] = vrep.simxGetObjectHandle(clientID, "rightLegJoint5", vrep.simx_opmode_blocking) returnCode06, legJoints[9] = vrep.simxGetObjectHandle(clientID, "rightLegJoint4", vrep.simx_opmode_blocking) returnCode07, legJoints[3] = vrep.simxGetObjectHandle(clientID, "leftLegJoint0", vrep.simx_opmode_blocking) returnCode08, legJoints[4] = vrep.simxGetObjectHandle(clientID, "leftLegJoint1", vrep.simx_opmode_blocking) returnCode09, legJoints[5] = vrep.simxGetObjectHandle(clientID, "leftLegJoint2", vrep.simx_opmode_blocking) returnCode010, legJoints[7] = vrep.simxGetObjectHandle(clientID, "leftLegJoint3", vrep.simx_opmode_blocking) returnCode011, legJoints[10] = vrep.simxGetObjectHandle(clientID, "leftLegJoint5", vrep.simx_opmode_blocking) returnCode012, legJoints[11] = vrep.simxGetObjectHandle(clientID, "leftLegJoint4", vrep.simx_opmode_blocking) returnCode013, head = vrep.simxGetObjectHandle(clientID, "Asti", vrep.simx_opmode_blocking) headLocation = [1,1,1]; returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming) for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming) #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ start_time = int(round(time.time())) while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.3 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)): for j in range(((subject * 101)),((subject * 101) + 202)): for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer) joint_data = model_iterator.getActual(j) newJointPositions = [0,0,0,0,0,0,0,0,0,0,0,0] for i in range(len(newJointPositions)): joint_data[i] = joint_data[i] * math.pi / 180 newJointPositions[i] = joint_data[i] * legJointInv[i] * hipJointInv[i] print(j) print(newJointPositions) print() vrep.simxPauseCommunication(clientID,1) for i in range(12): vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i],vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID,0) time.sleep(0.01) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.25) return
def cameraOrientation(clientID):
# Vision Sensor Object Handling
errorCode, cameraHandle = vrep.simxGetObjectHandle(
clientID, 'kinect_depth', vrep.simx_opmode_oneshot_wait)
# Setting Up First Call
error, orientation = vrep.simxGetObjectOrientation(
clientID, cameraHandle, -1, vrep.simx_opmode_streaming)
# Loop while connected
processLimit = 5
count = 0
while (vrep.simxGetConnectionId(clientID) != -1):
error, orientation = vrep.simxGetObjectOrientation(
clientID, cameraHandle, -1, vrep.simx_opmode_buffer)
if (error == vrep.simx_return_ok):
count += 1
if count > processLimit:
break
# Assume only pitch possible in camera
pitch = math.degrees(orientation[1]) - 90
return pitch
def get_connection_status(self): """ Function to inform if the connection with the server is active. Returns: connectionId: -1 if the client is not connected to the server. Different connection IDs indicate temporary disconections in-between. """ return vrep.simxGetConnectionId(self.clientID)
def __init__(self):
print 'started'
# Init vrep client
vrep.simxFinish(-1)
objectsList = ['Wall1','Wall2','Wall3','Wall4','Wall5','Wall6']
resolution = 0.01
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if self.clientID != -1:
#The quad helper contains function implementations for various quadcopter functions
quad_functions = quad_helper.quad_helper(self.clientID)
print('Main Script Started')
quad_functions.init_sensors()
quad_functions.start_sim()
#Setting initial time
init_time = time.time()
d1=0
#Initialize flags
self.obstacleAvoidFlag = False
positions = [[0,0,3],[0,1,3],[-1,1,3],[-2,1,3],[-2,2,3],[-2,3,3],[-1,3,3],[0,3,3],[1,3,3]]
# Rate init
self.rate = rospy.Rate(20.0) # MUST be more then 20Hz
#Initializing publishers
rospy.Subscriber("/quad_explore/target_position", Pose, self.posCb)
#Code to shift the origin from the center to the bottom left
obstOccMat = self.createObstacleOccupancyMat(objectsList,self.clientID,resolution)
# self.cellDecomposition(obstOccMat)
err,self.quadObjectHandle = vrep.simxGetObjectHandle(self.clientID,'Quadricopter',vrep.simx_opmode_blocking)
while not rospy.is_shutdown() and vrep.simxGetConnectionId(self.clientID) != -1:
#Getting object position with respect to first joint
err,obj_pos = vrep.simxGetObjectPosition(self.clientID,self.quadObjectHandle,-1,vrep.simx_opmode_blocking)
current_time = time.time()
elapsed_time = current_time-init_time
#Setting the position of the quadcopter
ind = int(elapsed_time/3)
#Looping the movement
if ind > 2*len(positions)-1:
print 'finished one'
init_time = time.time()
elif ind > len(positions)-1:
ind_rev = len(positions)-1-ind
self.quad_pos = positions[ind_rev]
else:
self.quad_pos = positions[ind]
# print 'moving quad'
quad_functions.move_quad(self.quad_pos)
vrep.simxSynchronousTrigger(self.clientID);
self.rate.sleep()
quad_functions.stop_sim()
def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, 1)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# enable streaming of the iteration counter:
res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration",
vrep.simx_opmode_streaming)
print("iteration1: ", iteration1)
#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_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID) != -1):
if iteration1 < 30:
vrep.simxSynchronousTrigger(clientID)
res, iteration1 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok: iteration1 = -1
iteration2 = iteration1
while iteration2 == iteration1: # wait until the iteration counter has changed
res, iteration2 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok:
iteration2 = -1
print(iteration2)
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' +
str(iteration2))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
# vrep.simxSynchronousTrigger(clientID)
print('End of Simulation')
def loop(self, interval, learning, learnLoop):
cnt=0
while vrep.simxGetConnectionId(self.getClientID())!=-1 and ((not learning) or cnt<learnLoop):
for item in self.__items:
item.loop()
for rob in self.__robs:
rob.observe()
self.robPerception(rob)
if cnt>1:
rob.setRewards()
# print rob.getLastAction()
rob.act()
# print rob.getName(), rob.getPosition()
time.sleep(interval)
cnt+=1
if vrep.simxGetConnectionId(self.getClientID())==-1:
print >> sys.stderr, "Disconnected: Exiting from the main loop!"
time.sleep(1)
exit()
def get_sensor_image(vision_sensor):
cnt = 0
while (vrep.simxGetConnectionId(clientID) != -1):
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, vision_sensor, 0, vrep.simx_opmode_buffer)
cnt += 1
if err == vrep.simx_return_ok:
image_buffer = change_color_and_resize(image, resolution)
return image_buffer
if cnt % 2048 == 0:
logging.warning('get_sensor_image retry %d tiems' % cnt)
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
nn = newNeuralNetwork()
#print("Neural Network num_inputs:{n}".format(n=nn.num_inputs))
#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_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
im = im.crop((160, 0, 480, 480))
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
count = COUNTER
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
im = im.crop((160, 0, 480, 480))
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.axis("off")
plt.show()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
if (nn):
# feed the neural network with the image and get its prediction (if it has a plant)
trainer = Trainer("live", imageIntoFeatures(im))
answer = nn.predict(trainer.inputs)
print(answer)
#assertAnswer(answer)
#assertAnswerStrong(answer)
if (SAVE_VISION and count % 60 == 0):
name = str(count//60) + ".png"
print("Saving " + name)
im.save(name)
count += 1
print('End of Simulation')
def main():
ur5 = UR5()
ur5.connect()
ur5.ankleinit()
clientID = ur5.get_clientID()
camera = Camera(clientID)
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP:
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
# Camera achieve data
cur_depth, cur_color = camera.get_camera_data()
cur_depth_path, cur_img_path = camera.save_image(
cur_depth, cur_color, currCmdTime)
# Call affordance map function
affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str(
currCmdTime) + '_results.h5'
try:
os.system(affordance_cmd)
print('-- Successfully create affordance map!')
except:
print(
'!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map'
)
hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5',
currCmdTime)
# TODO: get the push u,v coordinate
u = 256
v = 212
camera_coor = pixel2camera(u, v, cur_depth, camera.intri,
camera.Dis_FAR)
_, ur5_position = vrep.simxGetObjectPosition(
clientID, ur5.get_handle(), -1, vrep.simx_opmode_oneshot_wait)
location = camera2ur5(camera_coor, ur5_position, camera.cam_position)
# Move ur5
# location = random.randint(100, 200, (1, 3))
# location = location[0] / 400
print("UR5 Move to %s" % (location))
ur5.ur5moveto(location[0], location[1], location[2] - 0.05)
count += 1
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
ur5.disconnect()
def getVisionSensor(visionSensorName,clientID):
#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_streaming)
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
print 'End of Simulation'
def loop(self, interval, learning, learnLoop):
cnt = 0
while vrep.simxGetConnectionId(self.getClientID()) != -1 and (
(not learning) or cnt < learnLoop):
for item in self.__items:
item.loop()
for rob in self.__robs:
rob.observe()
self.robPerception(rob)
if cnt > 1:
rob.setRewards()
# print rob.getLastAction()
rob.act()
# print rob.getName(), rob.getPosition()
time.sleep(interval)
cnt += 1
if vrep.simxGetConnectionId(self.getClientID()) == -1:
print >> sys.stderr, "Disconnected: Exiting from the main loop!"
time.sleep(1)
exit()
def streamVisionSensor(visionSensorName,clientID,pause,sample_time):
#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_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID)!=-1):
start = time.time()
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
#Set the image for naoqi
imagepixel = []#*(resolution[0]*resolution[1])
for i in xrange(resolution[0] * resolution[1]):
r = i*3
g = r+1
b = r+2
imagepixel.append((image[b], image[g], image[r]))
img.putdata(imagepixel)
# print image
# print imagepixel
data = img.tostring()
im = im.rotate(180)
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.tostring())
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'
def collectImageData(clientID, action_arg):
list_of_images = []
collector = []
if clientID!=-1:
res,v0=vrep.simxGetObjectHandle(clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
res,v1=vrep.simxGetObjectHandle(clientID,'PassiveVision_sensor',vrep.simx_opmode_oneshot_wait)
ret_code, left_handle = vrep.simxGetObjectHandle(clientID,'DynamicLeftJoint', vrep.simx_opmode_oneshot_wait)
ret_code, right_handle = vrep.simxGetObjectHandle(clientID,'DynamicRightJoint', vrep.simx_opmode_oneshot_wait)
ret_code, base_handle = vrep.simxGetObjectHandle(clientID, 'LineTracerBase', vrep.simx_opmode_oneshot_wait)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v0,0,vrep.simx_opmode_streaming)
ret_code, euler_angles = vrep.simxGetObjectOrientation(clientID, base_handle, -1, vrep.simx_opmode_streaming)
count = 0
action = 2
inference_counter = 0
steps = 50
action_length = 0
while (vrep.simxGetConnectionId(clientID)!=-1 and count < steps):
tim = time.time()
for i in range(10):
vrep.simxSynchronousTrigger(clientID)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v0,0,vrep.simx_opmode_buffer)
if res==vrep.simx_return_ok:
img = np.array(image,dtype=np.uint8)
img.resize([resolution[1],resolution[0],3])
rotate_img = img.copy()
rotate_img = np.flipud(img)
list_of_images.append(rotate_img)
ret_code, pos = vrep.simxGetObjectPosition(clientID, base_handle, -1, vrep.simx_opmode_oneshot)
ret_code, velo, angle_velo = vrep.simxGetObjectVelocity(clientID, base_handle, vrep.simx_opmode_oneshot)
ret_code, euler_angles = vrep.simxGetObjectOrientation(clientID, base_handle, -1, vrep.simx_opmode_buffer)
collector.append([pos[0], pos[1], pos[2], velo[0], velo[1], velo[2], euler_angles[0], euler_angles[1], euler_angles[2], action])
if action_length < 1 and action_arg == -1:
action_length = random.randint(2,10)
action = np.random.randint(0, 5)
elif action_arg > -1:
action = action_arg
action_length = action_length - 1
velo = (action -2) * 15
return_val = vrep.simxSetJointTargetVelocity(clientID, left_handle, velo, vrep.simx_opmode_oneshot)
return_val2 = vrep.simxSetJointTargetVelocity(clientID, right_handle, velo, vrep.simx_opmode_oneshot_wait)
count+=1
return list_of_images, collector
else:
sys.exit()
def getVisionSensor(visionSensorName,clientID):
#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_streaming)
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
print 'End of Simulation'
def reset_target(self):
if vrep.simxGetConnectionId(self.client_id) == -1:
self.connection()
for i in range(3):
random_value = np.random.randint(self.workspace[i][0],
self.workspace[i][1])
self.target_input[i] = random_value / 1000.0
vrep.simxSetObjectPosition(self.client_id, self.target_handle, -1,
self.target_input, vrep.simx_opmode_oneshot)
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 run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error, jointHandle = vrep.simxGetObjectHandle(
clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0,
vrep.simx_opmode_oneshot_wait)
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle,
vrep.simx_opmode_streaming)
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(
clientID) != -1: # while we are connected to the server..
(error, position) = vrep.simxGetJointPosition(clientID, jointHandle,
vrep.simx_opmode_buffer)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error == vrep.simx_error_noerror:
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi / 2.0:
break
elif not passed90Degrees and position > math.pi / 2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0,
vrep.simx_opmode_oneshot_wait)
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def getDepthImage(clientID):
imageCount = 0 # to keep track of how many images have been processed
processLimit = 5
# print ('Vision Sensor object handling')
errorCode, rgbCamHandle = vrep.simxGetObjectHandle(
clientID, 'kinect_rgb', vrep.simx_opmode_oneshot_wait)
errorCode, xyzCamHandle = vrep.simxGetObjectHandle(
clientID, 'kinect_pointcloud', vrep.simx_opmode_oneshot_wait)
# print ('Setting up First Calls for Reading Images and Sensor Values')
# Note that the image takes a while to display so cannot load first image but
# have to set up a buffer
errorRGB, resolution, rgbImage = vrep.simxGetVisionSensorImage(
clientID, rgbCamHandle, 0, vrep.simx_opmode_streaming)
# api return value for the coordinate extraction filter in camera is as follows:
# (1) point count along x, (2) point count along Y, (3) point x1, (4) point y1,
# (5) point z1, (6) distance point1, (7) point x2, etc.
# and auxPacket[1] holds data in the format described above
errorXYZ, detectionState, auxPackets = vrep.simxReadVisionSensor(
clientID, xyzCamHandle, vrep.simx_opmode_streaming)
# loop while connected to simulation
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in camera information (Note operating mode changed to buffer)
errorRGB, resolution, rgbImage = vrep.simxGetVisionSensorImage(
clientID, rgbCamHandle, 0, vrep.simx_opmode_buffer)
errorXYZ, detectionState, auxPackets = vrep.simxReadVisionSensor(
clientID, xyzCamHandle, vrep.simx_opmode_buffer)
errorCodes = [errorRGB, errorXYZ]
# Check if have both image and xyz coordinate data available
if (all(errors == vrep.simx_return_ok for errors in errorCodes)):
# print ("image OK!!!")
imageCount += 1
elif (all(errors == vrep.simx_return_novalue_flag
for errors in errorCodes)):
# print ("no image yet")
pass
else:
# print (errorCodes)
pass
# exit if processed more than certain amount of images
if imageCount > processLimit:
break
return auxPackets, resolution, rgbImage
def loop(self, interval):
while vrep.simxGetConnectionId(self.getClientID()) != -1:
self.__cnt = self.__cnt + 1
for rob in self.__robs:
rob.loop()
self.robPerception(rob)
# print rob.getName(), rob.getPosition()
for item in self.__items:
item.loop()
time.sleep(interval)
print >> sys.stderr, "Disconnected: Exiting from the main loop!"
time.sleep(1)
exit()
def loop(self, interval):
while vrep.simxGetConnectionId(self.getClientID())!=-1:
self.__cnt=self.__cnt+1
for rob in self.__robs:
rob.loop()
self.robPerception(rob)
# print rob.getName(), rob.getPosition()
for item in self.__items:
item.loop()
time.sleep(interval)
print >> sys.stderr, "Disconnected: Exiting from the main loop!"
time.sleep(1)
exit()
def step(self, delta_rotor_thrusts):
if vrep.simxGetConnectionId(self.clientID) != -1:
self.curr_rotor_thrusts = (np.array(self.curr_rotor_thrusts) + delta_rotor_thrusts).tolist()
self.quad_functions.apply_rotor_thrust(self.curr_rotor_thrusts)
for i in range(self.run_time):
vrep.simxSynchronousTrigger(self.clientID)
self.get_curr_state()
reward = self.quad_functions.get_reward(self.curr_rotor_thrusts, self.curr_pos, self.curr_euler,
self.target_pos, self.target_euler)
goaldiffpos = np.linalg.norm(np.array(self.curr_pos) - np.array(self.target_pos))
goaldiffeuler = np.linalg.norm(np.array(self.curr_euler) - np.array(self.target_euler))
done = goaldiffpos < 0.5 and goaldiffeuler < 0.1 # TODO set appropriate threshold
return np.array(self.curr_state), reward, done, None
def run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait);
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming);
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server..
(error,position) = vrep.simxGetJointPosition(
clientID,
jointHandle,
vrep.simx_opmode_buffer
)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error==vrep.simx_error_noerror :
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi/2.0:
break
elif not passed90Degrees and position > math.pi/2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait);
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def conduct_action(self, a):
self.action = a / 1000.0
if vrep.simxGetConnectionId(self.client_id) == -1:
self.connection()
#print("action2", a)
lasttime = time.time()
while True:
for i in range(3):
vrep.simxSetFloatSignal(self.client_id, "myTestValue" + str(i),
self.action[i],
vrep.simx_opmode_oneshot)
currtime = time.time()
if currtime - lasttime > 0.1:
break
def connectToVREP(self, VREP_World=None):
"Connect to VREP and load the correct world if needed"
"FIXME: SHOULD LAUNCH VREP IF NOT RUNNING"
VREP_exec = 'vrep'
if self.VREPSimulation == None:
self.VREPSimulation = VREP_World
'1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect'
if VREP_exec not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation))
else:
"Check if we are connected with the passed clientId already"
if self._VREP_clientId is not None:
print "ClientId = " ,self._VREP_clientId
connId = vrep.simxGetConnectionId(self._VREP_clientId)
print "My connId is " , connId
if connId == -1: # we are not: set client Id to none and re-connect
print "Disconnecting all existing connections to V-REP"
vrep.simxFinish(-1)
self._VREP_clientId = None
while self._VREP_clientId is None:
self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5)
if not self._VREP_clientId == -1:
eCode = vrep.simxSynchronous(self._VREP_clientId, True)
if eCode != 0:
raise Exception("Failed to connect to VREP simulation. Bailing out")
# print " we are connected with clientId ", self._VREP_clientId
"2. Check the correct world is running"
if self.VREPSimulation is not None:
VREP_Scene = split(self.VREPSimulation)[1]
if VREP_Scene not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception(("Could not load into V-REP the world", self.VREPSimulation))
"3. Make sure VREP has bees set to the correct directory for saving data"
self.setDataDir(self.dataDir)
'4. Start simulation'
eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("VREP simulation cannot get started")
else:
print "V-REP simulation is running with clientId: ", self._VREP_clientId
return self._VREP_clientId
angleModel[0] = orientation[1]
(velizq, velder, targetReached) = setSpeeds(x, y, angleModel, lockMode)
if (joystick != oldJoystick) or targetReached:
oldJoystick = joystick
#print('x: '+str(x)+' y: '+str(y)+' vel: izq '+str(velizq)+' der '+str(velder)+' Lock Mode: '+str(lockMode))
vrep.simxSetJointTargetVelocity(clientID,lwmotor,velizq,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID,rwmotor,velder,vrep.simx_opmode_oneshot)
if (position != oldPosition):
data.update({"item": "robotData"}, {"$set":{"position": {"x": round(position[0],4), "y": round(position[1],4), "z": round(position[2],4)}}})
oldPosition = position
if (orientation!= oldOrientation):
data.update({"item": "robotData"}, {"$set":{"orientation": {"alpha": round(math.degrees(orientation[0]),2), "beta": round(math.degrees(orientation[1]),2), "gamma": round(math.degrees(orientation[2]),2)}}})
oldOrientation = orientation
if (speed!= oldSpeed):
data.update({"item": "robotData"}, {"$set":{"speed": {"vx": round(speed[0],2), "vy": round(speed[1],2), "vz": round(speed[2],2)}}})
oldSpeed = speed
if (angularSpeed!= oldAngularSpeed):
data.update({"item": "robotData"}, {"$set":{"angularSpeed": {"dAlpha": round(angularSpeed[0],2), "dBeta": round(angularSpeed[1],2), "dGamma": round(angularSpeed[2],2)}}})
oldAngularSpeed = angularSpeed
#Check connection
if vrep.simxGetConnectionId(clientID)==-1:
count += 1
if count>2:
break
close()
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,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'
print 'Conectado ao motor direito!'
#inicialização dos sensores (remoteApi)
for i in range(16):
erro, sensorHandle[i] = vrep.simxGetObjectHandle(clientID,
"Pioneer_p3dx_ultrasonicSensor%d" % (i+1),vrep.simx_opmode_oneshot_wait)
if erro <> 0:
print "Handle do sensor Pioneer_p3dx_ultrasonicSensor%d nao encontrado!" % (i+1)
else:
print "Conectado ao sensor Pioneer_p3dx_ultrasonicSensor%d!" % (i+1)
erro, state, coord, detectedObjectHandle,
detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID,
sensorHandle[i],vrep.simx_opmode_streaming)
#desvio e velocidade do robo
while vrep.simxGetConnectionId(clientID) != -1:
for i in range(16):
erro, state, coord, detectedObjectHandle,
detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID,
sensorHandle[i],vrep.simx_opmode_buffer)
if erro == 0:
dist = coord[2]
if state > 0 and dist < noDetectionDist:
if dist < maxDetectionDist:
dist = maxDetectionDist
detect[i] = 1-((dist-maxDetectionDist) /
(noDetectionDist-maxDetectionDist))
else:
detect[i] = 0
else:
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]]
