def create_quintic_spline(p0=Pose(), p1=Pose()):
"""Helper function to return a QuinticHermiteSpline object from 2 Poses"""
scale = 1.2 * p0.translation.distance(p1.translation)
return QuinticHermiteSpline(p0.translation.x, p1.translation.x,
p0.rotation.cos_angle * scale,
p1.rotation.cos_angle * scale, 0, 0,
p0.translation.y, p1.translation.y,
p0.rotation.sin_angle * scale,
p1.rotation.sin_angle * scale, 0, 0)
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_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 get_segment_arc(spline, points, t0, t1, max_dx, max_dy, max_dtheta):
"""Recursively adds a TrajectoryPoint to the list if the transformation is less than the max allowable"""
p0 = spline.get_point(t0)
p1 = spline.get_point(t1)
r0 = spline.get_heading(t0)
r1 = spline.get_heading(t1)
transformation = Pose(
Translation(p1.x - p0.x, p1.y - p0.y).rotate(r0.inverse()),
r1.rotate(r0.inverse()))
twist = Pose().log(transformation)
# Recursively divide segment arc in half until twist is smaller than max
twist.dtheta -= math.copysign(
math.pi, twist.dtheta) if abs(twist.dtheta) > (math.pi / 2) else 0
if (abs(twist.dy) > max_dy or abs(twist.dx) > max_dx
or abs(twist.dtheta) > max_dtheta):
get_segment_arc(spline, points, t0, (t0 + t1) / 2.0, max_dx, max_dy,
max_dtheta)
get_segment_arc(spline, points, (t0 + t1) / 2.0, t1, max_dx, max_dy,
max_dtheta)
else:
points.append(TrajectoryPoint(spline.get_pose(t1)))
def __init__(self,
pose=Pose(),
time=0,
velocity=0,
acceleration=0,
index_floor=0,
index_ceil=0,
distance=0):
"""Constructs a TrajectoryPoint instance"""
self.pose = pose
self.t = time
self.velocity = velocity
self.acceleration = acceleration
self.index_floor = index_floor
self.index_ceil = index_ceil
# Variables for time parameterization
self.min_accelation = -acceleration
self.max_acceleration = acceleration
self.distance = distance
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 get_end_pose(self):
"""Returns end pose of the spline"""
return Pose(Translation(self.x1, self.y1),
Rotation(self.dx1, self.dy1, True))
def get_start_pose(self):
"""Returns start pose of the spline"""
return Pose(Translation(self.x0, self.y0),
Rotation(self.dx0, self.dy0, True))