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