コード例 #1
0
    def test_init_elink_branched(self):
        robot = ERobot([
            ELink(ETS.rz(), name='link1'),
            ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(),
                  name='link2',
                  parent='link1'),
            ELink(ETS.tx(1), name='ee_1', parent='link2'),
            ELink(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(),
                  name='link3',
                  parent='link1'),
            ELink(ETS.tx(1), name='ee_2', parent='link3')
        ])
        self.assertEqual(robot.n, 3)
        for i in range(5):
            self.assertIsInstance(robot[i], ELink)
        self.assertTrue(robot[0].isrevolute)
        self.assertTrue(robot[1].isrevolute)
        self.assertTrue(robot[3].isrevolute)

        self.assertIs(robot[0].parent, None)
        self.assertIs(robot[1].parent, robot[0])
        self.assertIs(robot[2].parent, robot[1])
        self.assertIs(robot[3].parent, robot[0])
        self.assertIs(robot[4].parent, robot[3])

        self.assertEqual(robot[0].children, [robot[1], robot[3]])
        self.assertEqual(robot[1].children, [robot[2]])
        self.assertEqual(robot[2].children, [])
        self.assertEqual(robot[3].children, [robot[4]])
        self.assertEqual(robot[2].children, [])
コード例 #2
0
    def test_jindex(self):
        e1 = ELink(rtb.ETS.rz(), jindex=0)
        e2 = ELink(rtb.ETS.rz(), jindex=1, parent=e1)
        e3 = ELink(rtb.ETS.rz(), jindex=2, parent=e2)
        e4 = ELink(rtb.ETS.rz(), jindex=0, parent=e3)

        # with self.assertRaises(ValueError):
        ERobot([e1, e2, e3, e4], gripper_links=e4)
コード例 #3
0
    def test_init_bases(self):
        e1 = ELink()
        e2 = ELink()
        e3 = ELink(parent=e1)
        e4 = ELink(parent=e2)

        with self.assertRaises(ValueError):
            ERobot([e1, e2, e3, e4])
コード例 #4
0
    def test_init(self):
        l0 = ELink()
        l1 = ELink(parent=l0)
        r = ERobot([l0, l1], base=sm.SE3.Rx(1.3), base_link=l1)
        r.base_link = l1

        with self.assertRaises(TypeError):
            ERobot(l0, base=sm.SE3.Rx(1.3))

        with self.assertRaises(TypeError):
            ERobot([1, 2], base=sm.SE3.Rx(1.3))
コード例 #5
0
    def test_init_elink_autoparent(self):
        links = [
            ELink(ETS.rx(), name='link1'),
            ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2'),
            ELink(ETS.tx(1), name='ee_1')
        ]
        robot = ERobot(links)
        self.assertEqual(robot.n, 2)
        self.assertIsInstance(robot[0], ELink)
        self.assertIsInstance(robot[1], ELink)
        self.assertIsInstance(robot[2], ELink)
        self.assertTrue(robot[0].isrevolute)
        self.assertTrue(robot[1].isprismatic)
        self.assertIs(robot[0].parent, None)
        self.assertIs(robot[1].parent, robot[0])
        self.assertIs(robot[2].parent, robot[1])

        self.assertEqual(robot[0].children, [robot[1]])
        self.assertEqual(robot[1].children, [robot[2]])
        self.assertEqual(robot[2].children, [])
コード例 #6
0
    def test_complex(self):
        l0 = ELink(rtb.ETS.tx(0.1), rtb.ETS.rx())
        l1 = ELink(rtb.ETS.tx(0.1), rtb.ETS.ry(), parent=l0)
        l2 = ELink(rtb.ETS.tx(0.1), rtb.ETS.rz(), parent=l1)
        l3 = ELink(rtb.ETS.tx(0.1), rtb.ETS.tx(), parent=l2)
        l4 = ELink(rtb.ETS.tx(0.1), rtb.ETS.ty(), parent=l3)
        l5 = ELink(rtb.ETS.tx(0.1), rtb.ETS.tz(), parent=l4)

        r = ERobot([l0, l1, l2, l3, l4, l5])
        q = [1, 2, 3, 1, 2, 3]

        ans = np.array([[
            -0., 0.08752679, -0.74761985, 0.41198225, 0.05872664, 0.90929743
        ],
                        [
                            1.46443609, 2.80993063, 0.52675075, -0.68124272,
                            -0.64287284, 0.35017549
                        ],
                        [
                            -1.04432, -1.80423571, -2.20308833, 0.60512725,
                            -0.76371834, -0.2248451
                        ], [1., 0., 0.90929743, 0., 0., 0.],
                        [0., 0.54030231, 0.35017549, 0., 0., 0.],
                        [0., 0.84147098, -0.2248451, 0., 0., 0.]])

        nt.assert_array_almost_equal(r.jacob0(q), ans)
