diff --git a/src/roboticstoolbox/ets/ETS2.py b/src/roboticstoolbox/ets/ETS2.py index 4c488dd12..721c8388c 100644 --- a/src/roboticstoolbox/ets/ETS2.py +++ b/src/roboticstoolbox/ets/ETS2.py @@ -338,7 +338,7 @@ def eval( # append if l > 1: T[k, :, :] = Tk - # ret.append(SE2(Tk, check=False)) # type: ignore + # ret.append(SE2(Tk, check=False)) else: T = Tk # ret = SE2(Tk, check=False) diff --git a/src/roboticstoolbox/models/DH/TwoLink.py b/src/roboticstoolbox/models/DH/TwoLink.py index fb59c0ca5..d3b4d4c1c 100644 --- a/src/roboticstoolbox/models/DH/TwoLink.py +++ b/src/roboticstoolbox/models/DH/TwoLink.py @@ -73,12 +73,12 @@ def __init__( zero = sym.zero() pi = sym.pi() - a1, a2 = sym.symbol("a1 a2") # link lengths # type: ignore - m1, m2 = sym.symbol("m1 m2") # link masses # type: ignore - c1, c2 = sym.symbol("c1 c2") # link CoMs location relative to link frames# type: ignore + a1, a2 = sym.symbol("a1 a2") # link lengths + m1, m2 = sym.symbol("m1 m2") # link masses + c1, c2 = sym.symbol("c1 c2") # link CoMs location relative to link frames if inertia: - r = sym.symbol("r") # link radius, assumed tubular, for inertia calculation # type: ignore - I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM # type: ignore + r = sym.symbol("r") # link radius, assumed tubular, for inertia calculation + I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM else: I1, I2 = None, None g = sym.symbol("g") @@ -98,7 +98,7 @@ def __init__( c2 = 0.5 # CoM location of second link relative to second link frame r = 0.1 # radius of links, assumed tubular, for inertia calculation if inertia: - I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM # type: ignore + I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM else: I1, I2 = None, None g = 9.8 @@ -120,7 +120,7 @@ def __init__( c2 = -0.5 # CoM location of second link relative to second link frame r = 0.1 # radius of links, assumed tubular, for inertia calculation if inertia: - I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM # type: ignore + I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2) # moments of inertia about CoM else: I1, I2 = None, None g = 9.8 diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index e7e598b93..921a41afb 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -1937,15 +1937,16 @@ def recurse(link: Link) -> Any: segs = [link.parent] while True: segs.append(link) - if link.nchildren == 0: + children = link.children or [] + if len(children) == 0: return [segs] - elif link.nchildren == 1: - link = link.children[0] # type: ignore + elif len(children) == 1: + link = children[0] continue - elif link.nchildren > 1: + elif len(children) > 1: segs = [segs] # type: ignore[list-item] - for child in link.children: # type: ignore + for child in children: segs.extend(recurse(child)) return segs diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index 6f8a43c62..997146bb6 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -721,14 +721,15 @@ def I(self) -> NDArray: @I.setter @_dirties_frne def I(self, I_new: ArrayLike) -> None: + I_new = np.asarray(I_new) if ismatrix(I_new, (3, 3)): # 3x3 matrix passed - if np.any(np.abs(I_new - I_new.T) > 1e-8): # type: ignore + if np.any(np.abs(I_new - I_new.T) > 1e-8): raise ValueError("3x3 matrix is not symmetric") elif isvector(I_new, 9): # 3x3 matrix passed as a 1d vector - I_new = I_new.reshape(3, 3) # type: ignore + I_new = I_new.reshape(3, 3) if np.any(np.abs(I_new - I_new.T) > 1e-8): raise ValueError("3x3 matrix is not symmetric") @@ -1100,7 +1101,13 @@ def closest_point( """ if not skip: - self.robot._update_link_tf(self.robot.q) # type: ignore + robot = self.robot + if robot is None: + raise ValueError( + "the link does not belong to a robot, use skip=True to use " + "its current transform" + ) + robot._update_link_tf(robot.q) self.update() shape.update() @@ -1133,7 +1140,13 @@ def iscollided(self, shape: Shape, skip: bool = False) -> bool: """ if not skip: - self.robot._update_link_tf(self.robot.q) # type: ignore + robot = self.robot + if robot is None: + raise ValueError( + "the link does not belong to a robot, use skip=True to use " + "its current transform" + ) + robot._update_link_tf(robot.q) self.update() shape.update() diff --git a/tests/test_ELink.py b/tests/test_ELink.py index 614ed3811..d222c4655 100644 --- a/tests/test_ELink.py +++ b/tests/test_ELink.py @@ -110,6 +110,31 @@ def test_I(self): nt.assert_array_almost_equal(l1.I, I1) nt.assert_array_almost_equal(l2.I, I2) + def test_I_from_lists(self): + # a 3x3 matrix, and a 9-vector, given as lists rather than arrays + I0 = np.array([[1, 0, 0], [0, 2, 0], [0, 0, 3]]) + + l0 = rtb.Link(I=[[1, 0, 0], [0, 2, 0], [0, 0, 3]]) + l1 = rtb.Link(I=[1, 0, 0, 0, 2, 0, 0, 0, 3]) + + nt.assert_array_almost_equal(l0.I, I0) + nt.assert_array_almost_equal(l1.I, I0) + + with self.assertRaises(ValueError): + rtb.Link(I=[[1, 2, 0], [0, 2, 0], [0, 0, 3]]) # not symmetric + + @skip_no_collision_checking + def test_collision_checks_need_a_robot(self): + # a link that is not part of a robot cannot update its transform + link = rtb.Link() + shape = gm.Cuboid([1, 1, 1]) + + with self.assertRaises(ValueError): + link.iscollided(shape) + + with self.assertRaises(ValueError): + link.closest_point(shape) + def test_friction(self): l0 = rtb.Link(Tc=[2, -1], B=3, G=2)