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_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])
