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))