def reset_robot_pos(self):
#reset robot position to origin
returnCode = vrep.simxRemoveModel(self.clientID, self.robot_handle,
vrep.simx_opmode_oneshot_wait)
#print("Removing robot, robot handler:",self.robot_handle," | return code:",returnCode)
returnCode, self.robot_handle = vrep.simxGetObjectHandle(
self.clientID, 'turtlebot2i', vrep.simx_opmode_oneshot_wait)
while (returnCode == 0):
returnCode = vrep.simxRemoveModel(self.clientID, self.robot_handle,
vrep.simx_opmode_oneshot_wait)
rospy.loginfo(
"Previous removal failed. Remove robot again, robot handler:",
self.robot_handle, " | return code:", returnCode)
returnCode, self.robot_handle = vrep.simxGetObjectHandle(
self.clientID, 'turtlebot2i', vrep.simx_opmode_oneshot_wait)
returnCode, self.robot_handle = vrep.simxLoadModel(
self.clientID, self.model_location, 0,
vrep.simx_opmode_oneshot_wait)
#print("Loading robot, robot handler:",self.robot_handle," | return code:",returnCode)
while (returnCode != 0):
returnCode, self.robot_handle = vrep.simxLoadModel(
self.clientID, self.model_location, 0,
vrep.simx_opmode_oneshot_wait)
rospy.loginfo(
"Previous loading failed. Reload robot. robot handler:",
self.robot_handle, " | return code:", returnCode)
def seat(
self, data, chair
): #Seats person at desired chair. In VREP, switches model from a standing bill to a sitting bill
data.bills.remove(self.billID)
data.billID = getBillID(data.bills)
self.type = "Seated"
self.location = chair.location
self.vrepOrientation = [0, 0, chair.vrepOrientation[2] - math.pi / 2]
if self.vrepID != None:
vrep.simxRemoveModel(clientID, self.vrepID,
vrep.simx_opmode_oneshot)
self.vrepPos = [
chair.vrepPos[0],
chair.vrepPos[1] - math.cos(chair.vrepOrientation[2]) * -0.05, 0
]
error, self.vrepID = vrep.simxLoadModel(
clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
clientID, self.vrepID, -1,
(self.vrepPos[0], self.vrepPos[1], self.vrepPos[2] - 0.4),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, self.vrepID, -1,
self.vrepOrientation,
vrep.simx_opmode_oneshot)
error, self.rightLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, rLeg = vrep.simxGetJointPosition(clientID, self.rightLegID,
vrep.simx_opmode_streaming)
error, self.leftLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.neckID = vrep.simxGetObjectHandle(
clientID, 'Bill_neck' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
def reset(self):
_, robot = vrep.simxGetObjectHandle(self.clientID, 'Pioneer_p3dx',
vrep.simx_opmode_oneshot_wait)
# vrep.simxResetDynamicObject(self.clientID,robot)
vrep.simxRemoveModel(self.clientID, robot,
vrep.simx_opmode_oneshot_wait)
# vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot)
# a,b=vrep.simxLoadModel(self.clientID, '/home/rip/Downloads/V-REP_PRO_EDU_V3_4_0_Linux/models/Custom_model/Pioneer_p3dx.ttm',0, vrep.simx_opmode_blocking)
a, b = vrep.simxLoadModel(
self.clientID,
'/home/rip/BTP/V-REP_PRO_EDU_V3_4_0_Linux/models/Custom_model/Poineer_p3dx_3_sensors.ttm',
0, vrep.simx_opmode_blocking)
# _,prop = vrep.simxSetModelProperty(self.clientID,robot, vrep.sim_modelproperty_scripts_inactive,vrep.simx_opmode_oneshot)
#
# errorCode=vrep.simxSetJointTargetVelocity(self.clientID,self.left_motor_handle,0, vrep.simx_opmode_streaming)
# errorCode=vrep.simxSetJointTargetVelocity(self.clientID,self.right_motor_handle,0, vrep.simx_opmode_streaming)
# time.sleep(4)
errorCode, self.left_motor_handle = vrep.simxGetObjectHandle(
self.clientID, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_oneshot_wait)
errorCode, self.right_motor_handle = vrep.simxGetObjectHandle(
self.clientID, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_oneshot_wait)
# Preallocaation
self.sensor_h = [] #empty list for handles
sensor_val = np.array([]) #empty array for sensor measurements
#for loop to retrieve sensor arrays and initiate sensors
for x in range(1, 16 + 1):
errorCode, sensor_handle = vrep.simxGetObjectHandle(
self.clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(x),
vrep.simx_opmode_oneshot_wait)
self.sensor_h.append(sensor_handle) #keep list of handles
errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
self.clientID, sensor_handle, vrep.simx_opmode_streaming)
sensor_val = np.append(
sensor_val, np.linalg.norm(detectedPoint)) #get list of values
print(sensor_val)
# vrep.simxSetObjectPosition(self.clientID,robot,-1,[-1.4945,0.77500,0.13879],vrep.simx_opmode_oneshot_wait)
# vrep.simxSetJointPosition(self.clientID,self.left_motor_handle,-1,vrep.simx_opmode_oneshot)
# vrep.simxSetJointPosition(self.clientID,self.right_motor_handle,-1,vrep.simx_opmode_oneshot)
time.sleep(0.2)
# vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
state, done = self.discretize_observation(3)
return state
def reset(self):
""" reset the state
retruns: the current state after reset
"""
reset_object(self.client_id, self.target_reset)
vrep.simxRemoveModel(self.client_id, self.ref_frame,
vrep.simx_opmode_oneshot_wait)
vrep.simxLoadModel(
self.client_id,
"/home/user/V-REP/models/robots/mobile/pioneer_p3dx_script_disabled.ttm",
1, vrep.simx_opmode_oneshot_wait)
self._load_robot_handles()
self.vleft = 0
self.vright = 0
return self.get_state().to_array()
def remove_all_turtlebot2i(self):
turtlebot2i_namelist = [
'turtlebot2i', 'turtlebot2i#0', 'turtlebot2i#1', 'turtlebot2i#2',
'turtlebot2i#3', 'turtlebot2i#4', 'turtlebot2i#5', 'turtlebot2i#6',
'turtlebot2i#7', 'turtlebot2i#8', 'turtlebot2i#9',
'turtlebot_body_visual', 'turtlebot_reference',
'plate_middle_link_visual', 'plate_middle_link_respondable', 'GPS'
]
for turtlebot2i_name in turtlebot2i_namelist:
returnCode, temp_robot_handle = vrep.simxGetObjectHandle(
self.clientID, turtlebot2i_name, vrep.simx_opmode_oneshot_wait)
returnCode = vrep.simxRemoveModel(self.clientID, temp_robot_handle,
vrep.simx_opmode_oneshot_wait)
def _reset(self):
self.resetFlag = vrep.simxRemoveModel(
clientID=self.clientID,
objectHandle=self.robotObj,
operationMode=vrep.simx_opmode_streaming) #_wait
# print('Close Flag:', self.resetFlag)
time.sleep(2.0)
self.resetFlag, self.robotObj = vrep.simxLoadModel(
clientID=self.clientID,
modelPathAndName=
'/Users/xuanliu/Documents/V-REP_PRO_EDU_V3_4_0_Mac/models/robots/mobile/ACM-R5-disabled.ttm',
operationMode=vrep.simx_opmode_oneshot_wait, # _wait
options=0)
def remove(self):
"""Remove model from scene."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError("Could not remove {}: missing name or "
"handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not remove {}: object already "
"removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not remove {}: not connected to V-REP remote API "
"server.".format(self._name))
res = vrep.simxRemoveModel(client_id, self._handle,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not remove {}.".format(self._name))
self.set_removed()
def removeModel(clientID, name):
res, toRemove = vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)
vrep.simxRemoveModel(clientID, toRemove, vrep.simx_opmode_oneshot)
def analyze_model(clientID, model_path_and_name, qty_poses=9, save_location='imgs/models/'):
"""
Takes several qty_poses 'pictures' of a VREP model in different
orientations. Saves each of the pictures in save_location.
Each of the picture will be SIFT-described for instance detection.
clientID is the VREP scene ID.
qty_poses must be divisible by 3
"""
assert qty_poses % 3 == 0, 'qty_poses must be divisible by 3'
# finding model name
model_name = ''
for i in model_path_and_name:
if i == '/':
model_name = ''
elif i == '.':
break
else:
model_name += i
# creating models directory, if not existent
if model_name not in os.listdir(save_location):
os.mkdir(save_location + '/' + model_name)
# loading model and positioning
res, model_handle = vrep.simxLoadModel(clientID, modelPathAndName=model_path_and_name, options=1, operationMode=vrep.simx_opmode_oneshot_wait)
res = vrep.simxSetObjectPosition(clientID, model_handle, -1, (-0.8, 0, 5), vrep.simx_opmode_oneshot)
count = 0
# grabbing camera2 image, saving it and rotating model
if clientID!=-1:
res,v1=vrep.simxGetObjectHandle(clientID,'camera2',vrep.simx_opmode_oneshot_wait)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v1,0,vrep.simx_opmode_streaming)
while (vrep.simxGetConnectionId(clientID)!=-1) and count < qty_poses:
cv2.waitKey(200)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v1,0,vrep.simx_opmode_buffer)
if res==vrep.simx_return_ok:
#cv2.waitKey(200)
#img = img2rgbnp(image, resolution)
#cv2.imshow('foo', img)
#cv2.waitKey(50)
# Cases for count:
# when count < qty_poses / 3, poses are obtained rotating x axis
# when count >= qty_poses / 3 and < qty_poses*2/3, poses are obtained rotating y axis
# else, poses are obtained rotating z axis
"""
if count < qty_poses / 3:
cv2.waitKey(200)
res = vrep.simxSetObjectOrientation(clientID, model_handle,\
#model_handle, (360*3/qty_poses, 0, 0), vrep.simx_opmode_oneshot)
model_handle, (2*np.pi*3/qty_poses, 0, 0), vrep.simx_opmode_oneshot)
cv2.waitKey(200)
elif count < (qty_poses*2/3):
cv2.waitKey(200)
res = vrep.simxSetObjectOrientation(clientID, model_handle,\
model_handle, (0, 2*np.pi*3/qty_poses, 0), vrep.simx_opmode_oneshot)
cv2.waitKey(200)
else:
cv2.waitKey(200)
res = vrep.simxSetObjectOrientation(clientID, model_handle,\
model_handle, (0, 0, 2*np.pi*3/qty_poses), vrep.simx_opmode_oneshot)
cv2.waitKey(200)
"""
cv2.waitKey(200)
res = vrep.simxSetObjectOrientation(clientID, model_handle,\
model_handle, (0, 2*np.pi/qty_poses, 0), vrep.simx_opmode_oneshot)
cv2.waitKey(200)
#else:
# break
cv2.waitKey(200)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v1,0,vrep.simx_opmode_buffer)
img = img2rgbnp(image, resolution)
cv2.imwrite(save_location +'/' + model_name + '/' + model_name + '_' + str(count) + '.png', img)
count += 1
ret = vrep.simxRemoveModel(clientID, model_handle, vrep.simx_opmode_oneshot)
def learning():
frames = 100000
observation = int(frames/100)
instances = 5
epsilon = 1
batchSize = 60
buffer = 500
replay = []
gamma = 0.9
bufferCount = 0
sensorInformation = np.array([])
#capturing the current state
returnCode,collisionState=vrep.simxReadCollision(clientID,cH,vrep.simx_opmode_streaming)
for x in range(1,16+1):
errorCodeProximity3,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_streaming)
for x in range(1,16+1):
errorCodeProximity3,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_buffer)
sensorInformation=np.append(sensorInformation,np.linalg.norm(detectedPoint)) #get list of values
#sensorInformation=np.append(sensorInformation,detectedPoint[2])
print(detectedPoint)
state=sensorInformation
t = time.time()
# no iter
for i in range(frames):
noOfCollisions=0
#sensorInformation = np.array([])
#check for randomness or for intially observation before training begins
if (random.random() < epsilon) or (i < observation):
action = random.randint(0,4)
else:
qVal = model.predict(state)
action = np.argmax(qVal)
new_state = makeMove(state,action)
reward = rewardFunction(state,action)
replay.append((state,action,reward,new_state))
if i > observation:
if len(replay) > buffer:
replay.pop(0)
#!!!!!check for batchSize, it may not be correct
minibatch = random.sample(replay,batchSize)
Xtrain , ytrain = mini_batch_processing(minibatch,model)
#==============================================================================
# for j in range(batchSize):
# print("j is ",j)
#
#==============================================================================
for i in range(batchSize):
model.fit(Xtrain[i], ytrain[i], batch_size = 1, epochs = 1 , verbose = 1 )
state = new_state
if epsilon > 0.1 and i > observation:
epsilon -= (1/frames)
bufferCount+=1
print("\n\n\n\n\nGame no: " ,bufferCount)
print(state)
k=""
with open('stateFile.txt', 'a') as the_file:
for i in range(len(state)):
k=k+","+str(state[i])
the_file.write(k+"\n")
#write to file
time.sleep(0.02)
returnCode,collisionState=vrep.simxReadCollision(clientID,cH,vrep.simx_opmode_buffer)
print("Collison State: " , collisionState)
#if np.amin(state) >= 0.7 and bufferCount>10:
if collisionState==True:
sys.exit("HERE: ")
vrep.simxRemoveModel(clientID,modelHandle,vrep.simx_opmode_oneshot)
vrep.simxLoadModel(clientID,modelHandle,0,vrep.simx_opmode_blocking)
def makeFile(walls, obstacles, thermal, visual, startPos, uiWindow=None):
'''Create a file data string from the positions and scales'''
try:
returnCode, handler = vrep.simxGetObjectHandle(
clientID=clientID,
objectName="ResizableFloor_5_25",
operationMode=vrep.simx_opmode_blocking)
vrep.simxRemoveModel(clientID=clientID,
objectHandle=handler,
operationMode=vrep.simx_opmode_oneshot)
except:
print("Can not remove the ResizableFloor_5_25")
#Strings to hold the tile parts
allTiles = ""
#Strings to hold the boundaries for special tiles
allCheckpointBounds = ""
allTrapBounds = ""
allGoalBounds = ""
allSwampBounds = ""
#Upper left corner to start placing tiles from
width = len(walls[0])
height = len(walls)
startX = -(len(walls[0]) * 0.25 / 2.0)
startZ = -(len(walls) * 0.25 / 2.0)
#Id numbers used to give a unique but interable name to tile pieces
tileId = 0
checkId = 0
trapId = 0
goalId = 0
swampId = 0
#Iterate through all the tiles
for x in range(0, len(walls[0])):
for z in range(0, len(walls)):
#Check which corners and external walls and notches are needed
corners = checkForCorners([x, z], walls)
externals = checkForExternalWalls([x, z], walls)
notchData = checkForNotch([x, z], walls)
notch = ""
#Set notch string to correct value
if notchData[0]:
notch = "left"
if notchData[1]:
notch = "right"
#tile
# print( "V-REP add tile")
isTile = False
#wall
if walls[z][x][1][0]: #top
# print( "V-REP add top wall")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path + '/models/walls/' +
wall_color + 'top_wall.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX),
-1 * (z * 0.25 + startZ) + 0.125, pos_z),
operationMode=vrep.simx_opmode_oneshot)
if walls[z][x][1][1]: #right
# print( "V-REP add right wall")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path + '/models/walls/' +
wall_color + 'right_wall.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX) + 0.25,
-1 * (z * 0.25 + startZ) + 0.125, pos_z),
operationMode=vrep.simx_opmode_oneshot)
if walls[z][x][1][2]: #bottom
# print( "V-REP add bottom wall")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path + '/models/walls/' +
wall_color + 'bottom_wall.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX) + 0.125,
-1 * (z * 0.25 + startZ) - 0.25, pos_z),
operationMode=vrep.simx_opmode_oneshot)
if walls[z][x][1][3]: #left
# print( "V-REP add left wall")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path + '/models/walls/' +
wall_color + 'left_wall.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX),
-1 * (z * 0.25 + startZ) + 0.125, pos_z),
operationMode=vrep.simx_opmode_oneshot)
#checkpoint
if walls[z][x][2]:
#Add bounds to the checkpoint boundaries
print("V-REP add checkpoint")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path +
'/models/tiles/checkpoint_tile.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ),
pos_z),
operationMode=vrep.simx_opmode_oneshot)
# allCheckpointBounds = allCheckpointBounds + boundsPart.format("checkpoint", checkId, (x * 0.25 + startX) - 0.15, (z * 0.25 + startZ) - 0.15, (x * 0.25 + startX) + 0.15, (z * 0.25 + startZ) + 0.15)
#Increment id counter
checkId = checkId + 1
isTile = True
#trap
if walls[z][x][3]:
#Add bounds to the trap boundaries
print("V-REP add trap / hole tile")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path +
'/models/tiles/hole_tile.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ),
pos_z),
operationMode=vrep.simx_opmode_oneshot)
# allTrapBounds = allTrapBounds + boundsPart.format("trap", trapId, (x * 0.25 + startX) - 0.15, (z * 0.25 + startZ) - 0.15, (x * 0.25 + startX) + 0.15, (z * 0.25 + startZ) + 0.15)
#Increment id counter
trapId = trapId + 1
isTile = True
#goal
if walls[z][x][4]:
#Add bounds to the goal boundaries
print("V-REP add start tile")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path +
'/models/tiles/start_tile.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ),
pos_z),
operationMode=vrep.simx_opmode_oneshot)
resetCode, obj_robot = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path +
'/models/robots/simplus_e-puck.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj_robot,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
print("V-rep add robot")
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj_robot,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ),
pos_z),
operationMode=vrep.simx_opmode_oneshot)
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path + '/models/game_manager.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=(20, 20, 0),
operationMode=vrep.simx_opmode_oneshot)
#Increment id counter
goalId = goalId + 1
isTile = True
#swamp
if walls[z][x][5]:
print("V-REP add swamp tile")
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path +
'/models/tiles/speed_bump.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ),
pos_z),
operationMode=vrep.simx_opmode_oneshot)
#Add bounds to the swamp boundaries
# allSwampBounds = allSwampBounds + boundsPart.format("swamp", swampId, (x * 0.25 + startX) - 0.15, (z * 0.25 + startZ) - 0.15, (x * 0.25 + startX) + 0.15, (z * 0.25 + startZ) + 0.15)
#Increment id counter
swampId = swampId + 1
isTile = True
# white tile
if not isTile:
resetCode, obj = vrep.simxLoadModel(
clientID=clientID,
modelPathAndName=model_path +
'/models/tiles/tile_white.ttm',
options=0,
operationMode=vrep.simx_opmode_blocking)
returnCode, position = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
pos_z = position[2]
vrep.simxSetObjectPosition(
clientID=clientID,
objectHandle=obj,
relativeToObjectHandle=-1,
position=((x * 0.25 + startX), -1 * (z * 0.25 + startZ),
pos_z),
operationMode=vrep.simx_opmode_oneshot)
#Increment id counter
tileId = tileId + 1
#String to hold all the data for the obstacles
allObstacles = ""
allDebris = ""
#Id to give a unique name to the obstacles
obstacleId = 0
debrisId = 0
#Iterate obstalces
for obstacle in obstacles:
#If this is debris
if obstacle[3]:
#Add the debris object
# allDebris = allDebris + debrisPart.format(debrisId, obstacle[0], obstacle[1], obstacle[2])
#Increment id counter
debrisId = debrisId + 1
else:
#Add the obstacle
# allObstacles = allObstacles + obstaclePart.format(obstacleId, obstacle[0], obstacle[1], obstacle[2])
#Increment id counter
obstacleId = obstacleId + 1
#Return the file data as a string
return
import vrep #V-rep library
import sys
import time #used to keep track of time
import numpy as np
import matplotlib.pyplot as plt
import math
PI = math.pi
# Establish Communication
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000, 5)
_, robot = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx',
vrep.simx_opmode_oneshot_wait)
# vrep.simxResetDynamicObject(self.clientID,robot)
vrep.simxRemoveModel(clientID, robot, vrep.simx_opmode_oneshot_wait)
a, b = vrep.simxLoadModel(
clientID,
'/home/rip/BTP/V-REP_PRO_EDU_V3_4_0_Linux/models/Custom_model/Pioneer_p3dx.ttm',
0, vrep.simx_opmode_blocking)
if clientID != -1: #check if client connection successful
print('Connected to remote API server')
else:
print('Connection not successful')
sys.exit('Could not connect')
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
for i in range(4, 8):
vrep.simxSetJointTargetVelocity(clientID, jointhandles[i],
cur_genome[i + 24],
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, jointhandles[i],
cur_genome[i + 8],
vrep.simx_opmode_streaming)
time.sleep(1)
k = k + 1
returnCode, Data = vrep.simxGetObjectPosition(
clientID, model_handle, -1,
vrep.simx_opmode_blocking) #return code - 0,1, data - x,y,z
print("Position:")
print(Data)
cur_fitness = fitness(Data) #fitness calculator
print("Fitness:")
print(cur_fitness)
fitness_track.append(cur_fitness) #comparision
# for m in range (0,len(fitness_track)):
# fl.append(m)
# plt.plot(i,fitness_track)
# plt.show()
if cur_fitness > best_fitness:
best_genome = cur_genome[:]
best_fitness = cur_fitness
vrep.simxRemoveModel(clientID, model_handle, vrep.simx_opmode_blocking)
jointhandles = []
generation = generation + 1
