def test_fit_parabola(self):
"""Tests fit_parabola"""
# Symmetric downwards parabola (vertex x coordinate at 0)
x = fit_parabola(Translation(-2.0, 2.0), Translation(0.0, 4.0),
Translation(2.0, 2.0))
self.assertAlmostEqual(0, x)
def get_point(self, t):
"""Returns Translation at t (from 0 to 1)"""
x = (self.Ax * t * t * t * t * t) + (self.Bx * t * t * t * t) + (
self.Cx * t * t * t) + (self.Dx * t * t) + (self.Ex *
t) + (self.Fx)
y = (self.Ay * t * t * t * t * t) + (self.By * t * t * t * t) + (
self.Cy * t * t * t) + (self.Dy * t * t) + (self.Ey *
t) + (self.Fy)
return Translation(x, y)
def exp(self, delta):
"""Lie Algebra Exponential map: returns Pose (transform from one Pose to the next)"""
sin_theta = math.sin(delta.dtheta)
cos_theta = math.cos(delta.dtheta)
s = c = 0
if abs(delta.dtheta) < EPSILON:
s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta
c = .5 * delta.dtheta
else:
s = sin_theta / delta.dtheta
c = (1.0 - cos_theta) / delta.dtheta
return Pose(Translation(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s),
Rotation(cos_theta, sin_theta, False))
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 mirror(self):
"""Returns mirrored point about y = 0"""
return Pose(Translation(self.translation.x, -self.translation.y), self.rotation.inverse(),-self.curvature, -self.dCurvature)
def __init__(self, translation=Translation(), rotation=Rotation(), curvature=0.0, dCurvature=0.0):
"""Constructs a Pose object"""
self.translation = translation
self.rotation = rotation
self.curvature = curvature
self.dCurvature = dCurvature
def to_pose(x=0.0, y=0.0, degrees=0.0, curvature=0.0, dCurvature=0.0):
"""Helper function to create a Pose from (x, y, degrees)"""
return Pose(Translation(x,y), from_degrees(degrees), curvature, dCurvature)
def to_translation(self):
"""Returns Rotation as a translation"""
return Translation(self.cos_angle, self.sin_angle)
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_translation(self):
"""Tests a Translation"""
# Test constructors
trans = Translation()
self.assertAlmostEqual(0, trans.x)
self.assertAlmostEqual(0, trans.y)
self.assertAlmostEqual(0, trans.norm())
trans = Translation(3.0, 4.0)
self.assertAlmostEqual(3.0, trans.x)
self.assertAlmostEqual(4.0, trans.y)
self.assertAlmostEqual(5.0, trans.norm())
# Test inversion
trans1 = Translation(3.152, 4.1666)
trans2 = trans1.inverse()
self.assertAlmostEqual(-trans1.x, trans2.x)
self.assertAlmostEqual(-trans1.y, trans2.y)
self.assertAlmostEqual(trans1.norm(), trans2.norm())
# Test rotate
trans = Translation(2.0, 0.0)
rot = from_degrees(90.)
trans1 = trans.rotate(rot)
self.assertAlmostEqual(0.0, trans1.x)
self.assertAlmostEqual(2.0, trans1.y)
self.assertAlmostEqual(trans.norm(), trans1.norm())
rot = from_degrees(-45)
trans1 = trans.rotate(rot)
self.assertAlmostEqual(math.sqrt(2.0), trans1.x)
self.assertAlmostEqual(-math.sqrt(2.0), trans1.y)
self.assertAlmostEqual(trans.norm(), trans1.norm())
# Test translate
trans1 = Translation(2.0, 1.0)
trans2 = Translation(-2.0, 0.0)
trans3 = trans1.translate(trans2)
self.assertAlmostEqual(0.0, trans3.x)
self.assertAlmostEqual(1.0, trans3.y)
self.assertAlmostEqual(1, trans3.norm())
# Test inverse
trans = Translation()
trans1 = Translation(2.16612, -23.55)
trans2 = trans1.translate(trans1.inverse())
self.assertAlmostEqual(trans.x, trans2.x)
self.assertAlmostEqual(trans.y, trans2.y)
self.assertAlmostEqual(trans.norm(), trans2.norm())
# Test interpolation
trans = Translation(0.0, 1.0)
trans1 = Translation(10.0, -1.0)
trans2 = trans.interpolate(trans1, .75)
self.assertAlmostEqual(7.5, trans2.x)
self.assertAlmostEqual(-.5, trans2.y)
def run_optimization_iteration(splines):
"""Runs one optimization iteration on list of splines"""
if (len(splines) <= 1):
return
control_points = []
magnitude = 0.0
for i in range(len(splines) - 1):
if (splines[i].get_start_pose().is_collinear(
splines[i + 1].get_start_pose())) or (
splines[i].get_end_pose().is_collinear(
splines[i + 1].get_end_pose())):
# Skip optimization if consecutive splines are collinear
continue
original = sum_dCurvature2(splines)
tmp1 = splines[i]
tmp2 = splines[i + 1]
control_points.append(ControlPoint())
# Calculate partial derivatives of sum_dCurvature2
# Partial x
splines[i] = QuinticHermiteSpline(tmp1.x0, tmp1.x1, tmp1.dx0, tmp1.dx1,
tmp1.ddx0, tmp1.ddx1 + EPSILON,
tmp1.y0, tmp1.y1, tmp1.dy0, tmp1.dy1,
tmp1.ddy0, tmp1.ddy1)
splines[i + 1] = QuinticHermiteSpline(tmp2.x0, tmp2.x1, tmp2.dx0,
tmp2.dx1, tmp2.ddx0 + EPSILON,
tmp2.ddx1, tmp2.y0, tmp2.y1,
tmp2.dy0, tmp2.dy1, tmp2.ddy0,
tmp2.ddy1)
control_points[i].ddx = (sum_dCurvature2(splines) - original) / EPSILON
# Partial y
splines[i] = QuinticHermiteSpline(tmp1.x0, tmp1.x1, tmp1.dx0, tmp1.dx1,
tmp1.ddx0, tmp1.ddx1, tmp1.y0,
tmp1.y1, tmp1.dy0, tmp1.dy1,
tmp1.ddy0, tmp1.ddy1 + EPSILON)
splines[i + 1] = QuinticHermiteSpline(tmp2.x0, tmp2.x1, tmp2.dx0,
tmp2.dx1, tmp2.ddx0, tmp2.ddx1,
tmp2.y0, tmp2.y1, tmp2.dy0,
tmp2.dy1, tmp2.ddy0 + EPSILON,
tmp2.ddy1)
control_points[i].ddy = (sum_dCurvature2(splines) - original) / EPSILON
# Reset splines
splines[i] = tmp1
splines[i + 1] = tmp2
magnitude += control_points[i].ddx * control_points[
i].ddx + control_points[i].ddy * control_points[i].ddy
magnitude = math.sqrt(magnitude)
# Minimize along the direction of the gradient by calculating 3 points along it.
p2 = Translation(
0, sum_dCurvature2(splines)) # Middle point is the current location
for i in range(len(splines) - 1):
if (splines[i].get_start_pose().is_collinear(
splines[i + 1].get_start_pose())) or (
splines[i].get_end_pose().is_collinear(
splines[i + 1].get_end_pose())):
# Skip optimization if consecutive splines are collinear
continue
# Normalize to step size
control_points[i].ddx *= STEPSIZE / magnitude
control_points[i].ddy *= STEPSIZE / magnitude
# Move opposite the gradient by STEPSIZE
splines[i].ddx1 -= control_points[i].ddx
splines[i].ddy1 -= control_points[i].ddy
splines[i + 1].ddx0 -= control_points[i].ddx
splines[i + 1].ddy0 -= control_points[i].ddy
# Recompute spline coefficients to account for new 2nd derivatives
splines[i].compute_coefficients()
splines[i + 1].compute_coefficients()
p1 = Translation(-STEPSIZE, sum_dCurvature2(splines))
for i in range(len(splines) - 1):
if (splines[i].get_start_pose().is_collinear(
splines[i + 1].get_start_pose())) or (
splines[i].get_end_pose().is_collinear(
splines[i + 1].get_end_pose())):
# Skip optimization if consecutive splines are collinear
continue
# Move in direction of the gradient by 2 * STEPSIZE (return to original position and 1 more)
splines[i].ddx1 += 2 * control_points[i].ddx
splines[i].ddy1 += 2 * control_points[i].ddy
splines[i + 1].ddx0 += 2 * control_points[i].ddx
splines[i + 1].ddy0 += 2 * control_points[i].ddy
# Recompute spline coefficients to account for new 2nd derivatives
splines[i].compute_coefficients()
splines[i + 1].compute_coefficients()
p3 = Translation(STEPSIZE, sum_dCurvature2(splines))
step_size = fit_parabola(
p1, p2, p3
) # Approximate step size to minimize sum_dCurvature2 along the grandient
for i in range(len(splines) - 1):
if (splines[i].get_start_pose().is_collinear(
splines[i + 1].get_start_pose())) or (
splines[i].get_end_pose().is_collinear(
splines[i + 1].get_end_pose())):
# Skip optimization if consecutive splines are collinear
continue
# Normalize to step size (+1 to offset for the final transformation to find p3)
control_points[i].ddx *= 1 + step_size / STEPSIZE
control_points[i].ddy *= 1 + step_size / STEPSIZE
splines[i].ddx1 += control_points[i].ddx
splines[i].ddy1 += control_points[i].ddy
splines[i + 1].ddx0 += control_points[i].ddx
splines[i + 1].ddy0 += control_points[i].ddy
# Recompute spline coefficients to account for new 2nd derivatives
splines[i].compute_coefficients()
splines[i + 1].compute_coefficients()
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))