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