def Rxyz(theta, which):
theta = np.radians(theta)
if which == "X":
return SO3.Rx(theta)
elif which == "Y":
return SO3.Ry(theta)
elif which == "Z":
return SO3.Rz(theta)
def test_constructor(self):
# null constructor
R = SO3()
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(3))
self.assertIsInstance(R, SO3)
# empty constructor
R = SO3.Empty()
nt.assert_equal(len(R), 0)
self.assertIsInstance(R, SO3)
# construct from matrix
R = SO3(rotx(0.2))
nt.assert_equal(len(R), 1)
array_compare(R, rotx(0.2))
self.assertIsInstance(R, SO3)
# construct from canonic rotation
R = SO3.Rx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, rotx(0.2))
self.assertIsInstance(R, SO3)
R = SO3.Ry(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, roty(0.2))
self.assertIsInstance(R, SO3)
R = SO3.Rz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, rotz(0.2))
self.assertIsInstance(R, SO3)
# OA
R = SO3.OA([0, 1, 0], [0, 0, 1])
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(3))
self.assertIsInstance(R, SO3)
# random
R = SO3.Rand()
nt.assert_equal(len(R), 1)
self.assertIsInstance(R, SO3)
# copy constructor
R = SO3.Rx(pi / 2)
R2 = SO3(R)
R = SO3.Ry(pi / 2)
array_compare(R2, rotx(pi / 2))
def test_arith_vect(self):
rx = SO3.Rx(pi / 2)
ry = SO3.Ry(pi / 2)
rz = SO3.Rz(pi / 2)
u = SO3()
# multiply
R = SO3([rx, ry, rz])
a = R * rx
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * rx)
array_compare(a[1], ry * rx)
array_compare(a[2], rz * rx)
a = rx * R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * rx)
array_compare(a[1], rx * ry)
array_compare(a[2], rx * rz)
a = R * R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * rx)
array_compare(a[1], ry * ry)
array_compare(a[2], rz * rz)
a = R * 2
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * 2)
array_compare(a[1], ry * 2)
array_compare(a[2], rz * 2)
a = 2 * R
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * 2)
array_compare(a[1], ry * 2)
array_compare(a[2], rz * 2)
a = R
a *= rx
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * rx)
array_compare(a[1], ry * rx)
array_compare(a[2], rz * rx)
a = rx
a *= R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * rx)
array_compare(a[1], rx * ry)
array_compare(a[2], rx * rz)
a = R
a *= R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * rx)
array_compare(a[1], ry * ry)
array_compare(a[2], rz * rz)
a = R
a *= 2
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * 2)
array_compare(a[1], ry * 2)
array_compare(a[2], rz * 2)
# SO3 x vector
vx = np.r_[1, 0, 0]
vy = np.r_[0, 1, 0]
vz = np.r_[0, 0, 1]
a = R * vx
array_compare(a[:, 0], (rx * vx).flatten())
array_compare(a[:, 1], (ry * vx).flatten())
array_compare(a[:, 2], (rz * vx).flatten())
a = rx * np.vstack((vx, vy, vz)).T
array_compare(a[:, 0], (rx * vx).flatten())
array_compare(a[:, 1], (rx * vy).flatten())
array_compare(a[:, 2], (rx * vz).flatten())
# divide
R = SO3([rx, ry, rz])
a = R / rx
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx / rx)
array_compare(a[1], ry / rx)
array_compare(a[2], rz / rx)
a = rx / R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx / rx)
array_compare(a[1], rx / ry)
array_compare(a[2], rx / rz)
a = R / R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], np.eye(3))
array_compare(a[1], np.eye(3))
array_compare(a[2], np.eye(3))
a = R / 2
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx / 2)
array_compare(a[1], ry / 2)
array_compare(a[2], rz / 2)
a = R
a /= rx
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx / rx)
array_compare(a[1], ry / rx)
array_compare(a[2], rz / rx)
a = rx
a /= R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx / rx)
array_compare(a[1], rx / ry)
array_compare(a[2], rx / rz)
a = R
a /= R
self.assertIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], np.eye(3))
array_compare(a[1], np.eye(3))
array_compare(a[2], np.eye(3))
a = R
a /= 2
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx / 2)
array_compare(a[1], ry / 2)
array_compare(a[2], rz / 2)
# add
R = SO3([rx, ry, rz])
a = R + rx
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx + rx)
array_compare(a[1], ry + rx)
array_compare(a[2], rz + rx)
a = rx + R
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx + rx)
array_compare(a[1], rx + ry)
array_compare(a[2], rx + rz)
a = R + R
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx + rx)
array_compare(a[1], ry + ry)
array_compare(a[2], rz + rz)
a = R + 1
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx + 1)
array_compare(a[1], ry + 1)
array_compare(a[2], rz + 1)
# subtract
R = SO3([rx, ry, rz])
a = R - rx
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx - rx)
array_compare(a[1], ry - rx)
array_compare(a[2], rz - rx)
a = rx - R
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx - rx)
array_compare(a[1], rx - ry)
array_compare(a[2], rx - rz)
a = R - R
self.assertNotIsInstance(a, SO3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx - rx)
array_compare(a[1], ry - ry)
array_compare(a[2], rz - rz)
def test_arith(self):
R = SO3()
# sum
a = R + R
self.assertNotIsInstance(a, SO3)
array_compare(a, np.array([[2, 0, 0], [0, 2, 0], [0, 0, 2]]))
a = R + 1
self.assertNotIsInstance(a, SO3)
array_compare(a, np.array([[2, 1, 1], [1, 2, 1], [1, 1, 2]]))
# a = 1 + R
# self.assertNotIsInstance(a, SO3)
# array_compare(a, np.array([ [2,1,1], [1,2,1], [1,1,2]]))
a = R + np.eye(3)
self.assertNotIsInstance(a, SO3)
array_compare(a, np.array([[2, 0, 0], [0, 2, 0], [0, 0, 2]]))
# a = np.eye(3) + R
# self.assertNotIsInstance(a, SO3)
# array_compare(a, np.array([ [2,0,0], [0,2,0], [0,0,2]]))
# this invokes the __add__ method for numpy
# difference
R = SO3()
a = R - R
self.assertNotIsInstance(a, SO3)
array_compare(a, np.zeros((3, 3)))
a = R - 1
self.assertNotIsInstance(a, SO3)
array_compare(a, np.array([[0, -1, -1], [-1, 0, -1], [-1, -1, 0]]))
# a = 1 - R
# self.assertNotIsInstance(a, SO3)
# array_compare(a, -np.array([ [0,-1,-1], [-1,0,-1], [-1,-1,0]]))
a = R - np.eye(3)
self.assertNotIsInstance(a, SO3)
array_compare(a, np.zeros((3, 3)))
# a = np.eye(3) - R
# self.assertNotIsInstance(a, SO3)
# array_compare(a, np.zeros((3,3)))
# multiply
R = SO3()
a = R * R
self.assertIsInstance(a, SO3)
array_compare(a, R)
a = R * 2
self.assertNotIsInstance(a, SO3)
array_compare(a, 2 * np.eye(3))
a = 2 * R
self.assertNotIsInstance(a, SO3)
array_compare(a, 2 * np.eye(3))
R = SO3()
R *= SO3.Rx(pi / 2)
self.assertIsInstance(R, SO3)
array_compare(R, rotx(pi / 2))
R = SO3()
R *= 2
self.assertNotIsInstance(R, SO3)
array_compare(R, 2 * np.eye(3))
array_compare(
SO3.Rx(pi / 2) * SO3.Ry(pi / 2) * SO3.Rx(-pi / 2), SO3.Rz(pi / 2))
array_compare(SO3.Ry(pi / 2) * [1, 0, 0], np.c_[0, 0, -1].T)
# SO3 x vector
vx = np.r_[1, 0, 0]
vy = np.r_[0, 1, 0]
vz = np.r_[0, 0, 1]
def cv(v):
return np.c_[v]
nt.assert_equal(isinstance(SO3.Rx(pi / 2) * vx, np.ndarray), True)
print(vx)
print(SO3.Rx(pi / 2) * vx)
print(cv(vx))
array_compare(SO3.Rx(pi / 2) * vx, cv(vx))
array_compare(SO3.Rx(pi / 2) * vy, cv(vz))
array_compare(SO3.Rx(pi / 2) * vz, cv(-vy))
array_compare(SO3.Ry(pi / 2) * vx, cv(-vz))
array_compare(SO3.Ry(pi / 2) * vy, cv(vy))
array_compare(SO3.Ry(pi / 2) * vz, cv(vx))
array_compare(SO3.Rz(pi / 2) * vx, cv(vy))
array_compare(SO3.Rz(pi / 2) * vy, cv(-vx))
array_compare(SO3.Rz(pi / 2) * vz, cv(vz))
# divide
R = SO3.Ry(0.3)
a = R / R
self.assertIsInstance(a, SO3)
array_compare(a, np.eye(3))
a = R / 2
self.assertNotIsInstance(a, SO3)
array_compare(a, roty(0.3) / 2)
# power
R = SO3.Rx(pi / 2)
R = R**2
array_compare(R, SO3.Rx(pi))
R = SO3.Rx(pi / 2)
R **= 2
array_compare(R, SO3.Rx(pi))
R = SO3.Rx(pi / 4)
R = R**(-2)
array_compare(R, SO3.Rx(-pi / 2))
R = SO3.Rx(pi / 4)
R **= -2
array_compare(R, SO3.Rx(-pi / 2))
