def load_trajectories(self): """Returns list of trajectories from file_name""" # Initialize data lists trajectories = [] data = {} # Read file with open(self.file_name, "r") as f: data = json.load(f) # Convert data from dicts/lists to Trajectories # Structure: # "{"Name1": [ # [reverse, start_velocity, end_velocity, max_velocity, max_abs_acceleration, max_centr_acceleration], # [[p1x, p1y, p1theta], [p2x, p2y, p2theta], ... ] # ], # "Name2": [...], # }" for name in data.keys(): #Easier to work with tmp_props = data[name][0] tmp_poses = data[name][1] poses = [] for p in tmp_poses: poses.append(to_pose(p[0], p[1], p[2])) trajectories.append( Trajectory(name, poses, tmp_props[0], tmp_props[1], tmp_props[2], tmp_props[3], tmp_props[4], tmp_props[5], tmp_props[6])) return trajectories
def add_trajectory(self, trajectory = None): """Adds a trajectory to the trajectory list""" if trajectory == None: # Find a valid trajectory name (Format: Trajectory1, Trajectory2, ... Trajectory100) name = "Trajectory" + str(1) valid = False for i in range(100): name = "Trajectory" + str(i + 1) valid = False for t in self.trajectories: if t.name == name: valid = False break valid = True if valid: break trajectory = Trajectory(name=name, poses=self.random_points(), current=False, reverse=self.current_trajectory.reverse, start_velocity=self.current_trajectory.start_velocity, end_velocity=self.current_trajectory.end_velocity, max_velocity=self.current_trajectory.max_velocity, max_abs_acceleration=self.current_trajectory.max_abs_acceleration, max_centr_acceleration=self.current_trajectory.max_centr_acceleration) # Add Trajectory to graphical list item = ItemDrawer(icon="chart-bell-curve-cumulative", text=trajectory.name) self.screen.ids.content_drawer.ids.md_list.add_widget( item ) # Add Trajectory to mathematical list self.trajectories.append(trajectory) self.set_active_trajectory(item)
def test_save_load(self): """Tests saving and loading: Trajectory should be the same""" poses = [] poses.append(to_pose()) poses.append(to_pose(100, 300, 40)) poses.append(to_pose(320, 350, 135)) poses.append(to_pose(90, -100, -180)) poses.append(to_pose(400, 50, 90)) trajectory = Trajectory("Trajectory1", poses, True, True, 10, 20, 30, 40, 50) tmp = Trajectory("White_Line_Shot", poses, False, True, 10, 20, 30, 40, 50) trajectories = [tmp, trajectory] IO = JsonIO(path="Test/", name="jsonIO_unittest.json") IO.save_trajectories(trajectories) trajectory2 = IO.load_trajectories()[0] self.assertEqual(trajectory.name, trajectory2.name) self.assertEqual(trajectory.reverse, trajectory2.reverse) self.assertEqual(trajectory.current, trajectory2.current) self.assertAlmostEqual(trajectory.start_velocity, trajectory2.start_velocity) self.assertAlmostEqual(trajectory.end_velocity, trajectory2.end_velocity) self.assertAlmostEqual(trajectory.max_velocity, trajectory2.max_velocity) self.assertAlmostEqual(trajectory.max_abs_acceleration, trajectory2.max_abs_acceleration) self.assertAlmostEqual(trajectory.max_centr_acceleration, trajectory2.max_centr_acceleration) for i in range(len(trajectory.poses)): with self.subTest(i=i): self.assertAlmostEqual(trajectory.poses[i].translation.x, trajectory2.poses[i].translation.x) self.assertAlmostEqual(trajectory.poses[i].translation.y, trajectory2.poses[i].translation.y) self.assertAlmostEqual( trajectory.poses[i].rotation.get_degrees(), trajectory2.poses[i].rotation.get_degrees())
def test_trajectory_iterator(self): """Tests Trajectory iterator class""" poses = [] poses.append(to_pose()) poses.append(to_pose(36.0, 0.0, 0.0)) poses.append(to_pose(60.0, 100.0, 0.0)) poses.append(to_pose(160.0, 100.0, 0.0)) poses.append(to_pose(200.0, 70.0, 45.0)) traj = Trajectory(poses=poses) traj.time_parameterize_splines() iterator = TrajectoryIterator(traj) print(iterator.end_t) with open("Trajectory_Iterator_Test.csv", "w") as file: t = 0.1 while not iterator.is_done(): sample = iterator.advance(t) file.write(sample.__str__() + "\n")
def test_trajectory(self): """Tests Trajectory class""" poses = [] poses.append(to_pose()) poses.append(to_pose(50, 100, 90)) poses.append(to_pose(100, 150, 0)) traj = Trajectory(poses=poses, reverse=False) self.assertAlmostEqual(3, len(traj.poses)) self.assertAlmostEqual(2, len(traj.splines)) with open("Trajectory_Test.csv", "w") as file: traj.update_pose(to_pose(50, 75, 135), 1) traj.time_parameterize_splines() for t in traj.points: file.write(t.__str__() + "\n") with open("Trajectory_Test.csv", "a") as file: traj.optimize_splines() traj.time_parameterize_splines() for t in traj.points: file.write(t.__str__() + "\n")
class RootWidget(GridLayout): #points = ListProperty([(500, 500), # [500, 100, 200, 0], # [300, 300, 400, 700]]) points = ListProperty([500, 500, 500, 100, 200, 0, 300, 300, 400, 700]) poses = [ to_pose(300, 300, 45), to_pose(400, 500, 90), to_pose(600, 500, 0.0) ] trajectory = Trajectory(poses=poses) def trajectory_to_list(self, trajectory): point_list = [] for point in trajectory.points: point_list.append(point.pose.translation.x) point_list.append(point.pose.translation.y) return point_list
class MainApp(MDApp): """Main app class""" poses = [to_pose(300, 800, 45), to_pose(400, 900, -45), to_pose(200, 500, 180.0)] trajectory = Trajectory(poses=poses) points = ListProperty() control_points = ListProperty() reverse = StringProperty("Forward Trajectory") jsonIO = JsonIO() trajectories = [] current_trajectory = Trajectory() iterator = TrajectoryIterator(current_trajectory) timeout = .5 max_vel = TimeDelayedBoolean(0, timeout) max_accel = TimeDelayedBoolean(0, timeout) max_centr_accel = TimeDelayedBoolean(0, timeout) start_vel = TimeDelayedBoolean(0, timeout) end_vel = TimeDelayedBoolean(0, timeout) prev_reverse = False prev_time = 0.0 animation_active = False current_time = 0 dt = .04 s = 4.7 origin = ListProperty([500,500]) angle = NumericProperty(0) scalar = NumericProperty(s) wheel = ListProperty([2.5, 1.25]) pos_scalar = NumericProperty(4) radius = ListProperty([s / 5, s / 5]) vel_length = NumericProperty(0) accel_length = NumericProperty(0) accel_angle = NumericProperty(0) vector_scalar = NumericProperty(.6) triangle_scalar = NumericProperty(2) center_scalar = NumericProperty(2) def __init__(self, **kwargs): super().__init__(**kwargs) self.screen = Builder.load_string(KV) def build(self): """Builds app and sets theme""" self.theme_cls.primary_palette = "Teal" self.theme_cls.accent_palette = "Cyan" self.theme_cls.theme_style = "Dark" return self.screen def close_application(self): """Callback for closing window""" #https://stackoverflow.com/questions/32425831/how-to-exit-a-kivy-application-using-a-button self.get_running_app().stop() def on_start(self): """Runs at start of application: Initializes stuff""" # Load Trajectories self.trajectories = self.jsonIO.load_trajectories() # Set Current Trajectory update = True for t in self.trajectories: if t.current: self.current_trajectory = t update = False if update and len(self.trajectories) > 0: self.current_trajectory = self.trajectories[0] self.iterator = TrajectoryIterator(self.current_trajectory) self.reset_animation() # Add each trajectory for trajectory in self.trajectories: self.screen.ids.content_drawer.ids.md_list.add_widget( ItemDrawer(icon="chart-bell-curve-cumulative", text=trajectory.name) ) # Highlight correct Trajectory in drawerlist length = len(self.screen.ids.content_drawer.ids.md_list.children) - 1 item = self.screen.ids.content_drawer.ids.md_list.children[length - self.get_current_index()] self.screen.ids.content_drawer.ids.md_list.set_color_item(item) self.screen.ids.title.text = self.current_trajectory.name self.set_constraints() self.set_reverse() # Sync GUI and Mathematical Poses self.update_poses() # Schedule saving trajectories every 500 ms Clock.schedule_interval(self.save_trajectories, .5) # Schedule updating constraints Clock.schedule_interval(self.update_constraints, self.dt) def on_stop(self): """Save Settings before leaving""" self.jsonIO.save_trajectories(self.trajectories) def save_trajectories(self, dt): """Save Trajectories to JSON Periodically""" self.jsonIO.save_trajectories(self.trajectories) def update_points(self): """Updates trajectory lines""" point_list = [] for point in self.current_trajectory.points: point_list.append(self.translate_x(point.pose.translation.x)) point_list.append(self.translate_y(point.pose.translation.y)) self.points = point_list with self.screen.ids.control_points.canvas.after: #Rotate(angle=-self.angle) self.screen.ids.control_points.canvas.after.clear() Color(self.theme_cls.primary_dark[0],self.theme_cls.primary_dark[1],self.theme_cls.primary_dark[2],self.theme_cls.primary_dark[3]) radius = 50 / 8 for pose in self.current_trajectory.poses: Line(circle=(self.translate_x(pose.translation.x), self.translate_y(pose.translation.y), radius), width= 1.1, color=self.theme_cls.primary_color) Ellipse(size=(radius,radius), pos=(self.translate_x(pose.translation.x) - radius/2, self.translate_y(pose.translation.y) - radius/2)) self.reset_animation() self.update_stats() def translate_x(self, x): """Returns x translation from inches to pixels""" return x * 1.5 + 174 def translate_y(self, y): """Returns y translation from inches to pixels""" return y * 1.5 + 689.5 def update_name(self, instance): """Updates name of current trajectory""" # Update trajectory name self.current_trajectory.name = instance.text # Update GUI name length = len(self.screen.ids.content_drawer.ids.md_list.children) - 1 self.screen.ids.content_drawer.ids.md_list.children[length - self.get_current_index()].text = instance.text def optimize_trajectory(self): """Callback for optimizing current trajectory""" self.current_trajectory.optimize_splines() self.update_stats() # Update points to match optimization self.update_points() def mirror_trajectory(self): """Mirrors current Trajectory and adds it as a new trajectory""" self.add_trajectory(mirror_trajectory(self.current_trajectory)) def animate_trajectory(self): if self.iterator.end_t < self.current_time: self.reset_animation() self.animation_active = not self.animation_active def add_point(self): """Adds a point to current trajectory""" dx = [50.0, 120.0] dy = [-30.0, 30.0] dt = [-20.0, 20.0] # Generate semi - random point (Previous point + dx, dy, dt) pose = self.current_trajectory.poses[len(self.current_trajectory.poses) - 1].transform(to_pose(uniform(dx[0], dx[1]), uniform(dy[0], dy[1]), uniform(dt[0], dt[1]))) pose.translation.x = limit2(pose.translation.x, 15, 614.25) pose.translation.y = limit2(pose.translation.y, -146.625, 146.625) # Add new point to trajectory self.current_trajectory.add_pose(pose) # Add new point to GUI self.screen.ids.point_list.add_widget(PointDrawer()) self.screen.ids.point_list.children[0].ids.left_container.initial_update(pose.translation.x, pose.translation.y, pose.rotation.get_degrees()) # Re-draw points to include new pose self.update_points() def add_trajectory(self, trajectory = None): """Adds a trajectory to the trajectory list""" if trajectory == None: # Find a valid trajectory name (Format: Trajectory1, Trajectory2, ... Trajectory100) name = "Trajectory" + str(1) valid = False for i in range(100): name = "Trajectory" + str(i + 1) valid = False for t in self.trajectories: if t.name == name: valid = False break valid = True if valid: break trajectory = Trajectory(name=name, poses=self.random_points(), current=False, reverse=self.current_trajectory.reverse, start_velocity=self.current_trajectory.start_velocity, end_velocity=self.current_trajectory.end_velocity, max_velocity=self.current_trajectory.max_velocity, max_abs_acceleration=self.current_trajectory.max_abs_acceleration, max_centr_acceleration=self.current_trajectory.max_centr_acceleration) # Add Trajectory to graphical list item = ItemDrawer(icon="chart-bell-curve-cumulative", text=trajectory.name) self.screen.ids.content_drawer.ids.md_list.add_widget( item ) # Add Trajectory to mathematical list self.trajectories.append(trajectory) self.set_active_trajectory(item) def random_points(self): """Returns list if multiple of pseudo-random poses, else 1 pose""" theta_range = [-20.0, 20.0] x_range = [15.0, 120.0] y_range = [-100.0, 100.0] x_delta = [60.0, 100.0] y_delta = [-30.0, 30.0] theta1 = uniform(theta_range[0], theta_range[1]) theta2 = uniform(theta_range[0], theta_range[1]) x = uniform(x_range[0], x_range[1]) y = uniform(y_range[0], y_range[1]) dx = uniform(x_delta[0], x_delta[1]) dy = uniform(y_delta[0], y_delta[1]) return [to_pose(x, y, theta1), to_pose(x + dx, y + dy, theta2)] def down(self, instance): """Callback that moves a trajectory pose down one""" index = instance.get_index() if index == len(self.current_trajectory.poses) - 1: return self.current_trajectory.move_pose(index, -1) self.update_poses() def up(self, instance): """Callback that moves a trajectory pose up one""" index = instance.get_index() if index == 0: return self.current_trajectory.move_pose(index, 1) self.update_poses() def delete_point(self, instance): """Callback to delete trajectory point""" if len(self.current_trajectory.poses) == 2: return self.current_trajectory.remove_pose(instance.get_index()) self.update_poses() def update_reverse(self, checkbox, value): """https://kivymd.readthedocs.io/en/latest/components/selection-controls/""" slider_state = checkbox.active self.reverse = "Reverse Trajectory" if slider_state else "Forward Trajectory" if self.prev_reverse != slider_state: self.current_trajectory.update_reverse(slider_state) self.prev_reverse = slider_state self.update_stats() def set_reverse(self): """Sets reverse slider""" self.screen.ids.reverse.active = self.current_trajectory.reverse def set_active_trajectory(self, selected_instance): """Updates Current Trajectory""" self.screen.ids.content_drawer.ids.md_list.set_color_item(selected_instance) # Set all to False for t in self.trajectories: t.current = False # Update Current Trajectory self.current_trajectory = self.trajectories[selected_instance.get_index()] self.current_trajectory.current = True self.screen.ids.title.text = self.current_trajectory.name # Update Trajectory Iterator self.iterator = TrajectoryIterator(self.current_trajectory) self.reset_animation() self.update_poses() # Update Constraints self.set_constraints() self.set_reverse() # Update Time Slider self.screen.ids.time.min = self.iterator.start_t self.screen.ids.time.max = self.iterator.end_t def get_current_index(self): """Returns index of current trajectory""" i = 0 for t in self.trajectories: if t == self.current_trajectory: return i i += 1 def update_poses(self): """Updates displayed poses to match current trajectory""" # Remove Children self.screen.ids.point_list.clear_widgets() # Re-add Children for p in self.current_trajectory.poses: self.screen.ids.point_list.add_widget(PointDrawer()) # Update values for i in range(len(self.screen.ids.point_list.children)): c = self.screen.ids.point_list.children[len(self.screen.ids.point_list.children) - i - 1] c.ids.left_container.initial_update(self.current_trajectory.poses[i].translation.x, self.current_trajectory.poses[i].translation.y, self.current_trajectory.poses[i].rotation.get_degrees()) self.update_stats() self.update_points() def update_stats(self): """Updates Generation, drive, and length""" self.screen.ids.generation.value = "{:.7f}".format(self.current_trajectory.generation_time) self.screen.ids.drive.value = "{:.3f}".format(self.current_trajectory.drive_time) self.screen.ids.length.value = "{:.3f}".format(self.current_trajectory.length) self.screen.ids.current_vel.value = "{:.3f}".format(self.iterator.current_sample.velocity) def update_constraints(self, dt): """Periodically called to update trajectory Constraints""" # Temporarily Save Values max_velocity = self.screen.ids.max_vel.ids.slider.value max_acceleration = self.screen.ids.max_accel.ids.slider.value max_centr_acceleration = self.screen.ids.max_centr_accel.ids.slider.value start_velocity = self.screen.ids.start_vel.ids.slider.value end_velocity = self.screen.ids.end_vel.ids.slider.value # Check and Update if Necessary updated = False if self.max_vel.update(max_velocity): self.current_trajectory.update_constraint(max_velocity, 0) updated = True if self.max_accel.update(max_acceleration): self.current_trajectory.update_constraint(max_acceleration, 1) updated = True if self.max_centr_accel.update(max_centr_acceleration): self.current_trajectory.update_constraint(max_centr_acceleration, 2) updated = True if self.start_vel.update(start_velocity): self.current_trajectory.update_constraint(start_velocity, 3) updated = True if self.end_vel.update(end_velocity): self.current_trajectory.update_constraint(end_velocity, 4) updated = True if updated: self.update_stats() self.reset_animation() if self.animation_active: self.update_animation(dt) self.update_stats() elif not (self.animation_active or epsilon_equals(self.prev_time, self.screen.ids.time.value)): self.calculate_model(self.screen.ids.time.value) self.update_stats() self.prev_time = self.screen.ids.time.value def set_constraints(self): """Sets constraints""" max_velocity = self.current_trajectory.max_velocity max_acceleration = self.current_trajectory.max_abs_acceleration max_centr_acceleration = self.current_trajectory.max_centr_acceleration start_velocity = self.current_trajectory.start_velocity end_velocity = self.current_trajectory.end_velocity self.max_vel = TimeDelayedBoolean(max_velocity, self.timeout) self.max_accel = TimeDelayedBoolean(max_acceleration, self.timeout) self.max_centr_accel = TimeDelayedBoolean(max_centr_acceleration, self.timeout) self.start_vel = TimeDelayedBoolean(start_velocity, self.timeout) self.end_vel = TimeDelayedBoolean(end_velocity, self.timeout) self.screen.ids.max_vel.ids.slider.value = max_velocity self.screen.ids.max_accel.ids.slider.value = max_acceleration self.screen.ids.max_centr_accel.ids.slider.value = max_centr_acceleration self.screen.ids.start_vel.ids.slider.value = start_velocity self.screen.ids.end_vel.ids.slider.value = end_velocity def reset_animation(self): """Resets Trajectory Animation""" self.calculate_model(0.0) self.animation_active = False self.iterator.reset() self.screen.ids.time.min = self.iterator.start_t self.screen.ids.time.max = self.iterator.end_t def update_animation(self, dt): """Updates Trajectory Animation""" self.calculate_model(self.current_time + dt) if self.current_time > self.iterator.end_t: self.animation_active = False def calculate_model(self, t): """Calculates parameters for the model and updates drawing""" # Updates Time and gets next trajectory point self.current_time = t self.screen.ids.time.value = t point = self.iterator.advance(t) # Update Velocity self.origin = [self.translate_x(point.pose.translation.x), self.translate_y(point.pose.translation.y)] self.angle = point.pose.rotation.get_degrees() self.vel_length = 5.4 * abs(point.velocity) / self.current_trajectory.max_velocity # Update Acceleration centr = point.velocity ** 2 * point.pose.curvature / self.current_trajectory.max_centr_acceleration linear = point.acceleration / self.current_trajectory.max_abs_acceleration # If zero width, it leaves previous shape, so have small width self.accel_length = 5.4 * math.hypot(centr, linear) if epsilon_equals(self.accel_length, 0): self.accel_length = .01 self.accel_angle = math.degrees(math.atan2(centr, linear))