diff --git a/src/roboticstoolbox/blocks/arm.py b/src/roboticstoolbox/blocks/arm.py index 9fd26ae1f..d0e27880d 100644 --- a/src/roboticstoolbox/blocks/arm.py +++ b/src/roboticstoolbox/blocks/arm.py @@ -11,7 +11,7 @@ from spatialmath import SE3 from roboticstoolbox import quintic_func, trapezoidal_func -from roboticstoolbox.tools.types import ArrayLike +from roboticstoolbox.tools.types import ArrayLike, NDArray """ Robot blocks: @@ -23,6 +23,11 @@ # The constructor of each class ``MyClass`` with a ``@block`` decorator becomes a method ``MYCLASS()`` of the BlockDiagram instance. +def _check_gravity(value: Any) -> Any: + """Gravity is None, for the robot's own, or a 3-vector, which replaces ``value``.""" + return None if value is None else smb.getvector(value, 3) + + # ------------------------------------------------------------------------ # class FKine(FunctionBlock): r""" @@ -80,6 +85,8 @@ def __init__(self, robot: Any = None, args: Any = {}, **blockargs: Any) -> None: self.robot = robot self.args = args + self.add_param("robot", when="never") # fixes the ports + self.add_param("args", coerce=False) self.inport_names(("q",)) self.outport_names(("T",)) @@ -172,13 +179,20 @@ def __init__( ik = robot.ikine_LM self.ik = ik self.args = args - self.seed = 0 + # the seed argument used to be ignored, which left it at 0 + self.seed = 0 if seed is None else seed + self.add_param("robot", when="never") # fixes the ports + self.add_param("q0", when="setup", coerce=False) # start() copies it + self.add_param("useprevious") + self.add_param("ik", coerce=False) + self.add_param("args", coerce=False) + self.add_param("seed", coerce=False) self.inport_names(("T",)) self.outport_names(("q",)) - def start(self) -> None: - super().start() + def start(self, simstate: Any) -> None: + super().start(simstate) if self.useprevious: self.qprev = self.q0 @@ -289,34 +303,85 @@ def __init__( self.robot = robot - if frame in (0, "0"): - if representation is None: - self.jfunc = robot.jacob0 - else: - self.jfunc = lambda q: robot.jacob0_analytical( - q, representation=representation - ) - elif frame == "e": - if representation is None: - self.jfunc = robot.jacobe - else: - raise ValueError("cannot compute analytical Jacobian in EE frame") - else: - raise ValueError("unknown frame") + self._frame = frame + self._representation = representation + self.jfunc = self._make_jfunc(frame, representation) - if inverse and robot.n != 6: - raise ValueError("cannot invert a non square Jacobian") - if inverse and pinv: - raise ValueError("can only set one of inverse and pinv") + self._inverse = False + self._pinv = False self.inverse = inverse self.pinv = pinv self.damping = damping self.transpose = transpose - self.representation = representation + + self.add_param("robot", when="never") # fixes the ports + self.add_param("frame") + self.add_param("representation", coerce=False) + self.add_param("inverse") + self.add_param("pinv") + self.add_param("damping", coerce=False) + self.add_param("transpose") self.inport_names(("q",)) self.outport_names(("J",)) + def _make_jfunc(self, frame: Any, representation: Any) -> Callable[[Any], Any]: + """The function that computes the Jacobian, or ValueError if not possible.""" + robot = self.robot + if frame in (0, "0"): + if representation is None: + return robot.jacob0 + return lambda q: robot.jacob0_analytical(q, representation=representation) + elif frame == "e": + if representation is None: + return robot.jacobe + raise ValueError("cannot compute analytical Jacobian in EE frame") + raise ValueError("unknown frame") + + @property + def frame(self) -> str: + """The frame the Jacobian is computed in, ``"0"`` or ``"e"``.""" + return self._frame + + @frame.setter + def frame(self, value: str) -> None: + self.jfunc = self._make_jfunc(value, self._representation) + self._frame = value + + @property + def representation(self) -> str | None: + """The representation of the analytical Jacobian, None for the geometric.""" + return self._representation + + @representation.setter + def representation(self, value: str | None) -> None: + self.jfunc = self._make_jfunc(self._frame, value) + self._representation = value + + @property + def inverse(self) -> bool: + """Output the inverse of the Jacobian, which must be square.""" + return self._inverse + + @inverse.setter + def inverse(self, value: bool) -> None: + if value and self.robot.n != 6: + raise ValueError("cannot invert a non square Jacobian") + if value and self._pinv: + raise ValueError("can only set one of inverse and pinv") + self._inverse = bool(value) + + @property + def pinv(self) -> bool: + """Output the pseudo-inverse of the Jacobian.""" + return self._pinv + + @pinv.setter + def pinv(self, value: bool) -> None: + if value and self._inverse: + raise ValueError("can only set one of inverse and pinv") + self._pinv = bool(value) + def output(self, t: Any, inports: Any, x: Any) -> Any: q = inports[0] @@ -404,6 +469,9 @@ def __init__( self.backend = backend self.q0 = q0 self.env: Any = None + self.add_param("robot", when="never") # fixes the input size + self.add_param("q0", when="setup", coerce=False) # start() plots it + self.add_param("backend", when="setup") def start(self, simstate: Any) -> None: # create the plot @@ -541,12 +609,16 @@ def __init__( self.T = T - def start(self, simstate: Any) -> None: - if self.T is None: - # use simulation tmax - self.T = simstate.tf + # start() computes the coefficients from all of these + self.add_param("q0", when="setup") + self.add_param("qf", when="setup") + self.add_param("qd0", when="setup") + self.add_param("qdf", when="setup") + self.add_param("T", when="setup", coerce=False) # None is the run length - tscal = self.T + def start(self, simstate: Any) -> None: + # None is the length of the run, which can differ from one run to the next + tscal = simstate.tf if self.T is None else self.T self.tscal = tscal q0 = self.q0 @@ -665,19 +737,23 @@ def __init__( self.T2 = T2 self.T = T self.trapezoidal = trapezoidal + self.add_param("T1", coerce=False) + self.add_param("T2", coerce=False) + self.add_param("T", when="setup", coerce=False) # None is the run length + self.add_param("trapezoidal", when="setup") def start(self, simstate: Any) -> None: - if self.T is None: - self.T = simstate.T + # None is the length of the run, which can differ from one run to the next + self._T_run = simstate.T if self.T is None else self.T if self.trapezoidal: - self.trapezoidalfunc = trapezoidal_func(0.0, 1.0, self.T) + self.trapezoidalfunc = trapezoidal_func(0.0, 1.0, self._T_run) def output(self, t: Any, inports: Any, x: Any) -> Any: s: Any if self.trapezoidal: s = self.trapezoidalfunc(t)[0] # get the position from the trapezoidal function else: - s = min(t / self.T, 1.0) + s = min(t / self._T_run, 1.0) return [self.T1.interp(self.T2, s)] @@ -747,34 +823,56 @@ def __init__( """ if phase is None: phase = 0 - # TODO # flag to rotate the frame rather than just translate it super().__init__(**blockargs) - - if unit == "rps": - omega = frequency * 2 * pi - phase = phase * 2 * pi - elif unit == "rad": - omega = frequency - - # Redundant assignment, commented for LGTM - # phase = phase - else: + if unit not in ("rps", "rad"): raise ValueError("bad units: rps or rad") - + self.unit = unit self.radius = radius if len(centre) != 3: raise ValueError("centre must be a 3 vector") self.centre = centre self.pose = pose - self.omega = omega + # frequency and phase are kept in the unit they were given in, and the + # angular rate and phase that output() uses are derived from them + self.frequency = frequency self.phase = phase - self.outport_names(("y",)) + self.add_param("radius") + self.add_param("centre", validate=self._check_centre, coerce=False) + self.add_param("pose", coerce=False) + self.add_param("frequency") + self.add_param("unit", when="never") # what frequency and phase mean + self.add_param("phase") + + @staticmethod + def _check_centre(value: Any) -> None: + if len(value) != 3: + raise ValueError("centre must be a 3 vector") + + @property + def frequency(self) -> float: + """Frequency of the circle, in the units of ``unit``.""" + return self._frequency + + @frequency.setter + def frequency(self, value: float) -> None: + self._frequency = value + self.omega = value * 2 * pi if self.unit == "rps" else value + + @property + def phase(self) -> float: + """Phase of the circle, in the units of ``unit`` (cycles or radians).""" + return self._phase + + @phase.setter + def phase(self, value: float) -> None: + self._phase = value + self._phase_rad = value * 2 * pi if self.unit == "rps" else value def output(self, t: Any, inports: Any, x: Any) -> Any: - theta = t * self.omega + self.phase + theta = t * self.omega + self._phase_rad x = self.radius * cos(theta) + self.centre[0] y = self.radius * sin(theta) + self.centre[1] p = (x, y, self.centre[2]) @@ -856,11 +954,16 @@ def __init__( self.T = T self.q0 = q0 self.qf = qf + # start() builds the trajectory from these + self.add_param("q0", when="setup", coerce=False) + self.add_param("qf", when="setup", coerce=False) + self.add_param("V", when="never") # accepted and ignored + self.add_param("T", when="setup", coerce=False) # None is the run length def start(self, simstate: Any) -> None: - if self.T is None: - self.T = simstate.T - self.trapezoidalfunc = trapezoidal_func(self.q0, self.qf, self.T) + # None is the length of the run, which can differ from one run to the next + T = simstate.T if self.T is None else self.T + self.trapezoidalfunc = trapezoidal_func(self.q0, self.qf, T) def output(self, t: Any, inports: Any, x: Any) -> Any: return list(self.trapezoidalfunc(t)) @@ -975,8 +1078,20 @@ def __init__( f"unknown traj {traj!r}, must be 'trapezoidal' or 'quintic'" ) self.traj = traj - self.outport_names(("y", "yd", "ydd")) + # start() builds the trajectory from these + self.add_param("y0", when="setup") + self.add_param("yf", when="setup") + self.add_param("T", when="setup", coerce=False) # None is the run length + self.add_param("time", when="never") # source or function block + self.add_param("traj", when="setup", validate=self._check_traj) + + @staticmethod + def _check_traj(value: str) -> None: + if value not in ("trapezoidal", "quintic"): + raise ValueError( + f"unknown traj {value!r}, must be 'trapezoidal' or 'quintic'" + ) def start(self, simstate: Any) -> None: # if self.time: @@ -1094,6 +1209,8 @@ def __init__(self, robot: Any, gravity: Any = None, **blockargs: Any) -> None: self.robot = robot self.gravity = None if gravity is None else smb.getvector(gravity, 3) + self.add_param("robot", when="never") # fixes the ports + self.add_param("gravity", validate=_check_gravity, coerce=False) # state vector is [q qd] @@ -1164,6 +1281,8 @@ def __init__(self, robot: Any, gravity: Any = None, **blockargs: Any) -> None: self.robot = robot self.gravity = None if gravity is None else smb.getvector(gravity, 3) + self.add_param("robot", when="never") # fixes the ports + self.add_param("gravity", validate=_check_gravity, coerce=False) self.inport_names(("q",)) self.outport_names(("$\tau$",)) @@ -1236,9 +1355,12 @@ def __init__( self.robot = robot self.gravity = None if gravity is None else smb.getvector(gravity, 3) + self.add_param("robot", when="never") # fixes the ports + self.add_param("gravity", validate=_check_gravity, coerce=False) self.inport_names(("q",)) self.outport_names(("$\tau$",)) self.representation = representation + self.add_param("representation") def output(self, t: Any, inports: Any, x: Any) -> Any: q = inports[0] @@ -1305,6 +1427,7 @@ def __init__(self, robot: Any, **blockargs: Any) -> None: # self.type = "inertia" self.robot = robot + self.add_param("robot", when="never") # fixes the ports self.inport_names(("q",)) self.outport_names(("M",)) @@ -1377,6 +1500,9 @@ def __init__( self.robot = robot self.representation = representation self.pinv = pinv + self.add_param("robot", when="never") # fixes the ports + self.add_param("representation") + self.add_param("pinv") self.inport_names(("q",)) self.outport_names(("M",)) @@ -1452,6 +1578,8 @@ def __init__(self, robot: Any, q0: Any = None, **blockargs: Any) -> None: self.robot = robot self.nstates = robot.n * 2 + self.add_param("robot", when="never") # fixes the ports and states + self.add_param("q0", when="setup") # state vector is [q qd] @@ -1465,6 +1593,16 @@ def __init__(self, robot: Any, q0: Any = None, **blockargs: Any) -> None: self._x0 = np.r_[q0, np.zeros((robot.n,))] self._qdd = None + @property + def q0(self) -> NDArray: + """Initial joint configuration, the first half of the initial state.""" + return self._x0[: self.robot.n].copy() + + @q0.setter + def q0(self, value: ArrayLike) -> None: + n = self.robot.n + self._x0 = np.r_[smb.getvector(value, n), self._x0[n:]] + def output(self, t: Any, inports: Any, x: Any) -> Any: n = self.robot.n q = x[:n] @@ -1570,6 +1708,11 @@ def __init__( self.gravcomp = gravcomp self.velcomp = velcomp self.representation = representation + self.add_param("robot", when="never") # fixes the ports and states + self.add_param("q0", when="setup") + self.add_param("gravcomp") + self.add_param("velcomp") + self.add_param("representation") # state vector is [q qd] @@ -1594,6 +1737,16 @@ def __init__( # case (stateful blocks whose output() needs a resolved input). self._qdd = np.zeros((robot.n,)) + @property + def q0(self) -> NDArray: + """Initial joint configuration, the first half of the initial state.""" + return self._x0[: self.robot.n].copy() + + @q0.setter + def q0(self, value: ArrayLike) -> None: + n = self.robot.n + self._x0 = np.r_[smb.getvector(value, n), self._x0[n:]] + def output(self, t: Any, inports: Any, x: Any) -> Any: n = self.robot.n q = x[:n] diff --git a/src/roboticstoolbox/blocks/mobile.py b/src/roboticstoolbox/blocks/mobile.py index 6e73f4342..2686d4511 100644 --- a/src/roboticstoolbox/blocks/mobile.py +++ b/src/roboticstoolbox/blocks/mobile.py @@ -20,7 +20,46 @@ # ------------------------------------------------------------------------ # -class Bicycle(ContinuousBlock): + + +class _VehicleLimits: + """The limits of the vehicle model that the block holds in ``self.vehicle``. + + The model keeps them in private attributes that it only sets when it is created, + and the block owns its model, so the block parameters write them directly. + """ + + vehicle: Any + + @property + def speed_max(self) -> float: + """Maximum speed.""" + return self.vehicle.speed_max + + @speed_max.setter + def speed_max(self, value: float) -> None: + self.vehicle._speed_max = value + + @property + def accel_max(self) -> float: + """Maximum acceleration.""" + return self.vehicle.accel_max + + @accel_max.setter + def accel_max(self, value: float) -> None: + self.vehicle._accel_max = value + + @property + def steer_max(self) -> float | None: + """Maximum steering angle, or rate for a vehicle that steers by wheel speed.""" + return self.vehicle.steer_max + + @steer_max.setter + def steer_max(self, value: float | None) -> None: + self.vehicle._steer_max = value + + +class Bicycle(_VehicleLimits, ContinuousBlock): r""" :blockname:`BICYCLE` @@ -126,15 +165,45 @@ def __init__( self.outport_names(("q",)) self.state_names(("x", "y", r"$\theta$")) + self.add_param("L") + self.add_param("speed_max") + self.add_param("accel_max") + self.add_param("steer_max", coerce=False) + self.add_param("x0", when="setup") + def output(self, t: float, inports: list[Any], x: NDArray) -> list[R3]: return [x] # one output which is ndarray(3) def deriv(self, t: float, inports: list[Any], x: NDArray) -> NDArray: return self.vehicle.deriv(x, inports) + @property + def L(self) -> float: + """Wheelbase of the vehicle.""" + return self.vehicle.l + + @L.setter + def L(self, value: float) -> None: + self.vehicle._l = value + # ------------------------------------------------------------------------ # -class Unicycle(ContinuousBlock): + + +class _VehicleWidth(_VehicleLimits): + """The limits, and the width between the wheels, of a vehicle that has no steering.""" + + @property + def w(self) -> float: + """Width of the vehicle, the distance between the wheels.""" + return self.vehicle._W + + @w.setter + def w(self, value: float) -> None: + self.vehicle._W = value + + +class Unicycle(_VehicleWidth, ContinuousBlock): r""" :blockname:`UNICYCLE` @@ -213,6 +282,11 @@ def __init__( self.vehicle = mobile.Unicycle( W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max ) + self.add_param("speed_max") + self.add_param("accel_max") + self.add_param("steer_max", coerce=False) + self.add_param("w") + self.add_param("x0", when="setup") # TODO, add support for origin shift # If ``a`` is non-zero then the planar velocity of that point $x=a$ @@ -236,7 +310,9 @@ def deriv(self, t: float, inports: list[Any], x: NDArray) -> NDArray: # ------------------------------------------------------------------------ # -class DiffSteer(ContinuousBlock): + + +class DiffSteer(_VehicleWidth, ContinuousBlock): r""" :blockname:`DIFFSTEER` @@ -319,6 +395,12 @@ def __init__( self.vehicle = mobile.DiffSteer( W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max ) + self.add_param("speed_max") + self.add_param("accel_max") + self.add_param("steer_max", coerce=False) + self.add_param("w") + self.add_param("R", when="never") # accepted and ignored + self.add_param("x0", when="setup") def output(self, t: float, inports: list[Any], x: NDArray) -> list[R3]: return [x] @@ -449,6 +531,8 @@ def __init__( super().__init__(**blockargs) self.xdata: list[float] = [] self.ydata: list[float] = [] + self.size = size + self.polygon_style = polygon_style # self.type = "vehicleplot" if init_func is not None: if not callable(init_func): @@ -482,6 +566,29 @@ def __init__( self.animation = animation + self.add_param("animation", when="never") # the animation object is built here + # shape cannot be a parameter, since a block already has a shape (its icon) + self.add_param( + "size", when="never" + ) # with polygon_style, builds the default animation + self.add_param("polygon_style", when="never") + # start() sets up the figure from the rest + self.add_param("path_style", when="setup", coerce=False) + self.add_param("labels", when="setup") + self.add_param("square", when="setup") + self.add_param("init_func", when="setup", coerce=False) + self.add_param( + "limits", when="setup", validate=self._check_limits, coerce=False + ) + + @staticmethod + def _check_limits(value: Any) -> Any: + """``"auto"`` or ``[xmin, xmax, ymin, ymax]``; ``[min, max]`` is for both axes.""" + if isinstance(value, str): + return value + value = list(value) + return value * 2 if len(value) == 2 else value + def start(self, simstate: SimulationState) -> None: super().start(simstate) diff --git a/src/roboticstoolbox/blocks/spatial.py b/src/roboticstoolbox/blocks/spatial.py index e5ca727af..9d76e33f5 100644 --- a/src/roboticstoolbox/blocks/spatial.py +++ b/src/roboticstoolbox/blocks/spatial.py @@ -166,6 +166,16 @@ def __init__(self, T: Any = None, **blockargs: Any) -> None: if T is None: T = SE3() self.pose = T + self.add_param("T", coerce=False) + + @property + def T(self) -> SE3: + """The pose whose rotation is used for every output.""" + return self.pose + + @T.setter + def T(self, value: SE3) -> None: + self.pose = value def output(self, t: Any, inports: Any, x: Any) -> Any: T = SE3.Rt(self.pose.R, t=inports[0]) diff --git a/src/roboticstoolbox/blocks/uav.py b/src/roboticstoolbox/blocks/uav.py index f6565b691..ea629dee0 100644 --- a/src/roboticstoolbox/blocks/uav.py +++ b/src/roboticstoolbox/blocks/uav.py @@ -6,6 +6,8 @@ import numpy as np from bdsim.block_types import ContinuousBlock, FunctionBlock, GraphicsBlock +from roboticstoolbox.tools.types import NDArray + class MultiRotor(ContinuousBlock): r""" @@ -187,23 +189,7 @@ def __init__( raise ValueError("Must have an even number of rotors") self.nstates = 12 - if x0 is None: - x0 = np.zeros((self.nstates,)) - else: - x0 = np.r_[x0] - if len(x0) == 6: - # assume all derivative are zero - x0 = np.r_[x0, np.zeros((6,))] - elif len(x0) == 4: - # assume x,y,z,yaw - x0 = np.r_[x0[:3], 0, 0, x0[3], np.zeros((6,))] - elif len(x0) == 3: - # assume x,y,z - x0 = np.r_[x0[:3], np.zeros((9,))] - elif len(x0) != self.nstates: - raise ValueError("x0 is the wrong length") - - self._x0 = x0 + self._x0 = self._initial_state(x0) self.nrotors = nrotors self.model = model @@ -225,6 +211,39 @@ def __init__( self.a1s = np.zeros((self.nrotors,)) self.b1s = np.zeros((self.nrotors,)) + self.add_param("model", when="never") # fixes the rotors, which fix the input + self.add_param("groundcheck") + self.add_param("speedcheck") + self.add_param("x0", when="setup", coerce=False) # the setter expands short forms + + @staticmethod + def _initial_state(x0: Any) -> NDArray: + """The 12-element initial state from a full state, or one of the short forms.""" + if x0 is None: + return np.zeros((12,)) + x0 = np.r_[x0] + if len(x0) == 6: + # assume all derivative are zero + return np.r_[x0, np.zeros((6,))] + elif len(x0) == 4: + # assume x,y,z,yaw + return np.r_[x0[:3], 0, 0, x0[3], np.zeros((6,))] + elif len(x0) == 3: + # assume x,y,z + return np.r_[x0[:3], np.zeros((9,))] + elif len(x0) != 12: + raise ValueError("x0 is the wrong length") + return x0 + + @property + def x0(self) -> NDArray: + """Initial state, a full state or the short forms x,y,z / x,y,z,yaw / pose.""" + return self._x0 + + @x0.setter + def x0(self, value: Any) -> None: + self._x0 = self._initial_state(value) + def output(self, t: Any, inports: Any, x: Any) -> Any: model = self.model @@ -567,6 +586,28 @@ def __init__( self.Minv = np.linalg.inv(self.M) self.signs = np.array(s) + self.add_param("model", when="never") # builds the mixer matrix + self.add_param("wmax") + self.add_param("wmin") + + @property + def wmax(self) -> float: + """Maximum rotor speed.""" + return float(np.sqrt(self.maxw)) + + @wmax.setter + def wmax(self, value: float) -> None: + self.maxw = value**2 + + @property + def wmin(self) -> float: + """Minimum rotor speed.""" + return float(np.sqrt(self.minw)) + + @wmin.setter + def wmin(self, value: float) -> None: + self.minw = value**2 + def output(self, t: Any, inports: Any, x: Any) -> Any: tau = inports @@ -703,6 +744,11 @@ def __init__( self.projection = projection self.flapscale = flapscale + self.add_param("model", when="never") # fixes the rotors + self.add_param("scale", when="setup", coerce=False) # start() sets the limits + self.add_param("flapscale") + self.add_param("projection", when="setup") # start() sets it + def start(self, simstate: Any) -> None: super().start(simstate) diff --git a/tests/test_block_parameters.py b/tests/test_block_parameters.py new file mode 100644 index 000000000..53090f292 --- /dev/null +++ b/tests/test_block_parameters.py @@ -0,0 +1,275 @@ +#!/usr/bin/env python3 +""" +Block parameters of the RTB blocks: ``Block.set_param``, ``Block.parameters`` and the +``--set`` option of bdsim. + +Every ``__init__`` argument of a block is registered with ``self.add_param(name)``, and +is one of + +* ``when="anytime"`` (the default): settable at any time, between evaluations +* ``when="setup"``: settable before the run starts, refused while it runs, since + ``start()`` has used the value to build something +* ``when="never"``: fixed when the block is created (``robot``, ``model``) + +A parameter whose value is not a plain attribute, because the block derived state from +it, is a Python ``property``: the setter does the work and the registration is the same. +""" + +import ast +import unittest +from pathlib import Path + +import bdsim +import numpy as np +import numpy.testing as nt + +import roboticstoolbox as rtb +from roboticstoolbox.blocks.quad_model import quadrotor + +BLOCKS = Path(rtb.__file__).parent / "blocks" + +# Arguments that cannot be block parameters, and why. +NOT_PARAMETERS: dict[str, str] = { + # a block has a shape of its own, its icon in the diagram + "VehiclePlot.shape": "the name is taken by Block.shape", +} + + +def block_parameters() -> dict[str, str]: + """{Class.param: registered or not} from the source of every block module.""" + found = {} + for path in sorted(BLOCKS.glob("*.py")): + tree = ast.parse(path.read_text(encoding="utf-8")) + for cls in [n for n in tree.body if isinstance(n, ast.ClassDef)]: + init = next( + ( + n + for n in cls.body + if isinstance(n, ast.FunctionDef) and n.name == "__init__" + ), + None, + ) + if init is None: + continue + a = init.args + params = [ + x.arg + for x in a.posonlyargs + a.args + a.kwonlyargs + if x.arg not in ("self", "blockargs", "kwargs") + ] + registered = { + node.args[0].value + for node in ast.walk(init) + if isinstance(node, ast.Call) + and isinstance(node.func, ast.Attribute) + and node.func.attr == "add_param" + and node.args + and isinstance(node.args[0], ast.Constant) + } + # parameters registered by a helper method of the class + for node in cls.body: + if isinstance(node, ast.FunctionDef) and node.name != "__init__": + for call in ast.walk(node): + if ( + isinstance(call, ast.Call) + and isinstance(call.func, ast.Attribute) + and call.func.attr == "add_param" + and call.args + and isinstance(call.args[0], ast.Constant) + ): + registered.add(call.args[0].value) + for p in params: + found[f"{cls.name}.{p}"] = "registered" if p in registered else "none" + return found + + +class EveryParameterIsAccountedForTest(unittest.TestCase): + def test_no_parameter_is_left_out(self): + left_out = sorted( + k + for k, how in block_parameters().items() + if how == "none" and k not in NOT_PARAMETERS + ) + self.assertEqual( + left_out, + [], + 'register with self.add_param(name) (when="never" if it is fixed)', + ) + + def test_not_parameters_are_real_arguments_that_are_not_registered(self): + found = block_parameters() + self.assertEqual( + sorted(k for k in NOT_PARAMETERS if found.get(k) != "none"), + [], + "remove from NOT_PARAMETERS", + ) + + +class ParamTestCase(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.sim = bdsim.BDSim(graphics=None, progress=False, banner=False, quiet=True) + cls.robot = rtb.models.DH.Puma560() + + def bd(self): + return self.sim.blockdiagram() + + +class ParametersTest(ParamTestCase): + def test_robot_is_fixed(self): + block = self.bd().FKINE(self.robot) + with self.assertRaises(ValueError): + block.set_param("robot", self.robot) + self.assertNotIn("robot", block.parameters()) + + def test_fdyn_q0_is_the_first_half_of_the_initial_state(self): + block = self.bd().FDYN(self.robot, self.robot.qn) + nt.assert_array_equal(block.q0, self.robot.qn) + block.set_param("q0", self.robot.qr) + nt.assert_array_equal(block.getstate0(), np.r_[self.robot.qr, np.zeros(6)]) + with self.assertRaises(ValueError): + block.set_param("q0", [1, 2, 3]) + + def test_jacobian_frame_and_representation_rebuild_the_function(self): + block = self.bd().JACOBIAN(self.robot) + q = self.robot.qn + nt.assert_array_almost_equal(block.jfunc(q), self.robot.jacob0(q)) + block.set_param("frame", "e") + nt.assert_array_almost_equal(block.jfunc(q), self.robot.jacobe(q)) + with self.assertRaises(ValueError): + block.set_param("representation", "rpy/xyz") # not in the EE frame + self.assertEqual(block.frame, "e") + + def test_jacobian_inverse_and_pinv_exclude_each_other(self): + block = self.bd().JACOBIAN(self.robot, inverse=True) + with self.assertRaises(ValueError): + block.set_param("pinv", True) + block.set_param("inverse", False) + block.set_param("pinv", True) + self.assertTrue(block.pinv) + + def test_gravity_is_checked_and_can_be_none(self): + block = self.bd().GRAVLOAD(self.robot) + self.assertIsNone(block.gravity) + block.set_param("gravity", [0, 0, -1]) + nt.assert_array_equal(block.gravity, [0, 0, -1]) + with self.assertRaises(ValueError): + block.set_param("gravity", [1, 2]) + block.set_param("gravity", None) + self.assertIsNone(block.gravity) + + def test_circlepath_frequency_and_phase_are_in_the_unit_given(self): + block = self.bd().CIRCLEPATH(frequency=1, phase=0.25) + self.assertAlmostEqual(block.omega, 2 * np.pi) + self.assertAlmostEqual(block._phase_rad, np.pi / 2) + block.set_param("frequency", 2.0) + self.assertAlmostEqual(block.omega, 4 * np.pi) + with self.assertRaises(ValueError): + block.set_param("unit", "rad") # what the numbers mean is fixed + radians = self.bd().CIRCLEPATH(frequency=3, unit="rad") + self.assertAlmostEqual(radians.omega, 3) + + def test_point2tr_T_is_the_pose(self): + from spatialmath import SE3 + + block = self.bd().POINT2TR() + pose = SE3.Rx(0.3) + block.set_param("T", pose) + self.assertIs(block.pose, pose) + + def test_vehicle_limits_reach_the_model(self): + block = self.bd().BICYCLE(L=2, speed_max=3) + self.assertEqual(block.L, 2) + block.set_param("L", 3.0) + block.set_param("speed_max", 5.0) + block.set_param("steer_max", 0.3) + self.assertEqual(block.vehicle.l, 3) + self.assertEqual(block.vehicle.speed_max, 5) + self.assertEqual(block.vehicle.steer_max, 0.3) + + block = self.bd().UNICYCLE(w=2) + block.set_param("w", 4.0) + self.assertEqual(block.vehicle._W, 4) + + def test_multirotor_x0_accepts_the_short_forms(self): + block = self.bd().MULTIROTOR(quadrotor) + block.set_param("x0", [1, 2, 3]) + nt.assert_array_equal(block.getstate0(), np.r_[1, 2, 3, np.zeros(9)]) + block.set_param("x0", np.arange(12.0)) + nt.assert_array_equal(block.getstate0(), np.arange(12.0)) + with self.assertRaises(ValueError): + block.set_param("x0", [1, 2]) + + def test_multirotormixer_speed_limits(self): + block = self.bd().MULTIROTORMIXER(quadrotor, wmax=800, wmin=10) + self.assertAlmostEqual(block.wmax, 800) + block.set_param("wmin", 20.0) + self.assertAlmostEqual(block.minw, 400) + + def test_vehicleplot_limits_are_normalised(self): + block = self.bd().VEHICLEPLOT() + block.set_param("limits", [-5, 5]) + self.assertEqual(block.limits, [-5, 5, -5, 5]) + block.set_param("limits", "auto") + self.assertEqual(block.limits, "auto") + + +class RunsTest(ParamTestCase): + """Parameters that start() reads are used by the next run, not by the first.""" + + def test_jtraj_with_no_T_follows_the_length_of_each_run(self): + bd = self.bd() + traj = bd.JTRAJ(self.robot.qz, self.robot.qr) + bd.compile() + for T in (1.0, 2.0): + out = self.sim.run(bd, T=T, dt=0.1, watch=[traj[0]]) + nt.assert_array_almost_equal(out.y[-1], self.robot.qr, decimal=3) + + def test_start_resolves_no_T_without_changing_the_parameter(self): + # a second run, longer than the first, must not keep the first run's length. + # The blocks are started by hand: Trapezoidal, CTraj and Traj cannot be + # compiled into a diagram, since compile() evaluates outputs before start(). + from types import SimpleNamespace + + from spatialmath import SE3 + + cases = { + "trapezoidal": (self.bd().TRAPEZOIDAL(0, 1), np.array([1.0])), + "traj": (self.bd().TRAJ(0, 1, time=True), np.array([1.0])), + } + for name, (block, final) in cases.items(): + with self.subTest(block=name): + for T in (1.0, 2.0): + block.start(SimpleNamespace(T=T, tf=T)) + nt.assert_array_almost_equal( + np.atleast_1d(block.output(T, [], None)[0]), final, decimal=3 + ) + self.assertIsNone(block.T) + + block = self.bd().CTRAJ(SE3(), SE3.Tx(1), None) + for T in (1.0, 2.0): + block.start(SimpleNamespace(T=T, tf=T)) + nt.assert_array_almost_equal(block.output(T, [], None)[0].t, [1, 0, 0]) + self.assertIsNone(block.T) + + def test_ikine_runs(self): + # start() used to take no simstate, which every run passes + robot = self.robot + bd = self.bd() + pose = bd.CONSTANT(robot.fkine(robot.qn)) + ik = bd.IKINE(robot, q0=robot.qn) + bd.connect(pose, ik) + bd.compile() + out = self.sim.run(bd, T=0.2, dt=0.1, watch=[ik[0]]) + nt.assert_array_almost_equal( + robot.fkine(out.y[-1]).A, robot.fkine(robot.qn).A, decimal=4 + ) + + def test_armplot_q0_is_a_setup_parameter(self): + block = self.bd().ARMPLOT(self.robot) + block.set_param("q0", self.robot.qn) + nt.assert_array_equal(block.q0, self.robot.qn) + + +if __name__ == "__main__": + unittest.main()