def testIdentity(self):
quat = Quaternion.from_roll_pitch_yaw(0, 0, 0)
vector = [0, 0, 1]
vector2 = quat.rotate(vector)
aeq(vector, vector2)
def testCompress1(self):
timestamps = array([1., 2.])
points = array([[10.5, 10.5], [50.5, 50.5], [0., 0.1], [0., 0.]])
t1, p1 = groundings.compressP(timestamps, points)
t2, p2 = groundings.compressC(timestamps, points)
aeq(t1, t2)
aeq(p1, p2)
def testRotateAwayFromOrigin(self):
pobj = PhysicalObject(
Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 0,
3), ["tires"],
path=Path.from_xyztheta([0, 1],
tp([(3, 3, 0, 0),
(3, 3, 0, math.pi / 4)])))
aeq(pobj.centroid2d, (0.5, 0.5))
newp = pobj.prismAtT(1)
self.assertEqual(
newp,
Prism.from_points_xy(
array([[0.5, 1.20710678, 0.5, -0.20710678],
[-0.20710678, 0.5, 1.20710678, 0.5]]), 0.0, 3.0))
aeq(newp.centroid2d(), (0.5, 0.5))
def testQuaternion(self):
vector = [1, 1, 1]
quat = Quaternion.from_roll_pitch_yaw(math.pi / 2, 0, 0)
vector2 = quat.rotate(vector)
aeq(vector2, [1, -1, 1])
aeq(vector, quat.conjugate.rotate(quat.rotate(vector)))
vector2 = Quaternion.from_roll_pitch_yaw(0, math.pi / 2,
0).rotate(vector)
aeq(vector2, [1, 1, -1])
vector2 = Quaternion.from_roll_pitch_yaw(math.pi / 2, 0,
0).rotate(vector)
aeq(vector2, [1, -1, 1])
def testPath(self):
pobj = PhysicalObject(
Prism.from_points_xy(tp([(0, 0), (1, 0), (1, 1), (0, 1)]), 0,
3), ["tires"],
path=Path.from_xyztheta([0, 1, 2],
tp([(3, 3, 0, 0), (3, 3, 0, math.pi / 4),
(4, 4, 1, math.pi / 4)])))
self.assertEqual(pobj.prismAtT(0), pobj.prism)
aeq(pobj.path.locationAtT(1), (3, 3, 0, math.pi / 4))
self.assertEqual(
pobj.prismAtT(1),
Prism.from_points_xy(
array([[0.5, 1.20710678, 0.5, -0.20710678],
[-0.20710678, 0.5, 1.20710678, 0.5]]), 0.0, 3.0))
aeq(pobj.path.locationAtT(2), (4, 4, 1, math.pi / 4))
self.assertEqual(
pobj.prismAtT(2),
Prism.from_points_xy(
array([[1.5, 2.20710678, 1.5, 0.79289322],
[0.79289322, 1.5, 2.20710678, 1.5]]), 1.0, 4.0))
aeq(pobj.path.locationAtT(-1),
pobj.path.locationAtT(len(pobj.path.timestamps)))
def testCompress(self):
timestamps = array([
1., 2., 3., 4., 5., 6., 7., 8., 9., 10., 11., 12., 13., 14., 15.,
16., 17., 18., 19., 20., 21., 22., 23.
])
points = array([[
15.13386494, 15.13386494, 15.23543712, 15.48414417, 15.73285121,
15.98155825, 16.2302653, 16.47897234, 16.72767938, 16.97638643,
17.22509347, 17.47380051, 17.72250755, 17.9712146, 18.21992164,
18.46862868, 18.71733573, 18.96604277, 19.21474981, 19.46345686,
19.7121639, 19.88544197, 19.93128986
],
[
48.36429004, 48.36429004, 48.35911822, 48.33372517,
48.30833213, 48.28293908, 48.25754603, 48.23215299,
48.20675994, 48.1813669, 48.15597385, 48.13058081,
48.10518776, 48.07979471, 48.05440167, 48.02900862,
48.00361558, 47.97822253, 47.95282949, 47.92743644,
47.90204339, 47.88435166, 47.87859746
],
[
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0.
],
[
0., 0., 6.18143766, 6.18143766, 6.18143766,
6.18143766, 6.18143766, 6.18143766, 6.18143766,
6.18143766, 6.18143766, 6.18143766, 6.18143766,
6.18143766, 6.18143766, 6.18143766, 6.18143766,
6.18143766, 6.18143766, 6.18143766, 6.18143766,
6.18143766, 6.13522597
]])
t1, p1 = groundings.compressP(timestamps, points)
t2, p2 = groundings.compressC(timestamps, points)
aeq(t1, t2)
aeq(p1, p2)
def testIntersectLinePlane(self):
line = [(0, 0, 0), (1, 1, 1)]
plane = [(0, 0, 0), (1, 1, 0), (3, 4, 0)]
intersect_point = math3d_intersect_line_plane(tp(line), tp(plane))
print "point", intersect_point
aeq(intersect_point, (0, 0, 0))
line = [(1, 1, 1), (1, 1, 0.5)]
plane = [(0, 0, 0), (1, 1, 0), (3, 4, 0)]
intersect_point = math3d_intersect_line_plane(tp(line), tp(plane))
print "point", intersect_point
aeq(intersect_point, (1, 1, 0))
line = [(-1, -1, -1), (-2, -2, -2)]
plane = [(0, 0, 0), (1, 1, 0), (3, 4, 0)]
intersect_point = math3d_intersect_line_plane(tp(line), tp(plane))
print "point", intersect_point
aeq(intersect_point, (0, 0, 0))
line = [(-1, -1, -1), (-2, -2, -2)]
plane = [(0, 0, 0), (1, 1, 1), (4, 4, 4)]
intersect_point = math3d_intersect_line_plane(tp(line), tp(plane))
print "point", intersect_point
aeq(intersect_point, [])
def testAxisAngle(self):
vector = [1, 1, 1]
quat = Quaternion.from_axis_angle(math.pi / 2, 0, 0)
vector2 = quat.rotate(vector)
aeq(vector2, [1, -1, 1])
vector2 = Quaternion.from_axis_angle(0, math.pi / 2, 0).rotate(vector)
aeq(vector2, [1, 1, -1])
vector2 = Quaternion.from_axis_angle(math.pi / 2, 0, 0).rotate(vector)
aeq(vector2, [1, -1, 1])
print "leg0"
quat = Quaternion.from_axis_angle(0.3, 1.56, -0.38)
print "rpy", [math.degrees(x) for x in quat.to_roll_pitch_yaw()]
aeq(quat.rotate([1, 0, 0]), [-0.02672053, -0.0458008, -0.99859316])
print "leg1"
quat = Quaternion.from_axis_angle(1.6, 0.95, -1.6)
print "rpy", [math.degrees(x) for x in quat.to_roll_pitch_yaw()]
aeq(quat.rotate([1, 0, 0]), [-0.01924605, 0.03368216, -0.99924727])
print "leg2"
quat = Quaternion.from_axis_angle(2.48, -0.74, -2.38)
print "rpy", [math.degrees(x) for x in quat.to_roll_pitch_yaw()]
aeq(quat.rotate([1, 0, 0]), [0.02983124, -0.03904517, -0.99879206])