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]
示例#3
0
    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])