コード例 #7
0
    def test_jindex_fail(self):
        e1 = ELink(rtb.ETS.rz(), jindex=0)
        e2 = ELink(rtb.ETS.rz(), jindex=1, parent=e1)
        e3 = ELink(rtb.ETS.rz(), jindex=2, parent=e2)
        e4 = ELink(rtb.ETS.rz(), jindex=5, parent=e3)

        with self.assertRaises(ValueError):
            ERobot([e1, e2, e3, e4])

        e1 = ELink(rtb.ETS.rz(), jindex=0)
        e2 = ELink(rtb.ETS.rz(), jindex=1, parent=e1)
        e3 = ELink(rtb.ETS.rz(), jindex=2, parent=e2)
        e4 = ELink(rtb.ETS.rz(), parent=e3)

        with self.assertRaises(ValueError):
            ERobot([e1, e2, e3, e4])
コード例 #8
0
    def test_init_elink(self):
        link1 = ELink(ETS.rx(), name='link1')
        link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(),
                      name='link2',
                      parent=link1)
        link3 = ELink(ETS.tx(1), name='ee_1', parent=link2)
        robot = ERobot([link1, link2, link3])
        self.assertEqual(robot.n, 2)
        self.assertIsInstance(robot[0], ELink)
        self.assertIsInstance(robot[1], ELink)
        self.assertIsInstance(robot[2], ELink)
        self.assertTrue(robot[0].isrevolute)
        self.assertTrue(robot[1].isprismatic)

        self.assertFalse(robot[2].isrevolute)
        self.assertFalse(robot[2].isprismatic)

        self.assertIs(robot[0].parent, None)
        self.assertIs(robot[1].parent, robot[0])
        self.assertIs(robot[2].parent, robot[1])

        self.assertEqual(robot[0].children, [robot[1]])
        self.assertEqual(robot[1].children, [robot[2]])
        self.assertEqual(robot[2].children, [])

        link1 = ELink(ETS.rx(), name='link1')
        link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(),
                      name='link2',
                      parent='link1')
        link3 = ELink(ETS.tx(1), name='ee_1', parent='link2')
        robot = ERobot([link1, link2, link3])
        self.assertEqual(robot.n, 2)
        self.assertIsInstance(robot[0], ELink)
        self.assertIsInstance(robot[1], ELink)
        self.assertIsInstance(robot[2], ELink)
        self.assertTrue(robot[0].isrevolute)
        self.assertTrue(robot[1].isprismatic)

        self.assertIs(robot[0].parent, None)
        self.assertIs(robot[1].parent, robot[0])
        self.assertIs(robot[2].parent, robot[1])

        self.assertEqual(robot[0].children, [robot[1]])
        self.assertEqual(robot[1].children, [robot[2]])
        self.assertEqual(robot[2].children, [])
コード例 #9
0
    def test_invdyn(self):
        # create a 2 link robot
        # Example from Spong etal. 2nd edition, p. 260
        E = rtb.ETS
        l1 = ELink(ets=E.ry(), m=1, r=[0.5, 0, 0], name='l1')
        l2 = ELink(ets=E.tx(1) * E.ry(),
                   m=1,
                   r=[0.5, 0, 0],
                   parent=l1,
                   name='l2')
        robot = ERobot([l1, l2], name="simple 2 link")
        z = np.zeros(robot.n)

        # check gravity load
        tau = robot.rne(z, z, z) / 9.81
        nt.assert_array_almost_equal(tau, np.r_[-2, -0.5])

        tau = robot.rne([0, -pi / 2], z, z) / 9.81
        nt.assert_array_almost_equal(tau, np.r_[-1.5, 0])

        tau = robot.rne([-pi / 2, pi / 2], z, z) / 9.81
        nt.assert_array_almost_equal(tau, np.r_[-0.5, -0.5])

        tau = robot.rne([-pi / 2, 0], z, z) / 9.81
        nt.assert_array_almost_equal(tau, np.r_[0, 0])

        # check velocity terms
        robot.gravity = [0, 0, 0]
        q = [0, -pi / 2]
        h = -0.5 * sin(q[1])

        tau = robot.rne(q, [0, 0], z)
        nt.assert_array_almost_equal(tau, np.r_[0, 0] * h)

        tau = robot.rne(q, [1, 0], z)
        nt.assert_array_almost_equal(tau, np.r_[0, -1] * h)

        tau = robot.rne(q, [0, 1], z)
        nt.assert_array_almost_equal(tau, np.r_[1, 0] * h)

        tau = robot.rne(q, [1, 1], z)
        nt.assert_array_almost_equal(tau, np.r_[3, -1] * h)

        # check inertial terms

        d11 = 1.5 + cos(q[1])
        d12 = 0.25 + 0.5 * cos(q[1])
        d21 = d12
        d22 = 0.25

        tau = robot.rne(q, z, [0, 0])
        nt.assert_array_almost_equal(tau, np.r_[0, 0])

        tau = robot.rne(q, z, [1, 0])
        nt.assert_array_almost_equal(tau, np.r_[d11, d21])

        tau = robot.rne(q, z, [0, 1])
        nt.assert_array_almost_equal(tau, np.r_[d12, d22])

        tau = robot.rne(q, z, [1, 1])
        nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22])