MAX_Y = 1.5
ENCODER_UNIT = 159.23
INIT_X = 0.0
INIT_Y = 0.0
INIT_ANGLE = 0
PRED_STEPS = 450
correction_x = 0
correction_y = 0
correction_theta = 0
# create the Robot instance.
robot = Supervisor()
robot_sup = robot.getFromDef("e-puck")
robot_trans = robot_sup.getField("translation")
compass = robot.getCompass("compass")
motorLeft = robot.getMotor("left wheel motor")
motorRight = robot.getMotor("right wheel motor")
positionLeft = robot.getPositionSensor("left wheel sensor")
positionRight = robot.getPositionSensor("right wheel sensor")
predictor = Predictor()
timestep = int(robot.getBasicTimeStep())
x = []
y = []
theta = []
distance_sensors_info = []
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"))
