def test_multiply(self):
T1 = SE3.Rx(pi / 4)
T2 = SE3.Rz(-pi / 3)
T = T1 * T2
d1 = UnitDualQuaternion(T1)
d2 = UnitDualQuaternion(T2)
d = d1 * d2
nt.assert_array_almost_equal(d.SE3().A, T.A)
def __init__(self, arm='left'):
links = [
RevoluteDH(d=0.27, a=0.069, alpha=-pi / 2),
RevoluteDH(d=0, a=0, alpha=pi / 2, offset=pi / 2),
RevoluteDH(d=0.102 + 0.262, a=0.069, alpha=-pi / 2),
RevoluteDH(d=0, a=0, alpha=pi / 2),
RevoluteDH(d=0.103 + 0.271, a=0.010, alpha=-pi / 2),
RevoluteDH(d=0, a=0, alpha=pi / 2),
RevoluteDH(d=0.28, a=0, alpha=0)
]
super().__init__(
links,
name=f"Baxter-{arm}",
manufacturer="Rethink Robotics",
)
# zero angles, L shaped pose
self.addconfiguration("qz", np.array([0, 0, 0, 0, 0, 0, 0]))
# ready pose, arm up
self.addconfiguration("qr", np.array([0, -pi / 2, -pi / 2, 0, 0, 0,
0]))
# straight and horizontal
self.addconfiguration("qs", np.array([0, 0, -pi / 2, 0, 0, 0, 0]))
# nominal table top picking pose
self.addconfiguration("qn",
np.array([0, pi / 4, pi / 2, 0, pi / 4, 0, 0]))
if arm == 'left':
self.base = SE3(0.064614, 0.25858, 0.119) * SE3.Rz(pi / 4)
else:
self.base = SE3(0.063534, -0.25966, 0.119) * SE3.Rz(-pi / 4)
l2 = -0.2337
l3 = 0.4318
l4 = 0.0203
l5 = 0.0837
l6 = 0.4318
e = ETS.tz(l1) * ETS.rz() * ETS.ry() * ETS.ty(l2) * ETS.tz(l3) * ETS.ry() \
* ETS.tx(l4) * ETS.ty(l5) * ETS.tz(l6) * ETS.rz() * ETS.ry() * ETS.rz()
print(e.joints())
print(e.config)
print(e.eval(np.zeros(6)))
ec = e.compile()
print(ec)
print(ec.eval(np.zeros(6)))
print(ETS.SE3(SE3.Rz(200, 'deg')))
a = ETS.rx()
b = ETS(a)
print(b)
a = ETS.tz()
print(b)
e = ETS.rz(j=5) * ETS.tx(1) * ETS.rx(j=7, flip=True) * ETS.tx(1)
print(e)
print(e.inv())
q = [1, 2, 3, 4, 5, 6, 7, 8]
print(e.eval(q))
print(e.inv().eval(q))
print(e.eval(q) * e.inv().eval(q))
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])
env.launch()
path = pathlib.Path(path) / 'rtb-data' / 'rtbdata' / 'data' / 'test-mesh'
# obj_bug = rtb.Mesh(
# filename=str(path / 'scene.gltf'),
# scale=[0.001, 0.001, 0.001],
# base=SE3(0, 0, 0.45) * SE3.Rx(np.pi/2) * SE3.Ry(np.pi/2)
# )
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)
