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_plot(self):
plt.close('all')
R = SO3.Rx(0.3)
R.plot(block=False)
R2 = SO3.Rx(0.6)
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 test_printline(self):
R = SO3.Rx(0.3)
R.printline()
# s = R.printline(file=None)
# self.assertIsInstance(s, str)
R = SO3.Rx([0.3, 0.4, 0.5])
s = R.printline(file=None)
def test_constructor_AngVec(self):
# angvec
R = SO3.AngVec(0.2, [1, 0, 0])
nt.assert_equal(len(R), 1)
array_compare(R, rotx(0.2))
self.assertIsInstance(R, SO3)
R = SO3.AngVec(0.3, [0, 1, 0])
nt.assert_equal(len(R), 1)
array_compare(R, roty(0.3))
self.assertIsInstance(R, SO3)
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 test_properties(self):
R = SO3()
self.assertEqual(R.isSO, True)
self.assertEqual(R.isSE, False)
array_compare(R.n, np.r_[1, 0, 0])
array_compare(R.n, np.r_[1, 0, 0])
array_compare(R.n, np.r_[1, 0, 0])
nt.assert_equal(R.N, 3)
nt.assert_equal(R.shape, (3, 3))
R = SO3.Rx(0.3)
array_compare(R.inv() * R, np.eye(3, 3))
def test_tests(self):
R = SO3()
self.assertEqual(R.isrot(), True)
self.assertEqual(R.isrot2(), False)
self.assertEqual(R.ishom(), False)
self.assertEqual(R.ishom2(), False)
def test_str(self):
R = SO3()
s = str(R)
self.assertIsInstance(s, str)
self.assertEqual(s.count('\n'), 3)
s = repr(R)
self.assertIsInstance(s, str)
self.assertEqual(s.count('\n'), 2)
def step(self, dt: float = 0.01) -> None: # pylint: disable=unused-argument
"""
Updates the robot joints (robot.q) used in computing kinematics
:param dt: the delta time since the last update, defaults to 0.01
:type dt: float, optional
"""
if self.readonly:
return
current_time = timeit.default_timer()
Kp = 1
self.publish_state()
# calculate joint velocities from desired cartesian velocity
if any(self.e_v):
if current_time - self.last_update > 0.1:
self.e_v *= 0.9 if np.sum(np.absolute(self.e_v)
) >= 0.0001 else 0
wTe = self.fkine(self.q, start=self.base_link, fast=True, end=self.gripper)
error = self.e_p @ np.linalg.inv(wTe)
# print(e)
trans = self.e_p[:3, 3] # .astype('float64')
trans += self.e_v[:3] * dt
rotation = SO3(self.e_p[:3, :3])
# Rdelta = SO3.EulerVec(self.e_v[3:])
# R = Rdelta * R
# R = R.norm()
# # print(self.e_p)
self.e_p = SE3.Rt(rotation, t=trans).A
v_t = self.e_v[:3] + Kp * error[:3, 3]
v_r = self.e_v[3:] # + (e[.rpy(]) * 0.5)
e_v = np.concatenate([v_t, v_r])
self.j_v = np.linalg.pinv(
self.jacob0(self.q, fast=True, end=self.gripper)) @ e_v
# apply desired joint velocity to robot
if any(self.j_v):
if current_time - self.last_update > 0.1:
self.j_v *= 0.9 if np.sum(np.absolute(self.j_v)
) >= 0.0001 else 0
self.qd = self.j_v
self.joint_publisher.publish(Float64MultiArray(data=self.qd))
self.last_tick = current_time
self.event.set()
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_constructor_Eul(self):
R = SO3.Eul([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2r([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SO3)
R = SO3.Eul(0.1, 0.2, 0.3)
nt.assert_equal(len(R), 1)
array_compare(R, eul2r([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SO3)
R = SO3.Eul(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2r([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SO3)
R = SO3.Eul([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, eul2r([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SO3)
R = SO3.Eul(10, 20, 30, unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, eul2r([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SO3)
# matrix input
angles = np.array([[0.1, 0.2, 0.3], [0.2, 0.3, 0.4], [0.3, 0.4, 0.5],
[0.4, 0.5, 0.6]])
R = SO3.Eul(angles)
self.assertIsInstance(R, SO3)
nt.assert_equal(len(R), 4)
for i in range(4):
array_compare(R[i], eul2r(angles[i, :]))
angles *= 10
R = SO3.Eul(angles, unit='deg')
self.assertIsInstance(R, SO3)
nt.assert_equal(len(R), 4)
for i in range(4):
array_compare(R[i], eul2r(angles[i, :], unit='deg'))
def test_shape(self):
a = SO3()
self.assertEqual(a._A.shape, a.shape)
def test_constructor(self):
# null constructor
R = SE3()
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(4))
self.assertIsInstance(R, SE3)
# construct from matrix
R = SE3(trotx(0.2))
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
# construct from canonic rotation
R = SE3.Rx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
R = SE3.Ry(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, troty(0.2))
self.assertIsInstance(R, SE3)
R = SE3.Rz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, trotz(0.2))
self.assertIsInstance(R, SE3)
# construct from canonic translation
R = SE3.Tx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0.2, 0, 0))
self.assertIsInstance(R, SE3)
R = SE3.Ty(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0, 0.2, 0))
self.assertIsInstance(R, SE3)
R = SE3.Tz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0, 0, 0.2))
self.assertIsInstance(R, SE3)
# triple angle
R = SE3.Eul([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.Eul(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.Eul([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SE3)
R = SE3.RPY([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.RPY(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.RPY([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SE3)
R = SE3.RPY([0.1, 0.2, 0.3], order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3], order='xyz'))
self.assertIsInstance(R, SE3)
# angvec
R = SE3.AngVec(0.2, [1, 0, 0])
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
R = SE3.AngVec(0.3, [0, 1, 0])
nt.assert_equal(len(R), 1)
array_compare(R, troty(0.3))
self.assertIsInstance(R, SE3)
# OA
R = SE3.OA([0, 1, 0], [0, 0, 1])
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(4))
self.assertIsInstance(R, SE3)
# random
R = SE3.Rand()
nt.assert_equal(len(R), 1)
self.assertIsInstance(R, SE3)
# random
T = SE3.Rand()
R = T.R
t = T.t
T = SE3.Rt(R, t)
self.assertIsInstance(T, SE3)
self.assertEqual(T.A.shape, (4, 4))
nt.assert_equal(T.R, R)
nt.assert_equal(T.t, t)
# copy constructor
R = SE3.Rx(pi / 2)
R2 = SE3(R)
R = SE3.Ry(pi / 2)
array_compare(R2, trotx(pi / 2))
# SO3
T = SE3(SO3())
nt.assert_equal(len(T), 1)
self.assertIsInstance(T, SE3)
nt.assert_equal(T.A, np.eye(4))
# SE2
T = SE3(SE2(1, 2, 0.4))
nt.assert_equal(len(T), 1)
self.assertIsInstance(T, SE3)
self.assertEqual(T.A.shape, (4, 4))
nt.assert_equal(T.t, [1, 2, 0])
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_about(self):
R = SO3()
R.about
plane = Mesh(filename=str(path / 'spitfire_assy-gear_up.stl'),
scale=(1. / (180 * 3), ) * 3,
color=[240, 103, 103])
print(path / 'spitfire_assy-gear_up.stl')
env.add(g1)
env.add(g2)
env.add(g3)
env.add(plane)
print('Supermarine Spitfire Mk VIII by Ed Morley @GRABCAD')
print('Gimbal models by Peter Corke using OpenSCAD')
# compute the three rotation matrices
BASE = SE3(0, 0, 0.5)
R1 = SO3()
R2 = SO3()
R3 = SO3()
# rotation angle sequence
sequence = "ZYX"
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)
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))
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_constructor_RPY(self):
R = SO3.RPY(0.1, 0.2, 0.3, order='zyx')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
self.assertIsInstance(R, SO3)
R = SO3.RPY(10, 20, 30, unit='deg', order='zyx')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([10, 20, 30], order='zyx', unit='deg'))
self.assertIsInstance(R, SO3)
R = SO3.RPY([0.1, 0.2, 0.3], order='zyx')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
self.assertIsInstance(R, SO3)
R = SO3.RPY(np.r_[0.1, 0.2, 0.3], order='zyx')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
self.assertIsInstance(R, SO3)
# check default
R = SO3.RPY([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
self.assertIsInstance(R, SO3)
# XYZ order
R = SO3.RPY(0.1, 0.2, 0.3, order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='xyz'))
self.assertIsInstance(R, SO3)
R = SO3.RPY(10, 20, 30, unit='deg', order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([10, 20, 30], order='xyz', unit='deg'))
self.assertIsInstance(R, SO3)
R = SO3.RPY([0.1, 0.2, 0.3], order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='xyz'))
self.assertIsInstance(R, SO3)
R = SO3.RPY(np.r_[0.1, 0.2, 0.3], order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='xyz'))
self.assertIsInstance(R, SO3)
# matrix input
angles = np.array([[0.1, 0.2, 0.3], [0.2, 0.3, 0.4], [0.3, 0.4, 0.5],
[0.4, 0.5, 0.6]])
R = SO3.RPY(angles, order='zyx')
self.assertIsInstance(R, SO3)
nt.assert_equal(len(R), 4)
for i in range(4):
array_compare(R[i], rpy2r(angles[i, :], order='zyx'))
angles *= 10
R = SO3.RPY(angles, unit='deg', order='zyx')
self.assertIsInstance(R, SO3)
nt.assert_equal(len(R), 4)
for i in range(4):
array_compare(R[i], rpy2r(angles[i, :], unit='deg', order='zyx'))