def gps_read(clientID):
#Read GPS Sensor
errorCode, gpsX = vrep.simxGetFloatSignal(clientID, 'gpsX',
vrep.simx_opmode_oneshot_wait)
errorCode, gpsY = vrep.simxGetFloatSignal(clientID, 'gpsY',
vrep.simx_opmode_oneshot_wait)
return (gpsX, gpsY)
def prepare(self):
self.clearSignal()
################## PREPARE SENSORS ############################
rCode, self.proxHandle = vrep.simxGetObjectHandle(
self.clientID, 'sensor#1', vrep.simx_opmode_oneshot_wait)
if rCode != vrep.simx_return_ok:
print("Could not get proximity sensor handle. Exiting.")
sys.exit()
error, state, point, handle, vector = vrep.simxReadProximitySensor(
self.clientID, self.proxHandle, vrep.simx_opmode_streaming)
################ GET ROBOT/GOAL POSITIONS, MAP DIM WITH RESOLUTION, TRANSFORM ###########################
r, self.robotHandle = vrep.simxGetObjectHandle(
self.clientID, 'body#1', vrep.simx_opmode_oneshot_wait)
r, self.goalHandle = vrep.simxGetObjectHandle(
self.clientID, 'Goal', vrep.simx_opmode_oneshot_wait)
r = -1
while (r != vrep.simx_return_ok):
r, self.robotPosition = vrep.simxGetObjectPosition(
self.clientID, self.robotHandle, -1, vrep.simx_opmode_streaming
) #-1 specifies we want the absolute position
r = -1
while (r != vrep.simx_return_ok):
r, self.goalPosition = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
rCode = -1
while (rCode != vrep.simx_return_ok):
rCode, angle = vrep.simxGetObjectOrientation(
self.clientID, self.robotHandle, -1,
vrep.simx_opmode_streaming) #just to prepare our vrep
self.robotPosition = self.transform(self.robotPosition)
self.goalPosition = self.transform(self.goalPosition)
self.mapDimensions = (self.goalPosition[0] - self.robotPosition[0] + 2,
self.goalPosition[1] - self.robotPosition[1] + 2,
2) #g, rhs
self.map = np.zeros(self.mapDimensions)
vrep.simxGetFloatSignal(
self.clientID, "ProxDistance",
vrep.simx_opmode_streaming) #just to get things started
############### NAIVELY FILL IN G AND RHS VALUES FOR MAP #########################
for i in range(self.mapDimensions[0]):
for j in range(self.mapDimensions[1]):
cost = self.euclidian(self.goalPosition, (i, j))
self.map[i, j, 0] = cost
self.map[i, j, 1] = cost
############# INITIALIZE PRIORITY QUEUE ######################
print("OVERALL GOAL POSITION: ", self.goalPosition)
heapq.heapify(self.open)
self.map[self.goalPosition[0], self.goalPosition[1], 1] = 0
heapq.heappush(self.open,
(self.key(self.goalPosition), self.goalPosition))
def backRobot(self):
r, distance = vrep.simxGetFloatSignal(self.clientID, "ProxDistance",
vrep.simx_opmode_buffer)
while distance < MAXDISTANCE and distance != -1: #while it is not far away enough and there is an obstacle in front of him
self.CycleFreqL = -2
self.CycleFreqR = -2
self.sendSignal()
r, distance = vrep.simxGetFloatSignal(self.clientID,
"ProxDistance",
vrep.simx_opmode_buffer)
self.stopRobot()
def prepare(self):
self.clearSignal()
################ GET ROBOT/GOAL POSITIONS, MAP DIM WITH RESOLUTION, TRANSFORM ###########################
r, self.robotHandle = vrep.simxGetObjectHandle(
self.clientID, 'body#1', vrep.simx_opmode_oneshot_wait)
r, self.goalHandle = vrep.simxGetObjectHandle(
self.clientID, 'Goal', vrep.simx_opmode_oneshot_wait)
r = -1
while (r != vrep.simx_return_ok):
r, robotPosition = vrep.simxGetObjectPosition(
self.clientID, self.robotHandle, -1, vrep.simx_opmode_streaming
) #-1 specifies we want the absolute position
self.env.updateRobotPosition(robotPosition)
r = -1
while (r != vrep.simx_return_ok):
r, goalPosition = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
self.env.updateGoal(goalPosition)
rCode = -1
while (rCode != vrep.simx_return_ok):
rCode, angle = vrep.simxGetObjectOrientation(
self.clientID, self.robotHandle, -1,
vrep.simx_opmode_streaming) #just to prepare our vrep
robotPosition = self.env.transform(robotPosition)
goalPosition = self.env.transform(goalPosition)
self.env.updateRobotPosition(robotPosition)
self.env.updateGoal(goalPosition)
self.env.initializeMap()
################## PREPARE SENSORS ############################
rCode, proxHandle = vrep.simxGetObjectHandle(
self.clientID, 'sensor#1', vrep.simx_opmode_oneshot_wait)
if rCode != vrep.simx_return_ok or rCode != vrep.simx_return_ok:
print("Could not get proximity sensor handle. Exiting.")
sys.exit()
self.proxHandles = [proxHandle]
error, state, point, handle, vector = vrep.simxReadProximitySensor(
self.clientID, self.proxHandles[0], vrep.simx_opmode_streaming)
rCode = -1
vrep.simxGetFloatSignal(
self.clientID, "ProxDistance",
vrep.simx_opmode_streaming) #just to get things started
vrep.simxGetStringSignal(self.clientID, "threeDimData",
vrep.simx_opmode_streaming)
############# INITIALIZE PRIORITY QUEUE ######################
print("OVERALL GOAL POSITION: ", self.env.getGoal())
heapq.heapify(self.open)
self.env.setMap(goalPosition[0], goalPosition[1], 1, 0)
heapq.heappush(self.open, (self.key(goalPosition), goalPosition))
def __init__(self, clientID):
self.clientID = clientID
_, self.handle=vrep.simxGetObjectHandle(
self.clientID, 'Ball', vrep.simx_opmode_oneshot_wait)
vrep.simxGetObjectPosition(
self.clientID, self.handle, -1, vrep.simx_opmode_streaming)
self.posm2 = self.getBallPose()
self.tm2 = time.time()
self.posm1 = self.getBallPose()
self.tm1 = time.time()
self.pos = self.getBallPose()
self.t = time.time()
self.T = 2.15 # time constant in [s]
vrep.simxGetFloatSignal(self.clientID, 'simTime', vrep.simx_opmode_streaming )
def getGyroData(self):
err, gyroX = vrep.simxGetFloatSignal(self.clientID, 'gyroX',
vrep.simx_opmode_oneshot)
err, gyroY = vrep.simxGetFloatSignal(self.clientID, 'gyroY',
vrep.simx_opmode_oneshot)
err, gyroZ = vrep.simxGetFloatSignal(self.clientID, 'gyroZ',
vrep.simx_opmode_oneshot)
#added noise
gyroX *= random.uniform(1 - self.sensErr, 1 + self.sensErr)
gyroY *= random.uniform(1 - self.sensErr, 1 + self.sensErr)
gyroZ *= random.uniform(1 - self.sensErr, 1 + self.sensErr)
self.gyroData = (gyroX, gyroY, gyroZ)
def __init__(self, clientID):
self.clientID = clientID
_, self.handle = vrep.simxGetObjectHandle(
self.clientID, 'Ball', vrep.simx_opmode_oneshot_wait)
vrep.simxGetObjectPosition(self.clientID, self.handle, -1,
vrep.simx_opmode_streaming)
self.posm2 = self.getBallPose()
self.tm2 = time.time()
self.posm1 = self.getBallPose()
self.tm1 = time.time()
self.pos = self.getBallPose()
self.t = time.time()
self.T = 2.15 # time constant in [s]
vrep.simxGetFloatSignal(self.clientID, 'simTime',
vrep.simx_opmode_streaming)
def getRange(self):
"get sensor range"
eCode, value = vrep.simxGetFloatSignal(self.robot, "HOMEO_SIGNAL_"+self._eye+"Eye"+"_MAX_LIGHT",getattr(vrep,self._opMode))
vrep.simxSynchronousTrigger(self.robot)
if eCode != 0:
raise TransducerException("Cannot get maxLight of VREP sensor: " + self._eye+"Eye")
return value
def updateStart(self, newPosition):
print("CurrPosition: ", self.robotPosition)
print("NewPosition:", newPosition)
while (True):
############ GET INITIAL POSITIONS/ANGLES. CHECK IF TOO CLOSE TO OBSTACLE ############
r, position = vrep.simxGetObjectPosition(
self.clientID, self.robotHandle, -1, vrep.simx_opmode_buffer
) #-1 specifies we want the absolute position
r, angle = vrep.simxGetObjectOrientation(self.clientID,
self.robotHandle, -1,
vrep.simx_opmode_buffer)
r, distance = vrep.simxGetFloatSignal(self.clientID,
"ProxDistance",
vrep.simx_opmode_buffer)
if r == vrep.simx_return_ok and distance != -1 and distance < MINDISTANCE:
print('BACK')
self.backRobot()
r, position = vrep.simxGetObjectPosition(
self.clientID, self.robotHandle, -1,
vrep.simx_opmode_buffer)
break
else:
########## TRAVEL TO THE NEW POSITION ##############
position = self.transform(position) #our coordinate frame
angle = self.radToDeg(
angle[2]
) #the angles that v-rep gives range from -pi to pi radians. This is hard to work with. Convert to 0 to 360 degrees
if (angle < 0):
angle += 360
xVec = newPosition[0] - position[0]
yVec = newPosition[1] - position[1]
desiredAngle = math.degrees(math.atan(
yVec / xVec)) if xVec != 0 else yVec * 90
if (desiredAngle < 0):
desiredAngle += 360
if desiredAngle - PADDING < angle and desiredAngle + PADDING > angle:
self.CycleFreqL = 3
self.CycleFreqR = 3
print("STRAIGHT")
else:
turnRight = ((360 - desiredAngle) + angle) % 360
turnLeft = ((360 - angle) + desiredAngle) % 360
if turnRight < turnLeft: #turn right if the work to turn right is less than turning left
self.CycleFreqL = 3
self.CycleFreqR = 1
print('RIGHT')
else: #turn left if the work to turn left is less than turning right
self.CycleFreqL = 1
self.CycleFreqR = 3
print('LEFT')
self.sendSignal()
######### BREAK AND UPDATE ROBOTPOSITION IF TRANSITIONED ###########
if position != self.robotPosition:
self.robotPosition = position
difference = self.euclidian(
position, self.robotPosition
) #the difference in heuristic is this anyway
self.keyFactor += difference
break
def checkProximity(self):
###### DETECT NEW OBSTACLE AND ADD TO OUR ARRAY SPACE GRAPH ############
r, distance = vrep.simxGetFloatSignal(
self.clientID, "ProxDistance", vrep.simx_opmode_buffer
) #retrieves distance of a detected object from child script
if r != vrep.simx_return_ok or distance == -1 or r == 1: #if nothing was detected
return (False, None, np.inf)
##### IF SOMETHING WAS DETECTED ##########
self.stopRobot()
r, angle = vrep.simxGetObjectOrientation(self.clientID,
self.robotHandle, -1,
vrep.simx_opmode_buffer)
r, currPosition = vrep.simxGetObjectPosition(self.clientID,
self.robotHandle, -1,
vrep.simx_opmode_buffer)
angle = angle[2] #computation is always in radians
xdist = math.cos(angle) * distance
ydist = math.sin(angle) * distance
worldx = xdist + currPosition[0]
worldy = ydist + currPosition[1]
location = (worldx, worldy)
location = self.transform(location)
####### IF IT IS A NEW OBSTACLE, RETURN THE RESULTS #########
if location in self.obstacles:
return (False, None, np.inf)
self.obstacles.add(location)
return (True, location, distance)
def getRange(self):
"get motor range"
eCode, value = vrep.simxGetFloatSignal(self.robot, "HOMEO_SIGNAL_"+self._wheel+"Wheel"+"_MAX_SPEED",getattr(vrep,self._opMode))
vrep.simxSynchronousTrigger(self.robot)
if eCode != 0:
raise TransducerException("Cannot get maxSpeed of VREP motor: " + self._wheel+"Wheel")
return value
def getState(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
# Retrieve IMU data
errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(self.clientID,
'stepSeconds', mode)
errorOrien, baseEuler = vrep.simxGetObjectOrientation(self.clientID,
self.Quadbase, -1, mode)
errorPos, basePos = vrep.simxGetObjectPosition(self.clientID,
self.Quadbase,-1, mode)
errorVel, linVel, angVel = vrep.simxGetObjectVelocity(self.clientID,
self.Quadbase, mode)
if initial:
if (errorSignal or errorOrien or errorPos or errorVel != vrep.simx_return_ok):
time.sleep(0.05)
pass
else:
# Convert Euler angles to pitch, roll, yaw
rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]), baseEuler[2])
pitchRad = -pitchRad
yawRad = -baseEuler[2]
baseRad = np.array([yawRad,rollRad,pitchRad])+0.0
self.state = np.asarray(np.concatenate((basePos,linVel,angVel,baseRad)),dtype=np.float32)
#print("data_core: " + str(self.state))
return self.state
def getState(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
# Retrieve IMU data
errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(
self.clientID, 'stepSeconds', mode)
errorOrien, baseEuler = vrep.simxGetObjectOrientation(
self.clientID, self.Quadbase, -1, mode)
errorPos, basePos = vrep.simxGetObjectPosition(self.clientID,
self.Quadbase, -1, mode)
errorVel, linVel, angVel = vrep.simxGetObjectVelocity(
self.clientID, self.Quadbase, mode)
if initial:
if (errorSignal or errorOrien or errorPos
or errorVel != vrep.simx_return_ok):
time.sleep(0.05)
pass
else:
# Convert Euler angles to pitch, roll, yaw
rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]),
baseEuler[2])
pitchRad = -pitchRad
yawRad = -baseEuler[2]
baseRad = np.array([yawRad, rollRad, pitchRad]) + 0.0
self.state = np.asarray(np.concatenate(
(basePos, linVel, angVel, baseRad)),
dtype=np.float32)
#print("data_core: " + str(self.state))
return self.state
def initialize():
global clientID
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
res, v1 = vrep.simxGetObjectHandle(clientID, 'Quadricopter',
vrep.simx_opmode_oneshot_wait)
print(res)
rc, sv = vrep.simxGetFloatSignal(clientID, 'particlesTargetVelocities',
vrep.simx_opmode_oneshot_wait)
print(rc, sv)
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# load the quadroter model
vrep.simxLoadModel(clientID, "C:\quadrotor.ttm", 0,
vrep.simx_opmode_blocking)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
# with Listener(
# on_press=on_press,
# on_release=on_release) as listener:
# listener.join()
return clientID
else:
print('Not Connected')
sys.exit('Not Connected')
return -1
def _get_StateReward(self, prefix="", init=False):
v_opmode = vrep.simx_opmode_streaming if init else vrep.simx_opmode_buffer
state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.id_, prefix + "states",
v_opmode)[1]))
reward = vrep.simxGetFloatSignal(self.id_, prefix + "reward",
v_opmode)[1]
return state, reward
def getAccelerometer(self, axis):
acc_name = 'accelerometer' + axis.upper()
print(acc_name)
#Recommended operation modes for this function are simx_opmode_streaming (the first call) and simx_opmode_buffer (the following calls)
ret, value = vrep.simxGetFloatSignal(self.clientID, acc_name ,vrep.simx_opmode_streaming)
if ret != 0:
return -1
else:
return value
def waitForRobot(order, patID):
time.sleep(1)
ptime = None
orderID = 16*(order == 'linen') + patID
signalTo = 'order_ToVREP'
signalBack = 'ptime_FromVREP'
# Send order ID to be processed
vrep.simxSetIntegerSignal(clientID,signalTo,orderID,vrep.simx_opmode_oneshot)
# Start the receiving process
err,ptime=vrep.simxGetFloatSignal(clientID,signalBack,vrep.simx_opmode_streaming)
# While we haven't received anything
while not ptime:
err,ptime=vrep.simxGetFloatSignal(clientID,signalBack,vrep.simx_opmode_buffer)
if err != vrep.simx_return_ok:
print('!!!\nAn error occured while receiving data from vrep...\n!!!')
# Clear signal
vrep.simxClearFloatSignal(clientID,signalBack,vrep.simx_opmode_oneshot)
# Return the signal received (processing time)
return ptime
def update_paramter(d_position):
print(d_position)
x = d_position[0]
y = d_position[1]
dirs = np.array([x, -x, y, -y])
d = np.argmax(dirs)
print(dirs[d])
res, p1 = vrep.simxGetFloatSignal(clientID, "p1",
vrep.simx_opmode_blocking)
res, p2 = vrep.simxGetFloatSignal(clientID, "p2",
vrep.simx_opmode_blocking)
res, p3 = vrep.simxGetFloatSignal(clientID, "p3",
vrep.simx_opmode_blocking)
res, p4 = vrep.simxGetFloatSignal(clientID, "p4",
vrep.simx_opmode_blocking)
print(d)
if d == 0:
p1 += delta
p2 -= delta
p3 -= delta
p4 += delta
elif d == 1:
p1 -= delta
p2 += delta
p3 += delta
p4 -= delta
elif d == 2:
p1 += delta
p2 += delta
p3 -= delta
p4 -= delta
elif d == 3:
p1 -= delta
p2 -= delta
p3 += delta
p4 += delta
p = np.array([p1, p2, p3, p4])
[p1, p2, p3, p4] = p
print(p1, p2, p3, p4)
vrep.simxSetFloatSignal(clientID, "p1", p1, vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, "p2", p2, vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, "p3", p3, vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID, "p4", p4, vrep.simx_opmode_blocking)
def __get(self):
vrep.simxGetPingTime(self.__ID)
self.state = np.array(
vrep.simxUnpackFloats(
vrep.simxGetStringSignal(self.__ID, "states",
vrep.simx_opmode_buffer)[1]))
self.reward = vrep.simxGetFloatSignal(self.__ID, "reward",
vrep.simx_opmode_buffer)[1]
self.done = bool(
vrep.simxGetIntegerSignal(self.__ID, "done",
vrep.simx_opmode_buffer)[1])
def getAccelData(self):
err, accelX = vrep.simxGetFloatSignal(self.clientID, 'accelerometerX',
vrep.simx_opmode_oneshot)
err, accelY = vrep.simxGetFloatSignal(self.clientID, 'accelerometerY',
vrep.simx_opmode_oneshot)
err, accelZ = vrep.simxGetFloatSignal(self.clientID, 'accelerometerZ',
vrep.simx_opmode_oneshot)
accelNormX = accelX / 9.81
accelNormY = accelY / 9.81
accelNormZ = accelZ / 9.81
accelDataNorm = (accelNormX, accelNormY, accelNormZ)
#added noise
accelNormX *= random.uniform(1 - self.sensErr, 1 + self.sensErr)
accelNormY *= random.uniform(1 - self.sensErr, 1 + self.sensErr)
accelNormZ *= random.uniform(1 - self.sensErr, 1 + self.sensErr)
self.accelData = accelDataNorm
def __init__(self, clientid):
self.clientID = clientid
self.accelData = (0, 0, 0)
self.gyroData = (0, 0, 0)
self.sensErr = 0.04
res, self.accelHandle = vrep.simxGetObjectHandle(
self.clientID, 'Accelerometer', vrep.simx_opmode_blocking)
res, self.gyroHandle = vrep.simxGetObjectHandle(
self.clientID, 'GyroSensor', vrep.simx_opmode_blocking)
res, self.magnHandle = vrep.simxGetObjectHandle(
self.clientID, 'Magnetometr', vrep.simx_opmode_blocking)
err, accelX = vrep.simxGetFloatSignal(self.clientID, 'accelerometerX',
vrep.simx_opmode_streaming)
err, accelY = vrep.simxGetFloatSignal(self.clientID, 'accelerometerY',
vrep.simx_opmode_streaming)
err, accelZ = vrep.simxGetFloatSignal(self.clientID, 'accelerometerZ',
vrep.simx_opmode_streaming)
err, gyroX = vrep.simxGetFloatSignal(self.clientID, 'gyroX',
vrep.simx_opmode_streaming)
err, gyroY = vrep.simxGetFloatSignal(self.clientID, 'gyroY',
vrep.simx_opmode_streaming)
err, gyroZ = vrep.simxGetFloatSignal(self.clientID, 'gyroZ',
vrep.simx_opmode_streaming)
res, euler = vrep.simxGetObjectOrientation(self.clientID,
self.magnHandle, 0,
vrep.simx_opmode_streaming)
def waitForRobot(order, patID):
time.sleep(1)
ptime = None
orderID = 16 * (order == 'linen') + patID
signalTo = 'order_ToVREP'
signalBack = 'ptime_FromVREP'
# Send order ID to be processed
vrep.simxSetIntegerSignal(clientID, signalTo, orderID,
vrep.simx_opmode_oneshot)
# Start the receiving process
err, ptime = vrep.simxGetFloatSignal(clientID, signalBack,
vrep.simx_opmode_streaming)
# While we haven't received anything
while not ptime:
err, ptime = vrep.simxGetFloatSignal(clientID, signalBack,
vrep.simx_opmode_buffer)
if err != vrep.simx_return_ok:
print('!!!\nAn error occured while receiving data from vrep...\n!!!')
# Clear signal
vrep.simxClearFloatSignal(clientID, signalBack, vrep.simx_opmode_oneshot)
# Return the signal received (processing time)
return ptime
def checkGround(self, robotPosition):
r, distance = vrep.simxGetFloatSignal(self.clientID, "GroundDistance",
vrep.simx_opmode_buffer)
r, table = vrep.simxGetStringSignal(self.clientID, "threeDimData",
vrep.simx_opmode_buffer)
if r == vrep.simx_return_ok:
table = vrep.simxUnpackFloats(table)
dim = int((len(table) / 3)**(1 / 2))
if dim * dim * 3 != len(table):
return set()
heights = np.array(table).reshape((dim, dim, 3))[:, :, 0]
cliffs = self.env.analyzeCliffs(
heights) #returns a set of relative locations of cliffs
return cliffs
else:
return set()
def checkProximity(self):
###### DETECT NEW OBSTACLE AND ADD TO OUR ARRAY SPACE GRAPH ############
r, distance = vrep.simxGetFloatSignal(
self.clientID, "ProxDistance", vrep.simx_opmode_buffer
) #retrieves distance of a detected object from child script
error, state, point, handle, vector = vrep.simxReadProximitySensor(
self.clientID, self.proxHandles[0], vrep.simx_opmode_buffer)
if r != vrep.simx_return_ok or distance == -1 or r == 1: #if nothing was detected
return (False, None)
##### IF SOMETHING WAS DETECTED ##########
self.stopRobot()
r, angle = vrep.simxGetObjectOrientation(self.clientID,
self.robotHandle, -1,
vrep.simx_opmode_buffer)
r, currPosition = vrep.simxGetObjectPosition(self.clientID,
self.robotHandle, -1,
vrep.simx_opmode_buffer)
vector = self.env.rotate(vector, angle)
slope = ((vector[0]**2 + vector[1]**2)**(1 / 2)) / vector[2]
slope = -slope if vector[
1] > 0 else slope #we use the y axis as our reference. If the normal vector is facing positive, then it must have a negative slope
xdist = math.cos(angle[2]) * distance
ydist = math.sin(angle[2]) * distance
worldx = xdist + currPosition[0]
worldy = ydist + currPosition[1]
location = (worldx, worldy)
location = self.env.transform(location)
if slope < SLOPETRAVEL: #if the detection has a slope considered travelable
if (location not in self.env.slopes):
self.env.updateSlope(location[0], location[1], slope)
self.env.updateHeight(location[0], location[1], vector)
self.env.slopes.add(location)
'''TODO: update the cost of travelling across this node given that the SLOPE has changed. Make edge weights proportional to the slope'''
return (False, None)
self.env.setMap(location[0], location[1], 2, np.inf)
####### IF IT IS A NEW OBSTACLE, RETURN THE RESULTS #########
neighbors = self.env.neighbors(location)
gValues = [
np.inf == self.env.getMap(n[0], n[1], 0) for n in neighbors
] #to be an obstacle, these all have to be True
if all(gValues):
return (False, None)
return (True, location)
def __init__(self, n_robots, base_name):
self.name = "line follower tester"
self.n_robots = n_robots
self.base_name = base_name
self.robot_names = [self.base_name]
for i in range(self.n_robots - 1):
self.robot_names.append(self.base_name + '#' + str(i))
# Establish connection to V-REP
vrep.simxFinish(-1) # just in case, close all opened connections
# Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
# (requires the simulation to be running)
if self.clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
sys.exit('Could not connect')
# Reset running simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.2)
# Get initial robots' positions
self.robot_handles = []
self.robot_pos0 = []
for rbt_name in self.robot_names:
res, rbt_tmp = vrep.simxGetObjectHandle(
self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
self.robot_handles.append(rbt_tmp)
# Initialize data stream
vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1],
-1, vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name + '_1',
vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name + '_2',
vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name + '_3',
vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in self.robot_handles:
res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1,
vrep.simx_opmode_buffer)
self.robot_pos0.append(pos)
def get_imu(clientID):
err, gX = vrep.simxGetFloatSignal(clientID, 'gyroX',
vrep.simx_opmode_buffer)
err, gY = vrep.simxGetFloatSignal(clientID, 'gyroY',
vrep.simx_opmode_buffer)
err, gZ = vrep.simxGetFloatSignal(clientID, 'gyroZ',
vrep.simx_opmode_buffer)
err, aX = vrep.simxGetFloatSignal(clientID, 'accelX',
vrep.simx_opmode_buffer)
err, aY = vrep.simxGetFloatSignal(clientID, 'accelY',
vrep.simx_opmode_buffer)
err, aZ = vrep.simxGetFloatSignal(clientID, 'accelZ',
vrep.simx_opmode_buffer)
# print('%.2f,%.2f,%.2f,%.2f,%.2f,%.2f') % (gX, gY, gZ, aX, aY, aZ)
return [gX, gY, gZ, aX, aY, aZ]
def __init__(self, n_robots, base_name):
self.name = "line follower tester"
self.n_robots = n_robots
self.base_name = base_name
self.robot_names = [self.base_name]
for i in range(self.n_robots-1):
self.robot_names.append(self.base_name+'#'+str(i))
# Establish connection to V-REP
vrep.simxFinish(-1) # just in case, close all opened connections
# Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
# (requires the simulation to be running)
if self.clientID != -1:
print ('Connected to remote API server')
else:
print ('Failed connecting to remote API server')
sys.exit('Could not connect')
# Reset running simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.2)
# Get initial robots' positions
self.robot_handles = []
self.robot_pos0 = []
for rbt_name in self.robot_names:
res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
self.robot_handles.append(rbt_tmp)
# Initialize data stream
vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in self.robot_handles:
res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
self.robot_pos0.append(pos)
def read_state(self):
# Retriece Center of Mass Values
return_code1, com_x = vrep.simxGetFloatSignal(self.client_id, 'COM_x',
self.opmode)
return_code2, com_y = vrep.simxGetFloatSignal(self.client_id, 'COM_y',
self.opmode)
return_code3, com_z = vrep.simxGetFloatSignal(self.client_id, 'COM_z',
self.opmode)
# if return_code1 != 0 or return_code2 != 0 or return_code3 != 0:
# raise RuntimeError('Read state failed!')
# Retrieve quaternion values
return_code1, q1 = vrep.simxGetFloatSignal(self.client_id, 'q1',
self.opmode)
return_code2, q2 = vrep.simxGetFloatSignal(self.client_id, 'q2',
self.opmode)
return_code3, q3 = vrep.simxGetFloatSignal(self.client_id, 'q3',
self.opmode)
return_code4, q4 = vrep.simxGetFloatSignal(self.client_id, 'q4',
self.opmode)
# if return_code1 != 0 or return_code2 != 0 or return_code3 != 0 or return_code4 != 0:
# raise RuntimeError('Read state failed!')
if q1 < 0:
q1 = -q1
q2 = -q2
q3 = -q3
q4 = -q4
state_vector = [com_x, com_y, com_z, q1, q2, q3, q4]
leftArm_pk = self.leftArm.get_joints_position(self.client_id)
#leftArm_pk[1]=0
#print(leftArm_pk)#Modify to introduce sensor(State Read) based faults
state_vector += leftArm_pk
rightArm_pk = self.rightArm.get_joints_position(self.client_id)
#rightArm_pk[1]=0
state_vector += rightArm_pk
state_vector += self.leftLeg.get_joints_position(self.client_id)
state_vector += self.rightLeg.get_joints_position(self.client_id)
return numpy.array(state_vector)
#Get motor handle
errorCode,left_motor=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
errorCode,right_motor=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
# Get ultrasound sensors handle
errorCode,usensor1=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor8',vrep.simx_opmode_oneshot_wait)
# Get camera sensors handle
errorCode,cam_handle=vrep.simxGetObjectHandle(clientID,'Camera',vrep.simx_opmode_oneshot_wait)
errorCode,resolution,image=vrep.simxGetVisionSensorImage(clientID,cam_handle,0,vrep.simx_opmode_oneshot_wait)
pX = pY = 0.0
for i in range (1,30) :
errorCode,gpsX=vrep.simxGetFloatSignal(clientID,'gpsX',vrep.simx_opmode_oneshot_wait);
errorCode,gpsY=vrep.simxGetFloatSignal(clientID,'gpsY',vrep.simx_opmode_oneshot_wait);
pX += gpsX
pY += gpsY
offSetX = (pX/30) - initX
offSetY = (pY/30) - initY
for i in range(1,11):
#while True :
print 'Iteration : ', i
# Get ultrasound sensors handle
#errorCode,usensor1=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor8',vrep.simx_opmode_oneshot_wait)
# Read ultrasound sensor
def _read_sensors(self):
"""
This method is used for read the ePuck's sensors. Don't use directly,
instead use 'step()'
"""
# Read differents sensors
for s in self._sensors_to_read:
if s == 'a':
# Accelerometer sensor in a non filtered way
if self._accelerometer_filtered:
parameters = ('A', 12, '@III')
else:
parameters = ('a', 6, '@HHH')
self._debug('WARNING: Accelerometer not yet implemented!')
elif s == 'n':
# Proximity sensors
res, prox = vrep.simxGetStringSignal(self._clientID, 'EPUCK_proxSens', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Proximity sensors readout failed: ", res)
else:
proxVals = vrep.simxUnpackFloats(prox)
# TODO: Find out actual needed scaling factor
proxVals = [int(x * 1000) for x in proxVals]
self._proximity = tuple(proxVals)
elif s == 'm':
# Floor sensors
res, floor1 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_0', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 1 sensor readout failed: ", res)
res, floor2 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_1', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 2 sensor readout failed: ", res)
res, floor3 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_2', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 3 sensor readout failed: ", res)
# Scale returned values to mimic real robot; current factor is just guessed
self._floor_sensors = (floor1*1800, floor2*1800, floor3*1800)
elif s == 'q':
# Motor position sensor
# First: Get the handles of both motor joints
res, leftMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Unable to get handle of left motor: ", res)
continue
res, rightMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Unable to get handle of right motor: ", res)
continue
# Second: Get the actual motor position (in radians)
res, leftPos = vrep.simxGetJointPosition(self._clientID, leftMotor, vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
res, rightPos = vrep.simxGetJointPosition(self._clientID, rightMotor, vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
self._motor_position = (leftPos, rightPos)
elif s == 'o':
# Light sensors
parameters = ('O', 16, '@HHHHHHHH')
self._debug('WARNING: Light sensors not yet implemented!')
elif s == 'u':
# Microphone
parameters = ('u', 6, '@HHH')
self._debug('WARNING: Microphones not yet implemented!')
elif s == 'e':
# Motor Speed
parameters = ('E', 4, '@HH')
self._debug('WARNING: Motor speed not yet implemented!')
elif s == 'i':
# Do nothing for the camera, is an independent process
pass
else:
self._debug('Unknow type of sensor to read')
def float_signal(cid, handle, signal_name):
err, val = vrep.simxGetFloatSignal(cid, signal_name, vrep_mode)
return [val]
k=i.split(',')
for j in range(2):
k[j]=float(k[j])
fpath.append(k)
print
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
else:
print('Connection unsuccessful!')
sys.exit("Could not connect")
errorCode,left_motor_handle=vrep.simxGetObjectHandle(clientID,'wheel_left_joint',vrep.simx_opmode_blocking)
errorCodeR,right_motor_handle=vrep.simxGetObjectHandle(clientID,'wheel_right_joint',vrep.simx_opmode_blocking)
returncode,bot = vrep.simxGetObjectHandle(clientID,'Turtlebot2',vrep.simx_opmode_blocking)
err,simtime = vrep.simxGetFloatSignal(clientID,'Turtlebot2_simulation_time',vrep.simx_opmode_streaming)
for i in fpath:
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,float(i[0]),vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,float(i[1]),vrep.simx_opmode_streaming)
_,simtime1 = vrep.simxGetFloatSignal(clientID,'Turtlebot2_simulation_time',vrep.simx_opmode_buffer)
_,simtime2 = vrep.simxGetFloatSignal(clientID,'Turtlebot2_simulation_time',vrep.simx_opmode_buffer)
while (simtime2 - simtime1) < 1.5:
_,simtime2 = vrep.simxGetFloatSignal(clientID,'Turtlebot2_simulation_time',vrep.simx_opmode_buffer)
vrep.simxSetJointTargetVelocity(clientID,left_motor_handle,0,vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID,right_motor_handle,0,vrep.simx_opmode_blocking)
vrep.simxFinish(-1)
def accelerometer(cid, handle):
err, accel_x = vrep.simxGetFloatSignal(cid, 'accelerometerX', vrep_mode)
err, accel_y = vrep.simxGetFloatSignal(cid, 'accelerometerY', vrep_mode)
err, accel_z = vrep.simxGetFloatSignal(cid, 'accelerometerZ', vrep_mode)
return [accel_x, accel_y, accel_z]
clientID, h_feature6, -1, vrep.simx_opmode_oneshot_wait)
error_code, feat7pos = vrep.simxGetObjectPosition(
clientID, h_feature7, -1, vrep.simx_opmode_oneshot_wait)
error_code, feat8pos = vrep.simxGetObjectPosition(
clientID, h_feature8, -1, vrep.simx_opmode_oneshot_wait)
error_code, feat9pos = vrep.simxGetObjectPosition(
clientID, h_feature9, -1, vrep.simx_opmode_oneshot_wait)
featposlist = [
feat1pos, feat2pos, feat3pos, feat4pos, feat5pos, feat6pos, feat7pos,
feat8pos, feat9pos
]
featpos = np.asarray(featposlist)
# Intitiate required V_REP odometry information
error_code, simtime1 = vrep.simxGetFloatSignal(
clientID, 'vrep_simtime',
vrep.simx_opmode_streaming) #some error_code here for some reason
error_code, theta_L1 = vrep.simxGetJointPosition(
clientID, h_motor_left, vrep.simx_opmode_streaming) # error_code
error_code, theta_R1 = vrep.simxGetJointPosition(
clientID, h_motor_right, vrep.simx_opmode_streaming) # error_code
time.sleep(0.1) # try to wait for vrep in order to get correct values
error_code, theta_R1 = vrep.simxGetJointPosition(clientID, h_motor_right,
vrep.simx_opmode_buffer)
error_code, simtime1 = vrep.simxGetFloatSignal(clientID, 'vrep_simtime',
vrep.simx_opmode_buffer)
error_code, theta_L1 = vrep.simxGetJointPosition(clientID, h_motor_left,
vrep.simx_opmode_buffer)
R_L = 0.1 / 2 # Radius of left wheel
R_R = R_L
B = 0.2 # Length between front and back wheels
#ping pong ball
#initialization of two balls
res, ball0 = vrep.simxGetObjectHandle(clientID, 'Sphere',
vrep.simx_opmode_blocking)
res, ballpos = vrep.simxGetObjectPosition(clientID, ball0, -1,
vrep.simx_opmode_streaming)
time.sleep(1)
res, ballOrigin = vrep.simxGetObjectPosition(clientID, ball0, -1,
vrep.simx_opmode_buffer)
time.sleep(1)
# #make ball jump
# ret_code, _, _, _, _ = vrep.simxCallScriptFunction(clientID, 'Sphere', vrep.sim_scripttype_childscript, 'shootBall', [], [], [], bytearray(), vrep.simx_opmode_blocking)
res, xval = vrep.simxGetFloatSignal(clientID, "xtarget",
vrep.simx_opmode_streaming)
res, yval = vrep.simxGetFloatSignal(clientID, "ytarget",
vrep.simx_opmode_streaming)
res, zval = vrep.simxGetFloatSignal(clientID, "ztarget",
vrep.simx_opmode_streaming)
time.sleep(1)
#while(1):
#make ball jump
#res,ballcurr = vrep.simxGetObjectPosition(clientID,ball0,-1,vrep.simx_opmode_buffer)
#if ballcurr[0] > -0.2:
# time.sleep(0.5)
# res = vrep.simxSetObjectPosition(clientID,ball0,-1,ballOrigin,vrep.simx_opmode_oneshot)
# time.sleep(1)
ret_code, _, force, _, _ = vrep.simxCallScriptFunction(
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
time = 0
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'wheel_left_joint', vrep.simx_opmode_blocking)
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'wheel_right_joint', vrep.simx_opmode_blocking)
r, signalValue = vrep.simxGetFloatSignal(clientID,
'Turtlebot2_simulation_time',
vrep.simx_opmode_streaming)
path_speeds = action_sequence[::-1]
for k in path_speeds:
time = 0
err_code1 = 1
err_code2 = 2
while (err_code1 != 0 and err_code2 != 0):
err_code1 = vrep.simxSetJointTargetVelocity(
clientID, left_motor_handle, k[0], vrep.simx_opmode_streaming)
err_code2 = vrep.simxSetJointTargetVelocity(
clientID, right_motor_handle, k[1], vrep.simx_opmode_streaming)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for sixth joint')
# Get "handle" to the end-effector of robot
result, end_handle = vrep.simxGetObjectHandle(clientID, 'UR10_link7_visible',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for end effector')
# ==================================================================================================== #
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# ******************************** Your robot control code goes here ******************************** #
time.sleep(0.1)
counter = vrep.simxGetFloatSignal(clientID, "counter",
vrep.simx_opmode_streaming)[1]
vrep.simxSetFloatSignal(clientID, "RG2_open", 1, vrep.simx_opmode_oneshot)
previousCounter = counter
# Goal_joint_angles = np.array([[0,0,-0.5*np.pi,0.5*np.pi,-0.5*np.pi,-0.5*np.pi], \
# [0.5*np.pi,0,-0.5*np.pi,0.5*np.pi,0.5*np.pi,-0.5*np.pi],\
# [-0.5*np.pi,-0.5*np.pi,-0.5*np.pi,0,-0.5*np.pi,-0.5*np.pi]])
Goal_joint_angles = np.array([\
[0,0,0,0,0,0], \
[0.5*np.pi,0,0,0,0,0], \
[0,0.25*np.pi,0,0,0,0], \
[0,0,0.5*np.pi,0,0,0], \
[0,0,0,0.5*np.pi,0,0], \
[0,0,0,0,0.5*np.pi,0], \
[0,0,0,0,0,0.5*np.pi], \
# [0,0,0.45*np.pi,0,-0.5*np.pi,0], \
[0,0.1*np.pi,0.35*np.pi,0.05*np.pi,-0.5*np.pi,0], \
reward_pool = []
steps = 0
for e in range(num_episode):
print('Episode : ', e)
time.sleep(1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
err_code, target = vrep.simxGetObjectHandle(clientID,
"Quadricopter_target",
vrep.simx_opmode_blocking)
err_code, bob = vrep.simxGetObjectHandle(clientID, "Sphere",
vrep.simx_opmode_blocking)
t = time.time()
err_code, pos = vrep.simxGetObjectPosition(clientID, target, -1,
vrep.simx_opmode_streaming)
err_code, vel = vrep.simxGetFloatSignal(clientID, 'vel2',
vrep.simx_opmode_streaming)
err_code, pos_b_x = vrep.simxGetFloatSignal(clientID, 'pos_b_x',
vrep.simx_opmode_streaming)
err_code, pos_b_z = vrep.simxGetFloatSignal(clientID, 'pos_b_z',
vrep.simx_opmode_streaming)
angle = getAngle(pos_b_x, pos_b_z)
state = [[pos[0], vel, angle]]
state = torch.FloatTensor(state)
state = Variable(state)
rev = 0
steps = 0
while steps < 1000:
while pos[0] < 0.001 and pos[0] > -0.001:
err_code, pos = vrep.simxGetObjectPosition(
clientID, target, -1, vrep.simx_opmode_streaming)
err_code, vel = vrep.simxGetFloatSignal(clientID, 'vel2',
def braiten2a(self):
"Seek light source"
"PARAMETERS"
intens = 100
ambientIntensRatio = 0
attVect = [0,0,1]
HOMEODIR = '/home/stefano/Documents/Projects/Homeostat/Simulator/Python-port/Homeo/'
dataDir = 'SimsData-'+strftime("%Y-%m-%d-%H-%M-%S", localtime(time()))
simsDataDir = os.path.join(HOMEODIR,"SimulationsData",dataDir)
os.mkdir(simsDataDir)
print "Saving to: ", simsDataDir
e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Message sent, error code: ", e
"END PARAMETERS"
for run in xrange(self.noRuns):
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Simulation started: run number %d, error code: %d"% (run+1, eCode)
"Wait until simulation is ready, otherwise we will miss a few movement commands"
# sleep(2)
np.random.seed(64)
# resetRobotInitPose(initPose, self.simulID, ePuckHandle)
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("NEWFILE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Starting a new trajectory file"
else:
print "ERROR: Could not start a new trajectory file"
timeStart = time()
for step in xrange(self.noSteps):
rightLight = vrep.simxGetFloatSignal(self.simulID, "HOMEO_SIGNAL_rightEye_LIGHT_READING", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftLight = vrep.simxGetFloatSignal(self.simulID, "HOMEO_SIGNAL_leftEye_LIGHT_READING", vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
# print "rightLight %.3f\t left light: %.3f" %(rightLight[1],leftLight[1])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, clip(leftLight[1],0,self.maxSpeed), vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, clip(rightLight[1],0, self.maxSpeed), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
timeElapsed = time() - timeStart
"Stop the robot"
self.stopRobot(self.simulID, [self.rightMotor, self.leftMotor])
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("SAVE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Saving trajectory file"
else:
print "ERROR: Could not save a new trajectory file"
sleep(.5)
robotPose = vrep.simxGetObjectPosition(self.simulID, self.robotHandle, -1, vrep.simx_opmode_oneshot_wait)[1][:2]
vrep.simxSynchronousTrigger(self.simulID)
print "%d: Robot is at: %.3f, %.3f Distance from target is: %.4f. Run took exactly %.3f seconds" % (run,
robotPose[0],
robotPose[1],
self.computeDistance(self.targetPose, robotPose),
timeElapsed) #
eCode = vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(1)
# eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait)
# vrep.simxSynchronousTrigger(self.simulID)
eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("CLOSEFILE"), vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
if eCode == 0:
print "Starting a new trajectory file"
else:
print "ERROR: Could not close a new trajectory file"
print "Done"
def run_trial(self, genomes):
# Set the signals for each robot
for genome, robot in zip(genomes, self.robot_names):
par = [genome[:4], genome[4:]]
for j in range(len(par)):
# For each motor
for k in range(len(par[j])):
# For each sensor (+ base value)
signal_name = robot+"_"+str(j+1)+"_"+str(k+1)
res = vrep.simxSetFloatSignal(self.clientID, signal_name, par[j][k],
vrep.simx_opmode_oneshot)
if res > 1:
print 'Error setting signal '+signal_name+': '+str(res)
# Start running simulation
# print 'Running trial'
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
# Initialize output arrays
sim_time = [[] for i in range(self.n_robots)]
robot_x = [[] for i in range(self.n_robots)]
robot_y = [[] for i in range(self.n_robots)]
init_pos = [[] for i in range(self.n_robots)]
fit_y = [0 for i in range(self.n_robots)]
t_step = 0.01 # how often to check the simulation's signals
t_flag = time.time()+t_step
once = True
new_data = [0 for i in range(self.n_robots)]
go_loop = True
while go_loop:
now = time.time()
if now > t_flag:
t_flag = now+t_step
# Get info from robot
for i in range(self.n_robots):
res1, in1 = vrep.simxGetFloatSignal(self.clientID,
self.robot_names[i]+'_1', vrep.simx_opmode_buffer)
res2, in2 = vrep.simxGetFloatSignal(self.clientID,
self.robot_names[i]+'_2', vrep.simx_opmode_buffer)
res3, in3 = vrep.simxGetFloatSignal(self.clientID,
self.robot_names[i]+'_3', vrep.simx_opmode_buffer)
if res1 == 0 and res2 == 0 and res3 == 0:
sim_time[i].append(in1)
robot_x[i].append(in2)
robot_y[i].append(in3)
new_data[i] = 1 # new data arrived
if once:
if sum(new_data) == self.n_robots:
for i in range(self.n_robots):
init_pos[i] = [robot_x[i][-1], robot_y[i][-1]]
once = False
else:
for i in range(self.n_robots):
if new_data[i] == 1:
fit_y[i] += abs(robot_y[i][-1])*(sim_time[i][-1]-sim_time[i][-2])
new_data[i] = 0
if sim_time[i][-1] > 3: # time limit for the simulation
go_loop = False
break
# # Pause the simulation
# vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot)
# time.sleep(0.15)
#
# trial_fitness = []
# # Get the trial results for each robot
# for i in range(len(self.robot_handles)):
# res, pos = vrep.simxGetObjectPosition(self.clientID, self.robot_handles[i], -1, vrep.simx_opmode_buffer)
# # print pos_tmp
#
# delta_x = pos[0] - self.robot_pos0[i][0]
# delta_y = pos[1] - self.robot_pos0[i][1]
#
# # trial_fitness.append(delta_x)
# trial_fitness.append([delta_x, 1./(1.+20*abs(delta_y))])
trial_fitness = []
for i in range(self.n_robots):
x_fit = robot_x[i][-1] - init_pos[i][0]
y_fit = 1/(1+20*fit_y[i])
trial_fitness.append([x_fit, y_fit])
# Stop and reset the simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.35)
return trial_fitness
def gyro(cid, handle):
err, accel_x = vrep.simxGetFloatSignal(cid, 'gyroX', vrep_mode)
err, accel_y = vrep.simxGetFloatSignal(cid, 'gyroY', vrep_mode)
err, accel_z = vrep.simxGetFloatSignal(cid, 'gyroZ', vrep_mode)
return [accel_x, accel_y, accel_z]
]
t = 0
# Iterate over the data and write it out row by row.
for i, default in enumerate(defaults):
worksheet.write(0, i, default)
i += 1
i = 1
try:
while i < 101:
# Pause Simulation
#vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
# Gyro Get
gyroX = vrep.simxGetFloatSignal(clientID, 'gyroX',
vrep.simx_opmode_oneshot)[1]
gyroY = vrep.simxGetFloatSignal(clientID, 'gyroY',
vrep.simx_opmode_oneshot)[1]
gyroZ = vrep.simxGetFloatSignal(clientID, 'gyroZ',
vrep.simx_opmode_oneshot)[1]
#print("Gyro : {:.5f} {:.5f} {:.5f}".format(gyroX, gyroY, gyroZ))
# Accelerometer Get
accelX = vrep.simxGetFloatSignal(clientID, 'accelerometerX',
vrep.simx_opmode_oneshot)[1]
accelY = vrep.simxGetFloatSignal(clientID, 'accelerometerY',
vrep.simx_opmode_oneshot)[1]
accelZ = vrep.simxGetFloatSignal(clientID, 'accelerometerZ',
vrep.simx_opmode_oneshot)[1]
#print("Accelerometer : {:.5f} {:.5f} {:.5f}".format(accelX, accelY, accelZ))
def _read_sensors(self):
"""
This method is used for read the ePuck's sensors. Don't use directly,
instead use 'step()'
"""
# Read differents sensors
for s in self._sensors_to_read:
if s == 'a':
# Accelerometer sensor in a non filtered way
if self._accelerometer_filtered:
parameters = ('A', 12, '@III')
else:
parameters = ('a', 6, '@HHH')
self._debug('WARNING: Accelerometer not yet implemented!')
elif s == 'n':
# Proximity sensors
res, prox = vrep.simxGetStringSignal(
self._clientID, 'EPUCK_proxSens',
vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Proximity sensors readout failed: ",
res)
else:
proxVals = vrep.simxUnpackFloats(prox)
# TODO: Find out actual needed scaling factor
proxVals = [int(x * 1000) for x in proxVals]
self._proximity = tuple(proxVals)
elif s == 'm':
# Floor sensors
res, floor1 = vrep.simxGetFloatSignal(
self._clientID, 'EPUCK_mylightSens_0',
vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 1 sensor readout failed: ",
res)
res, floor2 = vrep.simxGetFloatSignal(
self._clientID, 'EPUCK_mylightSens_1',
vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 2 sensor readout failed: ",
res)
res, floor3 = vrep.simxGetFloatSignal(
self._clientID, 'EPUCK_mylightSens_2',
vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 3 sensor readout failed: ",
res)
# Scale returned values to mimic real robot; current factor is just guessed
self._floor_sensors = (floor1 * 1800, floor2 * 1800,
floor3 * 1800)
elif s == 'q':
# Motor position sensor
# First: Get the handles of both motor joints
res, leftMotor = vrep.simxGetObjectHandle(
self._clientID, 'ePuck_leftJoint',
vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug(
"WARNING: Unable to get handle of left motor: ", res)
continue
res, rightMotor = vrep.simxGetObjectHandle(
self._clientID, 'ePuck_rightJoint',
vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug(
"WARNING: Unable to get handle of right motor: ", res)
continue
# Second: Get the actual motor position (in radians)
res, leftPos = vrep.simxGetJointPosition(
self._clientID, leftMotor, vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
res, rightPos = vrep.simxGetJointPosition(
self._clientID, rightMotor, vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
self._motor_position = (leftPos, rightPos)
elif s == 'o':
# Light sensors
parameters = ('O', 16, '@HHHHHHHH')
self._debug('WARNING: Light sensors not yet implemented!')
elif s == 'u':
# Microphone
parameters = ('u', 6, '@HHH')
self._debug('WARNING: Microphones not yet implemented!')
elif s == 'e':
# Motor Speed
parameters = ('E', 4, '@HH')
self._debug('WARNING: Motor speed not yet implemented!')
elif s == 'i':
# Do nothing for the camera, is an independent process
pass
else:
self._debug('Unknow type of sensor to read')
