def test_A(self):
l0 = rp.DHLink(sigma=0)
l1 = rp.DHLink(sigma=1)
l2 = rp.DHLink(sigma=0, mdh=0)
l3 = rp.DHLink(sigma=1, mdh=1)
l4 = rp.DHLink(flip=True)
T0 = sm.SE3(np.array([
[-1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
]))
T1 = sm.SE3(np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, np.pi],
[0, 0, 0, 1]
]))
nt.assert_array_almost_equal(l0.A(np.pi).A, T0.A)
nt.assert_array_almost_equal(l1.A(np.pi).A, T1.A)
nt.assert_array_almost_equal(l2.A(np.pi).A, T0.A)
nt.assert_array_almost_equal(l3.A(np.pi).A, T1.A)
nt.assert_array_almost_equal(l4.A(np.pi).A, T0.A)
def test_sigma(self):
l0 = rp.DHLink(sigma=0)
l1 = rp.DHLink(sigma=1)
self.assertEqual(l0.isrevolute(), True)
self.assertEqual(l0.isprismatic(), False)
self.assertEqual(l1.isrevolute(), False)
self.assertEqual(l1.isprismatic(), True)
def test_Tc(self):
l0 = rp.DHLink(Tc=1)
l1 = rp.DHLink(Tc=[1])
l2 = rp.DHLink(Tc=[1, 2])
Tc0 = np.array([1, -1])
Tc1 = np.array([1, -1])
Tc2 = np.array([1, 2])
nt.assert_array_almost_equal(l0.Tc, Tc0)
nt.assert_array_almost_equal(l1.Tc, Tc1)
nt.assert_array_almost_equal(l2.Tc, Tc2)
def test_I(self):
l0 = rp.DHLink(I=[1, 2, 3])
l1 = rp.DHLink(I=[0, 1, 2, 3, 4, 5])
l2 = rp.DHLink(I=np.eye(3))
I0 = np.array([[1, 0, 0], [0, 2, 0], [0, 0, 3]])
I1 = np.array([[0, 3, 5], [3, 1, 4], [5, 4, 2]])
I2 = np.eye(3)
nt.assert_array_almost_equal(l0.I, I0)
nt.assert_array_almost_equal(l1.I, I1)
nt.assert_array_almost_equal(l2.I, I2)
def test_nofriction(self):
l0 = rp.DHLink(Tc=2, B=3)
l1 = rp.DHLink(Tc=2, B=3)
l2 = rp.DHLink(Tc=2, B=3)
l3 = rp.DHLink(Tc=2, B=3)
n0 = l1.nofriction()
n1 = l2.nofriction(viscous=True)
n2 = l3.nofriction(coulomb=False)
nt.assert_array_almost_equal(n0.B, l0.B)
nt.assert_array_almost_equal(n0.Tc, [0, 0])
nt.assert_array_almost_equal(n1.B, 0)
nt.assert_array_almost_equal(n1.Tc, [0, 0])
nt.assert_array_almost_equal(n2.B, l0.B)
nt.assert_array_almost_equal(n2.Tc, l0.Tc)
def test_nofriction(self):
l0 = rp.DHLink(Tc=2, B=3)
l1 = rp.DHLink(Tc=2, B=3)
l2 = rp.DHLink(Tc=2, B=3)
l3 = rp.DHLink(Tc=2, B=3)
L = [l0, l1, l2, l3]
r0 = rp.DHRobot(L)
n0 = r0.nofriction()
n1 = r0.nofriction(viscous=True)
n2 = r0.nofriction(coulomb=False)
for i in range(4):
nt.assert_array_almost_equal(n0.links[i].B, L[i].B)
nt.assert_array_almost_equal(n0.links[i].Tc, [0, 0])
nt.assert_array_almost_equal(n1.links[i].B, 0)
nt.assert_array_almost_equal(n1.links[i].Tc, [0, 0])
nt.assert_array_almost_equal(n2.links[i].B, L[i].B)
nt.assert_array_almost_equal(n2.links[i].Tc, L[i].Tc)
def test_teach2_basic(self):
l0 = rp.DHLink(d=2)
r0 = rp.DHRobot([l0, l0])
e = r0.teach2(block=False)
e.step()
e.close()
def test_DHRobot(self):
l0 = rp.DHLink()
rp.DHRobot([l0])
def test_qlim(self):
l0 = rp.DHLink(qlim=[-1, 1])
self.assertEqual(l0.islimit(-0.9), False)
self.assertEqual(l0.islimit(-1.9), True)
self.assertEqual(l0.islimit(2.9), True)
def test_link(self):
rp.DHLink()
def test_properties(self):
l0 = rp.DHLink()
self.assertEqual(l0.m, 0.0)
nt.assert_array_almost_equal(l0.r, np.zeros(3))
self.assertEqual(l0.Jm, 0.0)
def test_add(self):
l0 = rp.DHLink()
l1 = rp.DHLink()
self.assertIsInstance(l0 + l1, rp.DHRobot)
self.assertRaises(TypeError, l0.__add__, 1)