def test_geometry_fail(self):
l0 = rp.ELink()
col = gm.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_jindex_4(self):
e1 = rtb.ELink(rtb.ETS.rz())
e2 = rtb.ELink(rtb.ETS.rz(), parent=e1)
e3 = rtb.ELink(rtb.ETS.rz(), parent=e2)
e4 = rtb.ELink(rtb.ETS.rz(), parent=e3)
e5 = rtb.ELink(rtb.ETS.rz(), parent=e4)
e6 = rtb.ELink(rtb.ETS.rz(), parent=e5)
e7 = rtb.ELink(rtb.ETS.rz(), parent=e6)
rtb.ERobot([e1, e2, e3, e4, e5, e6, e7], gripper_links=e3)
def test_complex(self):
l0 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.rx())
l1 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.ry(), parent=l0)
l2 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.rz(), parent=l1)
l3 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.tx(), parent=l2)
l4 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.ty(), parent=l3)
l5 = rtb.ELink(rtb.ETS.tx(0.1), rtb.ETS.tz(), parent=l4)
r = rtb.ERobot([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_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_A2(self):
rx = rp.ETS.rx(np.pi)
ry = rp.ETS.ry(np.pi)
tz = rp.ETS.tz()
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_jindex_fail(self):
e1 = rtb.ELink(rtb.ETS.rz(), jindex=0)
e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1)
e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2)
e4 = rtb.ELink(rtb.ETS.rz(), jindex=5, parent=e3)
with self.assertRaises(ValueError):
rtb.ERobot([e1, e2, e3, e4])
e1 = rtb.ELink(rtb.ETS.rz(), jindex=0)
e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1)
e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2)
e4 = rtb.ELink(rtb.ETS.rz(), parent=e3)
with self.assertRaises(ValueError):
rtb.ERobot([e1, e2, e3, e4])
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
r = 0 0 0
| 0 0 0 |
I = | 0 0.35 0 |
| 0 0 0 |
Jm = 0
B = 0
Tc = 0.4(+) -0.43(-)
G = -63
qlim = -2.8 to 2.8""")
:return: gear ratio
:rtype: float
- ``link.G = ...`` checks and sets the gear ratio
.. note::
- The ratio of motor motion : link motion
- The gear ratio can be negative, see also the ``flip`` attribute.
:seealso: :func:`flip`
"""
return self._G
@G.setter
@_listen_dyn
def G(self, G_new):
self._G = G_new
if __name__ == "__main__": # pragma nocover
import roboticstoolbox as rtb
from spatialmath.base import sym
ET = rtb.ETS
d, a, theta = sym.symbol('d, a, theta')
e = ET.tx(d) * ET.ty(a) * ET.rz(theta)
print(e)
link = rtb.ELink()
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_init_fail(self):
rx = rp.ETS.rx(1.543)
ty = rp.ETS.ty()
with self.assertRaises(TypeError):
rp.ELink([rx, ty])
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, np.zeros(3))
self.assertEqual(l0.Jm, 0.0)
def test_invdyn(self):
# create a 2 link robot
# Example from Spong etal. 2nd edition, p. 260
E = rtb.ETS
l1 = rtb.ELink(ets=E.ry(), m=1, r=[0.5, 0, 0], name='l1')
l2 = rtb.ELink(ets=E.tx(1) * E.ry(),
m=1,
r=[0.5, 0, 0],
parent=l1,
name='l2')
robot = rtb.ERobot([l1, l2], name="simple 2 link")
z = np.zeros(robot.n)
# check gravity load
tau = robot.rne(z, z, z) / 9.81
nt.assert_array_almost_equal(tau, np.r_[-2, -0.5])
tau = robot.rne([0, -pi / 2], z, z) / 9.81
nt.assert_array_almost_equal(tau, np.r_[-1.5, 0])
tau = robot.rne([-pi / 2, pi / 2], z, z) / 9.81
nt.assert_array_almost_equal(tau, np.r_[-0.5, -0.5])
tau = robot.rne([-pi / 2, 0], z, z) / 9.81
nt.assert_array_almost_equal(tau, np.r_[0, 0])
# check velocity terms
robot.gravity = [0, 0, 0]
q = [0, -pi / 2]
h = -0.5 * sin(q[1])
tau = robot.rne(q, [0, 0], z)
nt.assert_array_almost_equal(tau, np.r_[0, 0] * h)
tau = robot.rne(q, [1, 0], z)
nt.assert_array_almost_equal(tau, np.r_[0, -1] * h)
tau = robot.rne(q, [0, 1], z)
nt.assert_array_almost_equal(tau, np.r_[1, 0] * h)
tau = robot.rne(q, [1, 1], z)
nt.assert_array_almost_equal(tau, np.r_[3, -1] * h)
# check inertial terms
d11 = 1.5 + cos(q[1])
d12 = 0.25 + 0.5 * cos(q[1])
d21 = d12
d22 = 0.25
tau = robot.rne(q, z, [0, 0])
nt.assert_array_almost_equal(tau, np.r_[0, 0])
tau = robot.rne(q, z, [1, 0])
nt.assert_array_almost_equal(tau, np.r_[d11, d21])
tau = robot.rne(q, z, [0, 1])
nt.assert_array_almost_equal(tau, np.r_[d12, d22])
tau = robot.rne(q, z, [1, 1])
nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22])
