def test_jacob0v(self): pdh = rp.PandaMDH() panda = rp.Panda() q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) panda.q = q1 nt.assert_array_almost_equal(panda.jacob0v(), pdh.jacob0v(q1))
def test_control_type(self): panda = rp.PandaMDH() panda.control_type = 'p' with self.assertRaises(ValueError): panda.control_type = 'z'
def test_teach2(self): panda = rp.PandaMDH() panda.q = panda.qr e = panda.teach(block=False) e.close() e2 = panda.teach2(block=False, q=panda.qr) e2.close()
def test_PyPlot(self): panda = rp.PandaMDH() env = rp.backend.PyPlot() env.launch() env.add(panda) env.step() env._plot_handler(None, None) env.close()
def test_PyPlot_invisible(self): panda = rp.PandaMDH() env = rp.backend.PyPlot() env.launch() env.add(panda, display=False) env.step() env._plot_handler(None, None) env.close()
def test_base(self): panda = rp.PandaMDH() panda.base = sm.SE3.Rx(2) nt.assert_array_almost_equal(panda.base.A, sm.SE3.Rx(2).A) panda.base = sm.SE3.Ty(2).A nt.assert_array_almost_equal(panda.base.A, sm.SE3.Ty(2).A)
def test_islimit(self): panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] panda.q = q ans = [False, True, True, True, True, True, True] self.assertEqual(panda.islimit(q), ans) self.assertEqual(panda.islimit(), ans)
def test_twists(self): # TODO panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] panda.q = q panda.twists() panda.twists(q) pass
def test_fkine_panda(self): panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] T = np.array([[-0.8583, 0.1178, 0.4994, 0.1372], [0.1980, 0.9739, 0.1106, 0.3246], [-0.4734, 0.1938, -0.8593, 0.4436], [0, 0, 0, 1]]) nt.assert_array_almost_equal(panda.fkine(q).A, T, decimal=4)
def test_payload(self): panda = rp.PandaMDH() nt.assert_array_almost_equal(panda.r[:, 6], np.zeros(3)) nt.assert_array_almost_equal(panda.links[6].m, 0) m = 6 p = [1, 2, 3] panda.payload(m, p) nt.assert_array_almost_equal(panda.r[:, 6], p) nt.assert_array_almost_equal(panda.links[6].m, m)
def test_plot_with_vellipse2_fail(self): panda = rp.PandaMDH() panda.q = panda.qr e = rp.backend.PyPlot2() e.launch() e.add(panda.fellipse(q=panda.qr, centre=[0, 1])) # e.close() with self.assertRaises(ValueError): e.add(panda.fellipse(q=panda.qr, centre='ee', opt='rot')) e.close()
def test_ikine(self): panda = rp.PandaMDH() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T]) l0 = rp.Revolute(d=2.0) l1 = rp.Prismatic(theta=1.0) l2 = rp.Prismatic(theta=1, qlim=[0, 2]) r0 = rp.SerialLink([l0, l1]) r1 = rp.SerialLink([l0, l2]) qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] qa, success, err = panda.ikine(T) qa2, success, err = panda.ikine(Tt) qa3, success, err = panda.ikine(Tt, q0=np.zeros((7, 2))) qa4, success, err = panda.ikine(T, q0=np.zeros(7)) # Untested qa5, success, err = r0.ikine(T.A, mask=[1, 1, 0, 0, 0, 0], transpose=5, ilimit=5) qa5, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0]) qa6, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1) qa7, success, err = r1.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1) nt.assert_array_almost_equal(qa, qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 0], qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa3[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa4, qr, decimal=4) with self.assertRaises(ValueError): panda.ikine(Tt, q0=np.zeros(7)) with self.assertRaises(ValueError): r0.ikine(T) with self.assertRaises(ValueError): r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1)
def test_jacobe_panda(self): panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] panda.q = q Je = np.array( [[0.3058, 0.1315, -0.2364, -0.0323, 0.0018, 0.2095, 0], [0.0954, 0.0303, -0.0721, 0.1494, -0.0258, 0.0144, 0], [-0.1469, 0.3385, 0.0506, 0.0847, -0.0000, -0.0880, 0], [-0.4734, 0.8292, -0.0732, 0.8991, -0.2788, -0.0685, 0], [0.1938, 0.4271, 0.7224, 0.3461, -0.0191, 0.9976, 0], [-0.8593, -0.3605, 0.6876, -0.2679, -0.9602, 0.0000, 1.0000]]) nt.assert_array_almost_equal(panda.jacobe(q), Je, decimal=4) nt.assert_array_almost_equal(panda.jacobe(), Je, decimal=4)
def test_A(self): panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] panda.q = q T1 = np.array([[0.5403, -0.8415, 0, 0], [0.8415, 0.5403, 0, 0], [0, 0, 1, 0.333], [0, 0, 0, 1]]) T2 = np.array([[-0.3279, -0.9015, -0.2826, 0.2918], [0.9232, -0.3693, 0.1068, 0.06026], [-0.2006, -0.2258, 0.9533, 0.3314], [0, 0, 0, 1]]) nt.assert_array_almost_equal(panda.A(0, q).A, T1, decimal=4) nt.assert_array_almost_equal(panda.A([1, 4], q).A, T2, decimal=4) nt.assert_array_almost_equal(panda.A([1, 4]).A, T2, decimal=4)
def test_allfkine(self): pm = rp.PandaMDH() p = rp.Panda() q = [1, 2, 3, 4, 5, 6, 7] p.q = q pm.q = q p.allfkine() r2 = pm.allfkine() for i in range(7): nt.assert_array_almost_equal(p.ets[i]._fk.A, r2[i].A) p.allfkine(q) for i in range(7): nt.assert_array_almost_equal(p.ets[i]._fk.A, r2[i].A)
def test_qmincon(self): panda = rp.PandaMDH() panda.q = panda.qr q = panda.qr qt = np.c_[q, q] q0, s0, _ = panda.qmincon() q1, s1, _ = panda.qmincon(q) q2, _, _ = panda.qmincon(qt) qres = [-0.0969, -0.3000, 0.0870, -2.2000, 0.0297, 2.0000, 0.7620] nt.assert_array_almost_equal(q0, qres, decimal=4) nt.assert_array_almost_equal(q1, qres, decimal=4) nt.assert_array_almost_equal(q2[:, 0], qres, decimal=4) nt.assert_array_almost_equal(q2[:, 1], qres, decimal=4)
def test_pay(self): panda = rp.PandaMDH() panda.q = [1, 2, 3, 4, 5, 6, 7] w = [1, 2, 3, 4, 5, 6] wT = np.c_[w, w, w, w] qT = np.c_[panda.q, panda.q, panda.q, panda.q] tau = np.array( [6.0241, -4.4972, -7.2160, -4.2400, 7.0215, -4.6884, -6.0000]) tau0 = np.array( [-5.9498, 1.4604, -3.4544, 1.5026, -3.7777, -6.6578, 2.6047]) tauT = np.c_[tau, tau, tau, tau] tau0T = np.c_[tau0, tau0, tau0, tau0] Je = panda.jacobe() J0 = panda.jacob0() JeT = np.zeros((6, 7, 4)) for i in range(4): JeT[:, :, i] = Je panda.pay(w) nt.assert_array_almost_equal(panda.pay(w), tau, decimal=4) nt.assert_array_almost_equal(panda.pay(w, frame=0), tau0, decimal=4) nt.assert_array_almost_equal(panda.pay(w, q=panda.q), tau, decimal=4) nt.assert_array_almost_equal(panda.pay(wT, q=qT), tauT, decimal=4) nt.assert_array_almost_equal(panda.pay(wT, q=qT, frame=0), tau0T, decimal=4) nt.assert_array_almost_equal(panda.pay(w, J=Je), tau, decimal=4) nt.assert_array_almost_equal(panda.pay(w, J=J0), tau0, decimal=4) nt.assert_array_almost_equal(panda.pay(wT, J=JeT), tauT, decimal=4) with self.assertRaises(ValueError): panda.pay(wT, panda.q) with self.assertRaises(TypeError): panda.pay(wT)
def test_plot_fellipse(self): panda = rp.PandaMDH() panda.q = panda.qr e = panda.plot_fellipse(block=False, limits=[1, 2, 1, 2, 1, 2]) e.close() e = panda.plot_fellipse(block=False, q=panda.qr, centre='ee', opt='rot') e.step(0) e.close() with self.assertRaises(TypeError): panda.plot_fellipse(fellipse=10) with self.assertRaises(ValueError): panda.plot_fellipse(centre='ff')
def test_ikcon(self): panda = rp.PandaMDH() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T, T]) qr = [ 7.69161412e-04, 9.01409257e-01, -1.46372859e-02, -6.98000000e-02, 1.38978915e-02, 9.62104811e-01, 7.84926515e-01 ] qa, success, err = panda.ikcon(T.A, q0=np.zeros(7)) qa2, success, err = panda.ikcon(Tt) qa3, _, _ = panda.ikcon(Tt, q0=np.zeros((7, 3))) nt.assert_array_almost_equal(qa, qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 0], qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa3[:, 0], qr, decimal=4) nt.assert_array_almost_equal(qa3[:, 1], qr, decimal=4)
def test_allfkine(self): panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] panda.q = q t0 = np.array([[0.5403, -0.8415, 0, 0], [0.8415, 0.5403, 0, 0], [0, 0, 1, 0.333], [0, 0, 0, 1]]) t1 = np.array([[-0.2248, -0.4913, -0.8415, 0], [-0.3502, -0.7651, 0.5403, 0], [-0.9093, 0.4161, 0, 0.333], [0, 0, 0, 1]]) t2 = np.array([[0.1038, 0.8648, 0.4913, 0.1552], [0.4229, -0.4855, 0.7651, 0.2418], [0.9002, 0.1283, -0.4161, 0.2015], [0, 0, 0, 1]]) t3 = np.array([[-0.4397, -0.2425, -0.8648, 0.1638], [-0.8555, -0.1801, 0.4855, 0.2767], [-0.2735, 0.9533, -0.1283, 0.2758], [0, 0, 0, 1]]) t4 = np.array([[-0.9540, -0.1763, -0.2425, 0.107], [0.2229, -0.9581, -0.1801, 0.2781], [-0.2006, -0.2258, 0.9533, 0.6644], [0, 0, 0, 1]]) t5 = np.array([[-0.8482, -0.4994, 0.1763, 0.107], [0.2643, -0.1106, 0.9581, 0.2781], [-0.4590, 0.8593, 0.2258, 0.6644], [0, 0, 0, 1]]) t6 = np.array([[-0.5236, 0.6902, 0.4994, 0.08575], [0.8287, 0.5487, 0.1106, 0.3132], [-0.1977, 0.4718, -0.8593, 0.5321], [0, 0, 0, 1]]) Tall = panda.allfkine(q) Tall2 = panda.allfkine() nt.assert_array_almost_equal(Tall[0].A, t0, decimal=4) nt.assert_array_almost_equal(Tall[1].A, t1, decimal=4) nt.assert_array_almost_equal(Tall[2].A, t2, decimal=4) nt.assert_array_almost_equal(Tall[3].A, t3, decimal=4) nt.assert_array_almost_equal(Tall[4].A, t4, decimal=4) nt.assert_array_almost_equal(Tall[5].A, t5, decimal=4) nt.assert_array_almost_equal(Tall[6].A, t6, decimal=4) nt.assert_array_almost_equal(Tall2[0].A, t0, decimal=4)
#!/usr/bin/env python """ @author Jesse Haviland """ import ropy as rp import spatialmath as sm import numpy as np import time env = rp.backend.PyPlot() env.launch('Panda Resolved-Rate Motion Control Example') panda = rp.PandaMDH() panda.q = panda.qr Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2) arrived = False env.add(panda) dt = 0.05 while not arrived: start = time.time() v, arrived = rp.p_servo(panda.fkine(), Tep, 1) panda.qd = np.linalg.pinv(panda.jacobe()) @ v env.step(50) stop = time.time()
def test_teach_withq(self): panda = rp.PandaMDH() e = panda.teach(q=panda.qr, block=False) e.close()
def test_plot(self): panda = rp.PandaMDH() panda.q = panda.qr e = panda.plot(block=False) e.close()
def test_plot2_traj(self): panda = rp.PandaMDH() q = np.random.rand(7, 3) e = panda.plot2(block=False, q=q, dt=0) e.close()
def test_plot2(self): panda = rp.PandaMDH() panda.q = panda.qr e = panda.plot2(block=False, name=True) e.close()
def test_jointdynamics(self): # TODO panda = rp.PandaMDH() panda.jointdynamics(1, 2) pass
def test_plot_with_vellipse2(self): panda = rp.PandaMDH() panda.q = panda.qr e = panda.plot2(block=False, vellipse=True, limits=[1, 2, 1, 2]) e.step() e.close()
def test_plot_with_fellipse2(self): panda = rp.PandaMDH() panda.q = panda.qr e = panda.plot2(block=False, fellipse=True) e.close()
def test_A_error(self): panda = rp.PandaMDH() q = [1, 2, 3, 4, 5, 6, 7] with self.assertRaises(ValueError): panda.A(7, q).A
def test_name(self): panda = rp.PandaMDH() panda.name = 'new' self.assertEqual(panda.name, 'new')