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 test_ctraj(self):
# unit testing ctraj with T0 and T1 and N
T0 = SE3(1, 2, 3)
T1 = SE3(-1, -2, -3)
T = tr.ctraj(T0, T1, 3)
self.assertEqual(len(T), 3)
nt.assert_array_almost_equal(T[0].A, T0.A)
nt.assert_array_almost_equal(T[2].A, T1.A)
nt.assert_array_almost_equal(T[1].A, SE3().A)
# unit testing ctraj with T0 and T1 and S[i]
T = tr.ctraj(T0, T1, [1, 0, 0.5])
self.assertEqual(len(T), 3)
nt.assert_array_almost_equal(T[0].A, T1.A)
nt.assert_array_almost_equal(T[1].A, T0.A)
nt.assert_array_almost_equal(T[2].A, SE3().A)
T0 = SE3.Rx(-pi/2)
T1 = SE3.Rx(pi/2)
T = tr.ctraj(T0, T1, 3)
self.assertEqual(len(T), 3)
nt.assert_array_almost_equal(T[0].A, T0.A)
nt.assert_array_almost_equal(T[2].A, T1.A)
nt.assert_array_almost_equal(T[1].A, SE3().A)
with self.assertRaises(TypeError):
tr.ctraj(T0, T1, 'hello')
def to_dict(self):
'''
to_dict() returns the shapes information in dictionary form
:returns: All information about the shape
:rtype: dict
'''
if self.stype == 'cylinder':
fk = self.wT * SE3.Rx(np.pi/2)
else:
fk = self.wT
shape = {
'stype': self.stype,
'scale': self.scale.tolist(),
'filename': self.filename,
'radius': self.radius,
'length': self.length,
't': fk.t.tolist(),
'q': r2q(fk.R).tolist(),
'v': self.v.tolist(),
'color': list(self.color)
}
return shape
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 fk_dict(self):
'''
fk_dict() outputs shapes pose in dictionary form
:returns: The shape pose in translation and quternion form
:rtype: dict
'''
if self.stype == 'cylinder':
fk = self.wT * SE3.Rx(np.pi / 2)
else:
fk = self.wT
shape = {'t': fk.t.tolist(), 'q': r2q(fk.R).tolist()}
return shape
def __init__(self, symbolic=False):
if symbolic:
import spatialmath.base.symbolic as sym
zero = sym.zero()
pi = sym.pi()
a1, a2 = sym.symbol('a1 a2')
m1, m2 = sym.symbol('m1 m2')
c1, c2 = sym.symbol('c1 c2')
g = sym.symbol('g')
else:
from math import pi
zero = 0.0
a1 = 1
a2 = 1
m1 = 1
m2 = 1
c1 = -0.5
c2 = -0.5
g = 9.8
links = [
RevoluteDH(a=a1, alpha=zero, m=m1, r=[c1, 0, 0]),
RevoluteDH(a=a2, alpha=zero, m=m2, r=[c2, 0, 0])
]
super().__init__(links,
symbolic=symbolic,
name='2 link',
keywords=('planar', 'dynamics'))
self.addconfiguration("qz", [0, 0])
self.addconfiguration("q1", [0, pi / 2])
self.addconfiguration("q2", [pi / 2, -pi / 2])
self.addconfiguration("qn", [pi / 6, -pi / 6])
self.base = SE3.Rx(pi / 2)
self.gravity = [0, 0, g]
X = r @ np.cos(theta)
Y = r @ sintheta
m = len(r)
Z = h * np.linspace(0, 1, m).reshape((m, 1)) @ np.ones((1, n))
if symmetric:
Z = Z - h / 2
if pose is not None:
P = np.row_stack((X.flatten(), Y.flatten(), Z.flatten()))
P = pose * P
X = P[0, :].reshape(X.shape)
Y = P[1, :].reshape(X.shape)
Z = P[2, :].reshape(X.shape)
return X, Y, Z
if __name__ == "__main__":
from spatialmath import SE3
S = mkcylinder(pose=SE3())
r = np.linspace(0, 2 * pi, 50)
import matplotlib.pyplot as plt
S = mkcylinder(r=np.cos(r) + 1.5, symmetric=True, pose=SE3.Rx(pi / 2))
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot_wireframe(*S)
plt.show()
if len(self) == 0:
return "Twist([])"
elif len(self) == 1:
return "Twist([{:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}])".format(
*list(self.S))
else:
return "Twist([\n" + \
',\n'.join([" [{:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}]".format(*list(tw)) for tw in self.data]) +\
"\n])"
if __name__ == '__main__': # pragma: no cover
import pathlib
import os.path
x = SE3.Rx(0.3)
y = x.interp(np.linspace(0, 1, 10))
x = Twist.P([1, 2, 3])
a = Twist.isvalid(x.se3)
print(a)
a = Twist.isvalid(x.S)
print(a)
exec(
open(
os.path.join(
pathlib.Path(__file__).parent.absolute(),
"test_twist3d.py")).read())
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])
def test_init(self):
T = SE3.Rx(pi / 4)
dq = UnitDualQuaternion(T)
nt.assert_array_almost_equal(dq.SE3().A, T.A)
def test_norm(self):
T = SE3.Rx(pi / 4)
dq = UnitDualQuaternion(T)
nt.assert_array_almost_equal(dq.norm(), (1, 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)