def is_collinear(self, other):
"""returns True if self and other are collinear (lie on the same line)"""
if not self.rotation.isParallel(other.rotation):
return False
tmp = self.log(self.inverse().transform(other))
return epsilon_equals(tmp.dy, 0.0) and epsilon_equals(tmp.dtheta, 0.0)
def get_curvature(self, t):
"""Returns the curvature of the spline at t"""
num = (self.dx(t) * self.ddy(t) - self.ddx(t) * self.dy(t))
# Checks if dx(t) and dy(t) are zero to avoid ZeroDivisionError
if epsilon_equals(self.dx(t), 0) and epsilon_equals(self.dy(t), 0):
return math.inf * (-1.0 if num < 0 else 1.0)
return num / (
(self.dx(t) * self.dx(t) + self.dy(t) * self.dy(t)) *
math.sqrt(self.dx(t) * self.dx(t) + self.dy(t) * self.dy(t)))
def get_dCurvature(self, t):
"""Returns dCurvature of the spline at t"""
# Checks if dx(t) and dy(t) are zero to avoid ZeroDivisionError
if epsilon_equals(self.dx(t), 0) and epsilon_equals(self.dy(t), 0):
return 0
dx2dy2 = self.dx(t) * self.dx(t) + self.dy(t) * self.dy(t)
num = (self.dx(t) * self.dddy(t) -
self.dddx(t) * self.dy(t)) * dx2dy2 - 3 * (
self.dx(t) * self.ddy(t) - self.ddx(t) * self.dy(t)) * (
self.dx(t) * self.ddx(t) + self.dy(t) * self.ddy(t))
return num / (dx2dy2 * dx2dy2 * math.sqrt(dx2dy2))
def fit_parabola(p1, p2, p3):
"""Returns the x-coordinate of the vertex of the parabola"""
a = p3.x * (p2.y - p1.y) + p2.x * (p1.y - p3.y) + p1.x * (p3.y - p2.y)
b = p3.x * p3.x * (p1.y - p2.y) + p2.x * p2.x * (
p3.y - p1.y) + p1.x * p1.x * (p2.y - p3.y)
if epsilon_equals(a, 0):
# Raises a divide by zero error because they're colinear
return 0.0
return -b / (2 * a)
def get_pose(self, t):
"""Returns pose of the spline at t"""
# Avoid ZeroDivisionError
dCurvature_ds = 0 if epsilon_equals(
self.get_velocity(t),
0) else self.get_dCurvature(t) / self.get_velocity(t)
return Pose(self.get_point(t), self.get_heading(t),
self.get_curvature(t), dCurvature_ds)
def test_util(self):
"""Tests util methods"""
# Test limit2
self.assertAlmostEqual(1.0, limit2(1.0, -2.0, 4.0))
self.assertAlmostEqual(-0.0, limit2(1.0, -2.0, -0.0))
self.assertAlmostEqual(-2.0, limit2(-5.0, -2.0, -0.0))
# Test limit
self.assertAlmostEqual(1.0, limit(1.0, 4.0))
self.assertAlmostEqual(4.0, limit(6.0, 4.0))
self.assertAlmostEqual(-3.0, limit(-5.0, 3.0))
# Test interpolate
self.assertAlmostEqual(1.0, interpolate(.0, 4.0, .25))
self.assertAlmostEqual(1.0, interpolate(-1, 4.0, .4))
self.assertAlmostEqual(1.0, interpolate(4.0, 0.0, .75))
# Test epsilon_equals
self.assertTrue(epsilon_equals(1, 1.5, .5))
self.assertTrue(epsilon_equals(2, 1.5, .5))
self.assertFalse(epsilon_equals(1, 1.5))
def sample(self, t):
"""Returns interpolated trajectory point based on t"""
if t >= self.end_t:
return self.trajectory.points[self.trajectory.trajectory_length() -
1]
if t <= self.start_t:
return self.trajectory.points[0]
for i in range(1, self.trajectory.trajectory_length()):
s = self.trajectory.points[i]
if s.t >= t:
prev_s = self.trajectory.points[i - 1]
if epsilon_equals(s.t, prev_s.t):
return s
return prev_s.interpolate(s, (t - prev_s.t) / (s.t - prev_s.t))
def update(self, value):
"""Returns True if value changes and is equivalent for a set amount of time"""
# Not the same, so reset timer, update prev_value, and return False
if (not epsilon_equals(value, self.prev_value)):
self.start_time = time.perf_counter()
self.prev_value = value
self.first = True
return False
dt = time.perf_counter() - self.start_time
return_value = False
if (dt >= self.timeout) and (self.first):
self.first = False
return_value = True
self.prev_value = value
return return_value
def get_max_drivetrain_velocity(trajectory_point, max_velocity):
"""Returns the max absolute velocity for a tank drive"""
# Going straight
if epsilon_equals(trajectory_point.pose.curvature, 0.0):
return max_velocity
# Turning in place
if math.isinf(trajectory_point.pose.curvature):
return 0.0
# right speed if left at max velocity
right_left_max = max_velocity * (trajectory_point.pose.curvature + 1) / (
1.0 - trajectory_point.pose.curvature)
if abs(right_left_max) <= max_velocity:
return (max_velocity + right_left_max) / 2.0
# left speed if right at max velocity
left_right_max = max_velocity * (1 - trajectory_point.pose.curvature) / (
1.0 + trajectory_point.pose.curvature)
return (max_velocity + left_right_max) / 2.0
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 interpolate(self, other, x):
"""Returns interpolatation between self and other"""
new_t = interpolate(self.t, other.t, x)
delta_t = new_t - self.t
# Going for 2nd pass through timed states
if delta_t < 0.0:
return other.interpolate(self, 1.0 - x)
# Constant acceleration formulas
reversing = self.velocity < 0.0 or (epsilon_equals(0.0, self.velocity)
and self.acceleration < 0.0)
new_v = self.velocity + self.acceleration * delta_t
new_s = (-1.0 if reversing else
1.0) * (self.velocity * delta_t +
.5 * self.acceleration * delta_t * delta_t)
return TrajectoryPoint(
self.pose.interpolate(other.pose,
new_s / self.pose.distance(other.pose)),
new_t, new_v, self.acceleration, self.index_floor,
other.index_ceil, self.distance + new_s)
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 isParallel(self, other):
"""Returns True if two rotations are parallel (cross product of 0)"""
return epsilon_equals(self.to_translation().cross(other.to_translation()), 0)
def get_max_centripetal_velocity(trajectory_point, max_centr_accel):
"""Returns max centripetal velocity"""
if epsilon_equals(trajectory_point.pose.curvature, 0.0):
return 1e4
return math.sqrt(abs(max_centr_accel / trajectory_point.pose.curvature))
