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 update_pose(self, index, value, key): """Mutator to update a single value at index""" if key == "x": self.poses[index].translation.x = value elif key == "y": self.poses[index].translation.y = value if key == "theta": self.poses[index].rotation = from_degrees(value) self.reset()
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 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 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))