def test_qplot(self):
# 6 joints is special
q1 = np.r_[1, 2, 3, 4, 5, 6]
q2 = -q1
q = tr.jtraj(q1, q2, 50)
tr.qplot(q.q, block=False)
# 4 joints
q1 = np.r_[1, 2, 3, 4]
q2 = -q1
q = tr.jtraj(q1, q2, 50)
tr.qplot(q.q, block=False)
def test_jtraj(self):
# unit testing jtraj with
q1 = np.r_[1, 2, 3, 4, 5, 6]
q2 = -q1
tg = tr.jtraj(q1, q2, 11)
q = tg.q
qd = tg.qd
qdd = tg.qdd
self.assertAlmostEqual(q.shape, (11, 6))
self.assertTrue(np.allclose(q[0, :], q1))
self.assertTrue(np.allclose(q[-1, :], q2))
self.assertTrue(np.allclose(q[5, :], np.zeros(6,)))
self.assertAlmostEqual(qd.shape, (11, 6))
self.assertTrue(np.allclose(qd[0, :], np.zeros(6,)))
self.assertTrue(np.allclose(qd[-1, :], np.zeros(6,)))
self.assertAlmostEqual(qdd.shape, (11, 6))
self.assertTrue(np.allclose(qdd[0, :], np.zeros(6,)))
self.assertTrue(np.allclose(qdd[-1, :], np.zeros(6,)))
self.assertTrue(np.allclose(qdd[5, :], np.zeros(6,)))
# with a time vector
t = np.linspace(0, 2, 11)
tg = tr.jtraj(q1, q2, t)
q = tg.q
qd = tg.qd
qdd = tg.qdd
self.assertAlmostEqual(q.shape, (11, 6))
self.assertTrue(np.allclose(q[0, :], q1))
self.assertTrue(np.allclose(q[-1, :], q2))
self.assertTrue(np.allclose(q[5, :], np.zeros(6,)))
self.assertAlmostEqual(qd.shape, (11, 6))
self.assertTrue(np.allclose(qd[0, :], np.zeros(6,)))
self.assertTrue(np.allclose(qd[-1, :], np.zeros(6,)))
self.assertAlmostEqual(qdd.shape, (11, 6))
self.assertTrue(np.allclose(qdd[0, :], np.zeros(6,)))
self.assertTrue(np.allclose(qdd[-1, :], np.zeros(6,)))
self.assertTrue(np.allclose(qdd[5, :], np.zeros(6,)))
# test with boundary conditions
qone = np.ones((6,))
tg = tr.jtraj(q1, q2, 11, -qone, qone)
q = tg.q
qd = tg.qd
qdd = tg.qdd
self.assertAlmostEqual(q.shape, (11, 6))
self.assertTrue(np.allclose(q[0, :], q1))
self.assertTrue(np.allclose(q[-1, :], q2))
self.assertAlmostEqual(qd.shape, (11, 6))
self.assertTrue(np.allclose(qd[0, :], -qone))
self.assertTrue(np.allclose(qd[-1, :], qone))
self.assertAlmostEqual(qdd.shape, (11, 6))
self.assertTrue(np.allclose(qdd[0, :], np.zeros(6,)))
self.assertTrue(np.allclose(qdd[-1, :], np.zeros(6,)))
with self.assertRaises(ValueError):
tr.jtraj(q1, [1, 1, 2], t)
with self.assertRaises(ValueError):
tr.jtraj(q1, q2, t, qd1=[1, 1])
with self.assertRaises(ValueError):
tr.jtraj(q1, q2, t, qd0=[1, 1])