def test_init(self): l0 = rtb.ELink() l1 = rtb.ELink(parent=l0) r = rtb.ERobot([l0, l1], base=sm.SE3.Rx(1.3), base_link=l1) r.base_link = l1 with self.assertRaises(TypeError): rtb.ERobot(l0, base=sm.SE3.Rx(1.3)) with self.assertRaises(TypeError): rtb.ERobot([1, 2], base=sm.SE3.Rx(1.3))
def test_complex(self): l0 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.rx()) l1 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.ry(), parent=l0) l2 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.rz(), parent=l1) l3 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.tx(), parent=l2) l4 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.ty(), parent=l3) l5 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.tz(), parent=l4) r = rtb.ERobot([l0, l1, l2, l3, l4, l5]) r.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(), ans)
def test_jindex_fail(self): e1 = rtb.ELink(rtb.ETS.rz(), jindex=0) e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1) e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2) e4 = rtb.ELink(rtb.ETS.rz(), jindex=5, parent=e3) with self.assertRaises(ValueError): rtb.ERobot([e1, e2, e3, e4]) e1 = rtb.ELink(rtb.ETS.rz(), jindex=0) e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1) e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2) e4 = rtb.ELink(rtb.ETS.rz(), parent=e3) with self.assertRaises(ValueError): rtb.ERobot([e1, e2, e3, e4])
def test_jindex(self): e1 = rtb.ELink(rtb.ETS.rz(), jindex=0) e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1) e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2) e4 = rtb.ELink(rtb.ETS.rz(), jindex=0, parent=e3) # with self.assertRaises(ValueError): rtb.ERobot([e1, e2, e3, e4], gripper_links=e4)
def test_init_bases(self): e1 = rtb.ELink() e2 = rtb.ELink() e3 = rtb.ELink(parent=e1) e4 = rtb.ELink(parent=e2) with self.assertRaises(ValueError): rtb.ERobot([e1, e2, e3, e4])
def test_jindex_1(self): e1 = rtb.ELink(rtb.ETS.rz(), jindex=0) e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1) e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2) e5 = rtb.ELink(rtb.ETS.rz(), jindex=0, parent=e3) e7 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e5) rtb.ERobot([e1, e2, e3, e5, e7], gripper_links=e5)
def test_invdyn(self): # create a 2 link robot # Example from Spong etal. 2nd edition, p. 260 E = rtb.ETS l1 = rtb.ELink(ets=E.ry(), m=1, r=[0.5, 0, 0], name='l1') l2 = rtb.ELink( ets=E.tx(1) * E.ry(), m=1, r=[0.5, 0, 0], parent=l1, name='l2') robot = rtb.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])
def test_jindex_4(self): e1 = rtb.ELink(rtb.ETS.rz()) e2 = rtb.ELink(rtb.ETS.rz(), parent=e1) e3 = rtb.ELink(rtb.ETS.rz(), parent=e2) e4 = rtb.ELink(rtb.ETS.rz(), parent=e3) e5 = rtb.ELink(rtb.ETS.rz(), parent=e4) e6 = rtb.ELink(rtb.ETS.rz(), parent=e5) e7 = rtb.ELink(rtb.ETS.rz(), parent=e6) rtb.ERobot([e1, e2, e3, e4, e5, e6, e7], gripper_links=e3)
def __init__(self, chain: PyChain): """Abstract Kinematic Solver class. Location for URDF files are relative to: /Lib/site-packages/rtbdata/xacro """ self.chain = chain # dirname = os.path.dirname(__file__) # filepath = os.path.join(dirname, chain.urdf.path) links, name = rtb.ERobot.urdf_to_ets_args(self, 'mechatronics_arm.urdf') self._robot = rtb.ERobot(links) self._robot.name = name
def test_ets_init(self): ets = rtb.ETS.tx(-0.0825) * rtb.ETS.rz() * rtb.ETS.tx(-0.0825) \ * rtb.ETS.rz() * rtb.ETS.tx(0.1) rtb.ERobot(ets)