class SmartWalker(Widget): time = StringProperty("") safe = BooleanProperty(True) # 1 / 3 of arrow height and width arrow_height = 5 arrow_width = 5 forward_arrow_color = ListProperty() backward_arrow_color = ListProperty() left_arrow_color = ListProperty() right_arrow_color = ListProperty() def __init__(self, **kwargs): super(SmartWalker, self).__init__(**kwargs) self.set_dr_prescription() self.logger = Logger() self.forward_arrow_color = 1, 1, 1, 1 self.backward_arrow_color = 0, 1, 1, 1 self.left_arrow_color = 1, 0, 1, 1 self.right_arrow_color = 1, 1, 0, 1 if not TEST_ENVIRONMENT: self.hx0 = WeightSensor(27, 17) self.hx3 = WeightSensor(26, 13) self.hx1 = WeightSensor(10, 22) self.hx2 = WeightSensor(11, 9) self.hx0.initialize_weight_sensor() self.hx1.initialize_weight_sensor() self.hx2.initialize_weight_sensor() self.hx3.initialize_weight_sensor() self.bno = BNO055.BNO055(serial_port='/dev/ttyS0', rst=23) heading, roll, pitch = self.bno.read_euler() self.gyro.set_initial_values(heading, roll, pitch) if not self.bno.begin(): raise RuntimeError( 'Failed to initialize BNO055! Is the sensor connected?') self.tof = VL53L0X() self.tof.start_ranging(VL53L0X_BETTER_ACCURACY_MODE) else: heading, roll, pitch = 100, random.uniform(0, 0.1), random.uniform( 0, 0.1) self.gyro.set_initial_values(heading, roll, pitch) def set_dr_prescription(self): try: with open('dr_note.txt') as f: numbers = map(float, f.readline().split(' ')) except: raise NoDrPrescriptionFound() if numbers: PressureSensorWidget.set_max_dr_value(max(numbers[:4])) self.fl.set_dr_radius(numbers[0]) self.fr.set_dr_radius(numbers[1]) self.rl.set_dr_radius(numbers[2]) self.rr.set_dr_radius(numbers[3]) ProximityWidget.set_dr_value(int(numbers[4])) def get_4_weight_sensors(self): """returns rr, fr, rl, fl, calibration values: 6.1324866, 4.3640498, 4.50525366, 4.35680998 """ if TEST_ENVIRONMENT: import random return random.randrange(10000) - 9000, random.randrange( 10000) - 5000, random.randrange( 10000) - 5000, random.randrange(10000) - 5000 try: val0 = self.hx0.get_weight(1) val1 = self.hx1.get_weight(1) val2 = self.hx2.get_weight(1) val3 = self.hx3.get_weight(1) calibrated_val0 = 6.1324866 * val0 / 1000 calibrated_val1 = 4.3640498 * val1 / 1000 calibrated_val2 = 4.50525366 * val2 / 1000 calibrated_val3 = 4.35680998 * val3 / 1000 return calibrated_val0, calibrated_val1, calibrated_val2, calibrated_val3 except (KeyboardInterrupt, SystemExit): self.hx0.cleanAndExit() self.hx1.cleanAndExit() self.hx2.cleanAndExit() self.hx3.cleanAndExit() return 1, 1, 1, 1 def update_weights(self): rr_value, fr_value, rl_value, fl_value = self.get_4_weight_sensors() self.logger.add_data([fr_value, fl_value, rr_value, rl_value], DataSources.WEIGHT) self.rr.set_pressure(rr_value) self.fr.set_pressure(fr_value) self.rl.set_pressure(rl_value) self.fl.set_pressure(fl_value) def update_gyroscope(self): if not TEST_ENVIRONMENT: heading, roll, pitch = self.bno.read_euler() sys, gyro, acc, mag = self.bno.get_calibration_status() else: heading, roll, pitch = 100, random.uniform(-0.1, 0.1), random.uniform( -0.1, 0.1) sys, gyro, acc, mag = 20, 12, 10, 4 self.logger.add_data([heading, roll, pitch, sys, gyro, acc, mag], DataSources.GYROSCOPE) self.gyro.set_roll_pos(roll) self.gyro.set_pitch_pos(pitch) if roll < -40: self.forward_arrow_color = 1, 0, 0, 1 else: self.forward_arrow_color = 0, 1, 0, 1 if roll > 40: self.backward_arrow_color = 1, 0, 0, 1 else: self.backward_arrow_color = 0, 1, 0, 1 def update_proximity(self): if not TEST_ENVIRONMENT: proximity_value = self.tof.get_distance() else: import random proximity_value = random.randrange(1000) self.logger.add_data([proximity_value], DataSources.PROXIMITY) self.proximity.set_proximity(proximity_value) def update_safe(self, *args): self.safe = random.uniform(0, 1) < 0.6 def update(self, *args): self.time = str(time.asctime()) self.update_weights() self.update_gyroscope() self.update_proximity() self.logger.write_data_to_file()
class Application: def __init__(self): self.ez = Ezgame(700, 700) self.ez.fps = FPS self.loop = CURRENT_MODE self.arm = Arm() self.init() def init(self): if self.loop == JACOBIAN: self.ez.init(self.jacobian_loop) # needs a trajectory self.traj_step = TRAJ_STEP_MIN self.trajectory = Trajectory( step_length=self.traj_step, restart=self.reset) self.jacobian = JacobianControler(self.arm) self.errors = [] self.plot_errors = [] self.jacobian_logger = Logger("jacobian_control") # self.ez.gui = False elif self.loop == RANDOM_POSE: self.ez.init(self.random_pose_loop) self.arm2 = Arm() self.ez.fps = 2 elif self.loop == GENERATE_SAMPLES: self.ez.init(self.generate_samples_loop) self.arm2 = Arm() self.ez.gui = False # the file has in its name the range of delta theta in degrees # and the maximum step valid filename = "dataset_{}_{}-{}.csv".format(DT_VAL, MIN_STEP, MAX_STEP) csvfile = open(filename, 'a') self.dataset_writer = csv.writer(csvfile) self.sample_counter = 0 self.invalid_counter = 0 elif self.loop == TEST_NETWORK: self.ez.init(self.test_net_loop) # needs a trajectory self.traj_step = TRAJ_STEP_MIN self.trajectory = Trajectory( step_length=self.traj_step, restart=self.reset) modelid = MODELID self.net = Net(modelid, self.arm) self.errors = [] self.net_logger = Logger("net_control{}".format(modelid)) def test_net_loop(self): self.draw_arm(self.arm) self.draw_trajectory() # get current position t1 = self.arm.t1 t2 = self.arm.t2 ee1 = self.arm.endeffector() self.current_point = self.trajectory.current() self.net.control(self.current_point) self.errors.append(self.arm.error(self.current_point)) self.draw_info() self.current_point = self.trajectory.next() self.ez.point(self.arm.endeffector(), color='blue') def jacobian_loop(self): self.draw_arm(self.arm) self.draw_trajectory() self.current_point = self.trajectory.current() self.jacobian.control(self.current_point) self.errors.append(self.arm.error(self.current_point)) self.draw_info() self.current_point = self.trajectory.next() self.ez.point(self.arm.endeffector(), color='blue') def random_pose_loop(self): self.arm.set_random_pose() t1, t2 = self.arm.get_normalized_pose() self.arm2.set_normalized_pose(t1, t2) self.draw_arm(self.arm) self.draw_arm(self.arm2) def generate_samples_loop(self): sample = self.generate_sample() vector = Point2(sample[2], sample[3]) if vector.r > MIN_STEP and vector.r < MAX_STEP: norm_sample = self.normalize_sample(sample) self.dataset_writer.writerow(norm_sample) self.sample_counter += 1 else: self.invalid_counter += 1 if self.sample_counter % 10000 == 0: print "{} samples generated in this loop.".format(self.sample_counter) avg = self.sample_counter/float((self.sample_counter + self.invalid_counter)) print "{}% valid".format(avg*100.) if self.sample_counter >= 2000000: self.exit() def generate_sample(self): # generate random position self.arm.set_random_pose() #view # self.draw_arm(self.arm, color='blue') #endview t1, t2 = self.arm.t1, self.arm.t2 ee1 = self.arm.endeffector() # generate random movement dt1 = normal_in_range(MIN_DT, MAX_DT) dt2 = normal_in_range(MIN_DT, MAX_DT) # dt1 = scale(0.0, 1.0, MIN_DT, MAX_DT, np.random.random()) # dt2 = scale(0.0, 1.0, MIN_DT, MAX_DT, np.random.random()) # move arm self.arm.move(dt1, dt2) ee2 = self.arm.endeffector() dee = ee2 - ee1 #view # self.arm2.t1, self.arm2.t2 = self.arm.t1, self.arm.t2 # self.draw_arm(self.arm2, color='red') # print [t1,t2, dee.x, dee.y, dt1, dt2], ":", ee1, " ->", ee2 # raw_input() #endview return [t1,t2, dee.x, dee.y, dt1, dt2] def normalize_sample(self, sample): t1 = scale(MIN_THETA, MAX_THETA, 0.0, 1.0, sample[0]) t2 = scale(MIN_THETA, MAX_THETA, 0.0, 1.0, sample[1]) dx = scale(-MAX_STEP, MAX_STEP, 0.0, 1.0, sample[2]) dy = scale(-MAX_STEP, MAX_STEP, 0.0, 1.0, sample[3]) dt1 = scale(MIN_DT, MAX_DT, 0.0, 1.0, sample[4]) dt2 = scale(MIN_DT, MAX_DT, 0.0, 1.0, sample[5]) norm_sample = [t1, t2, dx, dy, dt1, dt2] for value in norm_sample: assert value >= 0.0 and value <= 1.0 return norm_sample def draw_arm(self, arm, color='black'): p1 = Point2().polar(arm.l1, arm.t1) p2 = Point2().polar(arm.l2, arm.t2+arm.t1) + p1 self.ez.line(Point2(), p1, color=color) self.ez.point(p1) self.ez.line(p1, p2, color=color) self.ez.point(p2) def draw_trajectory(self): self.ez.lines(self.trajectory.points) def draw_info(self): # tamanho do passo atual val = self.traj_step val_text = "Step size.....{0}".format(val) self.ez.text(val_text, Point2(10,10), onScreen=True) # passo atual val = self.trajectory.curr_idx() val_text = "Current step..{0}".format(val) self.ez.text(val_text, Point2(10,20), onScreen=True) # erro medio val = self.mse() val_text = "Avg. error....{0}".format(val) self.ez.text(val_text, Point2(10,30), onScreen=True) if self.loop == JACOBIAN: # temporary if len(self.plot_errors) > 2: self.ez.lines(self.plot_errors,color='blue') def mse(self): squared = [e*e for e in self.errors] # squared = [e for e in self.errors] return sum(squared)/float(len(squared)) def run(self): self.ez.run() def reset(self): mse = self.mse() print self.traj_step, mse if self.loop == TEST_NETWORK: self.net_logger.add_data(self.traj_step, mse) elif self.loop == JACOBIAN: self.jacobian_logger.add_data(self.traj_step, mse) self.traj_step += TRAJ_STEP_INC if self.traj_step > TRAJ_STEP_MAX: self.exit() self.trajectory = Trajectory( step_length=self.traj_step, restart=self.reset) self.errors = [] self.arm = Arm() if self.loop == JACOBIAN: self.jacobian.arm = self.arm # temporary self.plot_errors.append( Point2(self.traj_step * 10, mse*1000) ) elif self.loop == TEST_NETWORK: self.net.arm = self.arm def exit(self): if self.loop == TEST_NETWORK: self.net_logger.plot("Step Size", "MSE") if self.loop == JACOBIAN: self.jacobian_logger.plot("Step Size", "MSE") raw_input() exit()