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