def test_listpowers(self):
R = SE3()
R1 = SE3.Rx(0.2)
R2 = SE3.Ry(0.3)
R.append(R1)
R.append(R2)
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SE3)
array_compare(R[0], np.eye(4))
array_compare(R[1], R1)
array_compare(R[2], R2)
R = SE3([trotx(0.1), trotx(0.2), trotx(0.3)])
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SE3)
array_compare(R[0], trotx(0.1))
array_compare(R[1], trotx(0.2))
array_compare(R[2], trotx(0.3))
R = SE3([SE3.Rx(0.1), SE3.Rx(0.2), SE3.Rx(0.3)])
nt.assert_equal(len(R), 3)
self.assertIsInstance(R, SE3)
array_compare(R[0], trotx(0.1))
array_compare(R[1], trotx(0.2))
array_compare(R[2], trotx(0.3))
def __init__(self, base=None):
mm = 1e-3
deg = pi / 180
# details from
h = 53.0 * mm
r = 30.309 * mm
l2 = 170.384 * mm
l3 = 136.307 * mm
l4 = 86.00 * mm
c = 40.0 * mm
# tool_offset = l4 + c
# Turret, Shoulder, Elbow, Wrist, Claw
links = [
RevoluteDH(d=h, a=0, alpha=90 * deg), # Turret
RevoluteDH(
d=0,
a=l2,
alpha=0, # Shoulder
qlim=[10 * deg, 122.5 * deg]),
RevoluteDH(
d=0,
a=-l3,
alpha=0, # Elbow
qlim=[20 * deg, 340 * deg]),
RevoluteDH(
d=0,
a=l4 + c,
alpha=0, # Wrist
qlim=[45 * deg, 315 * deg])
]
super().__init__(links,
name="Orion 5",
manufacturer="RAWR Robotics",
base=SE3(r, 0, 0),
tool=SE3.Ry(90, 'deg'))
# zero angles, all folded up
self.addconfiguration("qz", np.r_[0, 90, 180, 180] * deg)
# stretched out vertically
self.addconfiguration("qv", np.r_[0, 90, 180, 180] * deg)
# arm horizontal, hand down
self.addconfiguration("qh", np.r_[0, 0, 180, 90] * deg)
def __init__(self):
# create the base
links = [
PrismaticDH(alpha=-pi / 2, qlim=[-1, 1]),
PrismaticDH(theta=-pi / 2, alpha=pi / 2, qlim=[-1, 1])
]
# stick the Puma on top
puma = models.DH.Puma560()
links.extend(puma.links)
super().__init__(links,
name="P8",
keywords=('mobile', 'redundant'),
base=SE3.Ry(pi / 2))
self.addconfiguration("qz", np.zeros((8, )))
def test_arith_vect(self):
rx = SE3.Rx(pi / 2)
ry = SE3.Ry(pi / 2)
rz = SE3.Rz(pi / 2)
u = SE3()
# multiply
T = SE3([rx, ry, rz])
a = T * rx
self.assertIsInstance(a, SE3)
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 * T
self.assertIsInstance(a, SE3)
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 = T * T
self.assertIsInstance(a, SE3)
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 = T * 2
self.assertNotIsInstance(a, SE3)
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 * T
self.assertNotIsInstance(a, SE3)
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 = T
a *= rx
self.assertIsInstance(a, SE3)
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 *= T
self.assertIsInstance(a, SE3)
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 = T
a *= T
self.assertIsInstance(a, SE3)
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 = T
a *= 2
self.assertNotIsInstance(a, SE3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx * 2)
array_compare(a[1], ry * 2)
array_compare(a[2], rz * 2)
# SE3 x vector
vx = np.r_[1, 0, 0]
vy = np.r_[0, 1, 0]
vz = np.r_[0, 0, 1]
a = T * 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
T = SE3([rx, ry, rz])
a = T / rx
self.assertIsInstance(a, SE3)
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 / T
self.assertIsInstance(a, SE3)
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 = T / T
self.assertIsInstance(a, SE3)
nt.assert_equal(len(a), 3)
array_compare(a[0], np.eye(4))
array_compare(a[1], np.eye(4))
array_compare(a[2], np.eye(4))
a = T / 2
self.assertNotIsInstance(a, SE3)
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 = T
a /= rx
self.assertIsInstance(a, SE3)
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 /= T
self.assertIsInstance(a, SE3)
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 = T
a /= T
self.assertIsInstance(a, SE3)
nt.assert_equal(len(a), 3)
array_compare(a[0], np.eye(4))
array_compare(a[1], np.eye(4))
array_compare(a[2], np.eye(4))
a = T
a /= 2
self.assertNotIsInstance(a, SE3)
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
T = SE3([rx, ry, rz])
a = T + rx
self.assertNotIsInstance(a, SE3)
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 + T
self.assertNotIsInstance(a, SE3)
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 = T + T
self.assertNotIsInstance(a, SE3)
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 = T + 1
self.assertNotIsInstance(a, SE3)
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
T = SE3([rx, ry, rz])
a = T - rx
self.assertNotIsInstance(a, SE3)
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 - T
self.assertNotIsInstance(a, SE3)
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 = T - T
self.assertNotIsInstance(a, SE3)
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 = T - 1
self.assertNotIsInstance(a, SE3)
nt.assert_equal(len(a), 3)
array_compare(a[0], rx - 1)
array_compare(a[1], ry - 1)
array_compare(a[2], rz - 1)
def test_arith(self):
T = SE3(1, 2, 3)
# sum
a = T + T
self.assertNotIsInstance(a, SE3)
array_compare(
a,
np.array([[2, 0, 0, 2], [0, 2, 0, 4], [0, 0, 2, 6], [0, 0, 0, 2]]))
a = T + 1
self.assertNotIsInstance(a, SE3)
array_compare(
a,
np.array([[2, 1, 1, 2], [1, 2, 1, 3], [1, 1, 2, 4], [1, 1, 1, 2]]))
# a = 1 + T
# self.assertNotIsInstance(a, SE3)
# array_compare(a, np.array([ [2,1,1], [1,2,1], [1,1,2]]))
a = T + np.eye(4)
self.assertNotIsInstance(a, SE3)
array_compare(
a,
np.array([[2, 0, 0, 1], [0, 2, 0, 2], [0, 0, 2, 3], [0, 0, 0, 2]]))
# a = np.eye(3) + T
# self.assertNotIsInstance(a, SE3)
# array_compare(a, np.array([ [2,0,0], [0,2,0], [0,0,2]]))
# this invokes the __add__ method for numpy
# difference
T = SE3(1, 2, 3)
a = T - T
self.assertNotIsInstance(a, SE3)
array_compare(a, np.zeros((4, 4)))
a = T - 1
self.assertNotIsInstance(a, SE3)
array_compare(
a,
np.array([[0, -1, -1, 0], [-1, 0, -1, 1], [-1, -1, 0, 2],
[-1, -1, -1, 0]]))
# a = 1 - T
# self.assertNotIsInstance(a, SE3)
# array_compare(a, -np.array([ [0,-1,-1], [-1,0,-1], [-1,-1,0]]))
a = T - np.eye(4)
self.assertNotIsInstance(a, SE3)
array_compare(
a,
np.array([[0, 0, 0, 1], [0, 0, 0, 2], [0, 0, 0, 3], [0, 0, 0, 0]]))
# a = np.eye(3) - T
# self.assertNotIsInstance(a, SE3)
# array_compare(a, np.zeros((3,3)))
a = T
a -= T
self.assertNotIsInstance(a, SE3)
array_compare(a, np.zeros((4, 4)))
# multiply
T = SE3(1, 2, 3)
a = T * T
self.assertIsInstance(a, SE3)
array_compare(a, transl(2, 4, 6))
a = T * 2
self.assertNotIsInstance(a, SE3)
array_compare(a, 2 * transl(1, 2, 3))
a = 2 * T
self.assertNotIsInstance(a, SE3)
array_compare(a, 2 * transl(1, 2, 3))
T = SE3(1, 2, 3)
T *= SE3.Ry(pi / 2)
self.assertIsInstance(T, SE3)
array_compare(
T,
np.array([[0, 0, 1, 1], [0, 1, 0, 2], [-1, 0, 0, 3], [0, 0, 0,
1]]))
T = SE3()
T *= 2
self.assertNotIsInstance(T, SE3)
array_compare(T, 2 * np.eye(4))
array_compare(
SE3.Rx(pi / 2) * SE3.Ry(pi / 2) * SE3.Rx(-pi / 2), SE3.Rz(pi / 2))
array_compare(SE3.Ry(pi / 2) * [1, 0, 0], np.c_[0, 0, -1].T)
# SE3 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(SE3.Tx(pi / 2) * vx, np.ndarray), True)
array_compare(SE3.Rx(pi / 2) * vx, cv(vx))
array_compare(SE3.Rx(pi / 2) * vy, cv(vz))
array_compare(SE3.Rx(pi / 2) * vz, cv(-vy))
array_compare(SE3.Ry(pi / 2) * vx, cv(-vz))
array_compare(SE3.Ry(pi / 2) * vy, cv(vy))
array_compare(SE3.Ry(pi / 2) * vz, cv(vx))
array_compare(SE3.Rz(pi / 2) * vx, cv(vy))
array_compare(SE3.Rz(pi / 2) * vy, cv(-vx))
array_compare(SE3.Rz(pi / 2) * vz, cv(vz))
# divide
T = SE3.Ry(0.3)
a = T / T
self.assertIsInstance(a, SE3)
array_compare(a, np.eye(4))
a = T / 2
self.assertNotIsInstance(a, SE3)
array_compare(a, troty(0.3) / 2)
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])
# )
dae = rtb.Mesh(filename=str(path / 'walle.dae'), base=SE3(0, -1.5, 0))
obj = rtb.Mesh(filename=str(path / 'walle.obj'), base=SE3(0, -0.5, 0))
glb = rtb.Mesh(filename=str(path / 'walle.glb'),
base=SE3(0, 0.5, 0) * SE3.Rz(-np.pi / 2))
ply = rtb.Mesh(filename=str(path / 'walle.ply'), base=SE3(0, 1.5, 0))
# wrl = rtb.Mesh(
# filename=str(path / 'walle.wrl'),
# base=SE3(0, 2.0, 0)
# )
pcd = rtb.Mesh(filename=str(path / 'pcd.pcd'),
base=SE3(0, 0, 1.5) * SE3.Rx(np.pi / 2) * SE3.Ry(np.pi / 2))
env.add(dae)
env.add(obj)
env.add(glb)
env.add(ply)
env.add(pcd)
time.sleep(2)
while (True):
# env.process_events()
env.step(0)
