def test_Tc(self):
l0 = rp.ELink(Tc=1)
l1 = rp.ELink(Tc=[1])
l2 = rp.ELink(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_init(self):
l0 = rp.ELink()
l1 = rp.ELink()
r = rp.ETS([l0, l1], base=sm.SE3.Rx(1.3), base_link=l1, ee_link=l0)
r.base_link = l1
r.base_link = 0
r.ee_link = 1
with self.assertRaises(TypeError):
rp.ETS(l0, base=sm.SE3.Rx(1.3))
with self.assertRaises(TypeError):
rp.ETS([1, 2], base=sm.SE3.Rx(1.3))
def test_I(self):
l0 = rp.ELink(I=[1, 2, 3])
l1 = rp.ELink(I=[0, 1, 2, 3, 4, 5])
l2 = rp.ELink(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_friction(self):
l0 = rp.ELink(Tc=[2, -1], B=3, G=2)
tau = -124
tau2 = 122
nt.assert_almost_equal(l0.friction(10), tau)
nt.assert_almost_equal(l0.friction(-10), tau2)
def test_init(self):
rx = rp.ET.TRx(1.543)
ry = rp.ET.TRy(1.543)
tz = rp.ET.Ttz()
ty = rp.ET.Tty()
with self.assertRaises(ValueError):
rp.ELink([rx, ry, tz, ty])
def test_nofriction(self):
l0 = rp.ELink(Tc=2, B=3)
l1 = rp.ELink(Tc=2, B=3)
l2 = rp.ELink(Tc=2, B=3)
l3 = rp.ELink(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_collision_fail(self):
l0 = rp.ELink()
col = rp.Shape.Box([1, 1, 1])
l0.collision = col
with self.assertRaises(TypeError):
l0.collision = [1, 1, 1]
with self.assertRaises(TypeError):
l0.collision = 1
def test_A(self):
rx = rp.ET.TRx(1.543)
ry = rp.ET.TRy(1.543)
tz = rp.ET.Ttz(1)
l0 = rp.ELink([rx, ry, tz])
ans = sm.SE3.Rx(1.543) * sm.SE3.Ry(1.543) * sm.SE3.Tz(1)
nt.assert_array_almost_equal(l0.A().A, ans.A)
def test_str_ets(self):
rx = rp.ET.TRx(1.543)
ry = rp.ET.TRy(1.543)
tz = rp.ET.Ttz(1)
l0 = rp.ELink([rx, ry, tz])
ans = '[Rx(88.4074), Ry(88.4074), tz(1)]'
self.assertEqual(str(l0), ans)
def test_geometry_fail(self):
l0 = rp.ELink()
col = rp.Shape.Box([1, 1, 1])
l0.geometry = col
l0.geometry = [col, col]
with self.assertRaises(TypeError):
l0.geometry = [1, 1, 1]
with self.assertRaises(TypeError):
l0.geometry = 1
def test_complex(self):
l0 = rp.ELink([rp.ET.Ttx(0.1), rp.ET.TRx()])
l1 = rp.ELink([rp.ET.Ttx(0.1), rp.ET.TRy()], parent=l0)
l2 = rp.ELink([rp.ET.Ttx(0.1), rp.ET.TRz()], parent=l1)
l3 = rp.ELink([rp.ET.Ttx(0.1), rp.ET.Ttx()], parent=l2)
l4 = rp.ELink([rp.ET.Ttx(0.1), rp.ET.Tty()], parent=l3)
l5 = rp.ELink([rp.ET.Ttx(0.1), rp.ET.Ttz()], parent=l4)
r = rp.ETS([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_A2(self):
rx = rp.ET.TRx(np.pi)
ry = rp.ET.TRy(np.pi)
tz = rp.ET.Ttz()
l0 = rp.ELink([rx, ry, tz])
ans = sm.SE3.Rx(np.pi) * sm.SE3.Ry(np.pi) * sm.SE3.Tz(1.2)
nt.assert_array_almost_equal(l0.A(1.2).A, ans.A)
with self.assertRaises(ValueError):
l0.A()
def test_dyn(self):
l0 = rp.ELink(Tc=[0.4, -0.43],
G=-62.61,
qlim=[-2.79, 2.79],
I=np.diag([0, 0.35, 0]))
s0 = l0.dyn()
self.assertEqual(
s0, "m = 0.00 \n"
"r = 0.00 0.00 0.00 \n"
" | 0.00 0.00 0.00 | \n"
"I = | 0.00 0.35 0.00 | \n"
" | 0.00 0.00 0.00 | \n"
"Jm = 0.00 \n"
"B = 0.00 \n"
"Tc = 0.40(+) -0.43(-) \n"
"G = -62.61 \n"
"qlim = -2.79 to 2.79")
def test_qlim(self):
l0 = rp.ELink(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_setB(self):
l0 = rp.ELink()
with self.assertRaises(TypeError):
l0.B = [1, 2]
def test_fail_parent(self):
with self.assertRaises(TypeError):
rp.ELink(parent=1)
def test_properties(self):
l0 = rp.ELink()
self.assertEqual(l0.m, 0.0)
nt.assert_array_almost_equal(l0.r.A, np.eye(4))
self.assertEqual(l0.Jm, 0.0)
