def update_gimbals(theta, ring):
global R1, R2, R3
# update the relevant transform, depending on which ring's slider moved
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)
if ring == 1:
R1 = Rxyz(theta, sequence[ring - 1])
elif ring == 2:
R2 = Rxyz(theta, sequence[ring - 1])
elif ring == 3:
R3 = Rxyz(theta, sequence[ring - 1])
# figure the transforms for each gimbal and the plane, and update their
# pose
def convert(R):
return BASE * SE3.SO3(R)
g3.base = convert(R1 * SO3.Ry(pi / 2))
g2.base = convert(R1 * R2 * SO3.Rz(pi / 2))
g1.base = convert(R1 * R2 * R3 * SO3.Rx(pi / 2))
plane.base = convert(R1 * R2 * R3 * SO3.Ry(pi / 2) * SO3.Rz(pi / 2))
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_listpowers(self):
R = SO3()
R1 = SO3.Rx(0.2)
R2 = SO3.Ry(0.3)
R.append(R1)
R.append(R2)
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SO3)
array_compare(R[0], np.eye(3))
array_compare(R[1], R1)
array_compare(R[2], R2)
R = SO3([rotx(0.1), rotx(0.2), rotx(0.3)])
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SO3)
array_compare(R[0], rotx(0.1))
array_compare(R[1], rotx(0.2))
array_compare(R[2], rotx(0.3))
R = SO3([SO3.Rx(0.1), SO3.Rx(0.2), SO3.Rx(0.3)])
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SO3)
array_compare(R[0], rotx(0.1))
array_compare(R[1], rotx(0.2))
array_compare(R[2], rotx(0.3))
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 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 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 CreateSimulatedCamera(x=1,
y=-1,
z=0.01,
roll=-92,
pitch=2,
yaw=50,
image_size=(1280, 1024),
f=0.015):
"""Create a Machine Vision Toolbox central camera model given 6 DoF pose, image size and f.
Args In:
x - position of camera in x-axis world frame (in metres)
y - position of camera in y-axis world frame (in metres)
z - position of camera in z-axis world frame (in metres)
roll - rotation of the camera about the x-axis world frame (in degrees)
pitch - rotation of the camera about the y-axis world frame (in degrees)
yaw - rotation of the camera about the z-axis world frame (in degrees)
image_size - two element tuple specifying the width and height of the image (in pixels)
f - focal length
Returns:
a camera model
"""
# Establish a camera position with respect to the world frame
# position
t_cam = np.r_[x, y, z]
# orientation
R = SO3.Rz(yaw, 'deg') * SO3.Ry(pitch, 'deg') * SO3.Rx(roll, 'deg')
# Create full transformation matrix
T = SE3(t_cam) * SE3.SO3(R)
# print(T)
# Create camera model
cam_model = CentralCamera(imagesize=image_size, f=f, pose=T)
return cam_model
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))