diff --git a/pyproject.toml b/pyproject.toml index cf781595c..6059eb0f2 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -254,37 +254,81 @@ module = [ "roboticstoolbox.backends.PyPlot.RobotPlot", "roboticstoolbox.backends.PyPlot.RobotPlot2", "roboticstoolbox.backends.PyPlot.EllipsePlot", -] -disallow_untyped_defs = true - -# The to-do list for the typing pass. A module is listed here because it still -# has type errors; fix a module, delete its line, and mypy then holds it to -# zero. The list should only ever get shorter. A module that is not listed -# is checked, and must stay clean. -[[tool.mypy.overrides]] -module = [ -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ - "roboticstoolbox.bin.rtbtool", -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ + "roboticstoolbox.blocks.arm", + "roboticstoolbox.blocks.mobile", + "roboticstoolbox.blocks.spatial", + "roboticstoolbox.blocks.uav", + "roboticstoolbox.blocks.quad_model", + "roboticstoolbox.tools.Ticker", + "roboticstoolbox.tools.data", + "roboticstoolbox.tools.jsingu", + "roboticstoolbox.tools.null", + "roboticstoolbox.tools.numerical", + "roboticstoolbox.tools.p_servo", + "roboticstoolbox.tools.params", + "roboticstoolbox.tools.plot", + "roboticstoolbox.tools.trchain", + "roboticstoolbox.tools.urdf.urdf", + "roboticstoolbox.tools.urdf.utils", "roboticstoolbox.demo.eigdemo", -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ + "roboticstoolbox.demo.tripleangledemo", + "roboticstoolbox.demo.twistdemo", + "roboticstoolbox.bin._bintools", + "roboticstoolbox.bin.rtbtool", + "roboticstoolbox.models.DH.AL5D", + "roboticstoolbox.models.DH.Ball", + "roboticstoolbox.models.DH.Baxter", + "roboticstoolbox.models.DH.Cobra600", + "roboticstoolbox.models.DH.Coil", + "roboticstoolbox.models.DH.Hyper", + "roboticstoolbox.models.DH.Hyper3d", + "roboticstoolbox.models.DH.IRB140", + "roboticstoolbox.models.DH.Jaco", + "roboticstoolbox.models.DH.KR5", + "roboticstoolbox.models.DH.LWR4", + "roboticstoolbox.models.DH.Mico", + "roboticstoolbox.models.DH.Orion5", + "roboticstoolbox.models.DH.P8", + "roboticstoolbox.models.DH.Panda", + "roboticstoolbox.models.DH.Planar2", + "roboticstoolbox.models.DH.Planar3", + "roboticstoolbox.models.DH.Puma560", + "roboticstoolbox.models.DH.Sawyer", + "roboticstoolbox.models.DH.Stanford", + "roboticstoolbox.models.DH.TwoLink", + "roboticstoolbox.models.DH.UR10", + "roboticstoolbox.models.DH.UR3", + "roboticstoolbox.models.DH.UR5", + "roboticstoolbox.models.DH.Uprighttl", + "roboticstoolbox.models.ETS.GenericSeven", + "roboticstoolbox.models.ETS.Planar2", + "roboticstoolbox.models.ETS.Planar_Y", + "roboticstoolbox.models.URDF.AL5D", + "roboticstoolbox.models.URDF.Fetch", + "roboticstoolbox.models.URDF.Frankie", + "roboticstoolbox.models.URDF.FrankieOmni", + "roboticstoolbox.models.URDF.Jaco", + "roboticstoolbox.models.URDF.KinovaGen3", + "roboticstoolbox.models.URDF.LBR", + "roboticstoolbox.models.URDF.PR2", + "roboticstoolbox.models.URDF.Panda", + "roboticstoolbox.models.URDF.Puma560", + "roboticstoolbox.models.URDF.UR10", + "roboticstoolbox.models.URDF.UR3", + "roboticstoolbox.models.URDF.UR5", + "roboticstoolbox.models.URDF.URDFRobot", + "roboticstoolbox.models.URDF.Valkyrie", + "roboticstoolbox.models.URDF.YuMi", + "roboticstoolbox.models.URDF.px100", + "roboticstoolbox.models.URDF.px150", + "roboticstoolbox.models.URDF.rx150", + "roboticstoolbox.models.URDF.rx200", + "roboticstoolbox.models.URDF.vx300", + "roboticstoolbox.models.URDF.vx300s", + "roboticstoolbox.models.URDF.wx200", + "roboticstoolbox.models.URDF.wx250", + "roboticstoolbox.models.URDF.wx250s", + "roboticstoolbox.models.catalog", "roboticstoolbox.ets.ET", "roboticstoolbox.ets.ET2", "roboticstoolbox.ets.ETS", @@ -292,56 +336,46 @@ module = [ "roboticstoolbox.ets._ET", "roboticstoolbox.ets._ETS", "roboticstoolbox.ets.fknm", -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ - "roboticstoolbox.mobile.ParticleFilter", -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ - "roboticstoolbox.models.DH.Planar2", "roboticstoolbox.models.ETS.Frankie", "roboticstoolbox.models.ETS.Omni", "roboticstoolbox.models.ETS.Panda", "roboticstoolbox.models.ETS.Puma560", "roboticstoolbox.models.ETS.XYPanda", - "roboticstoolbox.models.URDF.URDFRobot", - "roboticstoolbox.models.catalog", -] -ignore_errors = true - -[[tool.mypy.overrides]] -module = [ "roboticstoolbox.robot.BaseRobot", - "roboticstoolbox.robot.DHFactor", "roboticstoolbox.robot.DHLink", "roboticstoolbox.robot.DHRobot", "roboticstoolbox.robot.Dynamics", + "roboticstoolbox.robot.ELink", + "roboticstoolbox.robot.ERobot", + "roboticstoolbox.robot.Gripper", "roboticstoolbox.robot.IK", "roboticstoolbox.robot.Link", "roboticstoolbox.robot.PoERobot", "roboticstoolbox.robot.Robot", "roboticstoolbox.robot.RobotKinematics", "roboticstoolbox.robot.RobotPlottingMPL", + "roboticstoolbox.robot.RobotProto", "roboticstoolbox.robot.frne", + "roboticstoolbox.robot", +] +disallow_untyped_defs = true + +# The to-do list for the typing pass. A module is listed here because it still +# has type errors; fix a module, delete its line, and mypy then holds it to +# zero. The list should only ever get shorter. A module that is not listed +# is checked, and must stay clean. +[[tool.mypy.overrides]] +module = [ + "roboticstoolbox.mobile.ParticleFilter", ] ignore_errors = true [[tool.mypy.overrides]] module = [ - "roboticstoolbox.tools.DHFactor", - "roboticstoolbox.tools.Ticker", - "roboticstoolbox.tools.data", - "roboticstoolbox.tools.jsingu", - "roboticstoolbox.tools.numerical", "roboticstoolbox.tools.trajectory", - "roboticstoolbox.tools.trchain", "roboticstoolbox.tools.urdf.tests.test_urdf", - "roboticstoolbox.tools.urdf.urdf", + "roboticstoolbox.tools.DHFactor", + "roboticstoolbox.robot.DHFactor", ] ignore_errors = true diff --git a/src/roboticstoolbox/backends/Connector.py b/src/roboticstoolbox/backends/Connector.py index 00b52e17e..8d233cc38 100644 --- a/src/roboticstoolbox/backends/Connector.py +++ b/src/roboticstoolbox/backends/Connector.py @@ -16,6 +16,10 @@ class Connector(ABC): #: True if the backend can render EllipsePlot objects. supports_ellipse: bool = False + #: velocity and force ellipsoids shown by the teach pendant, set by it + _teach_vellipse: Any + _teach_fellipse: Any + def __init__(self) -> None: super().__init__() @@ -104,6 +108,19 @@ def remove(self, *args: Any, **kwargs: Any) -> Any: pass + def _add_teach_panel( + self, robot: Any, q: Any, handle: Any = None, block: bool = True + ) -> Any: + """ + Add the teach pendant controls for ``robot``, if the backend supports + them (see ``supports_teach``). + + :return: True if the backend has already blocked until dismissed + """ + raise NotImplementedError( + f"{self.__class__.__name__} does not have a teach pendant" + ) + @abstractmethod def hold(self, *args: Any, **kwargs: Any) -> Any: # pragma nocover """ diff --git a/src/roboticstoolbox/bin/rtbtool.py b/src/roboticstoolbox/bin/rtbtool.py index 3a970a799..2f01ac5b9 100755 --- a/src/roboticstoolbox/bin/rtbtool.py +++ b/src/roboticstoolbox/bin/rtbtool.py @@ -9,6 +9,10 @@ $ rtbtool myscript.py """ +from __future__ import annotations + +from typing import Any + # import stuff import argparse import pathlib @@ -51,7 +55,7 @@ _OPTIONS_ENVVAR = "RTB_OPTIONS" -def env_arguments(parser): +def env_arguments(parser: Any) -> Any: """Return command-line style options from the environment. :param parser: argument parser used for error reporting @@ -69,7 +73,7 @@ def env_arguments(parser): parser.error(f"invalid {_OPTIONS_ENVVAR}: {exc}") -def parse_arguments(): +def parse_arguments() -> Any: parser = argparse.ArgumentParser( description="Robotics Toolbox shell", formatter_class=LineWrapRawTextDefaultsHelpFormatter, @@ -180,7 +184,7 @@ def get_versions() -> list[str]: ] -def make_banner(): +def make_banner() -> Any: # banner template # https://patorjk.com/software/taag/#p=display&f=Cybermedium&t=Robotics%20Toolbox%0A @@ -218,12 +222,12 @@ def make_banner(): return banner -def examples_path(): +def examples_path() -> Any: # Repository layout: /src/roboticstoolbox/bin/rtbtool.py return pathlib.Path(__file__).resolve().parents[3] / "examples" -def startup(): +def startup() -> None: plt.ion() @@ -271,7 +275,7 @@ def run_smoke_test() -> bool: return n_passed == len(checks) -def main(): +def main() -> None: args, ipython_args = parse_arguments() if args.test: @@ -317,10 +321,11 @@ def main(): # set default backend for Robot.plot if args.swift: - Robot.default_backend = "swift" + # an attribute that shadows the class method, see findings + Robot.default_backend = "swift" # type: ignore[method-assign,assignment] # set matrix printing mode for spatialmath - SE3._ansimatrix = args.ansi + SE3._ansimatrix = args.ansi # type: ignore[name-defined] # from the star import # set default matplotlib backend if args.backend is not None: @@ -343,10 +348,10 @@ def main(): # drop into IPython class MyPrompt(Prompts): - def in_prompt_tokens(self, cli=None): + def in_prompt_tokens(self, cli: Any = None) -> Any: return [(Token.Prompt, args.prompt)] - def out_prompt_tokens(self, cli=None): + def out_prompt_tokens(self, cli: Any = None) -> Any: if args.resultprefix is None: # traditional behaviour return [ diff --git a/src/roboticstoolbox/blocks/mobile.py b/src/roboticstoolbox/blocks/mobile.py index d82f15f9e..981ed9348 100644 --- a/src/roboticstoolbox/blocks/mobile.py +++ b/src/roboticstoolbox/blocks/mobile.py @@ -187,14 +187,14 @@ class Unicycle(ContinuousBlock): def __init__( self, - w=1, - speed_max=np.inf, - accel_max=np.inf, - steer_max=np.inf, + w: float = 1, + speed_max: Any = np.inf, + accel_max: Any = np.inf, + steer_max: Any = np.inf, # a=0, implement this, RVC2 p111, change output and deriv - x0=None, - **blockargs, - ): + x0: Any = None, + **blockargs: Any, + ) -> None: r""" :param w: vehicle width, defaults to 1 diff --git a/src/roboticstoolbox/demo/eigdemo.py b/src/roboticstoolbox/demo/eigdemo.py index 632e12e9f..8ba03971d 100644 --- a/src/roboticstoolbox/demo/eigdemo.py +++ b/src/roboticstoolbox/demo/eigdemo.py @@ -12,6 +12,10 @@ """ +from __future__ import annotations + +from typing import Any + import sys import numpy as np import matplotlib.pyplot as plt @@ -19,7 +23,7 @@ from math import pi, sin, cos -def eigdemo(A): +def eigdemo(A: Any) -> None: print("matrix A =") print(A) @@ -31,7 +35,7 @@ def eigdemo(A): s = np.max(np.abs(e)) fig, ax = plt.subplots() - plt.axis([-s, s, -s, s]) + plt.axis((-s, s, -s, s)) plt.grid(True) plt.title("Eigenvector demonstration") plt.xlabel("x") @@ -51,7 +55,7 @@ def eigdemo(A): print("\nto exit: type q, or close the window") - def animate(theta): + def animate(theta: Any) -> Any: x = np.r_[cos(-theta), sin(-theta)] y = A @ x @@ -76,7 +80,7 @@ def animate(theta): plt.show(block=True) -def main(): +def main() -> None: # eigdemo() prints λ1/λ2 -- on Windows, stdout defaults to the legacy # cp1252 console codepage (which can't encode λ) whenever it isn't a # real console, e.g. piped/redirected output. reconfigure() (3.7+) @@ -84,7 +88,7 @@ def main(): if hasattr(sys.stdout, "reconfigure"): sys.stdout.reconfigure(encoding="utf-8") - def help(): + def help() -> None: print("eigdemo # uses default matrix [1 2; 3 4]") print("eigdemo a b c d # uses matrix [a b; c d]") sys.exit(0) diff --git a/src/roboticstoolbox/demo/tripleangledemo.py b/src/roboticstoolbox/demo/tripleangledemo.py index 43a5b2c05..39a91044d 100644 --- a/src/roboticstoolbox/demo/tripleangledemo.py +++ b/src/roboticstoolbox/demo/tripleangledemo.py @@ -4,6 +4,10 @@ @author Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import sys import swift from math import pi @@ -48,11 +52,11 @@ ) -def update_gimbals(theta, ring): +def update_gimbals(theta: Any, ring: Any) -> None: global R1, R2, R3 # update the relevant transform, depending on which ring's slider moved - def Rxyz(theta, which): + def Rxyz(theta: Any, which: Any) -> Any: theta = np.radians(theta) if which == "X": return SO3.Rx(theta) @@ -70,7 +74,7 @@ def Rxyz(theta, which): # figure the transforms for each gimbal and the plane, and update their # pose - def convert(R): + def convert(R: Any) -> Any: return BASE * SE3.Rt(R, t=[0, 0, 0]) g3.T = convert(R1 * SO3.Ry(pi / 2)) @@ -79,7 +83,7 @@ def convert(R): plane.T = convert(R1 * R2 * R3 * SO3.Ry(pi / 2) * SO3.Rz(pi / 2)) -def demo(test: bool = False): +def demo(test: bool = False) -> None: # TODO # rotate the rings according to the rotation axis, so that the axles # point the right way @@ -98,13 +102,13 @@ def demo(test: bool = False): print("Gimbal models by Peter Corke using OpenSCAD") # slider call backs, invoke the central handler - def set_one(x): + def set_one(x: Any) -> None: update_gimbals(float(x), 1) - def set_two(x): + def set_two(x: Any) -> None: update_gimbals(float(x), 2) - def set_three(x): + def set_three(x: Any) -> None: update_gimbals(float(x), 3) r_one = swift.Slider( @@ -141,7 +145,7 @@ def set_three(x): swift.Button(lambda x: set("ZYX"), label="Set to Zero") # button to reset joint angles - def reset(e): + def reset(e: Any) -> None: r_one.value = 0 r_two.value = 0 r_three.value = 0 @@ -149,12 +153,12 @@ def reset(e): zero_button = swift.Button(reset, label="Set to Zero") - def update_all_sliders(): + def update_all_sliders() -> None: update_gimbals(float(r_one.value), 1) update_gimbals(float(r_two.value), 2) update_gimbals(float(r_three.value), 3) - def change_sequence(new): + def change_sequence(new: Any) -> None: global sequence xyz = "XYZ" @@ -168,7 +172,7 @@ def change_sequence(new): update_all_sliders() # handle radio button on angle slider - def angle(index, ring): + def angle(index: Any, ring: Any) -> None: global sequence # print('angle', index, ring) @@ -231,7 +235,7 @@ def angle(index, ring): env.run(duration=0.5 if test else None) -def main(): +def main() -> None: demo(test="--test" in sys.argv[1:]) diff --git a/src/roboticstoolbox/demo/twistdemo.py b/src/roboticstoolbox/demo/twistdemo.py index 96147040a..3586bc863 100644 --- a/src/roboticstoolbox/demo/twistdemo.py +++ b/src/roboticstoolbox/demo/twistdemo.py @@ -4,6 +4,10 @@ @author Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import sys import swift from math import pi @@ -13,7 +17,7 @@ import numpy as np -def demo(test: bool = False): +def demo(test: bool = False) -> None: # TODO # rotate the rings according to the rotation axis, so that the axles # point the right way @@ -41,14 +45,14 @@ def demo(test: bool = False): twist_params = {"x": 0, "y": 0, "R": 0, "P": 0, "p": 0.1} twist = Twist3() - def update_twist_param(value, key): + def update_twist_param(value: Any, key: Any) -> None: nonlocal twist_params twist_params[key] = value update_twist() - def update_twist(): + def update_twist() -> None: nonlocal twist axis = SO3.Rx(twist_params["R"]) @ SO3.Ry(twist_params["P"]) @@ -59,7 +63,7 @@ def update_twist(): update_plane() # button to reset twist - def reset_twist(e): + def reset_twist(e: Any) -> None: nonlocal twist_params twist_params["x"] = 0 @@ -73,15 +77,15 @@ def reset_twist(e): update_plane() # env.step(0) - def update_plane(*args): + def update_plane(*args: Any) -> None: T = twist.SE3(twist_theta.value) * BASE plane.T = T - def update_screw_axis(axis, point): + def update_screw_axis(axis: Any, point: Any) -> None: screw.T = SE3.Rt(axis, point) - def update_all_sliders(): + def update_all_sliders() -> None: twist_x.value = twist_params["x"] twist_y.value = twist_params["y"] twist_rollangle.value = twist_params["R"] @@ -167,7 +171,7 @@ def update_all_sliders(): env.run(duration=0.5 if test else None) -def main(): +def main() -> None: demo(test="--test" in sys.argv[1:]) diff --git a/src/roboticstoolbox/ets/ET.py b/src/roboticstoolbox/ets/ET.py index cef5d2492..593e35950 100644 --- a/src/roboticstoolbox/ets/ET.py +++ b/src/roboticstoolbox/ets/ET.py @@ -4,6 +4,10 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import roboticstoolbox as rtb from numpy import array, ndarray, pi from spatialmath.base import trotx, troty, trotz @@ -12,16 +16,21 @@ # needs its own name here. from spatialmath import SE3 as SE3T +from typing import TYPE_CHECKING + from roboticstoolbox.ets._ET import BaseET, Sym, _AXIS_TO_INT, _resolve_param from roboticstoolbox.ets.fknm import ET_T, ET_init, ET_update +if TYPE_CHECKING: # pragma: nocover + from roboticstoolbox.ets.ETS import ETS + class ET(BaseET): # See BaseET._deepcopy_skip: the compiled acceleration handle is # rebuilt by _accel_init() rather than deep-copied. _deepcopy_skip = ("_ET__fknm",) - def __init__(self, **kwargs): + def __init__(self, **kwargs: Any) -> None: # Set before super().__init__() runs: BaseET.__init__ may invoke # the `param` setter (for a static ET), which calls _accel_update() # below. `None` here tells _accel_update() the compiled struct @@ -33,10 +42,10 @@ def __init__(self, **kwargs): # all final), do the one real build of the compiled struct. self._accel_init() - def __mul__(self, other: "ET") -> "rtb.ETS": - return rtb.ETS([self, other]) + def __mul__(self, other: "ET") -> "ETS": + return rtb.ETS([self, other]) # type: ignore[operator] - def __add__(self, other: "ET") -> "rtb.ETS": + def __add__(self, other: "ET") -> "ETS": return self.__mul__(other) # ------------------------------------------------------------------ @@ -113,7 +122,7 @@ def _accel_update(self) -> None: ) @property - def fknm(self): + def fknm(self) -> Any: return self.__fknm def A(self, q: float | Sym = 0.0) -> ndarray: @@ -169,7 +178,7 @@ def Rx( unit: str = "rad", *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET": """ Pure rotation about the x-axis @@ -204,7 +213,7 @@ def Ry( unit: str = "rad", *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET": """ Pure rotation about the y-axis @@ -239,7 +248,7 @@ def Rz( unit: str = "rad", *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET": """ Pure rotation about the z-axis @@ -273,7 +282,7 @@ def tx( param: float | Sym | str | None = None, *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET": """ Pure translation along the x-axis @@ -300,7 +309,7 @@ def tx( param = _resolve_param(param, eta) # this method is 3x faster than using lambda x: transl(x, 0, 0) - def axis_func(param): + def axis_func(param: Any) -> Any: # fmt: off return array([ [1, 0, 0, param], @@ -318,7 +327,7 @@ def ty( param: float | Sym | str | None = None, *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET": """ Pure translation along the y-axis @@ -344,7 +353,7 @@ def ty( """ param = _resolve_param(param, eta) - def axis_func(param): + def axis_func(param: Any) -> Any: # fmt: off return array([ [1, 0, 0, 0], @@ -362,7 +371,7 @@ def tz( param: float | Sym | str | None = None, *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET": """ Pure translation along the z-axis @@ -388,7 +397,7 @@ def tz( """ param = _resolve_param(param, eta) - def axis_func(param): + def axis_func(param: Any) -> Any: # fmt: off return array([ [1, 0, 0, 0], @@ -401,7 +410,7 @@ def axis_func(param): return cls(axis="tz", axis_func=axis_func, param=param, **kwargs) @classmethod - def SE3(cls, T: ndarray | SE3T, **kwargs) -> "ET": + def SE3(cls, T: ndarray | SE3T, **kwargs: Any) -> "ET": """ A static SE3 diff --git a/src/roboticstoolbox/ets/ET2.py b/src/roboticstoolbox/ets/ET2.py index 68a42c822..367861f38 100644 --- a/src/roboticstoolbox/ets/ET2.py +++ b/src/roboticstoolbox/ets/ET2.py @@ -4,6 +4,10 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import roboticstoolbox as rtb from numpy import array, ndarray from spatialmath.base import trot2, transl2 @@ -12,17 +16,22 @@ # needs its own name here. from spatialmath import SE2 as SE2T +from typing import TYPE_CHECKING + from roboticstoolbox.ets._ET import BaseET, Sym, _resolve_param +if TYPE_CHECKING: # pragma: nocover + from roboticstoolbox.ets.ETS2 import ETS2 + class ET2(BaseET): - def __init__(self, **kwargs): + def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - def __mul__(self, other: "ET2") -> "rtb.ETS2": - return rtb.ETS2([self, other]) + def __mul__(self, other: "ET2") -> "ETS2": + return rtb.ETS2([self, other]) # type: ignore[operator] - def __add__(self, other: "ET2") -> "rtb.ETS2": + def __add__(self, other: "ET2") -> "ETS2": return self.__mul__(other) @property @@ -43,7 +52,7 @@ def R( unit: str = "rad", *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET2": """ Pure rotation @@ -82,7 +91,7 @@ def tx( unit: str = "rad", *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET2": """ Pure translation along the x-axis @@ -115,7 +124,7 @@ def ty( unit: str = "rad", *, eta: float | None = None, - **kwargs, + **kwargs: Any, ) -> "ET2": """ Pure translation along the y-axis @@ -141,7 +150,7 @@ def ty( return cls(axis="ty", param=param, axis_func=lambda y: transl2(0, y), **kwargs) @classmethod - def SE2(cls, T: ndarray | SE2T, **kwargs) -> "ET2": + def SE2(cls, T: ndarray | SE2T, **kwargs: Any) -> "ET2": """ A static SE2 diff --git a/src/roboticstoolbox/ets/ETS.py b/src/roboticstoolbox/ets/ETS.py index 4ab2ccbf7..ebaa84fbd 100644 --- a/src/roboticstoolbox/ets/ETS.py +++ b/src/roboticstoolbox/ets/ETS.py @@ -39,13 +39,13 @@ from copy import deepcopy from roboticstoolbox.ets.ET import ET from roboticstoolbox.ets._ET import BaseET -from roboticstoolbox.ets._ETS import BaseETS, T, _dirties_fknm -from typing import overload, TypeVar +from roboticstoolbox.ets._ETS import BaseETS, _dirties_fknm +from typing import Any, Sequence, overload, TypeVar from typing import Literal as L from roboticstoolbox.tools.types import ArrayLike, NDArray -class ETS(BaseETS): +class ETS(BaseETS[ET]): """ This class implements an elementary transform sequence (ETS) for 3D @@ -93,9 +93,8 @@ class ETS(BaseETS): """ def __init__( - self, - arg: list[ETS | ET] | list[ET] | list[ETS] | ET | ETS | None = None, - ): + self, arg: list[ETS | ET] | list[ET] | list[ETS] | ET | ETS | None = None + ) -> None: super().__init__() if isinstance(arg, list): for item in arg: @@ -162,10 +161,10 @@ def __mul__(self, other: ET | ETS) -> "ETS": def __rmul__(self, other: ET | ETS) -> "ETS": return ETS([other, *self._data]) # pragma: nocover - def __imul__(self, rest: "ETS"): + def __imul__(self, rest: ET | ETS) -> "ETS": return self + rest # pragma: nocover - def __add__(self, rest) -> "ETS": + def __add__(self, rest: ET | ETS) -> "ETS": return self.__mul__(rest) # pragma: nocover def compile(self) -> "ETS": @@ -219,7 +218,7 @@ def compile(self) -> "ETS": ets *= ET.SE3(const) return ets - def insert( # type: ignore[override] + def insert( self, index: int, value: ET | ETS, @@ -309,7 +308,7 @@ def fkine( if fk.ndim == 3: for T in fk: - ret.append(SE3(T, check=False)) # type: ignore + ret.append(SE3(T, check=False)) else: ret = SE3(fk, check=False) @@ -613,7 +612,7 @@ def jacob0_analytical( q: ArrayLike, representation: str = "rpy/xyz", tool: NDArray | SE3 | None = None, - ): + ) -> Any: r""" Manipulator analytical Jacobian in the base frame @@ -704,10 +703,10 @@ def jacobm(self, q: ArrayLike) -> NDArray: def manipulability( self, - q, + q: Any, method: L["yoshikawa", "minsingular", "invcondition"] = "yoshikawa", axes: L["all", "trans", "rot"] | list[bool] = "all", - ): + ) -> Any: """ Manipulability measure @@ -782,7 +781,7 @@ def manipulability( else: raise ValueError("axes must be all, trans, rot or both") - def yoshikawa(robot, J, q, axes, **kwargs): + def yoshikawa(robot: Any, J: Any, q: Any, axes: Any, **kwargs: Any) -> Any: J = J[axes, :] if J.shape[0] == J.shape[1]: # simplified case for square matrix @@ -791,11 +790,11 @@ def yoshikawa(robot, J, q, axes, **kwargs): m2 = det(J @ J.T) return np.sqrt(abs(m2)) - def condition(robot, J, q, axes, **kwargs): + def condition(robot: Any, J: Any, q: Any, axes: Any, **kwargs: Any) -> Any: J = J[axes, :] return 1 / cond(J) - def minsingular(robot, J, q, axes, **kwargs): + def minsingular(robot: Any, J: Any, q: Any, axes: Any, **kwargs: Any) -> Any: J = J[axes, :] s = svd(J, compute_uv=False) return s[-1] # return last/smallest singular value of J @@ -878,9 +877,9 @@ def partial_fkine0(self, q: ArrayLike, n: int) -> NDArray: # The Hessian is the 2nd-order so we start with c = 2 c = 2 - def add_indices(indices, c): + def add_indices(indices: Any, c: Any) -> Any: total = len(indices * 2) - new_indices = [] + new_indices: list[Any] = [] for i in range(total): j = i // 2 @@ -897,9 +896,9 @@ def add_indices(indices, c): return new_indices - def add_pdi(pdi): + def add_pdi(pdi: Any) -> Any: total = len(pdi * 2) - new_pdi = [] + new_pdi: list[Any] = [] for i in range(total): j = i // 2 @@ -979,18 +978,18 @@ def add_pdi(pdi): idx1_n = idx1[0] # Use our indices to select the rotational column from pdr0 and pdr1 - col0_rot = pdr0[(*idx0_slices, slice(3, 6), idx0_n)] - col1_rot = pdr1[(*idx1_slices, slice(3, 6), idx1_n)] + col0_rot = pdr0[tuple(idx0_slices) + (slice(3, 6), idx0_n)] + col1_rot = pdr1[tuple(idx1_slices) + (slice(3, 6), idx1_n)] # Use our indices to select the translational column from pdr1 - col1_trn = pdr1[(*idx1_slices, slice(0, 3), idx1_n)] + col1_trn = pdr1[tuple(idx1_slices) + (slice(0, 3), idx1_n)] # Perform the cross product as described in the maths above rot += np.cross(col0_rot, col1_rot) trn += np.cross(col0_rot, col1_trn) - pd[(*np.flip(count[1:]), slice(0, 3), count[0])] = trn - pd[(*np.flip(count[1:]), slice(3, 6), count[0])] = rot + pd[tuple(np.flip(count[1:])) + (slice(0, 3), count[0])] = trn + pd[tuple(np.flip(count[1:])) + (slice(3, 6), count[0])] = rot count[0] += 1 for j in range(len(count)): @@ -1390,8 +1389,8 @@ def ikine_LM( km: float = 0.0, ps: float = 0.0, pi: NDArray | float = 0.3, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Levenberg-Marquardt numerical inverse kinematics solver @@ -1565,8 +1564,8 @@ def ikine_NR( km: float = 0.0, ps: float = 0.0, pi: NDArray | float = 0.3, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Newton-Raphson numerical inverse kinematics solver @@ -1684,8 +1683,8 @@ def ikine_GN( km: float = 0.0, ps: float = 0.0, pi: NDArray | float = 0.3, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Gauss-Newton numerical inverse kinematics solver @@ -1813,14 +1812,14 @@ def ikine_QP( mask: ArrayLike | None = None, joint_limits: bool = True, seed: int | None = None, - kj=1.0, - ks=1.0, + kj: float = 1.0, + ks: float = 1.0, kq: float = 0.0, km: float = 0.0, ps: float = 0.0, pi: NDArray | float = 0.3, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Quadratic programming numerical inverse kinematics solver @@ -1979,7 +1978,7 @@ def ikine_QP( return solver.solve(ets=self, Tep=Tep, q0=q0) @staticmethod - def _template_consume(elements: list["BaseET"], template: list[str]) -> int: + def _template_consume(elements: Sequence["BaseET"], template: list[str]) -> int: """ Greedily match a run of ETs against an ordered template of ``kind`` strings, where each template slot is optional but present slots must @@ -2000,7 +1999,9 @@ def _template_consume(elements: list["BaseET"], template: list[str]) -> int: break return consumed - def _split_convention(self, template: list[str], joint_slots: set[int], exposed: str): + def _split_convention( + self, template: list[str], joint_slots: set[int], exposed: str + ) -> Any: """ Split according to a DH-like convention. @@ -2045,11 +2046,11 @@ def joint_slot(et: "ET") -> int: end[i] = k + 1 + consumed if i + 1 < len(idx): - gap = self[end[i] : idx[i + 1]] + gap_ets = self[end[i] : idx[i + 1]] gap_consumed = self._template_consume( - list(gap), template[0 : slots[i + 1]] + list(gap_ets), template[0 : slots[i + 1]] ) - if gap_consumed != len(gap): + if gap_consumed != len(gap_ets): raise ValueError( "ETS is not a valid DH/MDH parameterisation: " f"unexpected term at index {end[i] + gap_consumed}" @@ -2125,7 +2126,7 @@ def split(self, method: str = "last") -> list["ETS"]: ["tx", "Rx", "Rz", "tz"], {2, 3}, exposed="tail" ) case _: - return super().split(method) # type: ignore + return super().split(method) - return [head] + segments + [tail] # type: ignore + return [head] + segments + [tail] diff --git a/src/roboticstoolbox/ets/ETS2.py b/src/roboticstoolbox/ets/ETS2.py index 1e5de3e1f..d5086cfd2 100644 --- a/src/roboticstoolbox/ets/ETS2.py +++ b/src/roboticstoolbox/ets/ETS2.py @@ -6,6 +6,8 @@ """ from __future__ import annotations + +from typing import Any from functools import cached_property import numpy as np from numpy.random import uniform @@ -39,13 +41,13 @@ from copy import deepcopy from roboticstoolbox.ets.ET2 import ET2 from roboticstoolbox.ets._ET import BaseET -from roboticstoolbox.ets._ETS import BaseETS, T, _dirties_fknm +from roboticstoolbox.ets._ETS import BaseETS, _dirties_fknm from typing import overload, TypeVar from typing import Literal as L from roboticstoolbox.tools.types import ArrayLike, NDArray -class ETS2(BaseETS): +class ETS2(BaseETS[ET2]): """ This class implements an elementary transform sequence (ETS) for 2D @@ -86,9 +88,8 @@ class ETS2(BaseETS): """ def __init__( - self, - arg: list[ETS2 | ET2] | list[ET2] | list[ETS2] | ET2 | ETS2 | None = None, - ): + self, arg: list[ETS2 | ET2] | list[ET2] | list[ETS2] | ET2 | ETS2 | None = None + ) -> None: super().__init__() if isinstance(arg, list): for item in arg: @@ -153,10 +154,10 @@ def __mul__(self, other: ET2 | ETS2) -> "ETS2": def __rmul__(self, other: ET2 | ETS2) -> "ETS2": return ETS2([other, *self._data]) # pragma: nocover - def __imul__(self, rest: "ETS2"): + def __imul__(self, rest: ET2 | ETS2) -> "ETS2": return self + rest # pragma: nocover - def __add__(self, rest) -> "ETS2": + def __add__(self, rest: ET2 | ETS2) -> "ETS2": return self.__mul__(rest) # pragma: nocover def compile(self) -> "ETS2": @@ -196,7 +197,7 @@ def compile(self) -> "ETS2": ets *= ET2.SE2(const) return ets - def insert( # type: ignore[override] + def insert( self, i: int, arg: ET2 | ETS2, @@ -270,7 +271,7 @@ def fkine( if fk.ndim == 3: for T in fk: - ret.append(SE2(T, check=False)) # type: ignore + ret.append(SE2(T, check=False)) else: ret = SE2(fk, check=False) @@ -310,7 +311,7 @@ def eval( """ q = self._resolve_q(q, allow_trajectory=True) - l, _ = q.shape # type: ignore + l, _ = q.shape end = self[-1] if base is None: @@ -336,7 +337,7 @@ def eval( else: T = np.zeros((3, 3), dtype=object) - for k, qk in enumerate(q): # type: ignore + for k, qk in enumerate(q): link = end # start with last link jindex = 0 if link.jindex is None and link.isjoint else link.jindex @@ -401,7 +402,7 @@ def jacob0( axis = self[i].kind if axis == "R": dTdq = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) @ self[i].A( - q[jindex] # type: ignore + q[jindex] ) elif axis == "tx": dTdq = np.array([[0, 0, 1], [0, 0, 0], [0, 0, 0]]) @@ -426,10 +427,7 @@ def jacob0( return J - def jacobe( - self, - q: ArrayLike, - ): + def jacobe(self, q: ArrayLike) -> Any: r""" Jacobian in end-effector frame diff --git a/src/roboticstoolbox/ets/_ET.py b/src/roboticstoolbox/ets/_ET.py index a3f2e61fe..ae5a4fb90 100644 --- a/src/roboticstoolbox/ets/_ET.py +++ b/src/roboticstoolbox/ets/_ET.py @@ -10,6 +10,10 @@ without a cycle). """ +from __future__ import annotations + +from typing import Any + import re import warnings from copy import deepcopy @@ -24,9 +28,7 @@ _AXIS_TO_INT: dict[str, int] = {"Rx": 0, "Ry": 1, "Rz": 2, "tx": 3, "ty": 4, "tz": 5} if TYPE_CHECKING: # pragma: nocover - import sympy - - Sym = sympy.core.symbol.Symbol # type: ignore + from sympy import Symbol as Sym else: # pragma: nocover Sym = None @@ -86,7 +88,7 @@ def __init__( qlim: ArrayLike | None = None, *, eta: float | None = None, - ): + ) -> None: param = _resolve_param(param, eta) self._kind = axis @@ -94,7 +96,7 @@ def __init__( # A custom joint display name, e.g. "theta2" from a string `param` # descriptor below - printed by __str__ in place of the generic # "q2" when set. - self._joint_name = None + self._joint_name: str | None = None # A string `param` that doesn't parse as a plain number is a joint # descriptor (e.g. "theta2", "-q(3)", "θ_3"): regex-parsed for @@ -103,16 +105,17 @@ def __init__( # not also be given explicitly in this case - one form or the # other, not a silent merge of both. if isinstance(param, str): + descriptor: str = param try: - param = float(param) + param = float(descriptor) except ValueError: if jindex is not None or flip is not None: raise ValueError( "cannot specify `jindex` or `flip` alongside a string " "joint descriptor for `param`" ) - jindex, flip = _parse_joint_descriptor(param) - self._joint_name = param + jindex, flip = _parse_joint_descriptor(descriptor) + self._joint_name = descriptor param = None flip = bool(flip) # None (not given, and not set above) -> False @@ -130,11 +133,13 @@ def __init__( self._flip = flip self._jindex = jindex + self._qlim: NDArray | None if qlim is not None: - self._qlim: NDArray | None = getvector(qlim, 2, out="array") + self._qlim = getvector(qlim, 2, out="array") else: - self._qlim: NDArray | None = None + self._qlim = None + self._param: float | Sym | None if param is None: self._param = None if T is None: @@ -149,12 +154,14 @@ def __init__( if axis[0] == "R" and unit.lower().startswith("deg"): if not issymbol(param): param = deg2rad(float(param)) + # a string `param` has been converted to a float, or to None above + assert not isinstance(param, str) # This is a static joint. The `param` setter validates axis_func, # computes `_T`, sets `_isstaticsym`/`_joint`, and (for ET) # syncs the compiled acceleration struct. self.param = param - def __str__(self): + def __str__(self) -> str: param_str = "" if self.isjoint: @@ -197,7 +204,7 @@ def __str__(self): return f"{self.kind}({param_str})" - def __repr__(self): + def __repr__(self) -> str: s_param = "" if self.param is None else f"param={self.param}" s_T = ( f"T={repr(self._T)}" @@ -219,7 +226,7 @@ def __repr__(self): return f"{start}.{self.kind}({s_kwargs})" - def _repr_pretty_(self, p, cycle): + def _repr_pretty_(self, p: Any, cycle: Any) -> None: """ Pretty string for IPython @@ -242,7 +249,7 @@ def _repr_pretty_(self, p, cycle): # Python ET2; overridden by ET. _deepcopy_skip: tuple[str, ...] = () - def __deepcopy__(self, memo): + def __deepcopy__(self, memo: Any) -> Any: cls = self.__class__ result = cls.__new__(cls) memo[id(self)] = result @@ -254,10 +261,10 @@ def __deepcopy__(self, memo): result._accel_init() return result - def __eq__(self, other): + def __eq__(self, other: Any) -> Any: return repr(self) == repr(other) - def __radd__(self, other): + def __radd__(self, other: Any) -> Any: # lets sum() work without an explicit start value, since its # default start is the int 0, which has no idea how to compose # with an ET/ET2 @@ -540,9 +547,9 @@ def qlim(self) -> ndarray | None: @qlim.setter def qlim(self, qlim_new: ArrayLike | None) -> None: - if qlim_new is not None: - qlim_new = getvector(qlim_new, 2, out="array") - self._qlim = qlim_new + self._qlim = ( + None if qlim_new is None else getvector(qlim_new, 2, out="array") + ) self._accel_update() @property @@ -572,7 +579,7 @@ def jindex(self) -> int | None: return self._jindex @jindex.setter - def jindex(self, j): + def jindex(self, j: Any) -> None: if not isinstance(j, int) or j < 0: raise ValueError(f"jindex is {j}, must be an int >= 0") self._jindex = j @@ -599,7 +606,7 @@ def iselementary(self) -> bool: return self.kind[0] != "S" - def inv(self): + def inv(self) -> Any: r""" Inverse of ET @@ -658,7 +665,7 @@ def A(self, q: float | Sym = 0.0) -> ndarray: """ if self.isjoint: if self.isflip: - q = -q # type: ignore + q = -q if self.axis_func is not None: return self.axis_func(q) diff --git a/src/roboticstoolbox/ets/_ETS.py b/src/roboticstoolbox/ets/_ETS.py index e70d6c9a5..c24e02a88 100644 --- a/src/roboticstoolbox/ets/_ETS.py +++ b/src/roboticstoolbox/ets/_ETS.py @@ -12,7 +12,7 @@ """ from __future__ import annotations -from collections.abc import MutableSequence +from collections.abc import Iterable, MutableSequence from functools import wraps, cached_property import numpy as np from numpy.random import uniform @@ -44,45 +44,63 @@ ) from copy import deepcopy from roboticstoolbox.ets._ET import BaseET -from typing import overload, TypeVar +from typing import Any, Callable, cast, overload, TypeVar, TYPE_CHECKING +from typing_extensions import Self from typing import Literal as L from roboticstoolbox.tools.types import ArrayLike, NDArray -T = TypeVar("T", bound="BaseETS") +if TYPE_CHECKING: # pragma: nocover + from roboticstoolbox.ets.ETS import ETS +#: the element type of an ETS: ``ET`` for ``ETS``, ``ET2`` for ``ETS2`` +ETT = TypeVar("ETT", bound=BaseET) +_F = TypeVar("_F", bound=Callable[..., Any]) -def _dirties_fknm(func): + +def _dirties_fknm(func: _F) -> _F: @wraps(func) - def wrapper(self, *args, **kwargs): + def wrapper(self: Any, *args: Any, **kwargs: Any) -> Any: result = func(self, *args, **kwargs) self._fknm_stale = True return result - return wrapper + + return wrapper # type: ignore[return-value] -class BaseETS(MutableSequence): - def __init__(self): - self._data: list = [] +class BaseETS(MutableSequence[ETT]): + def __init__(self) -> None: + self._data: list[ETT] = [] self._fknm_stale = True - self._BaseETS__fknm = None + self._BaseETS__fknm: Any = None # ------------------------------------------------------------------ # MutableSequence abstract methods # ------------------------------------------------------------------ + def _new(self, data: Iterable[ETT]) -> Self: + """A new ETS of the same concrete type as ``self`` from elements""" + # the concrete subclasses, ETS and ETS2, take the list of elements + return self.__class__(data) # type: ignore[call-arg] + def __len__(self) -> int: return len(self._data) + @overload + def __setitem__(self, i: int, value: ETT) -> None: ... + + @overload + def __setitem__(self, i: slice, value: Iterable[ETT]) -> None: ... + @_dirties_fknm - def __setitem__(self, i, value): + def __setitem__(self, i: Any, value: Any) -> None: self._data[i] = value @_dirties_fknm - def __delitem__(self, i): + def __delitem__(self, i: int | slice) -> None: del self._data[i] @_dirties_fknm - def insert(self, index: int, value) -> None: + def insert(self, index: int, value: ETT) -> None: self._data.insert(index, value) def __repr__(self) -> str: @@ -95,7 +113,7 @@ def __eq__(self, other: object) -> bool: __hash__ = None # type: ignore[assignment] - def __radd__(self, other): + def __radd__(self, other: Any) -> Any: # lets sum() work without an explicit start value, since its # default start is the int 0, which has no idea how to compose # with an ETS/ETS2 @@ -108,20 +126,20 @@ def __radd__(self, other): # ------------------------------------------------------------------ @property - def _fknm(self): + def _fknm(self) -> Any: if self._fknm_stale: self._copy_to_cpp() return self._BaseETS__fknm - def _copy_to_cpp(self): + def _copy_to_cpp(self) -> None: self._BaseETS__fknm = ETS_init( - [et.fknm for et in self._data], + [et.fknm for et in self._data], # type: ignore[attr-defined] self.n, self.m, ) self._fknm_stale = False - def __str__(self, q: str | None = None): + def __str__(self, q: str | None = None) -> str: """ Pretty prints the ETS @@ -174,7 +192,7 @@ def __str__(self, q: str | None = None): es = [] j = 0 c = 0 - s = None + s = "" unicode = rtb_get_param("unicode") # An empty SE3 @@ -222,7 +240,7 @@ def __str__(self, q: str | None = None): if issymbol(et.param): s = f"{et.kind}({et.param})" else: - s = f"{et.kind}({et.param * 180 / np.pi:.4g}°)" + s = f"{et.kind}({cast(float, et.param) * 180 / np.pi:.4g}°)" elif et.istranslation: try: @@ -241,7 +259,7 @@ def __str__(self, q: str | None = None): else: # pragma: nocover return " * ".join(es) - def _repr_pretty_(self, p, cycle): + def _repr_pretty_(self, p: Any, cycle: bool) -> None: """ Pretty string for IPython @@ -261,7 +279,7 @@ def _repr_pretty_(self, p, cycle): print(self.__str__()) # pragma: nocover - def joint_idx(self) -> list[int]: + def joint_idx(self) -> NDArray: """ Get index of joint transforms @@ -279,9 +297,9 @@ def joint_idx(self) -> list[int]: """ - return np.where([e.isjoint for e in self])[0] # type: ignore + return np.where([e.isjoint for e in self])[0] - def joints(self) -> list[BaseET]: + def joints(self) -> list[ETT]: """ Get a list of the variable ETs with this ETS @@ -301,7 +319,7 @@ def joints(self) -> list[BaseET]: return [e for e in self if e.isjoint] - def split(self, method: str = "last") -> list["BaseETS"]: + def split(self, method: str = "last") -> list[Self]: """ Split ETS into link segments @@ -331,7 +349,7 @@ def split(self, method: str = "last") -> list["BaseETS"]: >>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}") """ - segments = [] + segments: list[Self] = [] joint_idx = self.joint_idx() head = self[0:0] tail = self[0:0] @@ -344,19 +362,19 @@ def split(self, method: str = "last") -> list["BaseETS"]: for k in joint_idx[1:]: ets_j = self[start: k] start = k - segments.append(self.__class__(ets_j)) - segments.append(self.__class__(self[joint_idx[-1]:])) + segments.append(self._new(ets_j)) + segments.append(self._new(self[joint_idx[-1]:])) case "last": start = 0 for k in joint_idx: ets_j = self[start : k + 1] start = k + 1 - segments.append(self.__class__(ets_j)) + segments.append(self._new(ets_j)) tail = self[start:] case _: raise ValueError(f"unknown split method '{method}'") - return [head] + segments + [tail] # type: ignore + return [head] + segments + [tail] def jindex_set(self) -> set[int]: # """ @@ -376,7 +394,7 @@ def jindex_set(self) -> set[int]: # """ - return set([self[j].jindex for j in self.joint_idx()]) # type: ignore + return set([self[j].jindex for j in self.joint_idx()]) @cached_property def jindices(self) -> NDArray: @@ -397,7 +415,7 @@ def jindices(self) -> NDArray: """ - return np.array([j.jindex for j in self.joints()]) # type: ignore + return np.array([j.jindex for j in self.joints()]) def _resolve_q(self, q: ArrayLike, allow_trajectory: bool = False) -> NDArray: """ @@ -435,7 +453,9 @@ def _resolve_q(self, q: ArrayLike, allow_trajectory: bool = False) -> NDArray: n = self.n jindices = self.jindices - q = getmatrix(q, (None, None)) if allow_trajectory else getvector(q, None) + q = np.asarray( + getmatrix(q, (None, None)) if allow_trajectory else getvector(q, None) + ) if n == 0 or jindices.dtype == object: # no joints, or at least one joint has no jindex assigned yet @@ -472,7 +492,7 @@ def _resolve_q(self, q: ArrayLike, allow_trajectory: bool = False) -> NDArray: ) @property - def qlim(self): + def qlim(self) -> NDArray: r""" Get/Set Joint limits @@ -501,6 +521,7 @@ def qlim(self): limits = np.zeros((2, self.n)) + v: ArrayLike for i, et in enumerate(self.joints()): if et.isrotation: if et.qlim is None: @@ -519,7 +540,7 @@ def qlim(self): return limits @qlim.setter - def qlim(self, new_qlim: ArrayLike): + def qlim(self, new_qlim: ArrayLike) -> None: new_qlim = np.array(new_qlim) if new_qlim.shape == (2,) and self.n == 1: @@ -609,30 +630,16 @@ def m(self) -> int: return len(self._data) - @overload - def data(self: "ETS") -> list[ET]: ... # pragma: nocover - - @overload - def data(self: "ETS2") -> list[ET2]: ... # pragma: nocover - @property - def data(self): + def data(self) -> list[ETT]: return self._data @data.setter - @overload - def data(self: "ETS", new_data: list[ET]): ... # pragma: nocover - - @data.setter - @overload - def data(self: "ETS", new_data: list[ET2]): ... # pragma: nocover - - @data.setter - def data(self, new_data): + def data(self, new_data: list[ETT]) -> None: self._data = new_data self._fknm_stale = True - def inv(self: T) -> T: + def inv(self) -> Self: r""" Inverse of ETS @@ -662,24 +669,15 @@ def inv(self: T) -> T: """ - return self.__class__([et.inv() for et in reversed(self._data)]) # type: ignore[call-arg] - - @overload - def __getitem__(self: "BaseETS", i: int) -> BaseET: ... - - @overload - def __getitem__(self: "ETS", i: int) -> ET: ... - - @overload - def __getitem__(self: "ETS", i: slice) -> "ETS": ... + return self._new([et.inv() for et in reversed(self._data)]) @overload - def __getitem__(self: "ETS2", i: int) -> ET2: ... + def __getitem__(self, i: int) -> ETT: ... @overload - def __getitem__(self: "ETS2", i: slice) -> "ETS2": ... + def __getitem__(self, i: slice) -> Self: ... - def __getitem__(self, i): + def __getitem__(self, i: int | slice) -> ETT | Self: """ Index or slice an ETS @@ -701,21 +699,20 @@ def __getitem__(self, i): """ if isinstance(i, slice): - return self.__class__(self._data[i]) + return self._new(self._data[i]) return self._data[i] - def __deepcopy__(self, memo): + def __deepcopy__(self, memo: dict[int, Any]) -> Self: new_data = [] for data in self: new_data.append(deepcopy(data)) - cls = self.__class__ - result = cls(new_data) + result = self._new(new_data) memo[id(self)] = result return result - def plot(self, *args, **kwargs): + def plot(self, *args: Any, **kwargs: Any) -> None: from roboticstoolbox.robot.Robot import Robot, Robot2 # Deferred (like Robot/Robot2 above): BaseETS can't import the # concrete ETS without a cycle (ETS.py imports BaseETS from here). @@ -728,7 +725,7 @@ def plot(self, *args, **kwargs): robot.plot(*args, **kwargs) - def teach(self, *args, **kwargs): + def teach(self, *args: Any, **kwargs: Any) -> None: from roboticstoolbox.robot.Robot import Robot, Robot2 from roboticstoolbox.ets.ETS import ETS @@ -847,7 +844,7 @@ def merge(self, i: int) -> None: raise ValueError("Transforms are both joints") # pragma: nocover else: - self._data[i].param = e1.param + e2.param + self._data[i].param = e1.param + e2.param # type: ignore[operator] del self._data[i + 1] self._fknm_stale = True diff --git a/src/roboticstoolbox/ets/fknm.py b/src/roboticstoolbox/ets/fknm.py index 1a212e8ce..1e8f7853d 100644 --- a/src/roboticstoolbox/ets/fknm.py +++ b/src/roboticstoolbox/ets/fknm.py @@ -9,12 +9,14 @@ from __future__ import annotations +from typing import Any + import numpy as np from spatialmath import SE3 from spatialmath.base import getvector, getmatrix, verifymatrix, tr2jac try: - from roboticstoolbox._fknm_c import ( # type: ignore[import] + from roboticstoolbox._fknm_c import ( ETS_init as _c_ETS_init, ETS_fkine as _c_ETS_fkine, ETS_jacob0 as _c_ETS_jacob0, @@ -37,7 +39,7 @@ _C_AVAILABLE = False -def _is_symbolic(q) -> bool: +def _is_symbolic(q: Any) -> bool: """True when q contains SymPy symbols (object dtype array).""" if q is None: return False @@ -49,7 +51,7 @@ def _is_symbolic(q) -> bool: # --------------------------------------------------------------------------- -def _python_fkine(data, q, base, tool, include_base): +def _python_fkine(data: Any, q: Any, base: Any, tool: Any, include_base: Any) -> Any: q = getmatrix(q, (None, None)) l, _ = q.shape # noqa: E741 end = data[-1] @@ -105,7 +107,7 @@ def _python_fkine(data, q, base, tool, include_base): return T -def _python_jacob0(data, n, q, tool): +def _python_jacob0(data: Any, n: Any, q: Any, tool: Any) -> Any: if tool is None: tools = np.eye(4) elif isinstance(tool, SE3): @@ -173,13 +175,13 @@ def _python_jacob0(data, n, q, tool): return J # symbolic elements — keep as object -def _python_jacobe(data, n, q, tool): +def _python_jacobe(data: Any, n: Any, q: Any, tool: Any) -> Any: T = _python_fkine(data, q, None, tool, False) return tr2jac(T.T) @ _python_jacob0(data, n, q, tool) -def _python_hessian(J): - def cross(a, b): +def _python_hessian(J: Any) -> Any: + def cross(a: Any, b: Any) -> Any: return np.array( [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]] ) @@ -200,14 +202,14 @@ def cross(a, b): # --------------------------------------------------------------------------- -def ETS_init(fknm_data, n, m): +def ETS_init(fknm_data: Any, n: Any, m: Any) -> Any: """Create the C++ ETS handle; returns None when C is absent or any ET is symbolic.""" if _C_AVAILABLE and all(h is not None for h in fknm_data): return _c_ETS_init(fknm_data, n, m) return None -def _to_q_array(q) -> np.ndarray: +def _to_q_array(q: Any) -> np.ndarray: """Convert q to C-contiguous float64 ndarray; raises TypeError for non-numeric.""" try: return np.ascontiguousarray(q, dtype=float) @@ -215,7 +217,7 @@ def _to_q_array(q) -> np.ndarray: raise TypeError(f"q must be numeric, not {type(q).__name__}") -def _to_f_array(x): +def _to_f_array(x: Any) -> Any: """Convert SE3 or ndarray to float64 ndarray; pass through None.""" if x is None: return None @@ -224,7 +226,9 @@ def _to_f_array(x): return np.asarray(x, dtype=float) -def ETS_fkine(fknm, q, base, tool, include_base, _data=None): +def ETS_fkine( + fknm: Any, q: Any, base: Any, tool: Any, include_base: Any, _data: Any = None +) -> Any: if fknm is not None and not _is_symbolic(q): return _c_ETS_fkine( fknm, _to_q_array(q), @@ -233,19 +237,21 @@ def ETS_fkine(fknm, q, base, tool, include_base, _data=None): return _python_fkine(_data, q, base, tool, include_base) -def ETS_jacob0(fknm, q, tool, _data=None, _n=None): +def ETS_jacob0(fknm: Any, q: Any, tool: Any, _data: Any = None, _n: Any = None) -> Any: if fknm is not None and not _is_symbolic(q): return _c_ETS_jacob0(fknm, _to_q_array(q), _to_f_array(tool)) return _python_jacob0(_data, _n, q, tool) -def ETS_jacobe(fknm, q, tool, _data=None, _n=None): +def ETS_jacobe(fknm: Any, q: Any, tool: Any, _data: Any = None, _n: Any = None) -> Any: if fknm is not None and not _is_symbolic(q): return _c_ETS_jacobe(fknm, _to_q_array(q), _to_f_array(tool)) return _python_jacobe(_data, _n, q, tool) -def ETS_hessian0(fknm, q, J0, tool, _data=None, _n=None): +def ETS_hessian0( + fknm: Any, q: Any, J0: Any, tool: Any, _data: Any = None, _n: Any = None +) -> Any: if fknm is not None and not _is_symbolic(q) and not _is_symbolic(J0): return _c_ETS_hessian0(fknm, _to_q_array(q), J0, _to_f_array(tool)) if J0 is None: @@ -258,7 +264,9 @@ def ETS_hessian0(fknm, q, J0, tool, _data=None, _n=None): return _python_hessian(J0) -def ETS_hessiane(fknm, q, Je, tool, _data=None, _n=None): +def ETS_hessiane( + fknm: Any, q: Any, Je: Any, tool: Any, _data: Any = None, _n: Any = None +) -> Any: if fknm is not None and not _is_symbolic(q) and not _is_symbolic(Je): return _c_ETS_hessiane(fknm, _to_q_array(q), Je, _to_f_array(tool)) if Je is None: @@ -276,7 +284,7 @@ def ETS_hessiane(fknm, q, Je, tool, _data=None, _n=None): # --------------------------------------------------------------------------- -def _ik_args_se3(args): +def _ik_args_se3(args: Any) -> Any: """Convert Tep (first positional arg) from SE3 to ndarray if needed.""" if not args: return args @@ -286,7 +294,7 @@ def _ik_args_se3(args): return (tep,) + args[1:] -def IK_NR_c(fknm, *args, **kwargs): +def IK_NR_c(fknm: Any, *args: Any, **kwargs: Any) -> Any: if fknm is None: raise RuntimeError( "ik_NR requires the _fknm_c C extension; use ikine_NR for the Python solver" @@ -294,7 +302,7 @@ def IK_NR_c(fknm, *args, **kwargs): return _c_IK_NR_c(fknm, *_ik_args_se3(args), **kwargs) -def IK_GN_c(fknm, *args, **kwargs): +def IK_GN_c(fknm: Any, *args: Any, **kwargs: Any) -> Any: if fknm is None: raise RuntimeError( "ik_GN requires the _fknm_c C extension; use ikine_GN for the Python solver" @@ -302,7 +310,7 @@ def IK_GN_c(fknm, *args, **kwargs): return _c_IK_GN_c(fknm, *_ik_args_se3(args), **kwargs) -def IK_LM_c(fknm, *args, **kwargs): +def IK_LM_c(fknm: Any, *args: Any, **kwargs: Any) -> Any: if fknm is None: raise RuntimeError( "ik_LM requires the _fknm_c C extension; use ikine_LM for the Python solver" @@ -315,7 +323,7 @@ def IK_LM_c(fknm, *args, **kwargs): # --------------------------------------------------------------------------- -def ET_init(*args): +def ET_init(*args: Any) -> Any: """Create a C ET handle; returns None when C extension is absent or inputs are symbolic.""" if _C_AVAILABLE: try: @@ -325,14 +333,14 @@ def ET_init(*args): return None -def ET_T(et_handle, q): +def ET_T(et_handle: Any, q: Any) -> Any: """Evaluate an ET at q via C. Raises TypeError when C absent so ET.A() falls back.""" if et_handle is None: raise TypeError("_fknm_c not available") return _c_ET_T(et_handle, q) -def ET_update(et_handle, *args): +def ET_update(et_handle: Any, *args: Any) -> None: """Update a C ET handle in-place; no-op when handle is None or inputs are symbolic.""" if et_handle is not None: try: @@ -346,20 +354,20 @@ def ET_update(et_handle, *args): # --------------------------------------------------------------------------- -def Angle_Axis(T, Td): +def Angle_Axis(T: Any, Td: Any) -> Any: """C angle-axis error; raises when C absent (p_servo.angle_axis has a Python fallback).""" if not _C_AVAILABLE: raise RuntimeError("_fknm_c not available") return _c_Angle_Axis(T, Td) -def Robot_link_T(link_ets, link_scene_nodes, q0, q): +def Robot_link_T(link_ets: Any, link_scene_nodes: Any, q0: Any, q: Any) -> None: """Update per-link scene-node transforms via C; no-op when C absent (visualisation only).""" if _C_AVAILABLE: _c_Robot_link_T(link_ets, link_scene_nodes, q0, q) -def r2q(R): +def r2q(R: Any) -> Any: """Convert rotation matrix to quaternion via C; raises when C absent.""" if not _C_AVAILABLE: raise RuntimeError("_fknm_c not available") diff --git a/src/roboticstoolbox/models/DH/AL5D.py b/src/roboticstoolbox/models/DH/AL5D.py index 942becc11..771e3d37d 100644 --- a/src/roboticstoolbox/models/DH/AL5D.py +++ b/src/roboticstoolbox/models/DH/AL5D.py @@ -2,6 +2,10 @@ @author: Tassos Natsakis """ +from __future__ import annotations + +from typing import Any + import numpy as np from roboticstoolbox import DHRobot, RevoluteMDH from spatialmath import SE3 @@ -38,7 +42,7 @@ class AL5D(DHRobot): .. codeauthor:: Tassos Natsakis """ # noqa - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/Ball.py b/src/roboticstoolbox/models/DH/Ball.py index 52a91b252..9b2d402e0 100755 --- a/src/roboticstoolbox/models/DH/Ball.py +++ b/src/roboticstoolbox/models/DH/Ball.py @@ -4,6 +4,10 @@ @author: Samuel Drew """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH from math import pi import numpy as np @@ -49,10 +53,10 @@ class Ball(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self, N=10): + def __init__(self, N: int = 10) -> None: links = [] - q1 = [] + q1: list[float] = [] for i in range(N): links.append(RevoluteDH(a=0.1, alpha=pi / 2)) @@ -67,7 +71,7 @@ def __init__(self, N=10): self.addconfiguration("qz", self.qz) -def _fract(i): +def _fract(i: Any) -> Any: # i is "i-1" as per the paper theta1 = 1 theta2 = -2 / 3 diff --git a/src/roboticstoolbox/models/DH/Baxter.py b/src/roboticstoolbox/models/DH/Baxter.py index ca18b3bca..0692ba2ae 100644 --- a/src/roboticstoolbox/models/DH/Baxter.py +++ b/src/roboticstoolbox/models/DH/Baxter.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any import numpy as np from math import pi from roboticstoolbox import DHRobot, RevoluteDH @@ -44,7 +48,7 @@ class Baxter(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, arm="left"): + def __init__(self, arm: str = "left") -> None: links = [ RevoluteDH(d=0.27, a=0.069, alpha=-pi / 2), diff --git a/src/roboticstoolbox/models/DH/Cobra600.py b/src/roboticstoolbox/models/DH/Cobra600.py index 55b46cbbe..a56733446 100755 --- a/src/roboticstoolbox/models/DH/Cobra600.py +++ b/src/roboticstoolbox/models/DH/Cobra600.py @@ -4,6 +4,10 @@ @author: Samuel Drew """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH from math import pi import numpy as np @@ -34,7 +38,7 @@ class Cobra600(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: deg = pi / 180 L = [ diff --git a/src/roboticstoolbox/models/DH/Coil.py b/src/roboticstoolbox/models/DH/Coil.py index 96ae9a1a5..693f43861 100644 --- a/src/roboticstoolbox/models/DH/Coil.py +++ b/src/roboticstoolbox/models/DH/Coil.py @@ -3,6 +3,10 @@ @author: Peter Corke """ +from __future__ import annotations + +from typing import Any + import numpy as np from roboticstoolbox import DHRobot, RevoluteDH @@ -46,7 +50,7 @@ class Coil(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, N=10, symbolic=False): + def __init__(self, N: int = 10, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/Hyper.py b/src/roboticstoolbox/models/DH/Hyper.py index 7609a600a..1cbd93f6b 100644 --- a/src/roboticstoolbox/models/DH/Hyper.py +++ b/src/roboticstoolbox/models/DH/Hyper.py @@ -3,6 +3,10 @@ @author: Peter Corke """ +from __future__ import annotations + +from typing import Any + # from math import pi import numpy as np from roboticstoolbox import DHRobot, RevoluteDH @@ -46,7 +50,7 @@ class Hyper(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, N=10, a=None, symbolic=False): + def __init__(self, N: int = 10, a: Any = None, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/Hyper3d.py b/src/roboticstoolbox/models/DH/Hyper3d.py index 1db199722..41d3705e3 100644 --- a/src/roboticstoolbox/models/DH/Hyper3d.py +++ b/src/roboticstoolbox/models/DH/Hyper3d.py @@ -3,6 +3,10 @@ @author: Peter Corke """ +from __future__ import annotations + +from typing import Any + # from math import pi import numpy as np from roboticstoolbox import DHRobot, RevoluteDH @@ -46,7 +50,7 @@ class Hyper3d(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, N=10, a=None, symbolic=False): + def __init__(self, N: int = 10, a: Any = None, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/IRB140.py b/src/roboticstoolbox/models/DH/IRB140.py index 5aa3e75c4..7cb6a6065 100644 --- a/src/roboticstoolbox/models/DH/IRB140.py +++ b/src/roboticstoolbox/models/DH/IRB140.py @@ -3,6 +3,10 @@ @author: Samuel Drew """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH from math import pi import numpy as np @@ -40,7 +44,7 @@ class IRB140(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self): + def __init__(self) -> None: deg = pi / 180 # robot length values (metres) diff --git a/src/roboticstoolbox/models/DH/Jaco.py b/src/roboticstoolbox/models/DH/Jaco.py index 7d72b611a..db1fabfc0 100644 --- a/src/roboticstoolbox/models/DH/Jaco.py +++ b/src/roboticstoolbox/models/DH/Jaco.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python from math import sin, cos @@ -39,7 +43,7 @@ class Jaco(DHRobot): :seealso: :func:`Mico` """ - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/KR5.py b/src/roboticstoolbox/models/DH/KR5.py index 210eb4c8c..90fbe0ced 100644 --- a/src/roboticstoolbox/models/DH/KR5.py +++ b/src/roboticstoolbox/models/DH/KR5.py @@ -5,6 +5,10 @@ @author: Samuel Drew """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH from math import pi import numpy as np @@ -42,7 +46,7 @@ class KR5(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self): + def __init__(self) -> None: deg = pi / 180 # Updated values form ARTE git. Old values left as comments diff --git a/src/roboticstoolbox/models/DH/LWR4.py b/src/roboticstoolbox/models/DH/LWR4.py index 5001a001f..ab48c82f1 100755 --- a/src/roboticstoolbox/models/DH/LWR4.py +++ b/src/roboticstoolbox/models/DH/LWR4.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -32,7 +36,7 @@ class LWR4(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self): + def __init__(self) -> None: # deg = np.pi/180 mm = 1e-3 diff --git a/src/roboticstoolbox/models/DH/Mico.py b/src/roboticstoolbox/models/DH/Mico.py index 32daaa999..673fa98d8 100644 --- a/src/roboticstoolbox/models/DH/Mico.py +++ b/src/roboticstoolbox/models/DH/Mico.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python from math import pi, sin, cos @@ -39,7 +43,7 @@ class Mico(DHRobot): :seealso: :func:`Mico` """ - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/Orion5.py b/src/roboticstoolbox/models/DH/Orion5.py index 114274d66..3b96fe686 100755 --- a/src/roboticstoolbox/models/DH/Orion5.py +++ b/src/roboticstoolbox/models/DH/Orion5.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python # Created by: Aditya Dua # 5 October 2017 @@ -41,7 +45,7 @@ class Orion5(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self, base=None): + def __init__(self, base: Any = None) -> None: mm = 1e-3 deg = pi / 180 diff --git a/src/roboticstoolbox/models/DH/P8.py b/src/roboticstoolbox/models/DH/P8.py index 1c2dc790c..00485c62d 100755 --- a/src/roboticstoolbox/models/DH/P8.py +++ b/src/roboticstoolbox/models/DH/P8.py @@ -4,6 +4,10 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + # 2/8/95 changed D3 to 150.05mm which is closer to data from Lee, AKB86 and # Tarn fixed errors in COG for links 2 and 3 # 29/1/91 to agree with data from Armstrong etal. Due to their use @@ -50,7 +54,7 @@ class P8(DHRobot): :seealso: :func:`models.DH.Puma560` """ # noqa - def __init__(self): + def __init__(self) -> None: # create the base links = [ diff --git a/src/roboticstoolbox/models/DH/Panda.py b/src/roboticstoolbox/models/DH/Panda.py index 7d242c737..2248635a7 100755 --- a/src/roboticstoolbox/models/DH/Panda.py +++ b/src/roboticstoolbox/models/DH/Panda.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class Panda(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: # deg = np.pi/180 mm = 1e-3 diff --git a/src/roboticstoolbox/models/DH/Planar2.py b/src/roboticstoolbox/models/DH/Planar2.py index 96d47a959..8b418f7e3 100644 --- a/src/roboticstoolbox/models/DH/Planar2.py +++ b/src/roboticstoolbox/models/DH/Planar2.py @@ -4,6 +4,10 @@ @author: Samuel Drew """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH import numpy as np @@ -35,14 +39,14 @@ class Planar2(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym zero = sym.zero() pi = sym.pi() - a1, a2 = sym.symbol("a1 a2") # type: ignore + a1, a2 = sym.symbol("a1 a2") else: from math import pi diff --git a/src/roboticstoolbox/models/DH/Planar3.py b/src/roboticstoolbox/models/DH/Planar3.py index 22aaf0b55..1d0e35086 100644 --- a/src/roboticstoolbox/models/DH/Planar3.py +++ b/src/roboticstoolbox/models/DH/Planar3.py @@ -4,6 +4,10 @@ @author: Samuel Drew """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH import numpy as np @@ -33,7 +37,7 @@ class Planar3(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: L = [RevoluteDH(a=1), RevoluteDH(a=1), RevoluteDH(a=1)] diff --git a/src/roboticstoolbox/models/DH/Puma560.py b/src/roboticstoolbox/models/DH/Puma560.py index a908815dd..f8f8dca90 100755 --- a/src/roboticstoolbox/models/DH/Puma560.py +++ b/src/roboticstoolbox/models/DH/Puma560.py @@ -4,6 +4,10 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + # 2/8/95 changed D3 to 150.05mm which is closer to data from Lee, AKB86 and # Tarn fixed errors in COG for links 2 and 3 # 29/1/91 to agree with data from Armstrong etal. Due to their use @@ -73,7 +77,7 @@ class Puma560(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym @@ -271,7 +275,7 @@ def ikine_a( config = self.config_validate(config, ("lr", "ud", "nf")) - def ik3(robot, T, config="lun"): + def ik3(robot: Any, T: Any, config: str = "lun") -> Any: # solve for the first three joints @@ -428,7 +432,7 @@ def ik3(robot, T, config="lun"): if __name__ == "__main__": # pragma nocover puma = Puma560(symbolic=False) print(puma) - print(puma.dynamics()) + print(puma.dynamics()) # type: ignore[func-returns-value] # T = puma.fkine(puma.qn) # print(puma.ikine_a(T, 'lu').q) # print(puma.ikine_a(T, 'ru').q) diff --git a/src/roboticstoolbox/models/DH/Sawyer.py b/src/roboticstoolbox/models/DH/Sawyer.py index 51dc282f1..38e912d7a 100644 --- a/src/roboticstoolbox/models/DH/Sawyer.py +++ b/src/roboticstoolbox/models/DH/Sawyer.py @@ -3,6 +3,10 @@ @author: Peter Corke """ +from __future__ import annotations + +from typing import Any + import numpy as np from roboticstoolbox import DHRobot, RevoluteDH @@ -36,7 +40,7 @@ class Sawyer(DHRobot): """ - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/Stanford.py b/src/roboticstoolbox/models/DH/Stanford.py index 61d23c8b0..b2347c812 100755 --- a/src/roboticstoolbox/models/DH/Stanford.py +++ b/src/roboticstoolbox/models/DH/Stanford.py @@ -3,6 +3,10 @@ @author: Peter Corke """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH from math import pi import numpy as np @@ -44,7 +48,7 @@ class Stanford(DHRobot): """ - def __init__(self): + def __init__(self) -> None: deg = pi / 180 inch = 0.0254 diff --git a/src/roboticstoolbox/models/DH/TwoLink.py b/src/roboticstoolbox/models/DH/TwoLink.py index 0bbf24d73..0ed4fb3f6 100644 --- a/src/roboticstoolbox/models/DH/TwoLink.py +++ b/src/roboticstoolbox/models/DH/TwoLink.py @@ -2,6 +2,10 @@ @author: Peter Corke """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox import DHRobot, RevoluteDH, RevoluteMDH # from math import pi @@ -59,7 +63,9 @@ class TwoLink(DHRobot): .. codeauthor:: Peter Corke """ - def __init__(self, symbolic:bool=False, mdh:bool=False, inertia:bool=False): + def __init__( + self, symbolic: bool = False, mdh: bool = False, inertia: bool = False + ) -> None: if symbolic: import spatialmath.base.symbolic as sym @@ -142,7 +148,7 @@ def __init__(self, symbolic:bool=False, mdh:bool=False, inertia:bool=False): self.gravity = [0, 0, g] -def _cylinder_inertia_x(m, r, L): +def _cylinder_inertia_x(m: Any, r: Any, L: Any) -> Any: ixx = 0.5 * m * r**2 iyy = (1.0 / 12.0) * m * (3 * r**2 + L**2) izz = iyy diff --git a/src/roboticstoolbox/models/DH/UR10.py b/src/roboticstoolbox/models/DH/UR10.py index 61075129e..8d52ec9d1 100644 --- a/src/roboticstoolbox/models/DH/UR10.py +++ b/src/roboticstoolbox/models/DH/UR10.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any import numpy as np from roboticstoolbox import DHRobot, RevoluteDH from spatialmath import SE3 @@ -43,7 +47,7 @@ class UR10(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/UR3.py b/src/roboticstoolbox/models/DH/UR3.py index 8b409f21f..450e04130 100644 --- a/src/roboticstoolbox/models/DH/UR3.py +++ b/src/roboticstoolbox/models/DH/UR3.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any import numpy as np from roboticstoolbox import DHRobot, RevoluteDH from spatialmath import SE3 @@ -38,7 +42,7 @@ class UR3(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/DH/UR5.py b/src/roboticstoolbox/models/DH/UR5.py index 85a2943b4..e0b27b6a0 100644 --- a/src/roboticstoolbox/models/DH/UR5.py +++ b/src/roboticstoolbox/models/DH/UR5.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any import numpy as np from roboticstoolbox import DHRobot, RevoluteDH from spatialmath import SE3 @@ -38,7 +42,7 @@ class UR5(DHRobot): .. codeauthor:: Peter Corke """ # noqa - def __init__(self, symbolic=False): + def __init__(self, symbolic: bool = False) -> None: if symbolic: import spatialmath.base.symbolic as sym diff --git a/src/roboticstoolbox/models/ETS/Frankie.py b/src/roboticstoolbox/models/ETS/Frankie.py index 1d9fa1ac6..9d8cb1fad 100644 --- a/src/roboticstoolbox/models/ETS/Frankie.py +++ b/src/roboticstoolbox/models/ETS/Frankie.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -35,7 +39,7 @@ class Frankie(Robot): Sequence, J. Haviland and P. Corke """ - def __init__(self): + def __init__(self) -> None: deg = np.pi / 180 mm = 1e-3 diff --git a/src/roboticstoolbox/models/ETS/GenericSeven.py b/src/roboticstoolbox/models/ETS/GenericSeven.py index 42347105c..1eed52203 100644 --- a/src/roboticstoolbox/models/ETS/GenericSeven.py +++ b/src/roboticstoolbox/models/ETS/GenericSeven.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -16,7 +20,7 @@ class GenericSeven(Robot): """ - def __init__(self): + def __init__(self) -> None: deg = np.pi / 180 mm = 1e-3 diff --git a/src/roboticstoolbox/models/ETS/Omni.py b/src/roboticstoolbox/models/ETS/Omni.py index e37765d70..aa38500a4 100644 --- a/src/roboticstoolbox/models/ETS/Omni.py +++ b/src/roboticstoolbox/models/ETS/Omni.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -36,7 +40,7 @@ class Omni(Robot): Sequence, J. Haviland and P. Corke """ - def __init__(self): + def __init__(self) -> None: l, w, h = 0.55, 0.40, 0.35 diff --git a/src/roboticstoolbox/models/ETS/Panda.py b/src/roboticstoolbox/models/ETS/Panda.py index 1bd0aa8e9..ab9aacbc3 100644 --- a/src/roboticstoolbox/models/ETS/Panda.py +++ b/src/roboticstoolbox/models/ETS/Panda.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -23,7 +27,7 @@ class Panda(Robot): """ - def __init__(self): + def __init__(self) -> None: deg = np.pi / 180 mm = 1e-3 diff --git a/src/roboticstoolbox/models/ETS/Planar2.py b/src/roboticstoolbox/models/ETS/Planar2.py index 6401cf356..52ff19ea2 100644 --- a/src/roboticstoolbox/models/ETS/Planar2.py +++ b/src/roboticstoolbox/models/ETS/Planar2.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -24,7 +28,7 @@ class Planar2(Robot2): """ - def __init__(self): + def __init__(self) -> None: a1 = 1 a2 = 1 diff --git a/src/roboticstoolbox/models/ETS/Planar_Y.py b/src/roboticstoolbox/models/ETS/Planar_Y.py index c85ace1b3..a2bef26c6 100644 --- a/src/roboticstoolbox/models/ETS/Planar_Y.py +++ b/src/roboticstoolbox/models/ETS/Planar_Y.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -24,7 +28,7 @@ class Planar_Y(Robot): """ - def __init__(self): + def __init__(self) -> None: # deg = np.pi / 180 # mm = 1e-3 diff --git a/src/roboticstoolbox/models/ETS/Puma560.py b/src/roboticstoolbox/models/ETS/Puma560.py index 5183b488b..13b5a5851 100644 --- a/src/roboticstoolbox/models/ETS/Puma560.py +++ b/src/roboticstoolbox/models/ETS/Puma560.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any from roboticstoolbox import ET as ET from roboticstoolbox import Robot import numpy as np @@ -29,7 +33,7 @@ class Puma560(Robot): """ # noqa - def __init__(self): + def __init__(self) -> None: # Puma dimensions (m) l1 = 0.672 l2 = -0.2337 diff --git a/src/roboticstoolbox/models/ETS/XYPanda.py b/src/roboticstoolbox/models/ETS/XYPanda.py index 8029c9a6f..3e6ea2727 100644 --- a/src/roboticstoolbox/models/ETS/XYPanda.py +++ b/src/roboticstoolbox/models/ETS/XYPanda.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -24,7 +28,7 @@ class XYPanda(Robot): """ - def __init__(self, workspace=5): + def __init__(self, workspace: float = 5) -> None: """ Create model of Franka-Emika Panda manipulator on an XY platform. diff --git a/src/roboticstoolbox/models/URDF/AL5D.py b/src/roboticstoolbox/models/URDF/AL5D.py index 09c95127d..d00cadb13 100644 --- a/src/roboticstoolbox/models/URDF/AL5D.py +++ b/src/roboticstoolbox/models/URDF/AL5D.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -27,7 +31,7 @@ class AL5D(URDFRobot): .. codeauthor:: Tassos Natsakis """ - def __init__(self): + def __init__(self) -> None: super().__init__( "lynxmotion_al5d_description/urdf/al5d_robot.urdf", manufacturer="Lynxmotion", diff --git a/src/roboticstoolbox/models/URDF/Fetch.py b/src/roboticstoolbox/models/URDF/Fetch.py index a54108f2c..787dfde9e 100644 --- a/src/roboticstoolbox/models/URDF/Fetch.py +++ b/src/roboticstoolbox/models/URDF/Fetch.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import re @@ -65,7 +69,7 @@ class Fetch(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "fetch", manufacturer="Fetch", diff --git a/src/roboticstoolbox/models/URDF/Frankie.py b/src/roboticstoolbox/models/URDF/Frankie.py index 423af88a1..8595a5999 100644 --- a/src/roboticstoolbox/models/URDF/Frankie.py +++ b/src/roboticstoolbox/models/URDF/Frankie.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -39,7 +43,7 @@ class Frankie(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "panda", manufacturer="Franka Emika", diff --git a/src/roboticstoolbox/models/URDF/FrankieOmni.py b/src/roboticstoolbox/models/URDF/FrankieOmni.py index 2d73a673b..a6b781384 100644 --- a/src/roboticstoolbox/models/URDF/FrankieOmni.py +++ b/src/roboticstoolbox/models/URDF/FrankieOmni.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -42,7 +46,7 @@ class FrankieOmni(Robot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: links_base, _, urdf_filepath = URDF_read( "clearpath_ridgeback_description/urdf/ridgeback.urdf.xacro" diff --git a/src/roboticstoolbox/models/URDF/Jaco.py b/src/roboticstoolbox/models/URDF/Jaco.py index 2047eb4d3..56227db55 100644 --- a/src/roboticstoolbox/models/URDF/Jaco.py +++ b/src/roboticstoolbox/models/URDF/Jaco.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class Jaco(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "j2n6s200", diff --git a/src/roboticstoolbox/models/URDF/KinovaGen3.py b/src/roboticstoolbox/models/URDF/KinovaGen3.py index 454ed5a6f..b2d31e157 100644 --- a/src/roboticstoolbox/models/URDF/KinovaGen3.py +++ b/src/roboticstoolbox/models/URDF/KinovaGen3.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -29,7 +33,7 @@ class KinovaGen3(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "gen3", manufacturer="Kinova", diff --git a/src/roboticstoolbox/models/URDF/LBR.py b/src/roboticstoolbox/models/URDF/LBR.py index 350713b99..30e586624 100644 --- a/src/roboticstoolbox/models/URDF/LBR.py +++ b/src/roboticstoolbox/models/URDF/LBR.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class LBR(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: # The bundled file does $(find kuka_lbr_iiwa_support) to locate its # own macro file, but rtb-data ships that content under the diff --git a/src/roboticstoolbox/models/URDF/PR2.py b/src/roboticstoolbox/models/URDF/PR2.py index 1acb055f4..2d89ba5ff 100644 --- a/src/roboticstoolbox/models/URDF/PR2.py +++ b/src/roboticstoolbox/models/URDF/PR2.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -25,7 +29,7 @@ class PR2(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__("pr2", manufacturer="Willow Garage") diff --git a/src/roboticstoolbox/models/URDF/Panda.py b/src/roboticstoolbox/models/URDF/Panda.py index fa8234b23..d7dc3e1fa 100644 --- a/src/roboticstoolbox/models/URDF/Panda.py +++ b/src/roboticstoolbox/models/URDF/Panda.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -43,7 +47,7 @@ class Panda(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self, use_robot_descriptions: bool = False): + def __init__(self, use_robot_descriptions: bool = False) -> None: if use_robot_descriptions: super().__init__( diff --git a/src/roboticstoolbox/models/URDF/Puma560.py b/src/roboticstoolbox/models/URDF/Puma560.py index 06b5fcdbe..d4e052b9b 100644 --- a/src/roboticstoolbox/models/URDF/Puma560.py +++ b/src/roboticstoolbox/models/URDF/Puma560.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -37,7 +41,7 @@ class Puma560(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "unimation_puma560_description/urdf/puma560_robot.urdf.xacro", diff --git a/src/roboticstoolbox/models/URDF/UR10.py b/src/roboticstoolbox/models/URDF/UR10.py index 2e77674ee..13ff8193d 100644 --- a/src/roboticstoolbox/models/URDF/UR10.py +++ b/src/roboticstoolbox/models/URDF/UR10.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class UR10(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__("ur10", manufacturer="Universal Robotics") diff --git a/src/roboticstoolbox/models/URDF/UR3.py b/src/roboticstoolbox/models/URDF/UR3.py index 8c89fa750..a08908c75 100644 --- a/src/roboticstoolbox/models/URDF/UR3.py +++ b/src/roboticstoolbox/models/URDF/UR3.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class UR3(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__("ur3", manufacturer="Universal Robotics") diff --git a/src/roboticstoolbox/models/URDF/UR5.py b/src/roboticstoolbox/models/URDF/UR5.py index c728e76f8..988d4dd19 100644 --- a/src/roboticstoolbox/models/URDF/UR5.py +++ b/src/roboticstoolbox/models/URDF/UR5.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -31,7 +35,7 @@ class UR5(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: # Name-based lookup, not a raw positional index: robot_descriptions # 3.0.0 changed which upstream repo "ur5" resolves to, reordering diff --git a/src/roboticstoolbox/models/URDF/URDFRobot.py b/src/roboticstoolbox/models/URDF/URDFRobot.py index a6a620e64..7045b3a9d 100644 --- a/src/roboticstoolbox/models/URDF/URDFRobot.py +++ b/src/roboticstoolbox/models/URDF/URDFRobot.py @@ -9,6 +9,12 @@ - URDFRobot — base class for simple URDF-loaded robot models """ +from __future__ import annotations + +from typing import Any + +from roboticstoolbox.tools.types import NDArray + from pathlib import Path import importlib import re @@ -28,7 +34,7 @@ from xacro import XacroException except ImportError: # pragma nocover - class XacroException(Exception): + class XacroException(Exception): # type: ignore[no-redef] pass @@ -36,7 +42,7 @@ class XacroException(Exception): from xacrodoc.packages import PackageNotFoundError except ImportError: # pragma nocover - class PackageNotFoundError(Exception): + class PackageNotFoundError(Exception): # type: ignore[no-redef] pass @@ -94,7 +100,7 @@ def _find_rd_rename(robot_name: str, tried: list[str]) -> str | None: return None -def _load_rd_module(robot_name: str): +def _load_rd_module(robot_name: str) -> Any: """Import the robot_descriptions submodule for ``robot_name``, trying the current name first and falling back to older/newer naming schemes. @@ -309,7 +315,7 @@ def _attach_expanded(err: URDFError, urdf_str: str, source: str) -> None: break -def _parse_urdf(urdf_str: str, source: str = ""): +def _parse_urdf(urdf_str: str, source: str = "") -> Any: """Parse a URDF string into (elinks, name). :raises URDFError: if the text is not well-formed XML or is not a valid @@ -324,7 +330,9 @@ def _parse_urdf(urdf_str: str, source: str = ""): raise -def _inertia_in_link_frame(inertial) -> "tuple[np.ndarray | None, np.ndarray | None]": +def _inertia_in_link_frame( + inertial: Any, +) -> tuple[NDArray | None, NDArray | None]: """Centre of mass and inertia tensor of a URDF link, in the link frame. URDF gives the inertia tensor in the link's *inertial* frame, whose pose @@ -347,7 +355,7 @@ def _inertia_in_link_frame(inertial) -> "tuple[np.ndarray | None, np.ndarray | N return r, I -def _links_from_urdf(urdf: URDF): +def _links_from_urdf(urdf: URDF) -> Any: """Convert a parsed :class:`URDF` into (elinks, name).""" elinks = [] elinkdict = {} @@ -359,6 +367,7 @@ def _links_from_urdf(urdf: URDF): elinkdict[link.name] = elink try: + # geometry has no setter, so this raises AttributeError and is ignored elink.geometry = [v.geometry.ob for v in link.visuals] except AttributeError: pass @@ -397,6 +406,7 @@ def _links_from_urdf(urdf: URDF): trans = SE3(joint.origin).t rot = joint.rpy + ets: Any if np.count_nonzero(joint.axis) < 2: ets = ET.SE3(SE3(trans) * SE3.RPY(rot)) else: @@ -596,8 +606,8 @@ def __init__( gripper_link_name: "str | list[str] | None" = None, patch: "Callable[[str], str] | None" = None, extra_packages: "dict[str, str] | None" = None, - **kwargs, - ): + **kwargs: Any, + ) -> None: elinks, name, filepath = URDF_file( urdf_path, patch=patch, extra_packages=extra_packages ) diff --git a/src/roboticstoolbox/models/URDF/Valkyrie.py b/src/roboticstoolbox/models/URDF/Valkyrie.py index 5cc3cddc4..292567508 100644 --- a/src/roboticstoolbox/models/URDF/Valkyrie.py +++ b/src/roboticstoolbox/models/URDF/Valkyrie.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import re @@ -61,7 +65,7 @@ class Valkyrie(Robot): .. codeauthor:: Peter Corke """ - def __init__(self, variant="A"): + def __init__(self, variant: str = "A") -> None: if not variant in "ABCD": raise ValueError("variant must be in the range A-D") diff --git a/src/roboticstoolbox/models/URDF/YuMi.py b/src/roboticstoolbox/models/URDF/YuMi.py index b8154f90a..861d96332 100644 --- a/src/roboticstoolbox/models/URDF/YuMi.py +++ b/src/roboticstoolbox/models/URDF/YuMi.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python from roboticstoolbox.robot.Link import Link @@ -36,7 +40,7 @@ class YuMi(Robot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: links, name, urdf_filepath = URDF_read("yumi") diff --git a/src/roboticstoolbox/models/URDF/px100.py b/src/roboticstoolbox/models/URDF/px100.py index 58b171f24..6f5bc6f2c 100644 --- a/src/roboticstoolbox/models/URDF/px100.py +++ b/src/roboticstoolbox/models/URDF/px100.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class px100(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/px100.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/px150.py b/src/roboticstoolbox/models/URDF/px150.py index f1fef2e75..7e61ca56f 100644 --- a/src/roboticstoolbox/models/URDF/px150.py +++ b/src/roboticstoolbox/models/URDF/px150.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class px150(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/px150.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/rx150.py b/src/roboticstoolbox/models/URDF/rx150.py index a0ff57605..ecb302664 100644 --- a/src/roboticstoolbox/models/URDF/rx150.py +++ b/src/roboticstoolbox/models/URDF/rx150.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class rx150(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/rx150.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/rx200.py b/src/roboticstoolbox/models/URDF/rx200.py index 6382837eb..94dcd7a41 100644 --- a/src/roboticstoolbox/models/URDF/rx200.py +++ b/src/roboticstoolbox/models/URDF/rx200.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class rx200(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/rx200.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/vx300.py b/src/roboticstoolbox/models/URDF/vx300.py index 8c099e269..886807961 100644 --- a/src/roboticstoolbox/models/URDF/vx300.py +++ b/src/roboticstoolbox/models/URDF/vx300.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class vx300(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/vx300.urdf.xacro", manufacturer="Interbotix", diff --git a/src/roboticstoolbox/models/URDF/vx300s.py b/src/roboticstoolbox/models/URDF/vx300s.py index b5e48cb11..0e0bcdbac 100644 --- a/src/roboticstoolbox/models/URDF/vx300s.py +++ b/src/roboticstoolbox/models/URDF/vx300s.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class vx300s(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/vx300s.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/wx200.py b/src/roboticstoolbox/models/URDF/wx200.py index 1e3670a15..9cc22bdd1 100755 --- a/src/roboticstoolbox/models/URDF/wx200.py +++ b/src/roboticstoolbox/models/URDF/wx200.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class wx200(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/wx200.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/wx250.py b/src/roboticstoolbox/models/URDF/wx250.py index 6ed00358d..4dc131cb6 100644 --- a/src/roboticstoolbox/models/URDF/wx250.py +++ b/src/roboticstoolbox/models/URDF/wx250.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class wx250(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/wx250.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/URDF/wx250s.py b/src/roboticstoolbox/models/URDF/wx250s.py index 27c443593..0c1dbbee3 100644 --- a/src/roboticstoolbox/models/URDF/wx250s.py +++ b/src/roboticstoolbox/models/URDF/wx250s.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -30,7 +34,7 @@ class wx250s(URDFRobot): .. sectionauthor:: Peter Corke """ - def __init__(self): + def __init__(self) -> None: super().__init__( "trossen_descriptions/urdf/wx250s.urdf.xacro", manufacturer="Interbotix" diff --git a/src/roboticstoolbox/models/catalog.py b/src/roboticstoolbox/models/catalog.py index 09f817fe8..d6b6260c2 100644 --- a/src/roboticstoolbox/models/catalog.py +++ b/src/roboticstoolbox/models/catalog.py @@ -1,3 +1,7 @@ + +from __future__ import annotations + +from typing import Any import warnings from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.tools import rtb_get_param @@ -9,7 +13,13 @@ # import importlib -def catalog(keywords=None, dof=None, mtype=None, sorton=None, border="thin"): +def catalog( + keywords: Any = None, + dof: Any = None, + mtype: Any = None, + sorton: Any = None, + border: str = "thin", +) -> None: """ Display all robot models in summary form @@ -86,7 +96,7 @@ def catalog(keywords=None, dof=None, mtype=None, sorton=None, border="thin"): f"sorton must be one of {sorted(sort_map)}, got {sorton!r}" ) - def make_table(border=None): + def make_table(border: Any = None) -> None: table = ANSITable( Column("class", headalign="^", colalign="<"), Column("robot name", headalign="^", colalign="<"), @@ -112,7 +122,7 @@ def make_table(border=None): if inspect.isclass(cls) and issubclass(cls, Robot): # we found a BaseRobot subclass, instantiate it try: - robot = cls() + robot = cls() # type: ignore[call-arg] # subclasses take no args except Exception: print(f"failed to load {cls}") continue @@ -152,7 +162,7 @@ def make_table(border=None): table.sort(sort_col_main, key=sort_key) table.print() - def make_rd_table(border=None): + def make_rd_table(border: Any = None) -> None: from robot_descriptions import DESCRIPTIONS from robot_descriptions._descriptions import Format @@ -198,9 +208,13 @@ def make_rd_table(border=None): make_rd_table(border=border) -def list( # pragma nocover - keywords=None, dof=None, mtype=None, sorton=None, border="thin" -): +def list( + keywords: Any = None, + dof: Any = None, + mtype: Any = None, + sorton: Any = None, + border: str = "thin", +) -> Any: """ Display all robot models in summary form (deprecated) diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index b732f5c50..00ab68b9c 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -6,6 +6,8 @@ """ from __future__ import annotations + +from typing import Any # import sys import io from abc import ABC @@ -48,7 +50,7 @@ from roboticstoolbox.robot.RobotPlottingMPL import RobotPlottingMPLMixin if TYPE_CHECKING: - from matplotlib.cm import Color # pragma nocover + from matplotlib.typing import ColorType as Color # pragma nocover else: Color = None @@ -76,6 +78,9 @@ def _is_symbolic(value: Any) -> bool: class BaseRobot(SceneNode, DynamicsMixin, RobotPlottingMPLMixin, ABC, Generic[LinkType]): + # set when the links are processed in the constructor + _base_link: LinkType + def __init__( self, links: list[LinkType], @@ -90,7 +95,7 @@ def __init__( symbolic: bool = False, configs: dict[str, NDArray] | None = None, check_jindex: bool = True, - ): + ) -> None: # Initialise the scene node SceneNode.__init__(self) @@ -140,7 +145,7 @@ def __init__( self.manufacturer = manufacturer self.comment = comment self._symbolic = symbolic - self._reach = None + self._reach: float | None = None self._hasdynamics = False self._hasgeometry = False self._hascollision = False @@ -156,7 +161,8 @@ def __init__( if link.hasdynamics: self._hasdynamics = True if link.geometry: - self._hasgeometry = [] + # an empty list, not True (replaced just below if it has geometry) + self._hasgeometry = [] # type: ignore[assignment] if link.collision: self._hascollision = True @@ -211,10 +217,10 @@ def __init__( self._valid_qlim = True # SceneNode, set a reference to the first link - self.scene_children = [self.links[0]] # type: ignore + self.scene_children = [self.links[0]] self._default_backend = None - self._active_plot_env = None + self._active_plot_env: Any = None # --------------------------------------------------------------------- # # --------- Private Methods ------------------------------------------- # @@ -225,7 +231,7 @@ def _sort_links( links: list[LinkType], gripper_links: LinkType | list[LinkType] | None, check_jindex: bool, - ): + ) -> None: """ This method does several things for setting up the links of a robot @@ -362,7 +368,7 @@ def _sort_links( # No joints have an index jindex = [0] # "mutable integer" hack - def visit_link(link, jindex): + def visit_link(link: Any, jindex: Any) -> None: # if it's a joint, assign it a jindex and increment it if link.isjoint and link in links: link.jindex = jindex[0] @@ -405,7 +411,7 @@ def visit_link(link, jindex): # --------- self._links = orlinks - def dynchanged(self, what: str | None = None): + def dynchanged(self, what: str | None = None) -> None: """ Dynamic parameters have changed @@ -426,7 +432,7 @@ def dynchanged(self, what: str | None = None): # --------- Magic Methods --------------------------------------------- # # --------------------------------------------------------------------- # - def __iter__(self): + def __iter__(self) -> Any: self._iter = 0 return self @@ -514,7 +520,7 @@ def __str__(self) -> str: if link.isjoint: jname = link.jindex else: - jname = "" + jname = "" # type: ignore[assignment] table.row( # link.jindex, k, @@ -630,7 +636,7 @@ def n(self) -> int: return self._n @property - def nlinks(self): + def nlinks(self) -> Any: """ Number of links @@ -700,7 +706,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, name_new: str): + def name(self, name_new: str) -> None: self._name = name_new @property @@ -717,11 +723,11 @@ def comment(self) -> str: return self._comment @comment.setter - def comment(self, comment_new: str): + def comment(self, comment_new: str) -> None: self._comment = comment_new @property - def manufacturer(self): + def manufacturer(self) -> Any: """ Get/set robot manufacturer's name @@ -733,7 +739,7 @@ def manufacturer(self): return self._manufacturer @manufacturer.setter - def manufacturer(self, manufacturer_new): + def manufacturer(self, manufacturer_new: Any) -> None: self._manufacturer = manufacturer_new @property @@ -749,7 +755,7 @@ def symbolic(self) -> bool: return self._symbolic @property - def hasdynamics(self): + def hasdynamics(self) -> Any: """ Robot has dynamic parameters @@ -770,7 +776,7 @@ def hasdynamics(self): return self._hasdynamics @property - def hasgeometry(self): + def hasgeometry(self) -> Any: """ Robot has geometry model @@ -796,7 +802,7 @@ def hasgeometry(self): return self._hasgeometry @property - def hascollision(self): + def hascollision(self) -> Any: """ Robot has collision model @@ -821,7 +827,7 @@ def hascollision(self): return self._hascollision @property - def default_backend(self): + def default_backend(self) -> Any: """ Get default graphical backend @@ -836,7 +842,7 @@ def default_backend(self): return self._default_backend @default_backend.setter - def default_backend(self, be): + def default_backend(self, be: Any) -> None: self._default_backend = be @property @@ -858,7 +864,7 @@ def gravity(self) -> NDArray: return self._gravity @gravity.setter - def gravity(self, gravity_new: ArrayLike): + def gravity(self, gravity_new: ArrayLike) -> None: self._gravity = np.array(getvector(gravity_new, 3)) self.dynchanged() @@ -879,7 +885,7 @@ def q(self) -> NDArray: return self._q @q.setter - def q(self, q_new: ArrayLike): + def q(self, q_new: ArrayLike) -> None: self._q = np.array(getvector(q_new, self.n)) @property @@ -896,7 +902,7 @@ def qd(self) -> NDArray: return self._qd @qd.setter - def qd(self, qd_new: ArrayLike): + def qd(self, qd_new: ArrayLike) -> None: self._qd = np.array(getvector(qd_new, self.n)) @property @@ -912,7 +918,7 @@ def qdd(self) -> NDArray: return self._qdd @qdd.setter - def qdd(self, qdd_new: ArrayLike): + def qdd(self, qdd_new: ArrayLike) -> None: self._qdd = np.array(getvector(qdd_new, self.n)) @property @@ -943,6 +949,7 @@ def qlim(self) -> NDArray: limits = np.zeros((2, self.n)) j = 0 + v: ArrayLike for link in self.links: if link.isrevolute: @@ -969,7 +976,7 @@ def qlim(self) -> NDArray: return limits @qlim.setter - def qlim(self, new_qlim: ArrayLike): + def qlim(self, new_qlim: ArrayLike) -> None: new_qlim = np.array(new_qlim) if new_qlim.shape != (2, self.n): @@ -1099,7 +1106,7 @@ def control_mode(self) -> str: return self._control_mode @control_mode.setter - def control_mode(self, cn: str): + def control_mode(self, cn: str) -> None: if cn == "p" or cn == "v" or cn == "a": self._control_mode = cn else: @@ -1132,7 +1139,7 @@ def tool(self) -> SE3: return SE3(self._tool, check=False) @tool.setter - def tool(self, T: SE3 | NDArray): + def tool(self, T: SE3 | NDArray) -> None: if isinstance(T, SE3): if len(T) != 1: raise ValueError("tool must be a single SE3, not a batch") @@ -1160,7 +1167,7 @@ def base(self) -> SE3: return SE3(self._T, check=False) @base.setter - def base(self, T: NDArray | SE3): + def base(self, T: NDArray | SE3) -> None: if isinstance(self, rtb.Robot): # All 3D robots # Set the SceneNode T @@ -1191,10 +1198,7 @@ def get_path( """ def search( - start, - end, - explored: set[LinkType | Link], - path: list[LinkType | Link], + start: Any, end: Any, explored: set[LinkType | Link], path: list[LinkType | Link] ) -> list[LinkType | Link] | None: link = self._getlink(start, self.base_link) end = self._getlink(end, self.ee_links[0]) @@ -1237,6 +1241,8 @@ def search( if p is not None and len(p) > 0: return p + return None + end, start, tool = self._get_limit_links(end=end, start=start) path = search(start, end, set(), []) @@ -1249,7 +1255,7 @@ def search( if tool is None: tool = SE3() - return path, len(path), tool # type: ignore + return path, len(path), tool @lru_cache(maxsize=32) def _getlink( @@ -1301,7 +1307,7 @@ def _getlink( else: raise TypeError("unknown argument") - def _find_ets(self, start, end, explored, path) -> ETS | None: + def _find_ets(self, start: Any, end: Any, explored: Any, path: Any) -> ETS | None: """ Privade method which will recursively find the ETS of a path see ets() @@ -1344,6 +1350,8 @@ def _find_ets(self, start, end, explored, path) -> ETS | None: if p is not None: return p + return None + def _gripper_ets(self, gripper: Gripper) -> ETS: """ Privade method which will find the ETS of a gripper @@ -1421,7 +1429,7 @@ def _get_limit_links( start_ret = self._getlink(start) # because Gripper returns Link not LinkType - return end_ret, start_ret, tool # type: ignore + return end_ret, start_ret, tool @lru_cache(maxsize=32) def ets( @@ -1488,13 +1496,13 @@ def ets( else: if isinstance(end, Gripper): ets_end = self._gripper_ets(end) - end_link = end.links[0].parent # type: ignore + end_link = end.links[0].parent if end_link is None: # pragma nocover raise ValueError("Invalid robot link configuration") else: end_link = self._getlink(end, self.ee_links[0]) - explored = set() + explored: set[Any] = set() if link is end_link: ets = link.ets @@ -1514,14 +1522,15 @@ def ets( return ets - def copy(self): + def copy(self) -> Any: return deepcopy(self) - def __deepcopy__(self, memo): + def __deepcopy__(self, memo: Any) -> Any: links = [] # if isinstance(self, rtb.DHRobot): # cls = rtb.DHRobot + cls: type[BaseRobot[Any]] if isinstance(self, rtb.Robot2): cls = rtb.Robot2 else: @@ -1545,7 +1554,7 @@ def __deepcopy__(self, memo): name=name, manufacturer=manufacturer, comment=comment, - base=base, # type: ignore + base=base, tool=tool, gravity=gravity, keywords=keywords, @@ -1564,7 +1573,7 @@ def __deepcopy__(self, memo): # --------------------------------------------------------------------- # - def todegrees(self, q) -> NDArray: + def todegrees(self, q: Any) -> NDArray: """ Convert joint angles to degrees @@ -1599,7 +1608,7 @@ def todegrees(self, q) -> NDArray: else: return q - def toradians(self, q) -> NDArray: + def toradians(self, q: Any) -> NDArray: """ Convert joint angles to radians @@ -1635,7 +1644,7 @@ def toradians(self, q) -> NDArray: else: return q - def isrevolute(self, j) -> bool: + def isrevolute(self, j: Any) -> bool: """ Check if joint is revolute @@ -1660,7 +1669,7 @@ def isrevolute(self, j) -> bool: """ return self.revolutejoints[j] - def isprismatic(self, j) -> bool: + def isprismatic(self, j: Any) -> bool: """ Check if joint is prismatic @@ -1691,7 +1700,7 @@ def isprismatic(self, j) -> bool: def dfs_links( self, start: LinkType, - func: Callable[[LinkType | None, Any]] = None, + func: Callable[[LinkType | None], Any] | None = None, ) -> list[LinkType]: """ A link search method @@ -1706,7 +1715,7 @@ def dfs_links( visited = [] - def vis_children(link): + def vis_children(link: Any) -> None: visited.append(link) if func is not None: func(link) @@ -1719,7 +1728,7 @@ def vis_children(link): return visited - def addconfiguration_attr(self, name: str, q: ArrayLike, unit: str = "rad"): + def addconfiguration_attr(self, name: str, q: ArrayLike, unit: str = "rad") -> None: """ Add a named joint configuration as an attribute @@ -1755,7 +1764,7 @@ def addconfiguration_attr(self, name: str, q: ArrayLike, unit: str = "rad"): self._configs[name] = v setattr(self, name, v) - def addconfiguration(self, name: str, q: ArrayLike): + def addconfiguration(self, name: str, q: ArrayLike) -> None: """ Add a named joint configuration @@ -1783,11 +1792,11 @@ def addconfiguration(self, name: str, q: ArrayLike): self._configs[name] = np.array(getvector(q, self.n)) - def configurations_str(self, border="thin"): + def configurations_str(self, border: str = "thin") -> Any: deg = 180 / np.pi # TODO: factor this out of DHRobot - def angle(theta, fmt=None): + def angle(theta: Any, fmt: Any = None) -> Any: if fmt is not None: try: return fmt.format(theta * deg) + "\u00b0" @@ -1820,7 +1829,7 @@ def angle(theta, fmt=None): else: # pragma nocover return "" - def random_q(self): + def random_q(self) -> Any: """ Return a random joint configuration @@ -1863,7 +1872,7 @@ def hierarchy(self) -> None: """ - def recurse(link, indent=0): + def recurse(link: Any, indent: int = 0) -> None: print(" " * indent * 2, link.name) if link.children is not None: for child in link.children: @@ -1899,7 +1908,7 @@ def segments(self) -> list[list[LinkType | None]]: segments """ - def recurse(link: Link): + def recurse(link: Link) -> Any: segs = [link.parent] while True: segs.append(link) @@ -1909,36 +1918,37 @@ def recurse(link: Link): link = link.children[0] # type: ignore continue elif link.nchildren > 1: - segs = [segs] + segs = [segs] # type: ignore[list-item] for child in link.children: # type: ignore segs.extend(recurse(child)) return segs - return recurse(self.links[0]) # type: ignore + return recurse(self.links[0]) # --------------------------------------------------------------------- # # Scene Graph section # --------------------------------------------------------------------- # - def _update_link_tf(self, q: ArrayLike | None = None): + def _update_link_tf(self, q: ArrayLike | None = None) -> None: """ This private method updates the local transform of each link within this robot according to q (or self.q if q is none) """ @lru_cache(maxsize=2) - def get_link_ets(): + def get_link_ets() -> Any: return [link.ets._fknm for link in self.links] @lru_cache(maxsize=2) - def get_link_scene_node(): + def get_link_scene_node() -> Any: return [link._T_reference for link in self.links] Robot_link_T(get_link_ets(), get_link_scene_node(), self._q, q) - [gripper._update_link_tf() for gripper in self.grippers] + for gripper in self.grippers: + gripper._update_link_tf() # --------------------------------------------------------------------- # # --------- PyPlot Methods -------------------------------------------- # @@ -1998,14 +2008,14 @@ def plot( q: ArrayLike, backend: L["swift", "pyplot", "pyplot2"] | None = None, block: bool = False, - dt: float = 0.050, + dt: float = 0.05, limits: ArrayLike | None = None, vellipse: bool = False, fellipse: bool = False, fig: str | None = None, movie: str | None = None, loop: bool = False, - **kwargs, + **kwargs: Any, ) -> Connector: """ Graphical display and animation @@ -2254,7 +2264,7 @@ def teach( # --------- Utility Methods ------------------------------------------- # # --------------------------------------------------------------------- # - def showgraph(self, display_graph: bool = True, **kwargs) -> str | None: + def showgraph(self, display_graph: bool = True, **kwargs: Any) -> str | None: """ Display a link transform graph in browser @@ -2318,6 +2328,8 @@ def showgraph(self, display_graph: bool = True, **kwargs) -> str | None: else: return pdffile.name + return None + def dotfile( self, filename: str | IO[str], @@ -2325,7 +2337,7 @@ def dotfile( ets: L["full", "brief"] = "full", jtype: bool = False, static: bool = True, - ): + ) -> None: """ Write a link transform graph as a GraphViz dot file @@ -2368,6 +2380,7 @@ def dotfile( """ + file: IO[str] if isinstance(filename, str): file = open(filename, "w", encoding="utf-8") else: @@ -2377,7 +2390,7 @@ def dotfile( graph [rankdir=LR]; """ - def draw_edge(link, etsbox, jtype, static): + def draw_edge(link: Any, etsbox: Any, jtype: Any, static: Any) -> None: # draw the edge if jtype: if link.isprismatic: @@ -2488,7 +2501,7 @@ def graph( self, format: L["dot", "mermaid", "mermaid_fenced"] = "dot", filename: str | IO[str] | None = None, - **kwargs, + **kwargs: Any, ) -> str: """ Render a link transform graph as text @@ -2537,7 +2550,7 @@ def graph( lines = ["flowchart LR"] node_ids: dict[Any, str] = {} - def node_id(link) -> str: + def node_id(link: Any) -> str: return node_ids.setdefault(link, f"n{len(node_ids)}") lines.append(' BASE["BASE"]') @@ -2553,7 +2566,7 @@ def node_id(link) -> str: for link in gripper.links: lines.append(f' {node_id(link)}{{{{"{link.name}"}}}}') - def edge_label(link) -> str: + def edge_label(link: Any) -> str: if not link.isjoint: return "" kind = "P" if link.isprismatic else "R" diff --git a/src/roboticstoolbox/robot/DHFactor.py b/src/roboticstoolbox/robot/DHFactor.py index ad70586ef..88b188404 100644 --- a/src/roboticstoolbox/robot/DHFactor.py +++ b/src/roboticstoolbox/robot/DHFactor.py @@ -1,4 +1,5 @@ -from roboticstoolbox import ETS, ET +from roboticstoolbox.ets.ET import ET +from roboticstoolbox.ets.ETS import ETS import re import sympy import numpy as np diff --git a/src/roboticstoolbox/robot/DHLink.py b/src/roboticstoolbox/robot/DHLink.py index fbe2921c6..333bbae58 100644 --- a/src/roboticstoolbox/robot/DHLink.py +++ b/src/roboticstoolbox/robot/DHLink.py @@ -3,6 +3,10 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + # import numpy as np # from spatialmath import SE3 import roboticstoolbox as rp @@ -16,7 +20,7 @@ from roboticstoolbox.tools.types import ArrayLike -def _check_rne(func): +def _check_rne(func: Any) -> Any: """ @_check_rne decorator @@ -36,7 +40,7 @@ def dyn_param_setter(self, value): """ @wraps(func) - def wrapper_check_rne(*args, **kwargs): + def wrapper_check_rne(*args: Any, **kwargs: Any) -> Any: if args[0]._frne is None or args[0]._frne_stale: args[0]._copy_to_cpp() return func(*args, **kwargs) @@ -50,25 +54,25 @@ def wrapper_check_rne(*args, **kwargs): # print('Using SymPy') import sympy as sym - def _issymbol(x): # type: ignore + def _issymbol(x: Any) -> Any: return isinstance(x, sym.Expr) except ImportError: - def _issymbol(x): # pylint: disable=unused-argument + def _issymbol(x: Any) -> Any: # pylint: disable=unused-argument return False -def _cos(theta) -> float: +def _cos(theta: Any) -> float: if _issymbol(theta): - return sym.cos(theta) # type: ignore + return sym.cos(theta) else: return cos(theta) -def _sin(theta) -> float: +def _sin(theta: Any) -> float: if _issymbol(theta): - return sym.sin(theta) # type: ignore + return sym.sin(theta) else: return sin(theta) @@ -125,8 +129,8 @@ def __init__( offset: float = 0, flip: bool = False, qlim: ArrayLike | None = None, - **kwargs, - ): + **kwargs: Any, + ) -> None: super().__init__(**kwargs) ets = self._to_ets(sigma, theta, d, alpha, a, offset, flip, mdh) @@ -152,7 +156,8 @@ def __init__( self.offset = offset self.id = None self.mesh = None - self.number = None + # set when the link is added to a robot + self.number = None # type: ignore[assignment] def _to_ets(self, sigma: int, theta: float, d: float, alpha: float, a: float, offset: float, flip: bool, mdh: bool) -> ETS: ets = ETS() @@ -311,7 +316,7 @@ def isjoint(self) -> bool: # return new_links - def __add__(self, L): + def __add__(self, L: Any) -> Any: if isinstance(L, DHLink): return rp.DHRobot([self, L]) @@ -336,7 +341,7 @@ def __add__(self, L): else: raise TypeError("Cannot add a Link with a non Link object") - def __str__(self): + def __str__(self) -> str: s = "" @@ -359,9 +364,9 @@ def __str__(self): ) return s - def __repr__(self): + def __repr__(self) -> str: name = self.__class__.__name__ - args = [] + args: list[str] = [] if self.isrevolute: self._format_param(args, "d") else: @@ -371,7 +376,7 @@ def __repr__(self): args.extend(super()._params()) return name + "(" + ", ".join(args) + ")" - def __deepcopy__(self, memo): + def __deepcopy__(self, memo: Any) -> Any: kwargs = { "name": deepcopy(self.name), "joint_name": deepcopy(self._joint_name), @@ -417,7 +422,7 @@ def __deepcopy__(self, memo): # -------------------------------------------------------------------------- # @property - def theta(self): + def theta(self) -> Any: """ Get/set joint angle. @@ -431,7 +436,7 @@ def theta(self): @theta.setter @_dirties_frne - def theta(self, theta_new): + def theta(self, theta_new: Any) -> None: if not self.sigma and theta_new != 0.0: raise ValueError("theta is not valid for revolute joints") else: @@ -440,7 +445,7 @@ def theta(self, theta_new): # -------------------------------------------------------------------------- # @property - def d(self): + def d(self) -> Any: """ Get/set link offset. @@ -454,7 +459,7 @@ def d(self): @d.setter @_dirties_frne - def d(self, d_new): + def d(self, d_new: Any) -> None: if self.sigma and d_new != 0.0: raise ValueError("f is not valid for prismatic joints") else: @@ -463,7 +468,7 @@ def d(self, d_new): # -------------------------------------------------------------------------- # @property - def a(self): + def a(self) -> Any: """ Get/set link length. @@ -477,13 +482,13 @@ def a(self): @a.setter @_dirties_frne - def a(self, a_new): + def a(self, a_new: Any) -> None: self._a = a_new # -------------------------------------------------------------------------- # @property - def alpha(self): + def alpha(self) -> Any: """ Get/set link twist. @@ -497,13 +502,13 @@ def alpha(self): @alpha.setter @_dirties_frne - def alpha(self, alpha_new): + def alpha(self, alpha_new: Any) -> None: self._alpha = alpha_new # -------------------------------------------------------------------------- # @property - def sigma(self): + def sigma(self) -> Any: """ Get/set joint type. @@ -520,13 +525,13 @@ def sigma(self): @sigma.setter @_dirties_frne - def sigma(self, sigma_new): + def sigma(self, sigma_new: Any) -> None: self._sigma = sigma_new # -------------------------------------------------------------------------- # @property - def mdh(self): + def mdh(self) -> Any: """ Get/set kinematic convention. @@ -543,13 +548,13 @@ def mdh(self): @mdh.setter @_dirties_frne - def mdh(self, mdh_new): + def mdh(self, mdh_new: Any) -> None: self._mdh = int(mdh_new) # -------------------------------------------------------------------------- # @property - def offset(self): + def offset(self) -> Any: """ Get/set joint variable offset @@ -568,12 +573,12 @@ def offset(self): return self._offset @offset.setter - def offset(self, offset_new): + def offset(self, offset_new: Any) -> None: self._offset = offset_new # -------------------------------------------------------------------------- # - def A(self, q: float) -> SE3: + def A(self, q: float = 0.0) -> SE3: r""" Link transform matrix @@ -656,7 +661,7 @@ def A(self, q: float) -> SE3: return SE3(T, check=False) @property - def isrevolute(self): + def isrevolute(self) -> Any: """ Checks if the joint is of revolute type @@ -672,7 +677,7 @@ def isrevolute(self): return False @property - def isprismatic(self): + def isprismatic(self) -> Any: """ Checks if the joint is of prismatic type @@ -732,9 +737,15 @@ class RevoluteDH(DHLink): """ def __init__( - self, d: float = 0.0, a: float = 0.0, alpha: float = 0.0, offset: float = 0.0, - qlim: ArrayLike | None = None, flip: bool = False, **kwargs - ): + self, + d: float = 0.0, + a: float = 0.0, + alpha: float = 0.0, + offset: float = 0.0, + qlim: ArrayLike | None = None, + flip: bool = False, + **kwargs: Any, + ) -> None: theta = 0.0 sigma = 0 @@ -795,9 +806,15 @@ class PrismaticDH(DHLink): """ def __init__( - self, theta: float = 0.0, a: float = 0.0, alpha: float = 0.0, - offset: float = 0.0, qlim: ArrayLike | None = None, flip: bool = False, **kwargs - ): + self, + theta: float = 0.0, + a: float = 0.0, + alpha: float = 0.0, + offset: float = 0.0, + qlim: ArrayLike | None = None, + flip: bool = False, + **kwargs: Any, + ) -> None: d = 0.0 sigma = 1 @@ -861,9 +878,15 @@ class RevoluteMDH(DHLink): """ def __init__( - self, d: float = 0.0, a: float = 0.0, alpha: float = 0.0, offset: float = 0.0, - qlim: ArrayLike | None = None, flip: bool = False, **kwargs - ): + self, + d: float = 0.0, + a: float = 0.0, + alpha: float = 0.0, + offset: float = 0.0, + qlim: ArrayLike | None = None, + flip: bool = False, + **kwargs: Any, + ) -> None: theta = 0.0 sigma = 0 @@ -927,9 +950,15 @@ class PrismaticMDH(DHLink): """ def __init__( - self, theta: float = 0.0, a: float = 0.0, alpha: float = 0.0, - offset: float = 0.0, qlim: ArrayLike | None = None, flip: bool = False, **kwargs - ): + self, + theta: float = 0.0, + a: float = 0.0, + alpha: float = 0.0, + offset: float = 0.0, + qlim: ArrayLike | None = None, + flip: bool = False, + **kwargs: Any, + ) -> None: d = 0.0 sigma = 1 diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index 0a47bca16..3b626126a 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -6,6 +6,7 @@ from __future__ import annotations from collections import namedtuple from email import message +from typing import Any from roboticstoolbox.tools.data import rtb_path_to_datafile import warnings import copy @@ -77,7 +78,7 @@ class DHRobot(Robot): """ - def __init__(self, links, meshdir=None, **kwargs): + def __init__(self, links: Any, meshdir: Any = None, **kwargs: Any) -> None: # Verify L if not isinstance(links, list): raise TypeError("The links must be stored in a list.") @@ -144,10 +145,10 @@ def __init__(self, links, meshdir=None, **kwargs): self._frne = None @property - def links(self) -> list[DHLink]: # type: ignore[override] - return self._links # type: ignore[return-value] + def links(self) -> list[DHLink]: + return self._links - def __str__(self): + def __str__(self) -> str: """ Pretty prints the DH Model of the robot. Will output angles in degrees @@ -186,7 +187,7 @@ def __str__(self): dh = "standard" s += f", {dh} DH parameters\n" - def qstr(j, link): + def qstr(j: Any, link: Any) -> Any: j += 1 if link.isflip: s = f"-q{j:d}" @@ -202,7 +203,7 @@ def qstr(j, link): s += f" {sign} {offset * deg:.3g}\u00b0" return s - def angle(theta, fmt=None): + def angle(theta: Any, fmt: Any = None) -> Any: if sym.issymbol(theta): # pragma nocover return "<>" + str(theta) else: @@ -211,7 +212,7 @@ def angle(theta, fmt=None): else: return str(theta * deg) + "\u00b0" - def format_attr(attr) -> str: + def format_attr(attr: Any) -> str: if isinstance(attr, float): return f"{attr:.4g}" else: @@ -314,7 +315,7 @@ def format_attr(attr) -> str: return s - def __add__(self, L): + def __add__(self, L: Any) -> Any: nlinks = [] # TODO - Should I do a deep copy here a physically copy the DHLinks @@ -340,7 +341,7 @@ def __add__(self, L): gravity=self.gravity, ) - def __deepcopy__(self, memo): + def __deepcopy__(self, memo: Any) -> Any: links = [] for link in self.links: @@ -419,7 +420,7 @@ def __deepcopy__(self, memo): # --------------------------------------------------------------------- # - def _set_link_fk(self, q): + def _set_link_fk(self, q: Any) -> None: """ robot._set_link_fk(q) evaluates fkine for each link within a robot and stores that pose in a private variable within the link. @@ -452,7 +453,7 @@ def _set_link_fk(self, q): # --------------------------------------------------------------------- # @property - def mdh(self): + def mdh(self) -> Any: """ Modified Denavit-Hartenberg status @@ -474,7 +475,7 @@ def mdh(self): return self._mdh @property - def d(self): + def d(self) -> Any: r""" Link offset values @@ -500,7 +501,7 @@ def d(self): return v @property - def a(self): + def a(self) -> Any: r""" Link length values @@ -526,7 +527,7 @@ def a(self): return v @property - def theta(self): + def theta(self) -> Any: r""" Joint angle values @@ -552,7 +553,7 @@ def theta(self): return v @property - def alpha(self): + def alpha(self) -> Any: r""" Link twist values @@ -578,7 +579,7 @@ def alpha(self): return v @property - def r(self): + def r(self) -> Any: r""" Link centre of mass values @@ -600,7 +601,7 @@ def r(self): return v @property - def offset(self): + def offset(self) -> Any: r""" Joint offset values @@ -621,7 +622,7 @@ def offset(self): return v @property - def reach(self): + def reach(self) -> Any: r""" Reach of the robot @@ -653,7 +654,7 @@ def reach(self): return self._reach @property - def nbranches(self): + def nbranches(self) -> Any: """ Number of branches @@ -674,7 +675,7 @@ def nbranches(self): """ return 1 - def A(self, j, q=None): + def A(self, j: Any, q: Any = None) -> Any: """ Link forward kinematics @@ -717,7 +718,7 @@ def A(self, j, q=None): return T - def islimit(self, q=None): + def islimit(self, q: Any = None) -> Any: """ Joint limit test @@ -748,7 +749,7 @@ def islimit(self, q=None): return [link.islimit(qk) for (link, qk) in zip(self, q)] - def isspherical(self): + def isspherical(self) -> Any: """ Test for spherical wrist @@ -785,7 +786,7 @@ def isspherical(self): and L[2].sigma == 0 ) - def dhunique(self): + def dhunique(self) -> None: """ Print the unique DH parameters @@ -820,7 +821,7 @@ def dhunique(self): table.row(f"⍺{j}", link.alpha) table.print() - def twists(self, q=None): + def twists(self, q: Any = None) -> Any: """ Joint axes as twists @@ -859,9 +860,9 @@ def twists(self, q=None): # MDH case for j, link in enumerate(self): if link.sigma == 0: - tw[j] = Twist3.UnitRevolute(T[j].a, T[j].t) # type: ignore[union-attr] + tw[j] = Twist3.UnitRevolute(T[j].a, T[j].t) else: - tw[j] = Twist3.UnitPrismatic(T[j].a) # type: ignore[union-attr] + tw[j] = Twist3.UnitPrismatic(T[j].a) else: # DH case for j, link in enumerate(self): @@ -875,13 +876,13 @@ def twists(self, q=None): else: # subsequent links if link.sigma == 0: - tw[j] = Twist3.UnitRevolute(T[j - 1].a, T[j - 1].t) # type: ignore[union-attr] # revolute + tw[j] = Twist3.UnitRevolute(T[j - 1].a, T[j - 1].t) # revolute else: tw[j] = Twist3.UnitPrismatic(T[j - 1].a) # prismatic return tw, T[-1] - def ets(self, *args, **kwargs) -> ETS: + def ets(self, *args: Any, **kwargs: Any) -> ETS: # type: ignore[override] """ Robot kinematics as an elemenary transform sequence @@ -908,6 +909,7 @@ def ets(self, *args, **kwargs) -> ETS: else: tool = self.tool.A + ets: Any if base is None: ets = ETS() else: @@ -923,7 +925,7 @@ def ets(self, *args, **kwargs) -> ETS: return ets - def fkine(self, q, **kwargs): + def fkine(self, q: Any, **kwargs: Any) -> Any: # type: ignore[override] """ Forward kinematics @@ -974,17 +976,17 @@ def fkine(self, q, **kwargs): Tr = L.A(q) first = False else: - Tr *= L.A(q) # type: ignore + Tr *= L.A(q) if base is not None: - Tr = base * Tr # type: ignore + Tr = base * Tr if tool is not None: - Tr = Tr * tool # type: ignore - T.append(Tr) # type: ignore + Tr = Tr * tool + T.append(Tr) return T - def fkine_path(self, q, old=None): + def fkine_path(self, q: Any, old: Any = None) -> Any: """ Compute the pose of every link frame @@ -1016,12 +1018,12 @@ def fkine_path(self, q, old=None): return T - def segments(self): - segments = [None] + def segments(self) -> Any: + segments: list[Any] = [None] segments.extend(self.links) return [segments] - def fkine_all(self, q=None, old=True): + def fkine_all(self, q: Any = None, old: bool = True) -> Any: """ Forward kinematics for all link frames @@ -1069,7 +1071,7 @@ def fkine_all(self, q=None, old=True): Tall.append(Tj) return Tall - def jacobe(self, q, half=None, **kwargs): + def jacobe(self, q: Any, half: Any = None, **kwargs: Any) -> Any: # type: ignore[override] r""" Manipulator Jacobian in end-effector frame @@ -1104,14 +1106,14 @@ def jacobe(self, q, half=None, **kwargs): n = self.n L = self.links - J = np.zeros((6, self.n), dtype=q.dtype) # type: ignore + J = np.zeros((6, self.n), dtype=q.dtype) U: NDArray = np.asarray(self.tool.A) for j in range(n - 1, -1, -1): if self.mdh == 0: # standard DH convention - U = L[j].A(q[j]).A @ U # type: ignore + U = L[j].A(q[j]).A @ U if not L[j].sigma: # revolute axis @@ -1132,7 +1134,7 @@ def jacobe(self, q, half=None, **kwargs): if self.mdh != 0: # modified DH convention - U = L[j].A(q[j]).A @ U # type: ignore + U = L[j].A(q[j]).A @ U # return top or bottom half if asked if half is not None: @@ -1145,7 +1147,14 @@ def jacobe(self, q, half=None, **kwargs): return J - def jacob0(self, q=None, T=None, half=None, start=None, end=None): + def jacob0( # type: ignore[override] + self, + q: Any = None, + T: Any = None, + half: Any = None, + start: Any = None, + end: Any = None, + ) -> Any: r""" Manipulator Jacobian in world frame @@ -1203,7 +1212,7 @@ def jacob0(self, q=None, T=None, half=None, start=None, end=None): raise ValueError("bad half specified") return J0 - def jacob0_analytical(self, q, representation=None, T=None): + def jacob0_analytical(self, q: Any, representation: Any = None, T: Any = None) -> Any: # type: ignore[override] r""" Manipulator Jacobian in world frame @@ -1280,7 +1289,9 @@ def jacob0_analytical(self, q, representation=None, T=None): A = rotvelxform(gamma, representation=representation, inverse=True, full=True) return A @ J0 - def hessian0(self, q=None, J0=None, start=None, end=None): + def hessian0( # type: ignore[override] + self, q: Any = None, J0: Any = None, start: Any = None, end: Any = None + ) -> Any: r""" Manipulator Hessian in base frame @@ -1336,14 +1347,14 @@ def hessian0(self, q=None, J0=None, start=None, end=None): return self.ets().hessian0(q, J0) - def _get_limit_links(self, end=None, start=None): + def _get_limit_links(self, end: Any = None, start: Any = None) -> Any: # type: ignore[override] # For compatibility with ERobot return None, None, None # -------------------------------------------------------------------------- # - def _copy_to_cpp(self): + def _copy_to_cpp(self) -> None: # Compress link data into a 1D array L = np.zeros(24 * self.n) @@ -1368,7 +1379,7 @@ def _copy_to_cpp(self): self._frne = init(self.n, self.mdh, L) self._frne_stale = False - def delete_rne(self): + def delete_rne(self) -> None: """ Frees the memory holding the robot object in C if it has been initialised. Not normally needed — the destructor handles this @@ -1379,7 +1390,15 @@ def delete_rne(self): self._frne = None self._frne_stale = True - def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): + def rne( # type: ignore[override] + self, + q: Any, + qd: Any = None, + qdd: Any = None, + gravity: Any = None, + fext: Any = None, + base_wrench: bool = False, + ) -> Any: r""" Inverse dynamics @@ -1513,14 +1532,14 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): def rne_python( self, - Q, - QD=None, - QDD=None, - gravity=None, - fext=None, - debug=False, - base_wrench=False, - ): + Q: Any, + QD: Any = None, + QDD: Any = None, + gravity: Any = None, + fext: Any = None, + debug: bool = False, + base_wrench: bool = False, + ) -> Any: """ Compute inverse dynamics via recursive Newton-Euler formulation @@ -1567,7 +1586,7 @@ def rne_python( else: base = self.base.A - def removesmall(x): + def removesmall(x: Any) -> Any: return x n = self.n @@ -1656,7 +1675,7 @@ def removesmall(x): w = np.zeros((3,), dtype=dtype) # base has zero angular acceleration wd = np.zeros((3,), dtype=dtype) - vd = -gravity # type: ignore + vd = -gravity if base is not None: Rb = t2r(base).T @@ -1873,7 +1892,7 @@ def removesmall(x): # -------------------------------------------------------------------------- # - def ikine_6s(self, T, config, ikfunc): + def ikine_6s(self, T: Any, config: Any, ikfunc: Any) -> Any: """ Analytic inverse kinematics for a robot with a spherical wrist @@ -1939,7 +1958,7 @@ def ikine_6s(self, T, config, ikfunc): self.links[5].alpha ) - R = Td4.inv() * T13.inv() * Tk * Tt.inv() # type: ignore[union-attr] + R = Td4.inv() * T13.inv() * Tk * Tt.inv() # The spherical wrist implements Euler angles if "f" in config: @@ -1959,22 +1978,24 @@ def ikine_6s(self, T, config, ikfunc): else: # ikfunc can return None or a str reason if theta is None: - solution = IKSolution(q=None, success=False) + solution = IKSolution(q=None, success=False) # type: ignore[arg-type] else: - solution = IKSolution(q=None, success=False, reason=theta) + solution = IKSolution(q=None, success=False, reason=theta) # type: ignore[arg-type] solutions.append(solution) if len(T) == 1: return solutions[0] else: + # the 2nd and 3rd positional arguments are not what IKSolution + # takes (success, iterations): see findings return IKSolution( np.vstack([sol.q for sol in solutions]), - np.array([sol.success for sol in solutions]), - [sol.reason for sol in solutions], + np.array([sol.success for sol in solutions]), # type: ignore[arg-type] + [sol.reason for sol in solutions], # type: ignore[arg-type] ) - def config_validate(self, config, allowables): + def config_validate(self, config: Any, allowables: Any) -> Any: """ Validate a configuration string @@ -2020,12 +2041,9 @@ def config_validate(self, config, allowables): # -------------------------------------------------------------------------- # - def ikine_LM( - self, - Tep: np.ndarray | SE3, - q0: ArrayLike | None = None, - **ikargs, - ): + def ikine_LM( # type: ignore[override] + self, Tep: np.ndarray | SE3, q0: ArrayLike | None = None, **ikargs: Any + ) -> Any: """ Numerical inverse kinematics by Levenberg-Marquadt optimization @@ -2044,18 +2062,18 @@ def ikine_LM( .. seealso:: :meth:`ikine_LM` (:class:`~roboticstoolbox.ETS`) """ - return self.ets().ikine_LM(Tep=Tep, q0=q0, **ikargs) + return self.ets().ikine_LM(Tep=Tep, q0=q0, **ikargs) # type: ignore[arg-type] class SerialLink(DHRobot): - def __init__(self, *args, **kwargs): + def __init__(self, *args: Any, **kwargs: Any) -> None: warnings.warn( "SerialLink is deprecated, use DHRobot instead", DeprecationWarning ) super().__init__(*args, **kwargs) -def _cross(a, b): +def _cross(a: Any, b: Any) -> Any: return np.r_[ a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0] ] @@ -2074,7 +2092,7 @@ def _cross(a, b): # tw, T0 = puma.twists(puma.qz) # print(planar) - puma = rtb.models.DH.Puma560() + puma = rtb.models.DH.Puma560() # type: ignore[operator] # class, not module print(puma) # print(puma.jacob0(puma.qn, analytical="eul")) # puma.base = None diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index e7b5b1707..60d6e0940 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -27,8 +27,9 @@ alternative to fall back on the way ``DHRobot`` has two. """ -from collections import namedtuple -from typing import Any, Callable +from __future__ import annotations + +from typing import Any, Callable, Literal as L, NamedTuple import numpy as np from spatialmath.base import getvector, verifymatrix, isscalar, getmatrix, t2r, rot2jac from scipy import integrate, interpolate @@ -44,9 +45,17 @@ import warnings +class fdyn(NamedTuple): + """Result of :meth:`DynamicsMixin.fdyn`: time, joint angles and velocities""" + + t: NDArray + q: NDArray + qd: NDArray + + class DynamicsMixin: # --------------------------------------------------------------------- # - def dynamics(self: RobotProto): + def dynamics(self: RobotProto) -> None: """ Pretty print the dynamic parameters (Robot superclass) @@ -80,7 +89,7 @@ def dynamics(self: RobotProto): table.row(link.name, *link._dyn2list()) table.print() - def dynamics_list(self: RobotProto): + def dynamics_list(self: RobotProto) -> None: """ Print dynamic parameters (Robot superclass) @@ -153,7 +162,7 @@ def friction(self: RobotProto, qd: NDArray) -> NDArray: # --------------------------------------------------------------------- # - def nofriction(self: RobotProto, coulomb: bool = True, viscous: bool = False): + def nofriction(self: RobotProto, coulomb: bool = True, viscous: bool = False) -> Any: """ Remove manipulator joint friction @@ -195,7 +204,7 @@ def fdyn( solver_args: dict = {}, dt: float | None = None, progress: bool = False, - ): + ) -> Any: """ Integrate forward dynamics @@ -356,17 +365,17 @@ def fdyn( tnew = np.arange(0, T, dt) xnew = interp(tnew) - return namedtuple("fdyn", "t q qd")(tnew, xnew[:, :n], xnew[:, n:]) + return fdyn(tnew, xnew[:, :n], xnew[:, n:]) else: - return namedtuple("fdyn", "t q qd")(tarray, xarray[:, :n], xarray[:, n:]) + return fdyn(tarray, xarray[:, :n], xarray[:, n:]) def _fdyn( self: RobotProto, t: float, x: NDArray, - Qfunc: Callable[[Any, float, NDArray, NDArray], NDArray], + Qfunc: Callable[[Any, float, NDArray, NDArray], NDArray] | None, Qargs: dict, - ): + ) -> NDArray: """ Private function called by fdyn @@ -403,7 +412,7 @@ def _fdyn( return np.r_[qd, qdd] - def accel(self: RobotProto, q, qd, torque, gravity=None): + def accel(self: RobotProto, q: Any, qd: Any, torque: Any, gravity: Any = None) -> Any: r""" Compute acceleration due to applied torque @@ -496,7 +505,7 @@ def pay( q: NDArray | None = None, J: NDArray | None = None, frame: int = 1, - ): + ) -> Any: """ Generalised joint force/torque due to a payload wrench @@ -593,7 +602,7 @@ def pay( return tau - def payload(self: RobotProto, m: float, p=np.zeros(3)): + def payload(self: RobotProto, m: float, p: Any = np.zeros(3)) -> None: """ Add a payload to the end-effector @@ -658,7 +667,7 @@ def payload(self: RobotProto, m: float, p=np.zeros(3)): + m * (np.dot(dp, dp) * E - np.outer(dp, dp)) ) - def jointdynamics(self: RobotProto, q, qd=None): + def jointdynamics(self: RobotProto, q: Any, qd: Any = None) -> Any: """ Transfer function of joint actuator @@ -702,7 +711,7 @@ def jointdynamics(self: RobotProto, q, qd=None): return tf - def cinertia(self: RobotProto, q): + def cinertia(self: RobotProto, q: Any) -> None: """ Deprecated, use ``inertia_x`` @@ -769,7 +778,7 @@ def inertia(self: RobotProto, q: NDArray) -> NDArray: else: return In - def coriolis(self: RobotProto, q, qd): + def coriolis(self: RobotProto, q: Any, qd: Any) -> Any: r""" Coriolis and centripetal term @@ -865,10 +874,8 @@ def coriolis(self: RobotProto, q, qd): return C def gravload( - self: RobotProto, - q: ArrayLike | None = None, - gravity: ArrayLike | None = None, - ): + self: RobotProto, q: ArrayLike | None = None, gravity: ArrayLike | None = None + ) -> Any: """ Compute gravity load @@ -906,7 +913,7 @@ def gravload( """ - q = getmatrix(q, (None, self.n)) + q = np.asarray(getmatrix(q, (None, self.n))) if gravity is None: gravity = self.gravity @@ -925,8 +932,12 @@ def gravload( return taug def inertia_x( - self: RobotProto, q=None, pinv=False, representation="rpy/xyz", Ji=None - ): + self: RobotProto, + q: Any = None, + pinv: bool = False, + representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz", + Ji: Any = None, + ) -> Any: r""" Operational space inertia matrix @@ -1021,16 +1032,16 @@ def inertia_x( def coriolis_x( self: RobotProto, - q, - qd, - pinv=False, - representation="rpy/xyz", - J=None, - Ji=None, - Jd=None, - C=None, - Mx=None, - ): + q: Any, + qd: Any, + pinv: bool = False, + representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz", + J: Any = None, + Ji: Any = None, + Jd: Any = None, + C: Any = None, + Mx: Any = None, + ) -> Any: r""" Operational space Coriolis and centripetal term @@ -1157,12 +1168,12 @@ def coriolis_x( def gravload_x( self: RobotProto, - q=None, - gravity=None, - pinv=False, - representation="rpy/xyz", - Ji=None, - ): + q: Any = None, + gravity: Any = None, + pinv: bool = False, + representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz", + Ji: Any = None, + ) -> Any: r""" Operational space gravity load @@ -1271,13 +1282,13 @@ def gravload_x( def accel_x( self: RobotProto, - q, - xd, - wrench, - gravity=None, - pinv=False, - representation="rpy/xyz", - ): + q: Any, + xd: Any, + wrench: Any, + gravity: Any = None, + pinv: bool = False, + representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz", + ) -> Any: r""" Operational space acceleration due to applied wrench @@ -1375,7 +1386,7 @@ def accel_x( else: return xdd - def itorque(self: RobotProto, q, qdd): + def itorque(self: RobotProto, q: Any, qdd: Any) -> Any: r""" Inertia torque @@ -1437,7 +1448,7 @@ def paycap( tauR: NDArray, frame: int = 1, q: ArrayLike | None = None, - ): + ) -> Any: """Compute static payload capacity along a wrench direction. :param w: Nonzero payload wrench direction (6,), or (m, 6) for @@ -1459,8 +1470,8 @@ def paycap( - Tool transforms are taken into consideration for ``frame=1``. - Gravity compensation requires a robot model with valid dynamics. """ - q = getmatrix(self.q if q is None else q, (None, self.n)) - w = getmatrix(w, (None, 6)) + q = np.asarray(getmatrix(self.q if q is None else q, (None, self.n))) + w = np.asarray(getmatrix(w, (None, 6))) if q.shape[0] != w.shape[0]: raise ValueError("q and w must have the same number of rows") verifymatrix(tauR, (self.n, 2)) @@ -1491,7 +1502,7 @@ def paycap( return wmax[0, :], joint[0] return wmax, joint - def perturb(self: RobotProto, p=0.1): + def perturb(self: RobotProto, p: float = 0.1) -> Any: """ Perturb robot parameters @@ -1524,8 +1535,14 @@ def perturb(self: RobotProto, p=0.1): def _printProgressBar( - fraction, prefix="", suffix="", decimals=1, length=50, fill="█", printEnd="\r" -): + fraction: Any, + prefix: str = "", + suffix: str = "", + decimals: int = 1, + length: int = 50, + fill: str = "█", + printEnd: str = "\r", +) -> None: percent = ("{0:." + str(decimals) + "f}").format(fraction * 100) filledLength = int(length * fraction) @@ -1536,4 +1553,4 @@ def _printProgressBar( if __name__ == "__main__": # pragma nocover import roboticstoolbox as rtb - puma = rtb.models.DH.Puma560() + puma = rtb.models.DH.Puma560() # type: ignore[operator] # class, not module diff --git a/src/roboticstoolbox/robot/ELink.py b/src/roboticstoolbox/robot/ELink.py index 452b57e5a..5ce847c8e 100644 --- a/src/roboticstoolbox/robot/ELink.py +++ b/src/roboticstoolbox/robot/ELink.py @@ -4,11 +4,15 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox.robot.Link import Link, Link2 class ELink(Link): - def __init__(self, *args, **kwargs): + def __init__(self, *args: Any, **kwargs: Any) -> None: import warnings warnings.warn("ELink is deprecated, use Link instead", FutureWarning) @@ -16,7 +20,7 @@ def __init__(self, *args, **kwargs): class ELink2(Link2): - def __init__(self, *args, **kwargs): + def __init__(self, *args: Any, **kwargs: Any) -> None: import warnings warnings.warn("ELink2 is deprecated, use Link2 instead", FutureWarning) diff --git a/src/roboticstoolbox/robot/ERobot.py b/src/roboticstoolbox/robot/ERobot.py index 82803caa7..efa079b2c 100644 --- a/src/roboticstoolbox/robot/ERobot.py +++ b/src/roboticstoolbox/robot/ERobot.py @@ -3,11 +3,15 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + from roboticstoolbox.robot.Robot import Robot, Robot2 class ERobot(Robot): - def __init__(self, *args, **kwargs): + def __init__(self, *args: Any, **kwargs: Any) -> None: # warn("ERobot is deprecated, use iscollided instead", FutureWarning) @@ -18,7 +22,7 @@ def __init__(self, *args, **kwargs): class ERobot2(Robot2): - def __init__(self, *args, **kwargs): + def __init__(self, *args: Any, **kwargs: Any) -> None: # warn("ERobot2 is deprecated, use iscollided instead", FutureWarning) diff --git a/src/roboticstoolbox/robot/Gripper.py b/src/roboticstoolbox/robot/Gripper.py index b4f0fb9ec..99a7f5ea3 100644 --- a/src/roboticstoolbox/robot/Gripper.py +++ b/src/roboticstoolbox/robot/Gripper.py @@ -3,6 +3,10 @@ @author Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import numpy as np from spatialmath import SE3 import spatialmath as sm @@ -20,11 +24,8 @@ class Gripper(Generic[LinkType]): def __init__( - self, - links: list[LinkType], - name: str = "", - tool: NDArray | SE3 | None = None, - ): + self, links: list[LinkType], name: str = "", tool: NDArray | SE3 | None = None + ) -> None: self._n = 0 @@ -52,7 +53,7 @@ def __init__( ): jindex = [0] # "mutable integer" hack - def visit_link(link, jindex): + def visit_link(link: Any, jindex: Any) -> None: # if it's a joint, assign it a jindex and increment it if link.isjoint: link.jindex = jindex[0] @@ -80,7 +81,7 @@ def visit_link(link, jindex): "all gripper links must have a jindex, or none have a jindex" ) - def __str__(self): + def __str__(self) -> str: s = "Gripper(" if self.name is not None: s += f'"{self.name}"' @@ -91,7 +92,7 @@ def __str__(self): s += ")" return s - def __repr__(self): + def __repr__(self) -> str: links = [link.__repr__() for link in self.links] tool = None if np.all(self.tool.A == np.eye(4)) else self.tool.A.__repr__() @@ -99,9 +100,7 @@ def __repr__(self): return s - def dfs_links( - self, start: LinkType, func: Callable[[Link], None] | None = None - ): + def dfs_links(self, start: LinkType, func: Callable[[Link], None] | None = None) -> Any: """ Search links using depth first search. @@ -115,7 +114,7 @@ def dfs_links( """ visited = [] - def vis_children(link): + def vis_children(link: Any) -> None: visited.append(link) if func is not None: func(link) @@ -145,7 +144,7 @@ def tool(self) -> SE3: return SE3(self._tool, check=False) @tool.setter - def tool(self, T: SE3 | NDArray): + def tool(self, T: SE3 | NDArray) -> None: if isinstance(T, SE3): self._tool = T.A else: @@ -186,7 +185,7 @@ def q(self) -> NDArray: return self._q @q.setter - def q(self, q_new: ArrayLike): + def q(self, q_new: ArrayLike) -> None: self._q = np.array(getvector(q_new, self.n)) # --------------------------------------------------------------------- # @@ -218,25 +217,25 @@ def name(self) -> str: return self._name @name.setter - def name(self, new_name: str): + def name(self, new_name: str) -> None: self._name = new_name # --------------------------------------------------------------------- # # Scene Graph section # --------------------------------------------------------------------- # - def _update_link_tf(self, q: ArrayLike | None = None): + def _update_link_tf(self, q: ArrayLike | None = None) -> None: """ This private method updates the local transform of each link within this robot according to q (or self.q if q is none) """ @lru_cache(maxsize=2) - def get_link_ets(): + def get_link_ets() -> Any: return [link.ets._fknm for link in self.links] @lru_cache(maxsize=2) - def get_link_scene_node(): + def get_link_scene_node() -> Any: return [link._T_reference for link in self.links] Robot_link_T(get_link_ets(), get_link_scene_node(), self._q, q) diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 533691c07..30b798e6e 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -4,14 +4,21 @@ @author Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import numpy as np from abc import ABC, abstractmethod -from typing import Tuple, cast +from typing import TYPE_CHECKING, Tuple, cast import roboticstoolbox as rtb from dataclasses import dataclass from spatialmath import SE3 from roboticstoolbox.tools.types import ArrayLike, NDArray +if TYPE_CHECKING: # pragma: nocover + from roboticstoolbox.ets.ETS import ETS + try: import qpsolvers as qp @@ -82,7 +89,7 @@ class IKSolution: values for the whole solution, not for each pose. """ - q: np.ndarray + q: NDArray success: bool iterations: int = 0 searches: int = 0 @@ -104,7 +111,7 @@ def __len__(self) -> int: """ return 0 if self.q is None else self._rows().shape[0] - def __iter__(self): + def __iter__(self) -> Any: """ Iterate over the joint coordinates of each pose @@ -112,7 +119,7 @@ def __iter__(self): """ return iter(self._rows()) - def __getitem__(self, i): + def __getitem__(self, i: Any) -> Any: """ Joint coordinates of one or more poses @@ -147,7 +154,7 @@ def astuple(self) -> tuple: self.reason, ) - def __repr__(self): + def __repr__(self) -> str: return str(self) def _q_str(self) -> str | None: @@ -157,8 +164,10 @@ def _q_str(self) -> str | None: fmt = {"float": lambda x: "{:.4g}".format(0 if abs(x) < 1e-6 else x)} - def text(a): - return np.array2string(a, separator=", ", formatter=fmt) + def text(a: NDArray) -> str: + return np.array2string( + a, separator=", ", formatter=fmt # type: ignore[arg-type] + ) if self.q.ndim == 1 or len(self) <= 6: return text(self.q) @@ -168,7 +177,7 @@ def text(a): lines = [text(r) for r in rows[:3]] + ["..."] + [text(r) for r in rows[-2:]] return "[" + ",\n ".join(lines) + "]" - def __str__(self): + def __str__(self) -> str: analytic = self.iterations == 0 and self.searches == 0 # everything after q @@ -249,7 +258,7 @@ def __init__( mask: ArrayLike | None = None, joint_limits: bool = True, seed: int | None = None, - ): + ) -> None: # Solver parameters self.name = name self.slimit = slimit @@ -262,13 +271,13 @@ def __init__( if mask is None: mask = np.ones(6) - self.We = np.diag(mask) # type: ignore + self.We = np.diag(mask) self.joint_limits = joint_limits def solve( self, - ets: "rtb.ETS", - Tep: SE3 | np.ndarray, + ets: "ETS", + Tep: SE3 | ArrayLike, q0: ArrayLike | None = None, ) -> IKSolution: """ @@ -308,32 +317,31 @@ def solve( max_jindex: int = 0 for j in ets.joints(): - if j.jindex > max_jindex: # type: ignore - max_jindex = j.jindex # type: ignore + if j.jindex is not None and j.jindex > max_jindex: + max_jindex = j.jindex q0_method = np.zeros((self.slimit, max_jindex + 1)) if q0 is None: q0_method[:, ets.jindices] = self._random_q(ets, self.slimit) - elif not isinstance(q0, np.ndarray): - q0 = np.array(q0) + q0a: NDArray | None = None if q0 is None else np.asarray(q0) - if q0 is not None and q0.ndim == 1: + if q0a is not None and q0a.ndim == 1: q0_method[:, ets.jindices] = self._random_q(ets, self.slimit) - q0_method[0, ets.jindices] = q0 + q0_method[0, ets.jindices] = q0a - if q0 is not None and q0.ndim == 2: + if q0a is not None and q0a.ndim == 2: q0_method[:, ets.jindices] = self._random_q(ets, self.slimit) - q0_method[: q0.shape[0], ets.jindices] = q0 + q0_method[: q0a.shape[0], ets.jindices] = q0a q0 = q0_method traj = False - methTep: np.ndarray + methTep: NDArray if isinstance(Tep, SE3): if len(Tep) > 1: @@ -344,13 +352,13 @@ def solve( methTep[i] = T.A else: methTep = cast(NDArray, Tep.A) - elif Tep.ndim == 3: + elif np.ndim(Tep) == 3: traj = True - methTep = Tep - elif Tep.shape != (4, 4): + methTep = np.asarray(Tep) + elif np.shape(Tep) != (4, 4): raise ValueError("Tep must be an SE3, a 4x4 array or an (N, 4, 4) array") else: - methTep = Tep + methTep = np.asarray(Tep) if traj: q = np.empty((methTep.shape[0], ets.n)) @@ -386,7 +394,7 @@ def solve( return sol - def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution: + def _solve(self, ets: "ETS", Tep: NDArray, q0: NDArray) -> IKSolution: # Iteration count i = 0 total_i = 0 @@ -464,7 +472,7 @@ def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution: reason=reason, ) - def error(self, Te: np.ndarray, Tep: np.ndarray) -> Tuple[np.ndarray, float]: + def error(self, Te: NDArray, Tep: NDArray) -> Tuple[NDArray, float]: r""" Calculates the error between Te and Tep @@ -491,8 +499,8 @@ def error(self, Te: np.ndarray, Tep: np.ndarray) -> Tuple[np.ndarray, float]: @abstractmethod def step( - self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray - ) -> Tuple[float, np.ndarray]: + self, ets: "ETS", Tep: NDArray, q: NDArray + ) -> Tuple[float, NDArray]: """ Abstract step method @@ -509,7 +517,7 @@ def step( """ pass # pragma: nocover - def _random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray: + def _random_q(self, ets: "ETS", i: int = 1) -> NDArray: """ Generate a random valid joint configuration using a private RNG @@ -550,7 +558,7 @@ def _random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray: return q - def _normalise_q(self, ets: "rtb.ETS", q: np.ndarray) -> None: + def _normalise_q(self, ets: "ETS", q: NDArray) -> None: """ Choose equivalent revolute coordinates without changing prismatic joints. @@ -562,11 +570,11 @@ def _normalise_q(self, ets: "rtb.ETS", q: np.ndarray) -> None: ETS and applies :func:`normalise_q`. """ joints = ets.joints() - jindex = [joint.jindex for joint in joints] + jindex = cast(list[int], [joint.jindex for joint in joints]) revolute = np.array([joint.isrotation for joint in joints]) q[jindex] = normalise_q(q[jindex], ets.qlim, revolute) - def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool: + def _check_jl(self, ets: "ETS", q: NDArray) -> bool: """ Checks if the joints are within their respective limits @@ -643,7 +651,7 @@ def within_limits(q: ArrayLike, qlim: ArrayLike) -> bool: qlim = np.asarray(qlim, dtype=float) return not bool(np.any((q < qlim[0]) | (q > qlim[1]))) -def _null_Σ(ets: "rtb.ETS", q: NDArray, ps: float, pi: NDArray | float): +def _null_Σ(ets: "ETS", q: NDArray, ps: float, pi: ArrayLike | float) -> Any: """ Formulates a relationship between joint limits and the joint velocity. When this is projected into the null-space of the differential kinematics @@ -679,14 +687,14 @@ def _null_Σ(ets: "rtb.ETS", q: NDArray, ps: float, pi: NDArray | float): def _calc_qnull( - ets: "rtb.ETS", - q: np.ndarray, - J: np.ndarray, + ets: "ETS", + q: NDArray, + J: NDArray, λΣ: float, λm: float, ps: float, - pi: np.ndarray | float, -): + pi: ArrayLike | float, +) -> Any: """ Calculates the desired null-space motion according to the gains λΣ and λm. This is a helper method that is used within the `step` method of an IK solver @@ -814,9 +822,9 @@ def __init__( kq: float = 0.0, km: float = 0.0, ps: float = 0.0, - pi: np.ndarray | float = 0.3, - **kwargs, - ): + pi: ArrayLike | float = 0.3, + **kwargs: Any, + ) -> None: super().__init__( name=name, ilimit=ilimit, @@ -843,8 +851,8 @@ def __init__( self.name += " Jm" def step( - self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray - ) -> Tuple[float, np.ndarray]: + self, ets: "ETS", Tep: NDArray, q: NDArray + ) -> Tuple[float, NDArray]: r""" Performs a single iteration of the Newton-Raphson optimisation method @@ -977,13 +985,13 @@ def __init__( joint_limits: bool = True, seed: int | None = None, k: float = 1.0, - method="chan", + method: str = "chan", kq: float = 0.0, km: float = 0.0, ps: float = 0.0, - pi: np.ndarray | float = 0.3, - **kwargs, - ): + pi: ArrayLike | float = 0.3, + **kwargs: Any, + ) -> None: super().__init__( name=name, ilimit=ilimit, @@ -1019,7 +1027,7 @@ def __init__( if self.km > 0.0: self.name += " Jm" - def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray): + def step(self, ets: "ETS", Tep: NDArray, q: NDArray) -> Any: r""" Performs a single iteration of the Levenberg-Marquardt optimisation @@ -1212,9 +1220,9 @@ def __init__( kq: float = 0.0, km: float = 0.0, ps: float = 0.0, - pi: np.ndarray | float = 0.3, - **kwargs, - ): + pi: ArrayLike | float = 0.3, + **kwargs: Any, + ) -> None: super().__init__( name=name, ilimit=ilimit, @@ -1241,8 +1249,8 @@ def __init__( self.name += " Jm" def step( - self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray - ) -> Tuple[float, np.ndarray]: + self, ets: "ETS", Tep: NDArray, q: NDArray + ) -> Tuple[float, NDArray]: r""" Performs a single iteration of the Gauss-Newton optimisation method @@ -1382,14 +1390,14 @@ def __init__( mask: ArrayLike | None = None, joint_limits: bool = True, seed: int | None = None, - kj=0.01, - ks=1.0, + kj: float = 0.01, + ks: float = 1.0, kq: float = 0.0, km: float = 0.0, ps: float = 0.0, - pi: np.ndarray | float = 0.3, - **kwargs, - ): + pi: ArrayLike | float = 0.3, + **kwargs: Any, + ) -> None: if not _qp: # pragma: nocover raise ImportError( "the package qpsolvers is required for this class. \nInstall using 'pip" @@ -1423,8 +1431,8 @@ def __init__( self.name += " Jm" def step( - self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray - ) -> Tuple[float, np.ndarray]: + self, ets: "ETS", Tep: NDArray, q: NDArray + ) -> Tuple[float, NDArray]: r""" Performs a single iteration of the QP optimisation method diff --git a/src/roboticstoolbox/robot/Link.py b/src/roboticstoolbox/robot/Link.py index ccae7b6be..e73afe6a1 100644 --- a/src/roboticstoolbox/robot/Link.py +++ b/src/roboticstoolbox/robot/Link.py @@ -1,5 +1,7 @@ from __future__ import annotations +from typing import Any + from copy import deepcopy from abc import ABC from typing_extensions import Self @@ -28,7 +30,7 @@ # ETType = TypeVar("ETType", bound=BaseET) -def _copy_shapes(shapes, link_name): +def _copy_shapes(shapes: Any, link_name: Any) -> Any: """Copy a list of coal/geometry shape objects, warning and skipping any that do not support deepcopy (coal CollisionObject instances cannot be pickled).""" import warnings @@ -53,7 +55,7 @@ def _copy_shapes(shapes, link_name): return result -def _dirties_frne(func): +def _dirties_frne(func: Any) -> Any: """ @_dirties_frne @@ -80,7 +82,7 @@ def m(self, m_new): """ @wraps(func) - def wrapper_dirties_frne(*args): + def wrapper_dirties_frne(*args: Any) -> Any: if args[0]._robot is not None: args[0]._robot.dynchanged() args[0]._hasdynamics = True @@ -134,7 +136,7 @@ class BaseLink(SceneNode, ABC): def __init__( self, ets: ETS | ETS2 | ET | ET2 = ETS(), - name=None, + name: Any = None, parent: Self | str | None = None, joint_name: str | None = None, m: float | None = None, @@ -149,13 +151,13 @@ def __init__( tlim: float | None = None, geometry: list[Shape] = [], collision: list[Shape] = [], - **kwargs, - ): + **kwargs: Any, + ) -> None: # Initialise the scene node super().__init__() # Reference to parent robot - self._robot = None + self._robot: rtb.BaseRobot | None = None # Set name of link and joint() if name is None: @@ -172,7 +174,7 @@ def __init__( self._scene_children.append(self._collision) # Link dynamic Parameters - def dynpar(self, name, value, default): + def dynpar(self: Any, name: Any, value: Any, default: Any) -> Any: if value is None: value = default setattr(self, name, value) @@ -213,7 +215,7 @@ def dynpar(self, name, value, default): if parent is not None: if isinstance(parent, str): self.parent = None - self._parent_name = parent + self._parent_name: str | None = parent elif isinstance(parent, BaseLink): self.parent = parent self._parent_name = None @@ -221,11 +223,11 @@ def dynpar(self, name, value, default): else: raise TypeError("parent must be BaseLink subclass") else: - self._parent = None + self._parent: Self | None = None self._parent_name = None self._joint_name = joint_name - self._children = [] + self._children: list[Self] = [] self.number = 0 @@ -241,7 +243,7 @@ def dynpar(self, name, value, default): # -------------------------------------------------------------------------- # - def _init_Ts(self): + def _init_Ts(self) -> None: # Compute the leading, constant, part of the ETS if isinstance(self, Link2): @@ -301,10 +303,10 @@ def ets(self) -> ETS: """ - return self._ets # type: ignore[return-value] # _ets is ETS|ETS2; ETS for Link subclasses + return self._ets # _ets is ETS|ETS2; ETS for Link subclasses @ets.setter - def ets(self, new_ets: ETS | ETS2): + def ets(self, new_ets: ETS | ETS2) -> None: if new_ets.n > 1: raise ValueError("An elementary link can only have one joint variable") @@ -314,6 +316,7 @@ def ets(self, new_ets: ETS | ETS2): self._ets = new_ets self._init_Ts() + self._v: ET | ET2 | None if self._ets.n: self._v = self._ets[-1] self._isjoint = True @@ -361,7 +364,7 @@ def __str__(self) -> str: s += ")" return s - def _repr_pretty_(self, p, cycle): + def _repr_pretty_(self, p: Any, cycle: Any) -> None: """ Pretty string for IPython @@ -390,10 +393,10 @@ def copy(self: Self) -> Self: return deepcopy(self) - def _copy(self): + def _copy(self) -> None: raise DeprecationWarning("Use copy method of Link class") - def __deepcopy__(self, memo): + def __deepcopy__(self, memo: Any) -> Any: ets = deepcopy(self.ets) name = deepcopy(self.name) parent = self.parent @@ -427,8 +430,8 @@ def __deepcopy__(self, memo): qlim=qlim, qdlim=qdlim, tlim=tlim, - geometry=geometry, # type: ignore - collision=collision, # type: ignore + geometry=geometry, + collision=collision, ) if self._children: @@ -480,7 +483,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, name: str): + def name(self, name: str) -> None: self._name = name # -------------------------------------------------------------------------- # @@ -499,7 +502,7 @@ def robot(self) -> rtb.BaseRobot | None: return self._robot @robot.setter - def robot(self, robot_ref: "rtb.BaseRobot"): + def robot(self, robot_ref: "rtb.BaseRobot") -> None: """ Set the forward reference to the robot which owns this link """ @@ -532,7 +535,7 @@ def qlim(self) -> NDArray | None: return None @qlim.setter - def qlim(self, qlim_new: ArrayLike): + def qlim(self, qlim_new: ArrayLike) -> None: if self.v: self.ets.qlim = qlim_new else: @@ -559,7 +562,7 @@ def qdlim(self) -> float | None: return self._qdlim @qdlim.setter - def qdlim(self, qdlim_new: float): + def qdlim(self, qdlim_new: float) -> None: self._qdlim = qdlim_new @property @@ -583,7 +586,7 @@ def tlim(self) -> float | None: return self._tlim @tlim.setter - def tlim(self, tlim_new: float): + def tlim(self, tlim_new: float) -> None: self._tlim = tlim_new @property @@ -656,7 +659,7 @@ def m(self) -> float: @m.setter @_dirties_frne - def m(self, m_new: float): + def m(self, m_new: float) -> None: self._m = m_new # -------------------------------------------------------------------------- # @@ -676,11 +679,11 @@ def r(self) -> NDArray: - ``link.r = ...`` checks and sets the link centre of mass """ - return self._r # type: ignore + return self._r @r.setter @_dirties_frne - def r(self, r_new: ArrayLike): + def r(self, r_new: ArrayLike) -> None: self._r = getvector(r_new, 3) # -------------------------------------------------------------------------- # @@ -716,11 +719,11 @@ def I(self) -> NDArray: """ - return self._I # type: ignore + return self._I @I.setter @_dirties_frne - def I(self, I_new: ArrayLike): + def I(self, I_new: ArrayLike) -> None: if ismatrix(I_new, (3, 3)): # 3x3 matrix passed if np.any(np.abs(I_new - I_new.T) > 1e-8): # type: ignore @@ -729,7 +732,7 @@ def I(self, I_new: ArrayLike): elif isvector(I_new, 9): # 3x3 matrix passed as a 1d vector I_new = I_new.reshape(3, 3) # type: ignore - 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, 6): @@ -737,15 +740,15 @@ def I(self, I_new: ArrayLike): # [Ixx Iyy Izz Ixy Iyz Ixz] I_new = np.array( [ - [I_new[0], I_new[3], I_new[5]], # type: ignore - [I_new[3], I_new[1], I_new[4]], # type: ignore - [I_new[5], I_new[4], I_new[2]], # type: ignore + [I_new[0], I_new[3], I_new[5]], + [I_new[3], I_new[1], I_new[4]], + [I_new[5], I_new[4], I_new[2]], ] ) elif isvector(I_new, 3): # 3-vector passed, moments of inertia [Ixx Iyy Izz] - I_new = np.diag(I_new) # type: ignore + I_new = np.diag(I_new) else: raise ValueError("invalid shape passed: must be (3,3), (6,), (3,)") @@ -774,7 +777,7 @@ def Jm(self) -> float: @Jm.setter @_dirties_frne - def Jm(self, Jm_new: float): + def Jm(self, Jm_new: float) -> None: self._Jm = Jm_new # -------------------------------------------------------------------------- # @@ -799,7 +802,7 @@ def B(self) -> float: @B.setter @_dirties_frne - def B(self, B_new: float): + def B(self, B_new: float) -> None: if isscalar(B_new): self._B = B_new else: @@ -841,12 +844,12 @@ def Tc(self) -> NDArray: @Tc.setter @_dirties_frne - def Tc(self, Tc_new: ArrayLike): + def Tc(self, Tc_new: ArrayLike) -> None: try: # sets Coulomb friction parameters to [F -F], for a symmetric # Coulomb friction model. Tc = getvector(Tc_new, 1) - Tc_new = np.array([Tc[0], -Tc[0]]) # type: ignore + Tc_new = np.array([Tc[0], -Tc[0]]) except ValueError: # [FP FM] sets Coulomb friction to [FP FM], for an asymmetric # Coulomb friction model. FP>0 and FM<0. FP is applied for a @@ -880,7 +883,7 @@ def G(self) -> float: @G.setter @_dirties_frne - def G(self, G_new: float): + def G(self, G_new: float) -> None: self._G = G_new # -------------------------------------------------------------------------- # @@ -900,6 +903,15 @@ def geometry(self) -> SceneGroup: """ return self._geometry + @geometry.setter + def geometry(self, geom: SceneGroup | list[Shape] | Shape) -> None: + if isinstance(geom, list): + self.geometry.scene_children = geom + elif isinstance(geom, Shape): + self.geometry.scene_children.append(geom) + elif isinstance(geom, SceneGroup): + self._geometry = geom + @property def collision(self) -> SceneGroup: """ @@ -919,23 +931,14 @@ def collision(self) -> SceneGroup: return self._collision @collision.setter - def collision(self, coll: SceneGroup | list[Shape] | Shape): + def collision(self, coll: SceneGroup | list[Shape] | Shape) -> None: if isinstance(coll, list): - self.collision.scene_children = coll # type: ignore + self.collision.scene_children = coll elif isinstance(coll, Shape): self.collision.scene_children.append(coll) elif isinstance(coll, SceneGroup): self._collision = coll - @geometry.setter - def geometry(self, geom: SceneGroup | list[Shape] | Shape): - if isinstance(geom, list): - self.geometry.scene_children = geom # type: ignore - elif isinstance(geom, Shape): - self.geometry.scene_children.append(geom) - elif isinstance(geom, SceneGroup): - self._geometry = geom - # -------------------------------------------------------------------------- # @property @@ -990,7 +993,7 @@ def jindex(self) -> int | None: return None if not self.v else self.v._jindex @jindex.setter - def jindex(self, j: int): + def jindex(self, j: int) -> None: if self.v: self.v.jindex = j self.ets._auto_jindex = False @@ -1039,7 +1042,7 @@ def parent(self) -> Self | None: return self._parent @parent.setter - def parent(self, parent: Self | None): + def parent(self, parent: Self | None) -> None: self._parent = parent @property @@ -1081,7 +1084,7 @@ def nchildren(self) -> int: def closest_point( self, shape: Shape, inf_dist: float = 1.0, skip: bool = False - ) -> tuple[int | None, NDArray | None, NDArray | None]: + ) -> tuple[float | None, NDArray | None, NDArray | None]: """ Finds the closest point to a shape @@ -1104,7 +1107,7 @@ def closest_point( self.update() shape.update() - d = 10000 + d: float | None = 10000 p1 = None p2 = None @@ -1143,7 +1146,7 @@ def iscollided(self, shape: Shape, skip: bool = False) -> bool: return False - def collided(self, shape: Shape, skip: bool = False): + def collided(self, shape: Shape, skip: bool = False) -> Any: """ Checks for collision with a shape @@ -1157,7 +1160,7 @@ def collided(self, shape: Shape, skip: bool = False): warn("base kwarg is deprecated, use pose instead", FutureWarning) return self.iscollided(shape=shape, skip=skip) - def dyn(self, indent=0): + def dyn(self, indent: int = 0) -> Any: """ Inertial properties of link as a string @@ -1225,7 +1228,7 @@ def dyn(self, indent=0): return s - def _dyn2list(self, fmt="{: .3g}"): + def _dyn2list(self, fmt: str = "{: .3g}") -> Any: """ Inertial properties of link as a string @@ -1245,7 +1248,7 @@ def _dyn2list(self, fmt="{: .3g}"): border="thin", ) - def format(l, fmt, val): + def format(l: Any, fmt: Any, val: Any) -> None: if isinstance(val, np.ndarray): try: s = ", ".join([fmt.format(v) for v in val]) @@ -1260,7 +1263,7 @@ def format(l, fmt, val): s = str(val) l.append(s) - dyn = [] + dyn: list[str] = [] format(dyn, fmt, self.m) format(dyn, fmt, self.r) I = self.I.flatten() @@ -1274,12 +1277,12 @@ def format(l, fmt, val): def _format_param( self, - l, - name, - symbol=None, - ignorevalue=None, - indices=None, - ): # noqa # pragma nocover + l: list[str], + name: str, + symbol: str | None = None, + ignorevalue: Any = None, + indices: Any = None, + ) -> None: # noqa # pragma nocover # if value == ignorevalue then don't display it v = getattr(self, name) @@ -1313,9 +1316,9 @@ def _format_param( if s is not None: l.append(s) - def _params(self, name: bool = True): # pragma nocover + def _params(self, name: bool = True) -> Any: # pragma nocover - l = [] + l: list[str] = [] if name: self._format_param(l, "name") if self.parent_name is not None: @@ -1336,7 +1339,7 @@ def _params(self, name: bool = True): # pragma nocover return l - def islimit(self, q: float): + def islimit(self, q: float) -> Any: """ Checks if joint exceeds limit @@ -1358,7 +1361,7 @@ def islimit(self, q: float): else: return q < self.qlim[0] or q > self.qlim[1] - def nofriction(self, coulomb: bool = True, viscous: bool = False): + def nofriction(self, coulomb: bool = True, viscous: bool = False) -> Any: """ Clone link without friction @@ -1387,7 +1390,7 @@ def nofriction(self, coulomb: bool = True, viscous: bool = False): return link - def friction(self, qd: float, coulomb: bool = True): + def friction(self, qd: float, coulomb: bool = True) -> Any: r""" Compute joint friction @@ -1479,8 +1482,8 @@ class Link(BaseLink): """ def __init__( - self, ets: ETS | ET = ETS(), jindex: int | None = None, **kwargs - ): + self, ets: ETS | ET = ETS(), jindex: int | None = None, **kwargs: Any + ) -> None: # process common options super().__init__(ets=ets, **kwargs) @@ -1519,7 +1522,9 @@ def A(self, q: float = 0.0) -> SE3: class Link2(BaseLink): - def __init__(self, ets: ETS2 = ETS2(), jindex: int | None = None, **kwargs): + def __init__( + self, ets: ETS2 = ETS2(), jindex: int | None = None, **kwargs: Any + ) -> None: # process common options super().__init__(ets=ets, **kwargs) diff --git a/src/roboticstoolbox/robot/PoERobot.py b/src/roboticstoolbox/robot/PoERobot.py index 0a6a32ac8..b76ef6991 100644 --- a/src/roboticstoolbox/robot/PoERobot.py +++ b/src/roboticstoolbox/robot/PoERobot.py @@ -1,8 +1,13 @@ + +from __future__ import annotations + +from typing import Any # PoERobot import numpy as np from spatialmath import Twist3, SE3 from spatialmath.base import skew -from roboticstoolbox.robot import Link, Robot +from roboticstoolbox.robot.Link import Link +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.ets.ET import ET from roboticstoolbox.ets.ETS import ETS @@ -16,7 +21,7 @@ class PoELink(Link): :seealso: :class:`Link` """ - def __init__(self, twist, name: str | None = None): + def __init__(self, twist: Any, name: str | None = None) -> None: # get ETS of the link in the world frame, given by its twist ets = self._ets_world(twist) @@ -25,14 +30,14 @@ def __init__(self, twist, name: str | None = None): self.S = Twist3(twist) self.name = name if name is not None else "" - def __repr__(self): - s = f"PoELink({np.array2string(self.S.S, separator=',')}" # type: ignore[arg-type] + def __repr__(self) -> str: + s = f"PoELink({np.array2string(self.S.S, separator=',')}" if self.name: s += f', name="{self.name}"' s += ")" return s - def __str__(self): + def __str__(self) -> str: s = f"{self.__class__.__name__}[twist={self.S}" if self.name is not None: s += f', name="{self.name}"' @@ -109,7 +114,7 @@ def _ets_world(self, twist: Twist3) -> ETS: ET.Rx(rpy[0]), ] # remove ETs with empty transform (param=None means joint variable, skip) - et_list = [et for et in et_list if et.param is None or not np.isclose(et.param, 0.0)] # type: ignore[arg-type] + et_list = [et for et in et_list if et.param is None or not np.isclose(et.param, 0.0)] # assign joint variable at the end of list (if the frame is not base or tool # frame) @@ -122,7 +127,7 @@ def _ets_world(self, twist: Twist3) -> ETS: class PoERevolute(PoELink): - def __init__(self, axis, point, **kwargs): + def __init__(self, axis: Any, point: Any, **kwargs: Any) -> None: """ Construct revolute product of exponential link @@ -140,7 +145,7 @@ def __init__(self, axis, point, **kwargs): class PoEPrismatic(PoELink): - def __init__(self, axis, **kwargs): + def __init__(self, axis: Any, **kwargs: Any) -> None: """ Construct prismatic product of exponential link @@ -155,7 +160,7 @@ def __init__(self, axis, **kwargs): class PoERobot(Robot): - def __init__(self, links, T0, **kwargs): + def __init__(self, links: Any, T0: Any, **kwargs: Any) -> None: """ Product of exponential robot class @@ -182,10 +187,10 @@ def __init__(self, links, T0, **kwargs): self._update_ets() @property - def links(self) -> list[PoELink]: # type: ignore[override] - return self._links # type: ignore[return-value] + def links(self) -> list[PoELink]: + return self._links - def __str__(self): + def __str__(self) -> str: """ Pretty prints the PoE Model of the robot. :return: Pretty print of the robot model @@ -198,7 +203,7 @@ def __str__(self): s += f" T0: {self.T0.strline()}" return s - def __repr__(self): + def __repr__(self) -> str: s = "PoERobot([\n" s += "\n".join([" " + repr(link) + "," for link in self]) s += "\n ],\n" @@ -207,10 +212,10 @@ def __repr__(self): s += ")" return s - def nbranches(self): + def nbranches(self) -> Any: # type: ignore[override] return 0 - def fkine(self, q): + def fkine(self, q: Any) -> Any: # type: ignore[override] """ Forward kinematics @@ -231,7 +236,7 @@ def fkine(self, q): # T = reduce(lambda x, y: x * y, # [link.A(qk) for link, qk in zip(self, q)]) - def jacob0(self, q): + def jacob0(self, q: Any) -> Any: # type: ignore[override] """ Jacobian in world frame @@ -243,17 +248,17 @@ def jacob0(self, q): columns = [] T = SE3() for i in range(self.n): - columns.append(T.Ad() @ self.links[i + 1].S.S) # type: ignore[union-attr] + columns.append(T.Ad() @ self.links[i + 1].S.S) T *= self.links[i + 1].S.exp(q[i]) T *= self.T0 J = np.column_stack(columns) # convert Jacobian from velocity twist to spatial velocity Jsv = np.eye(6) - Jsv[:3, 3:] = -skew(T.t) # type: ignore[union-attr] + Jsv[:3, 3:] = -skew(T.t) return Jsv @ J - def jacobe(self, q): + def jacobe(self, q: Any) -> Any: # type: ignore[override] """ Jacobian in end-effector frame @@ -265,15 +270,15 @@ def jacobe(self, q): columns = [] T = SE3() for i in range(self.n): - columns.append(T.Ad() @ self.links[i + 1].S.S) # type: ignore[union-attr] + columns.append(T.Ad() @ self.links[i + 1].S.S) T *= self.links[i + 1].S.exp(q[i]) T *= self.T0 J = np.column_stack(columns) # convert velocity twist from world frame to EE frame - return T.inv().Ad() @ J # type: ignore[union-attr] + return T.inv().Ad() @ J - def _update_ets(self): + def _update_ets(self) -> None: """ Updates ETS of links when PoERobot is initialized according to joint order @@ -315,7 +320,7 @@ def _update_ets(self): ET.Rx(rpy[0]), ] # remove ETs with empty transform - et_list = [et for et in et_list if et.param is None or not np.isclose(et.param, 0.0)] # type: ignore[arg-type] + et_list = [et for et in et_list if et.param is None or not np.isclose(et.param, 0.0)] # assign joint variable with corresponding index if self.links[i].isrevolute: @@ -328,14 +333,14 @@ def _update_ets(self): if __name__ == "__main__": # pragma nocover - T0 = SE3.Trans(2, 0, 0) # type: ignore[call-overload] + T0 = SE3.Trans(2, 0, 0) # rotate about z-axis, through (0,0,0) link1 = PoERevolute([0, 0, 1], [0, 0, 0], name="foo") # rotate about z-axis, through (1,0,0) link2 = PoERevolute([0, 0, 1], [1, 0, 0]) # end-effector pose when q=[0,0] - TE0 = SE3.Trans(2, 0, 0) # type: ignore[call-overload] + TE0 = SE3.Trans(2, 0, 0) print(repr(link1)) print(link1) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 8f127640e..561095b40 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -12,7 +12,7 @@ from copy import deepcopy from warnings import warn from pathlib import PurePosixPath, Path -from typing import TextIO, TypeVar, Literal as L, overload, cast +from typing import Any, TextIO, TypeVar, Literal as L, overload, cast import numpy as np @@ -79,7 +79,7 @@ def __init__( symbolic: bool = False, configs: dict[str, NDArray] | None = None, check_jindex: bool = True, - ): + ) -> None: # Process links if isinstance(arg, Robot): # We're passed a Robot, clone it @@ -137,18 +137,18 @@ def __init__( # the first segment, so `base` is always empty and dropped; # `gripper` holds trailing constant content, if any, and # becomes one extra static (non-joint) link. - _, *segs, gripper = arg.split() - if gripper: - segs.append(gripper) + _, *segs, tail_ets = arg.split() + if tail_ets: + segs.append(tail_ets) parent = None for j, ets_j in enumerate(segs): elink = Link(ETS(ets_j), parent=parent, name=f"link{j:d}") if ( elink.qlim is None and elink.v is not None - and elink.v.qlim is not None # type: ignore[union-attr] + and elink.v.qlim is not None ): - elink.qlim = elink.v.qlim # type: ignore[union-attr] # pragma nocover + elink.qlim = elink.v.qlim # pragma nocover parent = elink links.append(elink) @@ -178,7 +178,7 @@ def __init__( # --------- Swift Methods --------------------------------------------- # # --------------------------------------------------------------------- # - def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0): + def _to_dict(self, robot_alpha: float = 1.0, collision_alpha: float = 0.0) -> Any: ob = [] for link in self.links: @@ -208,7 +208,7 @@ def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0): return ob - def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0): + def _fk_dict(self, robot_alpha: float = 1.0, collision_alpha: float = 0.0) -> Any: ob = [] # Do the robot @@ -343,7 +343,7 @@ def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0): # ) @classmethod - def URDF(cls, file, gripper=None, manufacturer=None): + def URDF(cls, file: Any, gripper: Any = None, manufacturer: Any = None) -> Any: """Deprecated. Use :class:`~roboticstoolbox.models.URDF.URDFRobot` as a base class, or call ``URDF_read()`` from ``roboticstoolbox.models.URDF.URDFRobot`` and construct ``Robot`` @@ -653,9 +653,9 @@ def reach(self) -> float: # given ets. Then use an lru_cache to speed up return if self._reach is None: - d_all = [] + d_all: list[float] = [] for link in self.ee_links: - d = 0 + d: float = 0 while True: for et in link.ets: if et.istranslation: @@ -669,8 +669,8 @@ def reach(self) -> float: elif et.qlim is not None: # pragma nocover d += max(et.qlim) else: - d += abs(et.param) - link = link.parent + d += abs(cast(float, et.param)) + link = link.parent # type: ignore[assignment] if link is None or isinstance(link, str): d_all.append(d) break @@ -722,7 +722,7 @@ def fkine_all(self, q: ArrayLike) -> SE3: if link.isjoint } - def recurse(Tall, Tparent, q, link): + def recurse(Tall: Any, Tparent: Any, q: Any, link: Any) -> None: # if joint?? T = Tparent while True: @@ -820,7 +820,7 @@ def manipulability( "yoshikawa", "asada", "minsingular", "invcondition" ] = "yoshikawa", axes: L["all", "trans", "rot"] | list[bool] = "all", - **kwargs, + **kwargs: Any, ) -> float | NDArray: # pragma nocover ... @@ -835,22 +835,20 @@ def manipulability( "yoshikawa", "asada", "minsingular", "invcondition" ] = "yoshikawa", axes: L["all", "trans", "rot"] | list[bool] = "all", - **kwargs, + **kwargs: Any, ) -> float | NDArray: # pragma nocover ... def manipulability( self, - q=None, - J=None, + q: Any = None, + J: Any = None, end: str | Link | Gripper | None = None, start: str | Link | Gripper | None = None, - method: L[ - "yoshikawa", "asada", "minsingular", "invcondition" - ] = "yoshikawa", + method: L["yoshikawa", "asada", "minsingular", "invcondition"] = "yoshikawa", axes: L["all", "trans", "rot"] | list[bool] = "all", - **kwargs, - ): + **kwargs: Any, + ) -> Any: """ Manipulability measure @@ -942,7 +940,7 @@ def manipulability( else: raise ValueError("axes must be all, trans, rot or both") - def yoshikawa(robot, J, q, axes_list): + def yoshikawa(robot: Any, J: Any, q: Any, axes_list: Any) -> Any: J = J[axes_list, :] if J.shape[0] == J.shape[1]: # simplified case for square matrix @@ -951,20 +949,20 @@ def yoshikawa(robot, J, q, axes_list): m2 = np.linalg.det(J @ J.T) return np.sqrt(abs(m2)) - def condition(robot, J, q, axes_list): + def condition(robot: Any, J: Any, q: Any, axes_list: Any) -> Any: J = J[axes_list, :] # return 1/cond(J) return 1 / np.linalg.cond(J) - def minsingular(robot, J, q, axes_list): + def minsingular(robot: Any, J: Any, q: Any, axes_list: Any) -> Any: J = J[axes_list, :] s = np.linalg.svd(J, compute_uv=False) # return last/smallest singular value of J return s[-1] - def asada(robot, J, q, axes_list): + def asada(robot: Any, J: Any, q: Any, axes_list: Any) -> Any: # dof = np.sum(axes_list) if np.linalg.matrix_rank(J) < 6: return 0 @@ -989,6 +987,7 @@ def asada(robot, J, q, axes_list): raise ValueError("Invalid method chosen") # Calculate manipulability based on supplied Jacobian + w: Any if J is not None: w = [mfunc(self, J, q, axes_list)] @@ -997,7 +996,6 @@ def asada(robot, J, q, axes_list): if q is None: raise ValueError("Either J or q must be supplied") - q = getmatrix(q, (None, self.n)) q = np.array(getmatrix(q, (None, self.n))) w = np.zeros(q.shape[0]) @@ -1017,7 +1015,7 @@ def jtraj( t: ArrayLike | int, qd0: ArrayLike | None = None, qd1: ArrayLike | None = None, - ): + ) -> Any: """ Joint-space trajectory between two joint configurations @@ -1086,11 +1084,11 @@ def jacob0_dot( def jacob0_dot( self, - q, + q: Any, qd: ArrayLike, - J0=None, + J0: Any = None, representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None, - ): + ) -> Any: r""" Time derivative of manipulator Jacobian in frame {0} @@ -1248,9 +1246,9 @@ def jacobm( def jacobm( self, - q=None, - J=None, - H=None, + q: Any = None, + J: Any = None, + H: Any = None, end: str | Link | Gripper | None = None, start: str | Link | Gripper | None = None, axes: L["all", "trans", "rot"] | list[bool] = "all", @@ -1326,15 +1324,15 @@ def jacobm( raise ValueError("Hessian must be numpy array of shape 6xnxn") manipulability = self.manipulability( - q, # type: ignore[arg-type] + q, J=J, start=start, end=end, - axes=axes, # type: ignore + axes=axes, ) - J = J[axes, :] # type: ignore - H = H[:, axes, :] # type: ignore + J = J[axes, :] + H = H[:, axes, :] b = np.linalg.inv(J @ np.transpose(J)) Jm = np.zeros((n, 1)) @@ -1351,7 +1349,7 @@ def jacobm( def closest_point( self, q: ArrayLike, shape: Shape, inf_dist: float = 1.0, skip: bool = False - ) -> tuple[int | None, NDArray | None, NDArray | None]: + ) -> tuple[float | None, NDArray | None, NDArray | None]: """ Find the closest point between robot and shape @@ -1374,14 +1372,14 @@ def closest_point( self.update() shape.update() - d = 10000 + d: float | None = 10000 p1 = None p2 = None for link in self.links: td, tp1, tp2 = link.closest_point(shape, inf_dist, skip=True) - if td is not None and td < d: + if td is not None and d is not None and td < d: d = td p1 = tp1 p2 = tp2 @@ -1391,7 +1389,7 @@ def closest_point( return d, p1, p2 - def iscollided(self, q, shape: Shape, skip: bool = False) -> bool: + def iscollided(self, q: Any, shape: Shape, skip: bool = False) -> bool: """ Check if the robot is in collision with a shape @@ -1421,7 +1419,7 @@ def iscollided(self, q, shape: Shape, skip: bool = False) -> bool: return False - def collided(self, q, shape: Shape, skip: bool = False) -> bool: + def collided(self, q: Any, shape: Shape, skip: bool = False) -> bool: """ Check if the robot is in collision with a shape @@ -1496,7 +1494,7 @@ def link_collision_damper( end: Link | None = None, start: Link | None = None, collision_list: list[Shape] | None = None, - ): + ) -> Any: """ Compute a collision constrain for QP motion control @@ -1533,7 +1531,7 @@ def get_link_joint_index(link: Link | None) -> int: curr = curr.parent return 0 - def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): + def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray) -> Any: d, wTlp, wTcp = link_col.closest_point(shape, di) if d is not None and wTlp is not None and wTcp is not None: @@ -1542,7 +1540,7 @@ def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): norm = lpTcp / d norm_h = np.expand_dims( np.concatenate((norm, [0.0, 0.0, 0.0])), - axis=0, # type: ignore + axis=0, ) # tool = (self.fkine(q, end=link).inv() * SE3(wTlp)).A[:3, 3] @@ -1578,7 +1576,7 @@ def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): col_list = [collision_list[j - 1]] # pragma nocover for link_col in col_list: - l_Ain, l_bin = indiv_calculation(link, link_col, q) # type: ignore + l_Ain, l_bin = indiv_calculation(link, link_col, q) if l_Ain is not None and l_bin is not None: if Ain is None: @@ -1598,14 +1596,14 @@ def vision_collision_damper( shape: CollisionShape, camera: Robot | SE3 | None = None, camera_n: int = 0, - q=None, - di=0.3, - ds=0.05, - xi=1.0, - end=None, - start=None, - collision_list=None, - ): # pragma nocover + q: ArrayLike | None = None, + di: float = 0.3, + ds: float = 0.05, + xi: float = 1.0, + end: str | Link | Gripper | None = None, + start: str | Link | Gripper | None = None, + collision_list: Any = None, + ) -> tuple[NDArray | None, NDArray | None]: # pragma nocover """ Compute a vision collision constrain for QP motion control @@ -1636,7 +1634,7 @@ def vision_collision_damper( Ain = None bin = None - def rotation_between_vectors(a, b): + def rotation_between_vectors(a: Any, b: Any) -> Any: a = a / np.linalg.norm(a) b = b / np.linalg.norm(b) @@ -1658,23 +1656,23 @@ def rotation_between_vectors(a, b): los_mid = SE3((wTcp + wTtp) / 2) los_orientation = rotation_between_vectors( np.array([0.0, 0.0, 1.0]), - wTcp - wTtp, # type: ignore + wTcp - wTtp, ) los = Cylinder( radius=0.001, - length=np.linalg.norm(wTcp - wTtp), # type: ignore + length=np.linalg.norm(wTcp - wTtp), base=(los_mid * los_orientation), ) - def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): + def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray) -> Any: d, wTlp, wTvp = link_col.closest_point(los, di) if d is not None and wTlp is not None and wTvp is not None: lpTvp = -wTlp + wTvp norm = lpTvp / d - norm_h = np.expand_dims(np.concatenate((norm, [0.0, 0.0, 0.0])), axis=0) # type: ignore + norm_h = np.expand_dims(np.concatenate((norm, [0.0, 0.0, 0.0])), axis=0) tool = SE3( (np.linalg.inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3] @@ -1684,11 +1682,11 @@ def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): Je[:3, :] = self._T[:3, :3] @ Je[:3, :] n_dim = Je.shape[1] - if isinstance(camera, "Robot"): + if isinstance(camera, Robot): Jv = camera.jacob0(camera.q) Jv[:3, :] = self._T[:3, :3] @ Jv[:3, :] - Jv *= np.linalg.norm(wTvp - shape.T[:3, -1]) / los.length # type: ignore + Jv *= np.linalg.norm(wTvp - shape.T[:3, -1]) / los.length dpc = norm_h @ Jv dpc = np.concatenate( @@ -1702,7 +1700,7 @@ def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): dpc = np.zeros((1, self.n + camera_n)) dpt = norm_h @ shape.v - dpt *= np.linalg.norm(wTvp - wTcp) / los.length # type: ignore + dpt *= np.linalg.norm(wTvp - wTcp) / los.length l_Ain = np.zeros((1, self.n + camera_n)) l_Ain[0, :n_dim] = norm_h @ Je @@ -1750,7 +1748,7 @@ def rne( qdd: NDArray, symbolic: bool = False, gravity: ArrayLike | None = None, - ): + ) -> Any: """ Compute inverse dynamics via recursive Newton-Euler formulation @@ -1913,7 +1911,7 @@ def rne( I_int = I_int + SpatialInertia(m=link.m, r=link.r, I=link.I) if link.v is not None: - s.append(link.v.s) # type: ignore[union-attr] + s.append(link.v.s) # type: ignore[attr-defined] # ET, not ET2 past_joint = True I[i] = I_int @@ -1966,8 +1964,8 @@ def rne( # own ETS (the joint variable is guaranteed to be the last # element of its segment -- see this method's docstring), # so no further composition is needed. - Xup_int = SE3(joint.A(qk[jindex])) - Xup[j] = Xup_int.inv() # type: ignore[union-attr] + Xup_int = SE3(joint.A(qk[jindex])) # type: ignore[arg-type] + Xup[j] = Xup_int.inv() # group_of_link_idx also covers a joint whose immediate # .parent is a static link, or a run of them, rather than @@ -2010,7 +2008,9 @@ def rne( jindex = joint.jindex Q[k, j] += ( joint.G**2 * joint.Jm * qddk[jindex] - - joint.friction(qdk[jindex], coulomb=not symbolic) + - joint.friction( + qdk[jindex], coulomb=not symbolic # type: ignore[arg-type] + ) ) # See the forward recursion above: group_of_link_idx @@ -2083,7 +2083,7 @@ def rne( class Robot2(BaseRobot[Link2]): - def __init__(self, arg, **kwargs): + def __init__(self, arg: Any, **kwargs: Any) -> None: if isinstance(arg, ETS2): # we're passed an ETS string links = [] @@ -2135,7 +2135,7 @@ def base(self) -> SE2: no base transform, but this property will return ``SE3()`` which is an identity matrix. """ - if self._base is None: # pragma nocover + if self._base is None: # type: ignore[has-type] # pragma nocover self._base = SE2() # return a copy, otherwise somebody with @@ -2143,19 +2143,19 @@ def base(self) -> SE2: return self._base.copy() @base.setter - def base(self, T): + def base(self, T: Any) -> None: if isinstance(T, SE2): self._base = T elif SE2.isvalid(T): # pragma nocover self._tool = SE2(T, check=True) - def jacob0(self, q, start=None, end=None): + def jacob0(self, q: Any, start: Any = None, end: Any = None) -> Any: return self.ets(start, end).jacob0(q) - def jacobe(self, q, start=None, end=None): + def jacobe(self, q: Any, start: Any = None, end: Any = None) -> Any: return self.ets(start, end).jacobe(q) - def fkine(self, q, end=None, start=None): + def fkine(self, q: Any, end: Any = None, start: Any = None) -> Any: return self.ets(start, end).fkine(q) @property @@ -2186,9 +2186,9 @@ def reach(self) -> float: # given ets. Then use an lru_cache to speed up return if self._reach is None: - d_all = [] + d_all: list[float] = [] for link in self.ee_links: - d = 0 + d: float = 0 while True: for et in link.ets: if et.istranslation: @@ -2202,8 +2202,8 @@ def reach(self) -> float: elif et.qlim is not None: # pragma nocover d += max(et.qlim) else: - d += abs(et.param) - link = link.parent + d += abs(cast(float, et.param)) + link = link.parent # type: ignore[assignment] if link is None or isinstance(link, str): d_all.append(d) break @@ -2238,7 +2238,7 @@ def fkine_all(self, q: ArrayLike) -> SE2: linkframes = Tbase.__class__.Alloc(self.nlinks + 1) linkframes[0] = Tbase - def recurse(Tall, Tparent, q, link): + def recurse(Tall: Any, Tparent: Any, q: Any, link: Any) -> None: # if joint?? T = Tparent while True: diff --git a/src/roboticstoolbox/robot/RobotKinematics.py b/src/roboticstoolbox/robot/RobotKinematics.py index 8fb2827c3..58a1ecabe 100644 --- a/src/roboticstoolbox/robot/RobotKinematics.py +++ b/src/roboticstoolbox/robot/RobotKinematics.py @@ -2,6 +2,10 @@ @author: Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import numpy as np from roboticstoolbox.robot.RobotProto import KinematicsProtocol from roboticstoolbox.tools.types import ArrayLike, NDArray @@ -272,10 +276,10 @@ def hessian0( def hessian0( self: KinematicsProtocol, - q=None, + q: Any = None, end: str | Link | Gripper | None = None, start: str | Link | Gripper | None = None, - J0=None, + J0: Any = None, tool: NDArray | SE3 | None = None, ) -> NDArray: r""" @@ -372,10 +376,10 @@ def hessiane( def hessiane( self: KinematicsProtocol, - q=None, + q: Any = None, end: str | Link | Gripper | None = None, start: str | Link | Gripper | None = None, - Je=None, + Je: Any = None, tool: NDArray | SE3 | None = None, ) -> NDArray: r""" @@ -454,7 +458,7 @@ def partial_fkine0( n: int = 3, end: str | Link | Gripper | None = None, start: str | Link | Gripper | None = None, - ): + ) -> Any: r""" Manipulator Forward Kinematics nth Partial Derivative @@ -499,7 +503,7 @@ def jacob0_analytical( end: str | Link | Gripper | None = None, start: str | Link | Gripper | None = None, tool: NDArray | SE3 | None = None, - ): + ) -> Any: r""" Manipulator analytical Jacobian in the ``start`` frame @@ -1000,8 +1004,8 @@ def ikine_LM( ps: float = 0.0, pi: NDArray | float = 0.3, tool: NDArray | SE3 | None = None, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Levenberg-Marquardt Numerical Inverse Kinematics Solver @@ -1199,8 +1203,8 @@ def ikine_NR( ps: float = 0.0, pi: NDArray | float = 0.3, tool: NDArray | SE3 | None = None, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Newton-Raphson Numerical Inverse Kinematics Solver @@ -1344,8 +1348,8 @@ def ikine_GN( ps: float = 0.0, pi: NDArray | float = 0.3, tool: NDArray | SE3 | None = None, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Gauss-Newton Numerical Inverse Kinematics Solver @@ -1498,15 +1502,15 @@ def ikine_QP( mask: ArrayLike | None = None, joint_limits: bool = True, seed: int | None = None, - kj=1.0, - ks=1.0, + kj: float = 1.0, + ks: float = 1.0, kq: float = 0.0, km: float = 0.0, ps: float = 0.0, pi: NDArray | float = 0.3, tool: NDArray | SE3 | None = None, - **kwargs, - ): + **kwargs: Any, + ) -> Any: r""" Quadratic Programming Numerical Inverse Kinematics Solver diff --git a/src/roboticstoolbox/robot/RobotPlottingMPL.py b/src/roboticstoolbox/robot/RobotPlottingMPL.py index f4c353cac..75d35c9a0 100644 --- a/src/roboticstoolbox/robot/RobotPlottingMPL.py +++ b/src/roboticstoolbox/robot/RobotPlottingMPL.py @@ -12,7 +12,7 @@ from typing import TYPE_CHECKING, Any, Literal as L if TYPE_CHECKING: # pragma: nocover - from roboticstoolbox.backends.PyPlot import PyPlot + from roboticstoolbox.backends.PyPlot.PyPlot import PyPlot from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot from roboticstoolbox.tools.types import ArrayLike @@ -24,7 +24,7 @@ class RobotPlottingMPLMixin: # Colour helpers # ------------------------------------------------------------------ - def linkcolormap(self, linkcolors: list[Any] | str = "viridis"): + def linkcolormap(self, linkcolors: list[Any] | str = "viridis") -> Any: """ Create a colormap for robot joints. @@ -67,7 +67,7 @@ def linkcolormap(self, linkcolors: list[Any] | str = "viridis"): if isinstance(linkcolors, list) and len(linkcolors) == self.n: # type: ignore[attr-defined] return colors.ListedColormap(linkcolors) else: - return colormaps.get_cmap(linkcolors).resampled(6) + return colormaps.get_cmap(linkcolors).resampled(6) # type: ignore[arg-type] # ------------------------------------------------------------------ # Ellipse creation @@ -112,7 +112,7 @@ def fellipse( if isinstance(self, rtb.ERobot): # pragma: nocover raise NotImplementedError("ERobot fellipse not implemented yet") - q = getunit(q, unit) # type: ignore[assignment] + q = getunit(q, unit) ell = EllipsePlot(self, q, "f", opt, centre=centre) if add: @@ -161,7 +161,7 @@ def vellipse( if isinstance(self, rtb.ERobot): # pragma: nocover raise NotImplementedError("ERobot vellipse not implemented yet") - q = getunit(q, unit) # type: ignore[assignment] + q = getunit(q, unit) ell = EllipsePlot(self, q, "v", opt, centre=centre, scale=scale) if add: @@ -212,7 +212,7 @@ def plot_ellipse( ``robot.plot_ellipse(ellipsoid)`` displays the ellipsoid. """ - from roboticstoolbox.backends.PyPlot import PyPlot + from roboticstoolbox.backends.PyPlot.PyPlot import PyPlot from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot if not isinstance(ellipse, EllipsePlot): # pragma: nocover diff --git a/src/roboticstoolbox/robot/RobotProto.py b/src/roboticstoolbox/robot/RobotProto.py index 57b918aae..a1aa765ae 100644 --- a/src/roboticstoolbox/robot/RobotProto.py +++ b/src/roboticstoolbox/robot/RobotProto.py @@ -18,6 +18,11 @@ class KinematicsProtocol(Protocol): @property def _T(self) -> NDArray: ... + @property + def tool(self) -> SE3: ... + + def _resolve_tool(self, tool: NDArray | SE3 | None) -> SE3: ... + def ets( self, start: Link | Gripper | str | None = None, @@ -40,6 +45,8 @@ class RobotProto(Protocol): @property def links(self) -> list[BaseLink]: ... + _links: list[BaseLink] + @property def ee_links(self) -> list[BaseLink]: ... @@ -53,7 +60,7 @@ def q(self) -> NDArray: ... def name(self) -> str: ... @name.setter - def name(self, new_name: str): ... + def name(self, new_name: str) -> None: ... @property def gravity(self) -> NDArray: ... @@ -152,7 +159,7 @@ def _fdyn( self, t: float, x: NDArray, - torqfun: Callable[[Any], NDArray], + torqfun: Callable[..., NDArray] | None, targs: dict, ) -> NDArray: ... diff --git a/src/roboticstoolbox/robot/frne.py b/src/roboticstoolbox/robot/frne.py index fdd16482e..44ee474ab 100644 --- a/src/roboticstoolbox/robot/frne.py +++ b/src/roboticstoolbox/robot/frne.py @@ -9,8 +9,10 @@ from __future__ import annotations +from typing import Any + try: - from roboticstoolbox._frne_c import ( # type: ignore[import] + from roboticstoolbox._frne_c import ( init as _c_init, frne as _c_frne, delete as _c_delete, @@ -21,14 +23,16 @@ _C_AVAILABLE = False -def init(njoints, mdh, L): +def init(njoints: Any, mdh: Any, L: Any) -> Any: """Create the C Robot struct handle; returns None when C extension is absent.""" if _C_AVAILABLE: return _c_init(njoints, mdh, L) return None -def frne(robot, q, qd, qdd, gravity, base_rot, fext): +def frne( + robot: Any, q: Any, qd: Any, qdd: Any, gravity: Any, base_rot: Any, fext: Any +) -> Any: """Run Newton-Euler via C over a whole trajectory in one call; caller must check that robot handle is not None. @@ -45,7 +49,7 @@ def frne(robot, q, qd, qdd, gravity, base_rot, fext): return _c_frne(robot, q, qd, qdd, gravity, base_rot, fext) -def delete(robot): +def delete(robot: Any) -> None: """Free the C Robot struct; no-op when handle is None (C absent).""" if robot is not None: _c_delete(robot) diff --git a/src/roboticstoolbox/tools/Ticker.py b/src/roboticstoolbox/tools/Ticker.py index 2ef4220b6..d50b67466 100644 --- a/src/roboticstoolbox/tools/Ticker.py +++ b/src/roboticstoolbox/tools/Ticker.py @@ -1,9 +1,12 @@ +from __future__ import annotations + +from typing import Any import threading import time class Ticker(threading.Thread): # pragma nocover - def __init__(self, period): + def __init__(self, period: Any) -> None: super().__init__() self.sem = threading.Semaphore(0) @@ -11,10 +14,10 @@ def __init__(self, period): self.period = period print("in init", period) - def wait(self): + def wait(self) -> None: self.sem.acquire() - def run(self): + def run(self) -> None: print("in run", self.period) start = time.time() stop = start @@ -25,7 +28,7 @@ def run(self): time.sleep(stop - start) self.sem.release() - def stop(self): + def stop(self) -> None: self.done = True self.join() @@ -34,8 +37,8 @@ def stop(self): t = Ticker(0.1) tprev = time.time() - tmax = 0 - tsum = 0 + tmax: float = 0 + tsum: float = 0 N = 100 t.start() diff --git a/src/roboticstoolbox/tools/data.py b/src/roboticstoolbox/tools/data.py index 8e4f04abe..8d382a09f 100644 --- a/src/roboticstoolbox/tools/data.py +++ b/src/roboticstoolbox/tools/data.py @@ -1,9 +1,12 @@ +from __future__ import annotations + +from typing import Any from pathlib import Path import sys import importlib -def _candidate_rtbdata_roots(): +def _candidate_rtbdata_roots() -> Any: """ Yield candidate roots where the ``rtbdata`` tree may exist. """ @@ -28,7 +31,7 @@ def _candidate_rtbdata_roots(): yield root -def rtb_load_matfile(filename): +def rtb_load_matfile(filename: Any) -> Any: """ Load toolbox mat format data file @@ -64,7 +67,7 @@ def rtb_load_matfile(filename): if isinstance(value, mat_struct): # extend dict with a dot access method class dictx(dict): - def __getattr__(self, key): + def __getattr__(self, key: Any) -> Any: try: return self[key] except KeyError as k: @@ -78,7 +81,7 @@ def __getattr__(self, key): return data -def rtb_load_jsonfile(filename): +def rtb_load_jsonfile(filename: Any) -> Any: """ Load toolbox JSON format data file @@ -103,7 +106,7 @@ def rtb_load_jsonfile(filename): return rtb_load_data(filename, lambda f: json.load(open(f, "r"))) -def rtb_load_data(filename, handler, **kwargs): +def rtb_load_data(filename: Any, handler: Any, **kwargs: Any) -> Any: """ Load toolbox data file @@ -133,7 +136,7 @@ def rtb_load_data(filename, handler, **kwargs): return handler(path, **kwargs) -def rtb_path_to_datafile(*filename, local=True): +def rtb_path_to_datafile(*filename: Any, local: bool = True) -> Any: """ Get absolute path to datafile @@ -160,19 +163,19 @@ def rtb_path_to_datafile(*filename, local=True): loadmat('~/foo.dat') # read $HOME/foo.dat """ - filename = Path(*filename) + fname = Path(*filename) if local: # check if file is in user's local filesystem - p = filename.expanduser() + p = fname.expanduser() p = p.resolve() if p.exists(): return p # otherwise, look for it in rtbdata locations for root in _candidate_rtbdata_roots(): - path = root / filename + path = root / fname if path.exists(): return path.resolve() @@ -181,7 +184,7 @@ def rtb_path_to_datafile(*filename, local=True): if __name__ == "__main__": house = rtb_load_matfile("data/house.mat") - a = rtb_loadmat("map1.mat") + a = rtb_loadmat("map1.mat") # type: ignore[name-defined] print(a) - a = rtb_loadmat("data/map1.mat") + a = rtb_loadmat("data/map1.mat") # type: ignore[name-defined] print(a) diff --git a/src/roboticstoolbox/tools/jsingu.py b/src/roboticstoolbox/tools/jsingu.py index e1ae58e6f..9f6256f33 100644 --- a/src/roboticstoolbox/tools/jsingu.py +++ b/src/roboticstoolbox/tools/jsingu.py @@ -1,3 +1,6 @@ +from __future__ import annotations + +from typing import Any import numpy as np from scipy.linalg import lu @@ -16,7 +19,7 @@ # [str(i) for i in range(j) if all(U[:, j] == U[:, i])])) -def jsingu(J): +def jsingu(J: Any) -> None: indep_columns = np.empty((J.shape[0], 0)) rank = 0 @@ -45,7 +48,7 @@ def jsingu(J): import roboticstoolbox as rtb # robot = rtb.models.DH.Puma560() - robot = rtb.models.URDF.UR5() + robot = rtb.models.URDF.UR5() # type: ignore[operator] # class, not module J = robot.jacob0(robot.qr) jsingu(J) diff --git a/src/roboticstoolbox/tools/null.py b/src/roboticstoolbox/tools/null.py index 947ae8888..b45343adc 100644 --- a/src/roboticstoolbox/tools/null.py +++ b/src/roboticstoolbox/tools/null.py @@ -3,10 +3,14 @@ @author Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import numpy as np -def null(A, atol=1e-13, rtol=0): +def null(A: Any, atol: float = 1e-13, rtol: float = 0) -> Any: """ Compute an approximate basis for the nullspace of A. diff --git a/src/roboticstoolbox/tools/numerical.py b/src/roboticstoolbox/tools/numerical.py index 454b3ec3f..7f07b433d 100644 --- a/src/roboticstoolbox/tools/numerical.py +++ b/src/roboticstoolbox/tools/numerical.py @@ -1,8 +1,11 @@ +from __future__ import annotations + +from typing import Any import numpy as np from spatialmath import base -def jacobian_numerical(f, x, dx=1e-8, N=0): +def jacobian_numerical(f: Any, x: Any, dx: float = 1e-8, N: int = 0) -> Any: r""" Numerically compute Jacobian of function @@ -45,7 +48,7 @@ def jacobian_numerical(f, x, dx=1e-8, N=0): return np.c_[Jcol].T -def hessian_numerical(J, x, dx=1e-8): +def hessian_numerical(J: Any, x: Any, dx: float = 1e-8) -> Any: r""" Numerically compute Hessian of Jacobian function @@ -84,7 +87,7 @@ def hessian_numerical(J, x, dx=1e-8): formatter={"float": lambda x: f"{x:8.4g}" if abs(x) > 1e-10 else f"{0:8.4g}"}, ) - robot = rtb.models.DH.Puma560() + robot = rtb.models.DH.Puma560() # type: ignore[operator] # class, not module q = robot.qn J = jacobian_numerical(lambda q: robot.fkine(q).A, q, N=3) diff --git a/src/roboticstoolbox/tools/p_servo.py b/src/roboticstoolbox/tools/p_servo.py index d787ba543..dff047dd8 100644 --- a/src/roboticstoolbox/tools/p_servo.py +++ b/src/roboticstoolbox/tools/p_servo.py @@ -1,3 +1,6 @@ +from __future__ import annotations + +from typing import Any #!/usr/bin/env python import numpy as np @@ -9,7 +12,7 @@ # ArrayLike = Union[list, np.ndarray, tuple, set] -def angle_axis(T, Td) -> NDArray: +def angle_axis(T: Any, Td: Any) -> NDArray: # deferred: roboticstoolbox.ets.fknm imports the ets package, which # depends on roboticstoolbox.tools being fully initialised first from roboticstoolbox.ets.fknm import Angle_Axis @@ -22,7 +25,7 @@ def angle_axis(T, Td) -> NDArray: return e -def angle_axis_python(T, Td): +def angle_axis_python(T: Any, Td: Any) -> Any: e = np.empty(6) e[:3] = Td[:3, -1] - T[:3, -1] R = Td[:3, :3] @ T[:3, :3].T @@ -46,8 +49,12 @@ def angle_axis_python(T, Td): def p_servo( - wTe, wTep, gain: Union[float, ArrayLike] = 1.0, threshold=0.1, method="rpy" -): + wTe: Any, + wTep: Any, + gain: Union[float, ArrayLike] = 1.0, + threshold: float = 0.1, + method: str = "rpy", +) -> Any: """ Position-based servoing. diff --git a/src/roboticstoolbox/tools/params.py b/src/roboticstoolbox/tools/params.py index d2f336a41..6cbb361fb 100644 --- a/src/roboticstoolbox/tools/params.py +++ b/src/roboticstoolbox/tools/params.py @@ -1,11 +1,15 @@ +from __future__ import annotations + +from typing import Any + _params = { "unicode": True, } -def rtb_set_param(param, value): +def rtb_set_param(param: Any, value: Any) -> None: _params[param] = value -def rtb_get_param(param): +def rtb_get_param(param: Any) -> Any: return _params[param] diff --git a/src/roboticstoolbox/tools/plot.py b/src/roboticstoolbox/tools/plot.py index 6eac3de61..05e51b672 100644 --- a/src/roboticstoolbox/tools/plot.py +++ b/src/roboticstoolbox/tools/plot.py @@ -1,19 +1,22 @@ +from __future__ import annotations + +from typing import Any import numpy as np import matplotlib.pyplot as plt def xplot( - x, - y=None, - wrist=False, - unwrap=False, - block=False, - labels=None, - loc=None, - grid=True, - stack=False, - **kwargs, -): + x: Any, + y: Any = None, + wrist: bool = False, + unwrap: bool = False, + block: bool = False, + labels: Any = None, + loc: Any = None, + grid: bool = True, + stack: bool = False, + **kwargs: Any, +) -> Any: r""" Plot trajectory data diff --git a/src/roboticstoolbox/tools/trchain.py b/src/roboticstoolbox/tools/trchain.py index b5df6b0d8..06b5e8bb9 100644 --- a/src/roboticstoolbox/tools/trchain.py +++ b/src/roboticstoolbox/tools/trchain.py @@ -7,6 +7,7 @@ from typing import NamedTuple import numpy as np +from roboticstoolbox.tools.types import ArrayLike, NDArray from spatialmath.base import symbolic as sym from spatialmath.base import transl, transl2, trot2, trotx, troty, trotz @@ -21,7 +22,7 @@ class TrChainToken(NamedTuple): op: str arg: str - index: int + index: int # type: ignore[assignment] # shadows tuple.index, kept for the API _TOKEN_RE = re.compile(r"(?PR.?|T.)\(") @@ -34,7 +35,7 @@ class TrChainToken(NamedTuple): ast.Mod: operator.mod, ast.Pow: operator.pow, } -_UNARY_OPERATORS: dict[type[ast.unaryop], Callable[[object], object]] = { +_UNARY_OPERATORS: dict[type[ast.unaryop], Callable[..., object]] = { ast.UAdd: operator.pos, ast.USub: operator.neg, } @@ -46,7 +47,7 @@ class TrChainToken(NamedTuple): "sqrt": sym.sqrt, } -_Transform = Callable[[object, str], np.ndarray] +_Transform = Callable[[object, str], NDArray] _TRANSFORMS_3D: dict[str, _Transform] = { "Rx": lambda value, unit: trotx(value, unit), "Ry": lambda value, unit: troty(value, unit), @@ -127,13 +128,13 @@ def evaluate(node: ast.AST) -> object: def _evaluate_chain( chain: str | Sequence[TrChainToken], - q: Sequence[object] | np.ndarray | None, + q: ArrayLike | Sequence[object] | None, unit: str, qvar: str, variables: Mapping[str, object] | None, transforms: Mapping[str, _Transform], size: int, -) -> tuple[np.ndarray, tuple[TrChainToken, ...]]: +) -> tuple[NDArray, tuple[TrChainToken, ...]]: if unit not in {"rad", "deg"}: raise ValueError("unit must be 'rad' or 'deg'") if not qvar.isidentifier(): @@ -172,13 +173,13 @@ def _evaluate_chain( def trchain( chain: str | Sequence[TrChainToken], - q: Sequence[object] | np.ndarray | None = None, + q: ArrayLike | Sequence[object] | None = None, unit: str = "rad", *, qvar: str = "q", variables: Mapping[str, object] | None = None, return_tokens: bool = False, -) -> np.ndarray | tuple[np.ndarray, tuple[TrChainToken, ...]]: +) -> NDArray | tuple[NDArray, tuple[TrChainToken, ...]]: """Compound SE(3) elementary transforms from a string. :param chain: transform chain or tokens returned by an earlier call @@ -203,13 +204,13 @@ def trchain( def trchain2( chain: str | Sequence[TrChainToken], - q: Sequence[object] | np.ndarray | None = None, + q: ArrayLike | Sequence[object] | None = None, unit: str = "rad", *, qvar: str = "q", variables: Mapping[str, object] | None = None, return_tokens: bool = False, -) -> np.ndarray | tuple[np.ndarray, tuple[TrChainToken, ...]]: +) -> NDArray | tuple[NDArray, tuple[TrChainToken, ...]]: """Compound SE(2) elementary transforms from a string. :param chain: transform chain or tokens returned by an earlier call diff --git a/src/roboticstoolbox/tools/urdf/urdf.py b/src/roboticstoolbox/tools/urdf/urdf.py index 68f3221b6..bb956a597 100644 --- a/src/roboticstoolbox/tools/urdf/urdf.py +++ b/src/roboticstoolbox/tools/urdf/urdf.py @@ -4,6 +4,10 @@ @author (Adapted by) Jesse Haviland """ +from __future__ import annotations + +from typing import Any + import numpy as np import roboticstoolbox as rtb import spatialgeometry as gm @@ -61,7 +65,7 @@ def __init__( element: str | None = None, location: list[str] | None = None, expanded_file: "str | os.PathLike | None" = None, - ): + ) -> None: super().__init__(msg) self.msg = msg self.stage = stage @@ -102,7 +106,7 @@ def __str__(self) -> str: return "\n".join(lines) -def _describe(node) -> str: +def _describe(node: Any) -> str: """Short description of an XML element for error messages.""" name = node.attrib.get("name") if name is None: @@ -110,7 +114,7 @@ def _describe(node) -> str: return f'<{node.tag} name="{name}">' -def _parse_child(cls, node, path): +def _parse_child(cls: Any, node: Any, path: Any) -> Any: """Call ``cls._from_xml(node, path)``, attaching element context to any error.""" try: return cls._from_xml(node, path) @@ -147,15 +151,15 @@ class URDFType(): containing this type of object. """ - _ATTRIBS = {} # Map from attrib name to (type, required) - _ELEMENTS = {} # Map from element name to (type, required, multiple) + _ATTRIBS: dict[str, Any] = {} # Map from attrib name to (type, required) + _ELEMENTS: dict[str, Any] = {} # Map from element name to (type, required, multiple) _TAG = "" # XML tag for this element - def __init__(self): # pragma nocover + def __init__(self) -> None: # pragma nocover pass @classmethod - def _parse_attrib(cls, val_type, val): + def _parse_attrib(cls, val_type: Any, val: Any) -> Any: """Parse an XML attribute into a python value. Parameters ---------- @@ -175,7 +179,7 @@ def _parse_attrib(cls, val_type, val): return val @classmethod - def _parse_simple_attribs(cls, node): + def _parse_simple_attribs(cls, node: Any) -> Any: """Parse all attributes in the _ATTRIBS array for this class. Parameters ---------- @@ -222,7 +226,7 @@ def _parse_simple_attribs(cls, node): return kwargs @classmethod - def _parse_simple_elements(cls, node, path): + def _parse_simple_elements(cls, node: Any, path: Any) -> Any: """Parse all elements in the _ELEMENTS array from the children of this node. Parameters @@ -264,7 +268,7 @@ def _parse_simple_elements(cls, node, path): return kwargs @classmethod - def _parse(cls, node, path): + def _parse(cls, node: Any, path: Any) -> Any: """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS arrays for a node. Parameters @@ -285,7 +289,7 @@ def _parse(cls, node, path): return kwargs @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: """Create an instance of this class from an XML node. Parameters ---------- @@ -318,19 +322,19 @@ class Box(URDFType): _ATTRIBS = {"size": (np.ndarray, True)} _TAG = "box" - def __init__(self, size): + def __init__(self, size: Any) -> None: self.size = size - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Box(size={})".format(self.size) @property - def size(self): + def size(self) -> Any: """(3,) float : The length, width, and height of the box in meters.""" return self._size @size.setter - def size(self, value): + def size(self, value: Any) -> None: self._size = np.asanyarray(value).astype(np.float64) @@ -350,29 +354,29 @@ class Cylinder(URDFType): } _TAG = "cylinder" - def __init__(self, radius, length): + def __init__(self, radius: Any, length: Any) -> None: self.radius = radius self.length = length - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Cylinder(radius={}, length={})".format(self.radius, self.length) @property - def radius(self): + def radius(self) -> Any: """float : The radius of the cylinder in meters.""" return self._radius @radius.setter - def radius(self, value): + def radius(self, value: Any) -> None: self._radius = float(value) @property - def length(self): + def length(self) -> Any: """float : The length of the cylinder in meters.""" return self._length @length.setter - def length(self, value): + def length(self, value: Any) -> None: self._length = float(value) @@ -389,19 +393,19 @@ class Sphere(URDFType): } _TAG = "sphere" - def __init__(self, radius): + def __init__(self, radius: Any) -> None: self.radius = radius - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Sphere(radius={})".format(self.radius) @property - def radius(self): + def radius(self) -> Any: """float : The radius of the sphere in meters.""" return self._radius @radius.setter - def radius(self, value): + def radius(self, value: Any) -> None: self._radius = float(value) @@ -420,20 +424,20 @@ class Mesh(URDFType): _ATTRIBS = {"filename": (str, True), "scale": (np.ndarray, False)} _TAG = "mesh" - def __init__(self, filename, scale=None): + def __init__(self, filename: Any, scale: Any = None) -> None: self.filename = filename self.scale = scale - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Mesh(filename={}, scale={})".format(self.filename, self.scale) @property - def filename(self): + def filename(self) -> Any: """str : The path to the mesh file for this object.""" return self._filename @filename.setter - def filename(self, value): + def filename(self, value: Any) -> None: global _base_path @@ -450,18 +454,18 @@ def filename(self, value): self._filename = str(value) @property - def scale(self): + def scale(self) -> Any: """(3,) float : A scaling for the mesh along its local XYZ axes.""" return self._scale @scale.setter - def scale(self, value): + def scale(self, value: Any) -> None: if value is not None: value = np.asanyarray(value).astype(np.float64) self._scale = value @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: kwargs = cls._parse(node, path) return Mesh(**kwargs) @@ -477,16 +481,16 @@ class Material(URDFType): texture : :class:`.Texture`, optional A texture for the material. """ - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Material(name={}, color={}, texture={})".format( self.name, self.color, self.texture ) _ATTRIBS = {"name": (str, True)} - _ELEMENTS = {} + _ELEMENTS: dict[str, Any] = {} _TAG = "material" - def __init__(self, name, color=None, texture=None): + def __init__(self, name: Any, color: Any = None, texture: Any = None) -> None: # if color is None: # color = name @@ -495,7 +499,7 @@ def __init__(self, name, color=None, texture=None): self.texture = texture @classmethod - def _from_xml(cls, node, path): # pragma nocover + def _from_xml(cls, node: Any, path: Any) -> Any: # pragma nocover kwargs = cls._parse(node, path) # Extract the color -- it's weirdly an attribute of a subelement @@ -531,7 +535,9 @@ class Geometry(URDFType): } _TAG = "geometry" - def __init__(self, box=None, cylinder=None, sphere=None, mesh=None): + def __init__( + self, box: Any = None, cylinder: Any = None, sphere: Any = None, mesh: Any = None + ) -> None: if ( box is None and cylinder is None and sphere is None and mesh is None ): # pragma nocover @@ -553,57 +559,57 @@ def __init__(self, box=None, cylinder=None, sphere=None, mesh=None): self.mesh = mesh self.ob = gm.Mesh(mesh.filename, scale=mesh.scale) - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Geometry(box={}, cylinder={}, sphere={}, mesh={})".format( self.box, self.cylinder, self.sphere, self.mesh ) @property - def box(self): + def box(self) -> Any: """:class:`.Box` : Box geometry.""" return self._box @box.setter - def box(self, value): + def box(self, value: Any) -> None: if value is not None and not isinstance(value, Box): # pragma nocover raise TypeError("Expected Box type") self._box = value @property - def cylinder(self): + def cylinder(self) -> Any: """:class:`.Cylinder` : Cylinder geometry.""" return self._cylinder @cylinder.setter - def cylinder(self, value): + def cylinder(self, value: Any) -> None: if value is not None and not isinstance(value, Cylinder): raise TypeError("Expected Cylinder type") # pragma nocover self._cylinder = value @property - def sphere(self): + def sphere(self) -> Any: """:class:`.Sphere` : Spherical geometry.""" return self._sphere @sphere.setter - def sphere(self, value): + def sphere(self, value: Any) -> None: if value is not None and not isinstance(value, Sphere): raise TypeError("Expected Sphere type") # pragma nocover self._sphere = value @property - def mesh(self): + def mesh(self) -> Any: """:class:`.Mesh` : Mesh geometry.""" return self._mesh @mesh.setter - def mesh(self, value): + def mesh(self, value: Any) -> None: if value is not None and not isinstance(value, Mesh): raise TypeError("Expected Mesh type") # pragma nocover self._mesh = value @property - def geometry(self): # pragma nocover + def geometry(self) -> Any: # pragma nocover """:class:`.Box`, :class:`.Cylinder`, :class:`.Sphere`, or :class:`.Mesh` : The valid geometry element. """ @@ -637,50 +643,50 @@ class Collision(URDFType): } _TAG = "collision" - def __init__(self, name, origin, geometry): + def __init__(self, name: Any, origin: Any, geometry: Any) -> None: self.geometry = geometry self.name = name self.origin = origin self.geometry.ob.T = origin - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Collision(name={}, origin={}, geometry={})".format( self.name, self.origin, self.geometry ) @property - def geometry(self): + def geometry(self) -> Any: """:class:`.Geometry` : The geometry of this element.""" return self._geometry @geometry.setter - def geometry(self, value): + def geometry(self, value: Any) -> None: if not isinstance(value, Geometry): # pragma nocover raise TypeError("Must set geometry with Geometry object") self._geometry = value @property - def name(self): + def name(self) -> Any: """str : The name of this collision element.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: if value is not None: value = str(value) self._name = value @property - def origin(self): + def origin(self) -> Any: """(4,4) float : The pose of this element relative to the link frame.""" return self._origin @origin.setter - def origin(self, value): + def origin(self, value: Any) -> None: self._origin = configure_origin(value) @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: kwargs = cls._parse(node, path) kwargs["origin"], _ = parse_origin(node) return Collision(**kwargs) @@ -708,7 +714,9 @@ class Visual(URDFType): } _TAG = "visual" - def __init__(self, geometry, name=None, origin=None, material=None): + def __init__( + self, geometry: Any, name: Any = None, origin: Any = None, material: Any = None + ) -> None: self.geometry = geometry geometry.ob.T = origin self.name = name @@ -724,44 +732,44 @@ def __init__(self, geometry, name=None, origin=None, material=None): self.geometry.ob.color = material.color - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Visual(name={}, origin={}, geometry={}, material={})".format( self.name, self.origin, self.geometry, self.material ) @property - def geometry(self): + def geometry(self) -> Any: """:class:`.Geometry` : The geometry of this element.""" return self._geometry @geometry.setter - def geometry(self, value): + def geometry(self, value: Any) -> None: if not isinstance(value, Geometry): # pragma nocover raise TypeError("Must set geometry with Geometry object") self._geometry = value @property - def name(self): + def name(self) -> Any: """str : The name of this visual element.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: if value is not None: value = str(value) self._name = value @property - def origin(self): + def origin(self) -> Any: """(4,4) float : The pose of this element relative to the link frame.""" return self._origin @origin.setter - def origin(self, value): + def origin(self, value: Any) -> None: self._origin = configure_origin(value) @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: kwargs = cls._parse(node, path) kwargs["origin"], _ = parse_origin(node) return Visual(**kwargs) @@ -782,46 +790,46 @@ class Inertial(URDFType): _TAG = "inertial" - def __init__(self, mass=None, inertia=None, origin=None): + def __init__(self, mass: Any = None, inertia: Any = None, origin: Any = None) -> None: self._mass = mass self._inertia = inertia self._origin = origin - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Inertial(mass={}, inertia={}, origin={})".format(self.mass, self.inertia, self.origin) @property - def mass(self): + def mass(self) -> Any: """float : The mass of the link in kilograms.""" return self._mass @mass.setter - def mass(self, value): + def mass(self, value: Any) -> None: self._mass = float(value) @property - def inertia(self): + def inertia(self) -> Any: """(3,3) float : The 3x3 symmetric rotational inertia matrix.""" return self._inertia @inertia.setter - def inertia(self, value): + def inertia(self, value: Any) -> None: value = np.asanyarray(value).astype(np.float64) if not np.allclose(value, value.T): # pragma nocover raise ValueError("Inertia must be a symmetric matrix") self._inertia = value @property - def origin(self): + def origin(self) -> Any: """(4,4) float : The pose of the inertials relative to the link frame.""" return self._origin @origin.setter - def origin(self, value): + def origin(self, value: Any) -> None: self._origin = configure_origin(value) @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: origin, _ = parse_origin(node) mass_node = node.find("mass") if mass_node is None or "value" not in mass_node.attrib: @@ -876,36 +884,36 @@ class JointCalibration(URDFType): # pragma nocover _ATTRIBS = {"rising": (float, False), "falling": (float, False)} _TAG = "calibration" - def __init__(self, rising=None, falling=None): + def __init__(self, rising: Any = None, falling: Any = None) -> None: self.rising = rising self.falling = falling - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "JointCalibration(rising={}, falling={})".format(self.rising, self.falling) @property - def rising(self): + def rising(self) -> Any: """float : description.""" return self._rising @rising.setter - def rising(self, value): + def rising(self, value: Any) -> None: if value is not None: value = float(value) self._rising = value @property - def falling(self): + def falling(self) -> Any: """float : description.""" return self._falling @falling.setter - def falling(self, value): + def falling(self, value: Any) -> None: if value is not None: value = float(value) self._falling = value - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: Any = None) -> Any: """Create a deep copy of the visual with the prefix applied to all names. Parameters ---------- @@ -940,31 +948,31 @@ class JointDynamics(URDFType): } _TAG = "dynamics" - def __init__(self, damping, friction): + def __init__(self, damping: Any, friction: Any) -> None: self.damping = damping self.friction = friction - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "JointDynamics(damping={}, friction={})".format(self.damping, self.friction) @property - def damping(self): # pragma nocover + def damping(self) -> Any: # pragma nocover """float : The damping value of the joint.""" return self._damping @damping.setter - def damping(self, value): + def damping(self, value: Any) -> None: if value is not None: value = float(value) self._damping = value @property - def friction(self): + def friction(self) -> Any: """float : The static friction value of the joint.""" return self._friction @friction.setter - def friction(self, value): + def friction(self, value: Any) -> None: if value is not None: value = float(value) self._friction = value @@ -993,53 +1001,55 @@ class JointLimit(URDFType): } _TAG = "limit" - def __init__(self, effort, velocity, lower=None, upper=None): + def __init__( + self, effort: Any, velocity: Any, lower: Any = None, upper: Any = None + ) -> None: self.effort = effort self.velocity = velocity self.lower = lower self.upper = upper - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "JointLimit(effort={}, velocity={}, lower={}, upper={})".format( self.effort, self.velocity, self.lower, self.upper ) @property - def effort(self): + def effort(self) -> Any: """float : The maximum joint effort.""" return self._effort @effort.setter - def effort(self, value): + def effort(self, value: Any) -> None: self._effort = float(value) @property - def velocity(self): + def velocity(self) -> Any: """float : The maximum joint velocity.""" return self._velocity @velocity.setter - def velocity(self, value): + def velocity(self, value: Any) -> None: self._velocity = float(value) @property - def lower(self): + def lower(self) -> Any: """float : The lower joint limit.""" return self._lower @lower.setter - def lower(self, value): + def lower(self, value: Any) -> None: if value is not None: value = float(value) self._lower = value @property - def upper(self): + def upper(self) -> Any: """float : The upper joint limit.""" return self._upper @upper.setter - def upper(self, value): + def upper(self, value: Any) -> None: if value is not None: value = float(value) self._upper = value @@ -1067,32 +1077,32 @@ class JointMimic(URDFType): # pragma nocover } _TAG = "mimic" - def __init__(self, joint, multiplier=None, offset=None): + def __init__(self, joint: Any, multiplier: Any = None, offset: Any = None) -> None: self.joint = joint self.multiplier = multiplier self.offset = offset - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "JointMimic(joint={}, multiplier={}, offset={})".format( self.joint, self.multiplier, self.offset ) @property - def joint(self): + def joint(self) -> Any: """float : The name of the joint to mimic.""" return self._joint @joint.setter - def joint(self, value): + def joint(self, value: Any) -> None: self._joint = str(value) @property - def multiplier(self): + def multiplier(self) -> Any: """float : The multiplier for the joint configuration.""" return self._multiplier @multiplier.setter - def multiplier(self, value): + def multiplier(self, value: Any) -> None: if value is not None: value = float(value) else: @@ -1100,12 +1110,12 @@ def multiplier(self, value): self._multiplier = value @property - def offset(self): + def offset(self) -> Any: """float : The offset for the joint configuration""" return self._offset @offset.setter - def offset(self, value): + def offset(self, value: Any) -> None: if value is not None: value = float(value) else: @@ -1140,25 +1150,29 @@ class SafetyController(URDFType): # pragma nocover _TAG = "safety_controller" def __init__( - self, k_velocity, k_position=None, soft_lower_limit=None, soft_upper_limit=None - ): + self, + k_velocity: Any, + k_position: Any = None, + soft_lower_limit: Any = None, + soft_upper_limit: Any = None, + ) -> None: self.k_velocity = k_velocity self.k_position = k_position self.soft_lower_limit = soft_lower_limit self.soft_upper_limit = soft_upper_limit - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "SafetyController(k_velocity={}, k_position={}, soft_lower_limit={}, soft_upper_limit={})".format( self.k_velocity, self.k_position, self.soft_lower_limit, self.soft_upper_limit ) @property - def soft_lower_limit(self): + def soft_lower_limit(self) -> Any: """float : The soft lower limit where the safety controller kicks in.""" return self._soft_lower_limit @soft_lower_limit.setter - def soft_lower_limit(self, value): + def soft_lower_limit(self, value: Any) -> None: if value is not None: value = float(value) else: @@ -1166,12 +1180,12 @@ def soft_lower_limit(self, value): self._soft_lower_limit = value @property - def soft_upper_limit(self): + def soft_upper_limit(self) -> Any: """float : The soft upper limit where the safety controller kicks in.""" return self._soft_upper_limit @soft_upper_limit.setter - def soft_upper_limit(self, value): + def soft_upper_limit(self, value: Any) -> None: if value is not None: value = float(value) else: @@ -1179,12 +1193,12 @@ def soft_upper_limit(self, value): self._soft_upper_limit = value @property - def k_position(self): + def k_position(self) -> Any: """float : A relation between the position and velocity limits.""" return self._k_position @k_position.setter - def k_position(self, value): + def k_position(self, value: Any) -> None: if value is not None: value = float(value) else: @@ -1192,12 +1206,12 @@ def k_position(self, value): self._k_position = value @property - def k_velocity(self): + def k_velocity(self) -> Any: """float : A relation between the effort and velocity limits.""" return self._k_velocity @k_velocity.setter - def k_velocity(self, value): + def k_velocity(self, value: Any) -> None: self._k_velocity = float(value) @@ -1224,44 +1238,46 @@ class Actuator(URDFType): } _TAG = "actuator" - def __init__(self, name, mechanicalReduction=None, hardwareInterfaces=None): + def __init__( + self, name: Any, mechanicalReduction: Any = None, hardwareInterfaces: Any = None + ) -> None: self.name = name self.mechanicalReduction = mechanicalReduction self.hardwareInterfaces = hardwareInterfaces - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Actuator(name={}, mechanicalReduction={}, hardwareInterfaces={})".format( self.name, self.mechanicalReduction, self.hardwareInterfaces ) @property - def name(self): # pragma nocover + def name(self) -> Any: # pragma nocover """str : The name of this actuator.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: self._name = str(value) @property - def mechanicalReduction(self): # pragma nocover + def mechanicalReduction(self) -> Any: # pragma nocover """str : A specifier for the type of mechanical reduction.""" return self._mechanicalReduction @mechanicalReduction.setter - def mechanicalReduction(self, value): + def mechanicalReduction(self, value: Any) -> None: if value is not None: value = str(value) self._mechanicalReduction = value @property - def hardwareInterfaces(self): # pragma nocover + def hardwareInterfaces(self) -> Any: # pragma nocover """list of str : The supported hardware interfaces.""" return self._hardwareInterfaces @hardwareInterfaces.setter - def hardwareInterfaces(self, value): # pragma nocover + def hardwareInterfaces(self, value: Any) -> None: # pragma nocover if value is None: value = [] else: @@ -1271,7 +1287,7 @@ def hardwareInterfaces(self, value): # pragma nocover self._hardwareInterfaces = value @classmethod - def _from_xml(cls, node, path): # pragma nocover + def _from_xml(cls, node: Any, path: Any) -> Any: # pragma nocover kwargs = cls._parse(node, path) mr = node.find("mechanicalReduction") if mr is not None: @@ -1299,31 +1315,31 @@ class TransmissionJoint(URDFType): } _TAG = "joint" - def __init__(self, name, hardwareInterfaces): + def __init__(self, name: Any, hardwareInterfaces: Any) -> None: self.name = name self.hardwareInterfaces = hardwareInterfaces - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "TransmissionJoint(name={}, hardwareInterfaces={})".format( self.name, self.hardwareInterfaces ) @property - def name(self): # pragma nocover + def name(self) -> Any: # pragma nocover """str : The name of this transmission joint.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: self._name = str(value) @property - def hardwareInterfaces(self): # pragma nocover + def hardwareInterfaces(self) -> Any: # pragma nocover """list of str : The supported hardware interfaces.""" return self._hardwareInterfaces @hardwareInterfaces.setter - def hardwareInterfaces(self, value): # pragma nocover + def hardwareInterfaces(self, value: Any) -> None: # pragma nocover if value is None: value = [] else: @@ -1333,7 +1349,7 @@ def hardwareInterfaces(self, value): # pragma nocover self._hardwareInterfaces = value @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: kwargs = cls._parse(node, path) hi = node.findall("hardwareInterface") if len(hi) > 0: @@ -1371,44 +1387,46 @@ class Transmission(URDFType): } _TAG = "transmission" - def __init__(self, name, trans_type, joints=None, actuators=None): + def __init__( + self, name: Any, trans_type: Any, joints: Any = None, actuators: Any = None + ) -> None: self.name = name self.trans_type = trans_type self.joints = joints self.actuators = actuators - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Transmission(name={}, trans_type={}, joints={}, actuators={})".format( self.name, self.trans_type, self.joints, self.actuators ) @property - def name(self): + def name(self) -> Any: """str : The name of this transmission.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: self._name = str(value) @property - def trans_type(self): # pragma nocover + def trans_type(self) -> Any: # pragma nocover """str : The type of this transmission.""" return self._trans_type @trans_type.setter - def trans_type(self, value): + def trans_type(self, value: Any) -> None: self._trans_type = str(value) @property - def joints(self): # pragma nocover + def joints(self) -> Any: # pragma nocover """:class:`.TransmissionJoint` : The joints the transmission is connected to. """ return self._joints @joints.setter - def joints(self, value): # pragma nocover + def joints(self, value: Any) -> None: # pragma nocover if value is None: value = [] else: @@ -1419,12 +1437,12 @@ def joints(self, value): # pragma nocover self._joints = value @property - def actuators(self): # pragma nocover + def actuators(self) -> Any: # pragma nocover """:class:`.Actuator` : The actuators the transmission is connected to.""" return self._actuators @actuators.setter - def actuators(self, value): # pragma nocover + def actuators(self, value: Any) -> None: # pragma nocover if value is None: value = [] else: @@ -1435,7 +1453,7 @@ def actuators(self, value): # pragma nocover self._actuators = value @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: kwargs = cls._parse(node, path) if node.find("type") is not None: kwargs["trans_type"] = node.find("type").text @@ -1502,19 +1520,19 @@ class Joint(URDFType): def __init__( self, - name, - joint_type, - parent, - child, - axis=None, - origin=None, - limit=None, - dynamics=None, - safety_controller=None, - calibration=None, - mimic=None, - rpy=None, - ): + name: Any, + joint_type: Any, + parent: Any, + child: Any, + axis: Any = None, + origin: Any = None, + limit: Any = None, + dynamics: Any = None, + safety_controller: Any = None, + calibration: Any = None, + mimic: Any = None, + rpy: Any = None, + ) -> None: self.name = name self.parent = parent self.child = child @@ -1528,57 +1546,57 @@ def __init__( self.calibration = calibration self.mimic = mimic - def __repr__(self): + def __repr__(self) -> str: return "Joint(name={}, joint_type={}, parent={}, child={})".format( self.name, self.joint_type, self.parent, self.child ) @property - def name(self): + def name(self) -> Any: """str : Name for this joint.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: self._name = str(value) @property - def joint_type(self): + def joint_type(self) -> Any: """str : The type of this joint.""" return self._joint_type @joint_type.setter - def joint_type(self, value): + def joint_type(self, value: Any) -> None: value = str(value) if value not in Joint.TYPES: # pragma nocover raise ValueError("Unsupported joint type {}".format(value)) self._joint_type = value @property - def parent(self): + def parent(self) -> Any: """str : The name of the parent link.""" return self._parent @parent.setter - def parent(self, value): + def parent(self, value: Any) -> None: self._parent = str(value) @property - def child(self): + def child(self) -> Any: """str : The name of the child link.""" return self._child @child.setter - def child(self, value): + def child(self, value: Any) -> None: self._child = str(value) @property - def axis(self): + def axis(self) -> Any: """(3,) float : The joint axis in the joint frame.""" return self._axis @axis.setter - def axis(self, value): + def axis(self, value: Any) -> None: if value is None: value = np.array([1.0, 0.0, 0.0], dtype=np.float64) else: @@ -1592,31 +1610,31 @@ def axis(self, value): self._axis = value @property - def origin(self): + def origin(self) -> Any: """(4,4) float : The pose of child and joint frames relative to the parent link's frame. """ return self._origin @origin.setter - def origin(self, value): + def origin(self, value: Any) -> None: self._origin = configure_origin(value) @property - def rpy(self): + def rpy(self) -> Any: return self._rpy @rpy.setter - def rpy(self, value): + def rpy(self, value: Any) -> None: self._rpy = value @property - def limit(self): + def limit(self) -> Any: """:class:`.JointLimit` : The limits for this joint.""" return self._limit @limit.setter - def limit(self, value): + def limit(self, value: Any) -> None: if value is None: if self.joint_type in ["prismatic", "revolute"]: # pragma nocover raise ValueError( @@ -1627,54 +1645,54 @@ def limit(self, value): self._limit = value @property - def dynamics(self): + def dynamics(self) -> Any: """:class:`.JointDynamics` : The dynamics for this joint.""" return self._dynamics @dynamics.setter - def dynamics(self, value): + def dynamics(self, value: Any) -> None: if value is not None: if not isinstance(value, JointDynamics): # pragma nocover raise TypeError("Expected JointDynamics type") self._dynamics = value @property - def safety_controller(self): # pragma nocover + def safety_controller(self) -> Any: # pragma nocover """:class:`.SafetyController` : The safety controller for this joint.""" return self._safety_controller @safety_controller.setter - def safety_controller(self, value): + def safety_controller(self, value: Any) -> None: if value is not None: if not isinstance(value, SafetyController): # pragma nocover raise TypeError("Expected SafetyController type") self._safety_controller = value @property - def calibration(self): # pragma nocover + def calibration(self) -> Any: # pragma nocover """:class:`.JointCalibration` : The calibration for this joint.""" return self._calibration @calibration.setter - def calibration(self, value): + def calibration(self, value: Any) -> None: if value is not None: if not isinstance(value, JointCalibration): # pragma nocover raise TypeError("Expected JointCalibration type") self._calibration = value @property - def mimic(self): # pragma nocover + def mimic(self) -> Any: # pragma nocover """:class:`.JointMimic` : The mimic for this joint.""" return self._mimic @mimic.setter - def mimic(self, value): + def mimic(self, value: Any) -> None: if value is not None: if not isinstance(value, JointMimic): # pragma nocover raise TypeError("Expected JointMimic type") self._mimic = value - def is_valid(self, cfg): # pragma nocover + def is_valid(self, cfg: Any) -> Any: # pragma nocover """Check if the provided configuration value is valid for this joint. Parameters ---------- @@ -1687,11 +1705,11 @@ def is_valid(self, cfg): # pragma nocover """ if self.joint_type not in ["fixed", "revolute"]: return True - if self.joint_limit is None: + if self.joint_limit is None: # type: ignore[attr-defined] return True cfg = float(cfg) - lower = -np.infty - upper = np.infty + lower = -np.inf + upper = np.inf if self.limit.lower is not None: lower = self.limit.lower if self.limit.upper is not None: @@ -1699,7 +1717,7 @@ def is_valid(self, cfg): # pragma nocover return cfg >= lower and cfg <= upper @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: kwargs = cls._parse(node, path) if "type" not in node.attrib: raise URDFError( @@ -1750,33 +1768,33 @@ class Link(URDFType): } _TAG = "link" - def __init__(self, name, inertial, visuals, collisions): + def __init__(self, name: Any, inertial: Any, visuals: Any, collisions: Any) -> None: self.name = name self.inertial = inertial self.visuals = visuals self.collisions = collisions - def __repr__(self): # pragma nocover + def __repr__(self) -> str: # pragma nocover return "Link(name={}, inertial={}, visuals={}, collisions={})".format( self.name, self.inertial, self.visuals, self.collisions ) @property - def name(self): + def name(self) -> Any: """str : The name of this link.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: self._name = str(value) @property - def inertial(self): + def inertial(self) -> Any: """:class:`.Inertial` : Inertial properties of the link.""" return self._inertial @inertial.setter - def inertial(self, value): + def inertial(self, value: Any) -> None: if value is not None and not isinstance(value, Inertial): raise TypeError("Expected Inertial object") # pragma nocover # Set default inertial @@ -1785,12 +1803,12 @@ def inertial(self, value): self._inertial = value @property - def visuals(self): + def visuals(self) -> Any: """list of :class:`.Visual` : The visual properties of this link.""" return self._visuals @visuals.setter - def visuals(self, value): + def visuals(self, value: Any) -> None: if value is None: # pragma nocover value = [] else: @@ -1801,12 +1819,12 @@ def visuals(self, value): self._visuals = value @property - def collisions(self): + def collisions(self) -> Any: """list of :class:`.Collision` : The collision properties of this link.""" return self._collisions @collisions.setter - def collisions(self, value): + def collisions(self, value: Any) -> None: if value is None: # pragma nocover value = [] else: @@ -1851,13 +1869,13 @@ class URDF(URDFType): def __init__( self, - name, - links, - joints=None, - transmissions=None, - materials=None, - other_xml=None, - ): + name: Any, + links: Any, + joints: Any = None, + transmissions: Any = None, + materials: Any = None, + other_xml: Any = None, + ) -> None: if joints is None: # pragma nocover joints = [] if transmissions is None: # pragma nocover @@ -1880,7 +1898,7 @@ def __init__( self._joints = list(joints) self._transmissions = list(transmissions) self._materials = list(materials) - self._material_map = {} + self._material_map: dict[str, Any] = {} for x in self._materials: if x.name in self._material_map: @@ -1902,16 +1920,16 @@ def __init__( ) @property - def name(self): + def name(self) -> Any: """str : The name of the URDF.""" return self._name @name.setter - def name(self, value): + def name(self, value: Any) -> None: self._name = str(value) @property - def links(self): + def links(self) -> Any: """list of :class:`.Link` : The links of the URDF. This returns a copy of the links array which cannot be edited directly. If you want to add or remove links, use @@ -1920,16 +1938,17 @@ def links(self): return copy.copy(self._links) @property - def link_map(self): # pragma nocover + def link_map(self) -> Any: # pragma nocover """dict : Map from link names to the links themselves. This returns a copy of the link map which cannot be edited directly. If you want to add or remove links, use the appropriate functions. """ - return copy.copy(self._link_map) + # `_link_map` is never set: AttributeError, see findings + return copy.copy(self._link_map) # type: ignore[attr-defined] @property - def joints(self): + def joints(self) -> Any: """list of :class:`.Joint` : The links of the URDF. This returns a copy of the joints array which cannot be edited directly. If you want to add or remove joints, use @@ -1938,16 +1957,16 @@ def joints(self): return copy.copy(self._joints) @property - def joint_map(self): # pragma nocover + def joint_map(self) -> Any: # pragma nocover """dict : Map from joint names to the joints themselves. This returns a copy of the joint map which cannot be edited directly. If you want to add or remove joints, use the appropriate functions. """ - return copy.copy(self._joint_map) + return copy.copy(self._joint_map) # type: ignore[attr-defined] @property - def transmissions(self): + def transmissions(self) -> Any: """list of :class:`.Transmission` : The transmissions of the URDF. This returns a copy of the transmissions array which cannot be edited directly. If you want to add or remove transmissions, use @@ -1956,39 +1975,39 @@ def transmissions(self): return copy.copy(self._transmissions) @property - def transmission_map(self): # pragma nocover + def transmission_map(self) -> Any: # pragma nocover """dict : Map from transmission names to the transmissions themselves. This returns a copy of the transmission map which cannot be edited directly. If you want to add or remove transmissions, use the appropriate functions. """ - return copy.copy(self._transmission_map) + return copy.copy(self._transmission_map) # type: ignore[attr-defined] @property - def other_xml(self): # pragma nocover + def other_xml(self) -> Any: # pragma nocover """str : Any extra XML that belongs with the URDF.""" return self._other_xml @other_xml.setter - def other_xml(self, value): + def other_xml(self, value: Any) -> None: self._other_xml = value @property - def actuated_joints(self): # pragma nocover + def actuated_joints(self) -> Any: # pragma nocover """list of :class:`.Joint` : The joints that are independently actuated. This excludes mimic joints and fixed joints. The joints are listed in topological order, starting from the base-most joint. """ - return self._actuated_joints + return self._actuated_joints # type: ignore[attr-defined] - def _merge_materials(self): + def _merge_materials(self) -> None: """Merge the top-level material set with the link materials.""" for link in self.links: for v in link.visuals: if v.material is None: continue - if v.material.name in self.material_map: + if v.material.name in self.material_map: # type: ignore[attr-defined] v.material = self._material_map[v.material.name] v.geometry.ob.color = v.material.color else: @@ -1996,7 +2015,7 @@ def _merge_materials(self): self._material_map[v.material.name] = v.material @staticmethod - def load(file_obj): # pragma nocover + def load(file_obj: Any) -> Any: # pragma nocover """Load a URDF from a file. Parameters ---------- @@ -2026,7 +2045,7 @@ def load(file_obj): # pragma nocover return URDF._from_xml(node, path) @staticmethod - def loadstr(str_obj, file_obj, base_path=None): + def loadstr(str_obj: Any, file_obj: Any, base_path: Any = None) -> Any: """Load a URDF from a file. Parameters ---------- @@ -2060,14 +2079,14 @@ def loadstr(str_obj, file_obj, base_path=None): return _parse_child(URDF, tree.getroot(), None) - def _validate_transmissions(self): + def _validate_transmissions(self) -> None: """Raise an exception if any transmissions are invalidly specified. Checks for the following: - Transmission joints have valid joint names. """ for t in self.transmissions: # pragma nocover for joint in t.joints: - if joint.name not in self._joint_map: + if joint.name not in self._joint_map: # type: ignore[attr-defined] raise ValueError( "Transmission {} has invalid joint name {}".format( t.name, joint.name @@ -2075,7 +2094,7 @@ def _validate_transmissions(self): ) @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: Any, path: Any) -> Any: valid_tags = set(["joint", "link", "transmission", "material"]) kwargs = cls._parse(node, path) diff --git a/src/roboticstoolbox/tools/urdf/utils.py b/src/roboticstoolbox/tools/urdf/utils.py index 7098e8438..24b5b5e76 100644 --- a/src/roboticstoolbox/tools/urdf/utils.py +++ b/src/roboticstoolbox/tools/urdf/utils.py @@ -1,10 +1,14 @@ """Utilities for URDF parsing.""" +from __future__ import annotations + +from typing import Any + import numpy as np import spatialmath as sm -def parse_origin(node): +def parse_origin(node: Any) -> Any: """Find the ``origin`` subelement of an XML node and convert it into a 4x4 homogenous transformation matrix. Parameters @@ -33,7 +37,7 @@ def parse_origin(node): return matrix.A, rpy -def configure_origin(value): +def configure_origin(value: Any) -> Any: """Convert a value into a 4x4 transform matrix. Parameters ----------