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 test_twist(self):
"""Tests Twist methods"""
# Test constructor
twist = Twist(1.0, 0.0, 0.0)
pose = Pose()
pose = pose.exp(twist)
self.assertAlmostEqual(1.0, pose.translation.x)
self.assertAlmostEqual(0.0, pose.translation.y)
self.assertAlmostEqual(0.0, pose.rotation.get_degrees())
# Test scaled
pose = pose.exp(twist.scaled(2.5))
self.assertAlmostEqual(2.5, pose.translation.x)
self.assertAlmostEqual(0.0, pose.translation.y)
self.assertAlmostEqual(0.0, pose.rotation.get_degrees())
# Test logarithm
pose = to_pose(2.0, 2.0, 90.0)
twist = pose.log(pose)
self.assertAlmostEqual(math.pi, twist.dx)
self.assertAlmostEqual(0.0, twist.dy)
self.assertAlmostEqual(math.pi / 2, twist.dtheta)
# Test exponentiation: inverse of logarithm
pose1 = pose.exp(twist)
self.assertAlmostEqual(pose1.translation.x, pose.translation.x)
self.assertAlmostEqual(pose1.translation.y, pose.translation.y)
self.assertAlmostEqual(pose1.rotation.get_degrees(), pose.rotation.get_degrees())
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 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)]
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
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")
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()
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))
def test_trajectory_point(self):
"""Tests TrajectoryPoint"""
# Test constructor
traj = TrajectoryPoint()
self.assertAlmostEqual(0, traj.pose.translation.x)
self.assertAlmostEqual(0, traj.pose.translation.y)
self.assertAlmostEqual(0, traj.pose.rotation.get_degrees())
self.assertAlmostEqual(0, traj.pose.curvature)
self.assertAlmostEqual(0, traj.pose.dCurvature)
self.assertAlmostEqual(0, traj.t)
self.assertAlmostEqual(0, traj.velocity)
self.assertAlmostEqual(0, traj.acceleration)
self.assertAlmostEqual(0, traj.index_floor)
self.assertAlmostEqual(0, traj.index_ceil)
traj1 = TrajectoryPoint(to_pose(0, 0, 0), 0, 0, 1, 6, 6)
self.assertAlmostEqual(0, traj1.pose.translation.x)
self.assertAlmostEqual(0, traj1.pose.translation.y)
self.assertAlmostEqual(0, traj1.pose.rotation.get_degrees())
self.assertAlmostEqual(0, traj1.pose.curvature)
self.assertAlmostEqual(0, traj1.pose.dCurvature)
self.assertAlmostEqual(0, traj1.t)
self.assertAlmostEqual(0, traj1.velocity)
self.assertAlmostEqual(1, traj1.acceleration)
self.assertAlmostEqual(6, traj1.index_floor)
self.assertAlmostEqual(6, traj1.index_ceil)
traj2 = TrajectoryPoint(to_pose(.5, 0, 0), 1, 1, 0, 7, 7)
self.assertAlmostEqual(.5, traj2.pose.translation.x)
self.assertAlmostEqual(0, traj2.pose.translation.y)
self.assertAlmostEqual(0, traj2.pose.rotation.get_degrees())
self.assertAlmostEqual(0, traj2.pose.curvature)
self.assertAlmostEqual(0, traj2.pose.dCurvature)
self.assertAlmostEqual(1, traj2.t)
self.assertAlmostEqual(1, traj2.velocity)
self.assertAlmostEqual(0, traj2.acceleration)
self.assertAlmostEqual(7, traj2.index_floor)
self.assertAlmostEqual(7, traj2.index_ceil)
traj3 = traj1.interpolate(traj2, .5)
self.assertAlmostEqual(.125, traj3.pose.translation.x)
self.assertAlmostEqual(0, traj3.pose.translation.y)
self.assertAlmostEqual(0, traj3.pose.rotation.get_degrees())
self.assertAlmostEqual(0, traj3.pose.curvature)
self.assertAlmostEqual(0, traj3.pose.dCurvature)
self.assertAlmostEqual(.5, traj3.t)
self.assertAlmostEqual(.5, traj3.velocity)
self.assertAlmostEqual(1, traj3.acceleration)
self.assertAlmostEqual(6, traj3.index_floor)
self.assertAlmostEqual(7, traj3.index_ceil)
# Test Reverse interpolation
traj3 = traj2.interpolate(traj1, .5)
self.assertAlmostEqual(.125, traj3.pose.translation.x)
self.assertAlmostEqual(0, traj3.pose.translation.y)
self.assertAlmostEqual(0, traj3.pose.rotation.get_degrees())
self.assertAlmostEqual(0, traj3.pose.curvature)
self.assertAlmostEqual(0, traj3.pose.dCurvature)
self.assertAlmostEqual(.5, traj3.t)
self.assertAlmostEqual(.5, traj3.velocity)
self.assertAlmostEqual(1, traj3.acceleration)
self.assertAlmostEqual(6, traj3.index_floor)
self.assertAlmostEqual(7, traj3.index_ceil)
def test_pose(self):
"""Tests pose methods"""
# Tests Constructor
pose = Pose()
self.assertAlmostEqual(0, pose.translation.x)
self.assertAlmostEqual(0, pose.translation.y)
self.assertAlmostEqual(0, pose.rotation.get_degrees())
self.assertAlmostEqual(0, pose.curvature)
self.assertAlmostEqual(0, pose.dCurvature)
pose = Pose(Translation(3.0, 4.0), from_degrees(45), .5, .1)
self.assertAlmostEqual(3, pose.translation.x)
self.assertAlmostEqual(4, pose.translation.y)
self.assertAlmostEqual(45, pose.rotation.get_degrees())
self.assertAlmostEqual(.5, pose.curvature)
self.assertAlmostEqual(.1, pose.dCurvature)
pose = to_pose(4.0, 3.0, -45, .4, -.2)
self.assertAlmostEqual(4, pose.translation.x)
self.assertAlmostEqual(3, pose.translation.y)
self.assertAlmostEqual(-45, pose.rotation.get_degrees())
self.assertAlmostEqual(.4, pose.curvature)
self.assertAlmostEqual(-.2, pose.dCurvature)
# Test transform
pose1 = to_pose(3.0, 4.0, 90.0, .4, .2)
pose2 = to_pose(1.0, 0.0, 0.0)
pose3 = pose1.transform(pose2)
self.assertAlmostEqual(3, pose3.translation.x)
self.assertAlmostEqual(5, pose3.translation.y)
self.assertAlmostEqual(90, pose3.rotation.get_degrees())
self.assertAlmostEqual(.4, pose3.curvature)
self.assertAlmostEqual(.2, pose3.dCurvature)
pose1 = to_pose(3.0, 4.0, 90.0)
pose2 = to_pose(1.0, 0.0, -90.0)
pose3 = pose1.transform(pose2)
self.assertAlmostEqual(3, pose3.translation.x)
self.assertAlmostEqual(5, pose3.translation.y)
self.assertAlmostEqual(0, pose3.rotation.get_degrees())
self.assertAlmostEqual(0, pose3.curvature)
self.assertAlmostEqual(0, pose3.dCurvature)
# Test inverse
identity = Pose()
pose1 = to_pose(3.12123424, 8.286395, 93.1235, .5, .3)
pose2 = pose1.transform(pose1.inverse())
self.assertAlmostEqual(identity.translation.x, pose2.translation.x)
self.assertAlmostEqual(identity.translation.y, pose2.translation.y)
self.assertAlmostEqual(identity.rotation.get_degrees(), pose3.rotation.get_degrees())
self.assertAlmostEqual(.5, pose2.curvature)
self.assertAlmostEqual(.3, pose2.dCurvature)
# Test interpolation
# Movement from pose1 to pose2 along a circle with radius 10 centered at (3, -6)
pose1 = to_pose(3.0, 4.0, 90.0, .5, .1)
pose2 = to_pose(13.0, -6.0, 0.0, 1.0, .2)
pose3 = pose1.interpolate(pose2, .5)
expected_angle_radians = math.pi / 4.0
self.assertAlmostEqual(3 + 10.0 * math.cos(expected_angle_radians), pose3.translation.x)
self.assertAlmostEqual(-6 + 10.0 * math.sin(expected_angle_radians), pose3.translation.y)
self.assertAlmostEqual(expected_angle_radians, pose3.rotation.get_radians())
self.assertAlmostEqual(.75, pose3.curvature)
self.assertAlmostEqual(.15, pose3.dCurvature)
pose1 = to_pose(3.0, 4.0, 90.0)
pose2 = to_pose(13.0, -6.0, 0.0)
pose3 = pose1.interpolate(pose2, .75)
expected_angle_radians = math.pi / 8.0
self.assertAlmostEqual(3 + 10.0 * math.cos(expected_angle_radians), pose3.translation.x)
self.assertAlmostEqual(-6 + 10.0 * math.sin(expected_angle_radians), pose3.translation.y)
self.assertAlmostEqual(expected_angle_radians, pose3.rotation.get_radians())
self.assertAlmostEqual(0.0, pose3.curvature)
self.assertAlmostEqual(0.0, pose3.dCurvature)
# Test distance
self.assertAlmostEqual(math.pi * 5, pose1.distance(pose2))
# Test mirror
pose = to_pose(4.0, 3.0, -45, .4, -.2)
pose1 = pose.mirror()
self.assertAlmostEqual(4, pose1.translation.x)
self.assertAlmostEqual(-3, pose1.translation.y)
self.assertAlmostEqual(45, pose1.rotation.get_degrees())
self.assertAlmostEqual(-.4, pose1.curvature)
self.assertAlmostEqual(.2, pose1.dCurvature)
# Test is_collinear
pose1 = to_pose(3.0, 4.0, 90.0)
pose2 = to_pose(13.0, -6.0, 0.0)
self.assertFalse(pose1.is_collinear(pose2))
pose1 = to_pose(3.0, 4.0, 90.0)
pose2 = to_pose(3.0, 6.0, 90.0)
self.assertTrue(pose1.is_collinear(pose2))
def test_spline_construction(self):
"""Tests Quintic Hermite Spline construction"""
s = create_quintic_spline()
self.assertAlmostEqual(0, s.get_start_pose().translation.x)
self.assertAlmostEqual(0, s.get_start_pose().translation.y)
self.assertAlmostEqual(0, s.get_start_pose().rotation.get_degrees())
self.assertAlmostEqual(0, s.get_end_pose().translation.x)
self.assertAlmostEqual(0, s.get_end_pose().translation.y)
self.assertAlmostEqual(0, s.get_end_pose().rotation.get_degrees())
self.assertAlmostEqual(0, s.get_velocity(0))
self.assertAlmostEqual(0, s.get_curvature(0))
self.assertAlmostEqual(0, s.get_dCurvature(0))
self.assertAlmostEqual(0, s.get_heading(0).get_degrees())
self.assertAlmostEqual(0, s.get_point(0).x)
self.assertAlmostEqual(0, s.get_point(0).y)
self.assertAlmostEqual(0, s.get_pose(0).translation.x)
self.assertAlmostEqual(0, s.get_pose(0).translation.y)
self.assertAlmostEqual(0, s.get_pose(0).rotation.get_degrees())
self.assertAlmostEqual(0, s.get_velocity(1))
self.assertAlmostEqual(0, s.get_curvature(1))
self.assertAlmostEqual(0, s.get_dCurvature(1))
self.assertAlmostEqual(0, s.get_heading(1).get_degrees())
self.assertAlmostEqual(0, s.get_point(1).x)
self.assertAlmostEqual(0, s.get_point(1).y)
self.assertAlmostEqual(0, s.get_pose(1).translation.x)
self.assertAlmostEqual(0, s.get_pose(1).translation.y)
self.assertAlmostEqual(0, s.get_pose(1).rotation.get_degrees())
s = create_quintic_spline(p1=to_pose(3.0, 4.0, 90.0))
self.assertAlmostEqual(0, s.get_start_pose().translation.x)
self.assertAlmostEqual(0, s.get_start_pose().translation.y)
self.assertAlmostEqual(0, s.get_start_pose().rotation.get_degrees())
self.assertAlmostEqual(3.0, s.get_end_pose().translation.x)
self.assertAlmostEqual(4.0, s.get_end_pose().translation.y)
self.assertAlmostEqual(90.0, s.get_end_pose().rotation.get_degrees())
self.assertAlmostEqual(6.0, s.get_velocity(0))
self.assertAlmostEqual(0, s.get_curvature(0))
#self.assertAlmostEqual(0, s.get_dCurvature(0))
self.assertAlmostEqual(0, s.get_heading(0).get_degrees())
self.assertAlmostEqual(0, s.get_point(0).x)
self.assertAlmostEqual(0, s.get_point(0).y)
self.assertAlmostEqual(0, s.get_pose(0).translation.x)
self.assertAlmostEqual(0, s.get_pose(0).translation.y)
self.assertAlmostEqual(0, s.get_pose(0).rotation.get_degrees())
self.assertAlmostEqual(6.0, s.get_velocity(1))
self.assertAlmostEqual(0, s.get_curvature(1))
#self.assertAlmostEqual(0, s.get_dCurvature(1))
self.assertAlmostEqual(90, s.get_heading(1).get_degrees())
self.assertAlmostEqual(3, s.get_point(1).x)
self.assertAlmostEqual(4, s.get_point(1).y)
self.assertAlmostEqual(3, s.get_pose(1).translation.x)
self.assertAlmostEqual(4, s.get_pose(1).translation.y)
self.assertAlmostEqual(90, s.get_pose(1).rotation.get_degrees())
def test_optimization(self):
"""Tests Spline optimization"""
# Optimization test 1
p1 = to_pose(0.0, 100.0, 270.0)
p2 = to_pose(50.0, 0.0, 0.0)
p3 = to_pose(100.0, 100.0, 90.0)
splines = []
splines.append(create_quintic_spline(p1, p2))
splines.append(create_quintic_spline(p2, p3))
start_time = time.perf_counter()
self.assertTrue(optimize_spline(splines) < 0.014)
print("Optimization time: {:.6}s".format(time.perf_counter() -
start_time))
# Optimization test 2
p4 = to_pose(degrees=90.0)
p5 = to_pose(0.0, 50.0, 0.0)
p6 = to_pose(100.0, 0.0, 90.0)
p7 = to_pose(100.0, 100.0, 0.0)
splines1 = []
splines1.append(create_quintic_spline(p4, p5))
splines1.append(create_quintic_spline(p5, p6))
splines1.append(create_quintic_spline(p6, p7))
start_time = time.perf_counter()
self.assertTrue(optimize_spline(splines1) < 0.16)
print("Optimization time: {:.6}s".format(time.perf_counter() -
start_time))
# Optimization test 3
p8 = to_pose()
p9 = to_pose(50.0, 0.0, 0.0)
p10 = to_pose(100.0, 50.0, 45.0)
p11 = to_pose(150.0, 0.0, 270.0)
p12 = to_pose(150.0, -50.0, 270.0)
splines2 = []
splines2.append(create_quintic_spline(p8, p9))
splines2.append(create_quintic_spline(p9, p10))
splines2.append(create_quintic_spline(p10, p11))
splines2.append(create_quintic_spline(p11, p12))
start_time = time.perf_counter()
self.assertTrue(optimize_spline(splines1) < 0.16)
print("Optimization time: {:.6}s".format(time.perf_counter() -
start_time))
def properties_to_json(self, trajectory):
"""Helper method to convert Trajectory properties to list"""
return [
trajectory.current, trajectory.reverse, trajectory.start_velocity,
trajectory.end_velocity, trajectory.max_velocity,
trajectory.max_abs_acceleration, trajectory.max_centr_acceleration
]
def points_to_json(self, trajectory):
"""Helper method to convert Trajectory points to list"""
data = []
# Add each point as a list
for p in trajectory.poses:
tmp = [p.translation.x, p.translation.y, p.rotation.get_degrees()]
data.append(tmp)
return data
if __name__ == "__main__":
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