def axes_calcs(self):
# Joint and ee poses
T = self.robot.fkine_all()
Te = self.robot.fkine()
Tb = self.robot.base
# Joint and ee position matrix
loc = np.zeros([3, self.robot.n + 2])
loc[:, 0] = Tb.t
loc[:, self.robot.n + 1] = Te.t
# Joint axes position matrix
joints = np.zeros((3, self.robot.n))
# Axes arrow transforms
Tjx = SE3.Tx(0.06)
Tjy = SE3.Ty(0.06)
Tjz = SE3.Tz(0.06)
# ee axes arrows
Tex = Te * Tjx
Tey = Te * Tjy
Tez = Te * Tjz
# Joint axes arrow calcs
for i in range(self.robot.n):
loc[:, i + 1] = T[i].t
if isinstance(self.robot, rp.DHRobot) \
or self.robot.ets[self.robot.q_idx[i]].axis == 'Rz' \
or self.robot.ets[self.robot.q_idx[i]].axis == 'tz':
Tji = T[i] * Tjz
# elif self.robot.ets[self.robot.q_idx[i]].axis == 'Ry' \
# or self.robot.ets[self.robot.q_idx[i]].axis == 'ty':
# Tji = T[i] * Tjy
# elif self.robot.ets[self.robot.q_idx[i]].axis == 'Rx' \
# or self.robot.ets[self.robot.q_idx[i]].axis == 'tx':
# Tji = T[i] * Tjx
joints[:, i] = Tji.t
return loc, joints, [Tex, Tey, Tez]
def axes_calcs(self):
# Joint and ee poses
T = self.robot.fkine_all(self.robot.q)
try:
Te = self.robot.fkine(self.robot.q)
except ValueError:
print(
"\nError: Branched robot's not yet supported "
"with PyPlot backend\n")
raise
Tb = self.robot.base
# Joint and ee position matrix
loc = np.zeros([3, len(self.robot.links) + 1])
loc[:, 0] = Tb.t
# Joint axes position matrix
joints = np.zeros((3, self.robot.n))
# Axes arrow transforms
Tjx = SE3.Tx(0.06)
Tjy = SE3.Ty(0.06)
Tjz = SE3.Tz(0.06)
# ee axes arrows
Tex = Te * Tjx
Tey = Te * Tjy
Tez = Te * Tjz
# Joint axes arrow calcs
if isinstance(self.robot, rp.ERobot):
i = 0
j = 0
for link in self.robot.links:
loc[:, i + 1] = link._fk.t
if link.isjoint:
if link.v.axis == 'Rz' or link.v.axis == 'tz':
Tji = link._fk * Tjz
elif link.v.axis == 'Ry' or link.v.axis == 'ty':
Tji = link._fk * Tjy
elif link.v.axis == 'Rx' or link.v.axis == 'tx':
Tji = link._fk * Tjx
joints[:, j] = Tji.t
j += 1
i += 1
loc = np.c_[loc, loc[:, -1]]
else:
# End effector offset (tool of robot)
loc = np.c_[loc, Te.t]
for i in range(self.robot.n):
loc[:, i + 1] = T[i].t
Tji = T[i] * Tjz
joints[:, i] = Tji.t
return loc, joints, [Tex, Tey, Tez]
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])
