robot = Supervisor() # set random seed for reproducibility np.random.seed(5) # degrees for left, right, up, down head_directions = [90, 270, 0, 180] timestep = 2000 # Initialize devices ps = [] psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7'] for i in range(8): ps.append(robot.getDistanceSensor(psNames[i])) ps[i].enable(timestep) cmpXY1 = robot.createCompass("compassXY_01") cmpXY1.enable(timestep) cmpZ1 = robot.createCompass("compassZ_01") cmpZ1.enable(timestep) # Initialize motors leftMotor = robot.getMotor('left wheel motor') rightMotor = robot.getMotor('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) leftMotor.setVelocity(0.0)
class UniversalController: def __init__(self, network, robot_name="standard", target_name="TARGET", num_of_inputs=4, eval_run_time=100, time_step=32): self.neural_network = network self.TIME_STEP = time_step #standard time step is 32 or 64 self.supervisor = Supervisor() self.inputs = num_of_inputs self.solution_threshold = 0.95 #call methods functions to intialise robot and target self.get_and_set_robot(robot_name) self.get_and_set_target(target_name) self.eval_run_time = eval_run_time #default to 1m x 1m space but can be edited directly or using method below self.max_distance = 1.4142 self.data_reporting = DataReporting() def get_and_set_robot(self, robot_name): # initialise robot node and components self.robot_node = self.supervisor.getFromDef( robot_name) #TARGET ROBOT MUST BE NAMED IN DEF FIELD self.robot_trans = self.robot_node.getField("translation") self.robot_rotation = self.robot_node.getField("rotation") #call start location function to store initial position self.get_and_set_robotStart() #call motors function self.get_and_set_motors() def get_and_set_robotStart(self): #establish robot starting position self.robot_starting_location = self.robot_trans.getSFVec3f() self.robot_starting_rotation = self.robot_rotation.getSFRotation() '''EDIT HERE TO EXPAND TO DIFFERENT NUMBERS OF WHEELS/MOTORS''' def get_and_set_motors(self): self.motors = [] self.motor_max_Vs = [] #get robot motors - currently works for all two wheel motor morphologies self.left_motor = self.supervisor.getMotor('left wheel motor') self.right_motor = self.supervisor.getMotor('right wheel motor') self.left_motor.setPosition(float('inf')) self.right_motor.setPosition(float('inf')) #get the max velocity each motor is capable of and set the max velocity self.left_motor_max = self.left_motor.getMaxVelocity() self.right_motor_max = self.right_motor.getMaxVelocity() #append necessary additional motors and max velocities here after enabling as above self.motors.append(self.left_motor) self.motors.append(self.right_motor) self.motor_max_Vs.append(self.left_motor_max) self.motor_max_Vs.append(self.right_motor_max) def get_and_set_target(self, target_name): #get target location self.target = self.supervisor.getFromDef( target_name) #TARGET MUST BE NAMED IN DEF FIELD #get and set the translation and location for retrieval when needed self.target_trans = self.target.getField("translation") self.target_location = self.target_trans.getSFVec3f() def set_dimensions(self, space_dimensions): #basic pythagorean calculation to find max distance possible in square space self.max_distance = np.sqrt(space_dimensions[0]**2 + space_dimensions[1]**2) #print(self.max_distance) def set_distance_sensors(self, distance_sensors): #takes in array of strings - names of distance sensors self.distance_sensors = [] self.min_DS_values = [] self.DS_value_range = [] for i in range(len(distance_sensors)): #set and enable each sensor self.distance_sensors.append( self.supervisor.getDistanceSensor(distance_sensors[i])) self.distance_sensors[i].enable(self.TIME_STEP) #get and store the min reading value of each sensor self.min_DS_values.append(self.distance_sensors[i].getMinValue()) #get and store the possible value range of each sensor self.DS_value_range.append(self.distance_sensors[i].getMaxValue() - self.min_DS_values[i]) #print(self.DS_value_range) def get_DS_values(self): #get distance sensor values values = [] for i in range(len(self.distance_sensors)): value = self.distance_sensors[i].getValue() #value = value/self.maxDSReading #practical max value value = value - self.min_DS_values[i] if value < 0.0: value = 0.0 #to account for gaussian noise value = value / (self.DS_value_range[i]) #account for gaussian noise providing higher than max reading if value > 1.0: value = 1.0 values.append(value) #return a list of the normalised sensor readings return values def compute_motors(self, DS_values): #get the outputs of the neural network and convert into wheel motor speeds #already fully flexible for multiple motors results = self.neural_network.forwardPass(DS_values) for i in range(len(self.motors)): self.motors[i].setVelocity(results[i] * self.motor_max_Vs[i]) def reset_all_physics(self): #reset robot physics and return to starting translation ready for next run self.robot_rotation.setSFRotation(self.robot_starting_rotation) self.robot_trans.setSFVec3f(self.robot_starting_location) self.robot_node.resetPhysics() '''SIMULATION FUNCTION - can also be used directly for manual DataVisualisationandTesting of weight arrays (to manually repeat successful solutions etc.)''' def evaluate_robot(self, individual, map_elites=False, all_novelty=False): self.neural_network.decodeEA( individual) #individual passed from algorithm #note simulator start time t = self.supervisor.getTime() if map_elites or all_novelty: velocity = [] angular_V = [] #run simulation for eval_run_time seconds while self.supervisor.getTime() - t < self.eval_run_time: #calculate the motor speeds from the sensor readings self.compute_motors(self.get_DS_values()) #check current objective fitness current_fit = self.calculate_fitness() if map_elites or all_novelty: currentV = self.robot_node.getVelocity() velocity.append( np.sqrt((currentV[0]**2) + (currentV[1]**2) + (currentV[2]**2))) angular_V.append(np.sqrt(currentV[4]**2)) #break if robot has reached the target if current_fit > self.solution_threshold: time_taken = self.supervisor.getTime() - t #safety measure due to time_step and thread lag if time_taken > 0.0: #then a legitimate solution fit = self.calculate_fitness() break if self.supervisor.step(self.TIME_STEP) == -1: quit() #Get only the X and Y coordinates to create the endpoint vector endpoint = self.robot_trans.getSFVec3f()[0:3:2] distance_FS = np.sqrt( (endpoint[0] - self.robot_starting_location[0])**2 + (endpoint[1] - self.robot_starting_location[2])**2) #reset the simulation self.reset_all_physics() #find fitness fit = self.calculate_fitness() if map_elites: average_velocity = np.average(velocity) #average_angular_V = np.average(angular_V) return average_velocity, distance_FS, fit if all_novelty: average_velocity = np.average(velocity) median_velocity = np.median(velocity) #print("fit = " + str(fit) + ", endpoint = " + str(endpoint[0]) + "," + str(endpoint[1]) + ", AV = " + str(average_velocity) + ", distanceFS = " + str(distance_FS)) return fit, endpoint, average_velocity, distance_FS, median_velocity return fit, endpoint def calculate_fitness(self): values = self.robot_trans.getSFVec3f() distance_from_target = np.sqrt( (self.target_location[0] - values[0])**2 + (self.target_location[2] - values[2])**2) fit = 1.0 - (distance_from_target / self.max_distance) return fit def sigma_test(self, my_EA, upper_limit=1.0, lower_limit=0.1): self.data_reporting.sigma_test(my_EA, upper_limit, lower_limit) def algorithm_test(self, my_EA, generations, total_runs, nipes=False, map_elites=False): self.data_reporting.algorithm_test(my_EA, generations, total_runs, nipes, map_elites) def control_group_test(self, generations=10000, total_runs=1): self.data_reporting.control_group_test( individual_size=self.neural_network.solution_size, eval_function=self.evaluate_robot, generations=generations, total_runs=total_runs)
currCell = 13 # define max speed MAX_SPEED = 6.283 #pi = 3.14159265359 wheelCircumferenceMeters = 0.20891591146 cellLengthInRadians = (0.4572) * (2*pi)/(wheelCircumferenceMeters) #????? # enable keyboard for controls keyboard = Keyboard() keyboard.enable(timestep) # get front left and right distance sensors # note: boebot has built in lds and rds at front l/R angles. Currently not enabled. frontDistanceSensor = robot.getDistanceSensor('front_ds') leftDistanceSensor = robot.getDistanceSensor('left_ds') rightDistanceSensor = robot.getDistanceSensor('right_ds') frontDistanceSensor.enable(timestep) leftDistanceSensor.enable(timestep) rightDistanceSensor.enable(timestep) # get left and right light sensors leftLightSensor = robot.getLightSensor('lls') rightLightSensor = robot.getLightSensor('rls') leftLightSensor.enable(timestep) rightLightSensor.enable(timestep) # get left and right LED leftLED = robot.getLED('left_led')
#get field from robot trans_field = robo_node.getField('translation') #randomize the x and z coordinates to change position of dummy car x = random.uniform(-2, 2) z = random.uniform(-2, 2) #trans_field.setSFVec3f([x,0,z]) #Enabling keyboard to control different funcitons keyboard = Keyboard() keyboard.enable(TIME_STEP) #initializing distanse sensors ds = [] dsNames = ['ds_right', 'ds_left', 'ds_back_left', 'ds_back_right'] for i in range(4): ds.append(supervisor.getDistanceSensor(dsNames[i])) ds[i].enable(TIME_STEP) #initializing wheels wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for i in range(4): wheels.append(supervisor.getMotor(wheelsNames[i])) wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) avoidObstacleCounter = 0 #initializing communications fasheqi = supervisor.getEmitter('emitter') fasheqi.setChannel(1) fasheqi.setRange(-1)
# name of the available distance sensors sensorsNames = [ 'front', 'front right 0', 'front right 1', 'front right 2', 'front left 0', 'front left 1', 'front left 2', 'rear', 'rear left', 'rear right', 'right', 'left' ] sensors = {} colorFields = {} supervisor = Supervisor() # get and enable the distance sensors for name in sensorsNames: sensors[name] = supervisor.getDistanceSensor('distance sensor ' + name) sensors[name].enable(TIME_STEP) defName = name.upper().replace(' ', '_') colorFields[name] = supervisor.getFromDef( defName + '_VISUALIZATION').getField('diffuseColor') # get the color fields childrenField = supervisor.getSelf().getField('children') for i in range(childrenField.getCount()): node = childrenField.getMFNode(i) if node.getTypeName() == 'DistanceSensorVisualization': colorFields[node.getDef()] = node.getField('diffuseColor') colorFields[node.getDef()].setSFColor([0.0, 1.0, 0.0]) while supervisor.step(TIME_STEP) != -1: # adjust the color according to the value returned by the front distance sensor
class RobotManager: def __init__(self, params): self.robot = Supervisor() self.robot_sup = self.robot.getFromDef("e-puck") self.robot_trans = self.robot_sup.getField("translation") self.robot_rotation = self.robot_sup.getField("rotation") self.compass = self.robot.getCompass("compass") self.motorLeft = self.robot.getMotor("left wheel motor") self.motorRight = self.robot.getMotor("right wheel motor") self.positionLeft = self.robot.getPositionSensor("left wheel sensor") self.positionRight = self.robot.getPositionSensor("right wheel sensor") self.params = params # real robot state self.x = [] self.y = [] self.theta = [] self.distance_sensors_info = [] # odometry estimation self.x_odometry = [] self.y_odometry = [] self.theta_odometry = [] self.sensorNames = [ 'ds0', 'ds1', 'ds2', 'ds3', 'ds4', 'ds5', 'ds6', 'ds7' ] # predicted data self.x_pred = [] self.y_pred = [] self.theta_pred = [] self.data_collector = DataCollector() self.movement_controller = MovementController() self.window_communicator = WindowCommunicator(self.robot) # predictors self.predictor = PredictorNNSensorsNotNormalized() self.predictorCoord = PredictorNNCoordinates() self.timestep = int(self.robot.getBasicTimeStep()) # particles filter initialization robot_initial_conf = RobotConfiguration( self.params.INIT_X, self.params.INIT_Y, self.convert_angle_to_xy_coordinates(self.params.INIT_ANGLE)) environment_conf = EnvironmentConfiguration(self.params.MAX_X, self.params.MAX_Y) self.particles_filter = ParticlesFilter(environment_conf, robot_initial_conf, self.predictor, self.params) self.movement_random = True pass def init_actuators(self): self.compass.enable(self.timestep) self.motorLeft.setPosition(float('inf')) self.motorRight.setPosition(float('inf')) self.positionRight.enable(self.timestep) self.positionLeft.enable(self.timestep) def robot_to_xy(self, x, y): return x + 1, y + 0.75 def xy_to_robot(self, x, y): return x - 1.00000, y - 0.750000 def init_robot_pos(self, x, y): x, y = self.xy_to_robot(x, y) self.robot_trans.setSFVec3f([y, 0, x]) self.robot_rotation.setSFRotation([0, 1, 0, 0]) self.robot_sup.resetPhysics() def get_bearing_degrees(self): north = self.compass.getValues() rad = np.arctan2(north[0], north[2]) bearing = (rad) / np.pi * 180 if bearing < 0.0: bearing += 360 bearing = 360 - bearing - 90 if bearing < 0.0: bearing += 360 return bearing def save_supervisor_coordinates(self): # true robot position information trans_info = self.robot_trans.getSFVec3f() # print('SUP COORD:', trans_info) x_coordinate, y_coordinate = self.robot_to_xy(trans_info[2], trans_info[0]) self.x.append(x_coordinate) self.y.append(y_coordinate) angle = self.get_bearing_degrees() self.theta.append(angle) def step(self): return self.robot.step(self.timestep) != -1 def save_odometry_coordinates(self, coordinate): # convert robot coordinates into global coordinate system self.x_odometry.append(coordinate.x) self.y_odometry.append(coordinate.y) self.theta_odometry.append( self.convert_angle_to_xy_coordinates(coordinate.theta)) def save_sensor_distances(self, distanceSensors): distances = [] for distanceSensor in distanceSensors: distance = distanceSensor.getValue() #there is no real messure. if distance == 10: distance = None distances.append(distance) self.distance_sensors_info.append(distances) def get_sensor_distance(self): # Read the sensors, like: distanceSensors = [] for sensorName in self.sensorNames: sensor = self.robot.getDistanceSensor(sensorName) sensor.enable(self.timestep) distanceSensors.append(sensor) return distanceSensors def convert_angle_to_xy_coordinates(self, angle): angle = angle * 180 / np.pi if angle < 0.0: angle = 360 + angle return angle def plot(self): # Enter here exit cleanup code. plt.ylim([0, self.params.MAX_Y]) plt.xlim([0, self.params.MAX_X]) plt.xlabel("x") plt.ylabel("y") plt.plot(self.x, self.y, label="real") plt.plot(self.x_odometry, self.y_odometry, label="odometry") plt.plot(self.x_pred, self.y_pred, 's', label="correction", marker='o') plt.title("Robot position estimation") plt.legend() plt.savefig("results/position.eps", format='eps') def move_robot_to_random_position(self): new_x = -(1 / 2 * self.params.MAX_X - 0.1) + np.random.random() * (self.params.MAX_X - 0.2) new_y = -(1 / 2 * self.params.MAX_Y - 0.1) + np.random.random() * (self.params.MAX_Y - 0.2) # old_z = robot_trans.getSFVec3f()[1] new_theta = np.random.random() * 2 * np.pi self.robot_trans.setSFVec3f([new_y, 0, new_x]) self.robot_rotation.setSFRotation([0, 1, 0, new_theta]) self.robot_sup.resetPhysics() def execute(self): self.init_actuators() self.init_robot_pos(self.params.INIT_X, self.params.INIT_Y) self.step() self.init_robot_pos(self.params.INIT_X, self.params.INIT_Y) self.window_communicator.sendInitialParams(self.params) odometry = Odometry( self.params.ENCODER_UNIT * (self.positionLeft.getValue()), self.params.ENCODER_UNIT * (self.positionRight.getValue()), self.params.INIT_X, self.params.INIT_Y, self.params.INIT_ANGLE) count = 0 particles = np.array([[], []]) last_move = 'none' errorPos = [] errorOdo = [] continue_running = True # principal robot step cycle while (continue_running): # if the number of steps is greater than EXPERIMENT_DURATION_STEPS then stop running if count == self.params.EXPERIMENT_DURATION_STEPS: continue_running = False # receive message from robot window message = self.window_communicator.receiveMessage() message = json.loads(message) if message else None if message and message['code'] == 'start_randomness': self.movement_random = True elif message and message['code'] == 'stop_randomness': self.movement_random = False elif message and message['code'] == 'params_modification': self.particles_filter.reset_particles( message["number_particles"], message["sigma_xy"], message["sigma_theta"], RobotConfiguration(self.x_pred[-1], self.y_pred[-1], self.theta_pred[-1])) # get odometry data odometry_info, delta_movement = odometry.track_step( self.params.ENCODER_UNIT * (self.positionLeft.getValue()), self.params.ENCODER_UNIT * (self.positionRight.getValue())) # transoform the angle to xy coordinates delta_movement[2] = self.convert_angle_to_xy_coordinates( delta_movement[2]) # for capturing robot positioning if not self.step() and self.params.CAPTURING_DATA: print('saving data') self.data_collector.collect( self.x, self.y, self.theta, np.array(self.distance_sensors_info)) self.plot() # get sensor distances distanceSensors = self.get_sensor_distance() # collect data: real, odometry, predicted self.save_sensor_distances(distanceSensors) self.save_odometry_coordinates(odometry_info) self.save_supervisor_coordinates() # calculate new velocity left_speed, right_speed = 0, 0 if self.movement_random: if self.params.GO_STRAIGHT_MOVE: left_speed, right_speed = self.movement_controller.calculate_velocity( distanceSensors) else: left_speed, right_speed = self.movement_controller.calculate_velocity_random_move( distanceSensors) last_move = 'none' else: # joystick control if message and message[ 'code'] == "move" and last_move != message['direction']: last_move = message['direction'] if last_move == 'UP': left_speed, right_speed = self.movement_controller.move_straight( ) elif last_move == 'DOWN': left_speed, right_speed = self.movement_controller.move_backwards( ) elif last_move == 'RIGHT': left_speed, right_speed = self.movement_controller.move_right( ) elif last_move == 'LEFT': left_speed, right_speed = self.movement_controller.move_left( ) self.motorLeft.setVelocity(left_speed) self.motorRight.setVelocity(right_speed) # predict position based on particles data if not self.params.CAPTURING_DATA and count % self.params.PRED_STEPS == 0 and count != 0: # get particles particles = self.particles_filter.get_particles( delta_movement, self.distance_sensors_info[-1], count % 2 == 0) # get weights sum weighted_sum = np.sum(particles[3]) # get weighted average from particles data x_prim = np.sum(particles[0] * particles[3]) / weighted_sum y_prim = np.sum(particles[1] * particles[3]) / weighted_sum theta_prim = np.sum(particles[2] * particles[3]) / weighted_sum # # get the position prediction given the sensor measurements # predicted_coord = predictorCoord.predict(distance_sensors_info[-1]) # # print(predicted_coord) # # combine both previous models # x_pred.append((x_prim + float(predicted_coord[0]))/2) # y_pred.append((y_prim + float(predicted_coord[1]))/2) self.x_pred.append(x_prim) self.y_pred.append(y_prim) self.theta_pred.append(theta_prim) # predictor.refit_models(x[-1], y[-1], theta[-1], distance_sensors_info[-1]) # calculate error if self.params.CALCULATE_PRED_ERROR: errorPos.append( np.sqrt((self.x[-1] - self.x_pred[-1])**2 + (self.y[-1] - self.y_pred[-1])**2)) if self.params.CALCULATE_ODO_ERROR: errorOdo.append( np.sqrt((self.x[-1] - self.x_odometry[-1])**2 + (self.y[-1] - self.y_odometry[-1])**2)) # send data to html page self.window_communicator.sendCoordinatesParticles( self.x, self.y, self.x_odometry, self.y_odometry, self.x_pred, self.y_pred, particles.tolist()) # move robot to a random position after a while if self.params.CAPTURING_DATA and count % self.params.MOVING_ROBOT_STEPS == 0: self.move_robot_to_random_position() count += 1 if self.params.CALCULATE_PRED_ERROR: print('Saving prediction SDE') pickle.dump(errorPos, open(self.params.PRED_ERROR_FILE, "wb")) if self.params.CALCULATE_ODO_ERROR: print('Saving odometry SDE') pickle.dump(errorOdo, open(self.params.ODO_ERROR_FILE, "wb"))
y = [] while (robot.step(timestep) != -1): # true robot position information trans_info = robot_trans.getSFVec3f() x_coordinate, y_coordinate = robot_to_xy(trans_info[2], trans_info[0]) x.append(x_coordinate) y.append(y_coordinate) print('coordinates: ', trans_info, get_bearing_degrees()) # Read the sensors, like: distanceSensors = [] sensorNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7'] for sensorName in sensorNames: sensor = robot.getDistanceSensor(sensorName) sensor.enable(timestep) distanceSensors.append(sensor) # Process sensor data here sensorValues = [ distanceSensor.getValue() for distanceSensor in distanceSensors ] rightObstacle = sensorValues[0] > 100.0 or sensorValues[ 1] > 100.0 or sensorValues[2] > 100.0 leftObstacle = sensorValues[5] > 100.0 or sensorValues[ 6] > 100.0 or sensorValues[7] > 100.0 left_speed = .5 * MAX_SPEED right_speed = .5 * MAX_SPEED