def log(self, transform):
"""Inverse of exp: returns Twist (Twist from Pose and transformation)"""
dtheta = transform.rotation.get_radians()
half_dtheta = .5 * dtheta
cos_minus_one = transform.rotation.cos_angle - 1.0
halfdtheta_by_tan_halfdtheta = 0
if abs(cos_minus_one) < EPSILON:
halfdtheta_by_tan_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta
else:
halfdtheta_by_tan_halfdtheta = -(half_dtheta * transform.rotation.sin_angle) / cos_minus_one
tmp_rot = Rotation(halfdtheta_by_tan_halfdtheta, -half_dtheta, False)
tmp_trans = transform.translation.rotate(tmp_rot)
return Twist(tmp_trans.x, tmp_trans.y, dtheta)
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 __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 test_rotation(self):
"""Tests a Rotation"""
# Test Constructors
rot = Rotation()
self.assertAlmostEqual(1, rot.cos_angle)
self.assertAlmostEqual(0, rot.sin_angle)
self.assertAlmostEqual(0, rot.get_degrees())
self.assertAlmostEqual(0, rot.get_radians())
rot = Rotation(1.0, 1.0, True)
self.assertAlmostEqual(math.sqrt(2.0)/2.0, rot.cos_angle)
self.assertAlmostEqual(math.sqrt(2.0)/2.0, rot.sin_angle)
self.assertAlmostEqual(45, rot.get_degrees())
self.assertAlmostEqual(math.pi/4, rot.get_radians())
rot = from_radians(math.pi / 2.0)
self.assertAlmostEqual(0, rot.cos_angle)
self.assertAlmostEqual(1, rot.sin_angle)
self.assertAlmostEqual(90, rot.get_degrees())
self.assertAlmostEqual(math.pi/2.0, rot.get_radians())
rot = from_degrees(270.0)
self.assertAlmostEqual(0, rot.cos_angle)
self.assertAlmostEqual(-1.0, rot.sin_angle)
self.assertAlmostEqual(-90, rot.get_degrees())
self.assertAlmostEqual(- math.pi/2, rot.get_radians())
# Test inverse
inverse = rot.inverse()
self.assertAlmostEqual(0, inverse.cos_angle)
self.assertAlmostEqual(1.0, inverse.sin_angle)
self.assertAlmostEqual(90, inverse.get_degrees())
self.assertAlmostEqual(math.pi/2, inverse.get_radians())
rot = from_degrees(1.0)
inverse = rot.inverse()
self.assertAlmostEqual(rot.cos_angle, inverse.cos_angle)
self.assertAlmostEqual(-rot.sin_angle, inverse.sin_angle)
self.assertAlmostEqual(-1, inverse.get_degrees())
# Test rotate
rot1 = from_degrees(45)
rot2 = from_degrees(45)
rot3 = rot1.rotate(rot2)
self.assertAlmostEqual(0, rot3.cos_angle)
self.assertAlmostEqual(1.0, rot3.sin_angle)
self.assertAlmostEqual(90, rot3.get_degrees())
self.assertAlmostEqual(math.pi/2, rot3.get_radians())
rot1 = Rotation()
rot2 = from_degrees(21.45)
rot3 = rot2.rotate(rot2.inverse())
self.assertAlmostEqual(rot1.cos_angle, rot3.cos_angle)
self.assertAlmostEqual(rot1.sin_angle, rot3.sin_angle)
self.assertAlmostEqual(rot1.get_degrees(), rot3.get_degrees())
# Test interpolation
rot1 = from_degrees(45)
rot2 = from_degrees(135)
rot3 = rot1.interpolate(rot2, .5)
self.assertAlmostEqual(90, rot3.get_degrees())
rot3 = rot1.interpolate(rot2, .75)
self.assertAlmostEqual(112.5, rot3.get_degrees())
rot1 = from_degrees(45)
rot2 = from_degrees(45)
rot3 = rot1.interpolate(rot2, .5)
self.assertAlmostEqual(45, rot3.get_degrees())
def get_heading(self, t):
"""Returns heading of the spline at t"""
return Rotation(self.dx(t), self.dy(t), True)
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))