roboticstoolbox-python 1.4.0__py3-none-any.whl

roboticstoolbox/blocks/mobile.py

@@ -0,0 +1,522 @@
+from typing import Type
+import warnings
+import numpy as np
+from math import sin, cos, atan2, tan, sqrt, pi
+
+import matplotlib.pyplot as plt
+from matplotlib.patches import Polygon
+import time
+
+from bdsim.block_types import GraphicsBlock, ContinuousBlock
+
+from roboticstoolbox import mobile
+
+
+# ------------------------------------------------------------------------ #
+class Bicycle(ContinuousBlock):
+    r"""
+    :blockname:`BICYCLE`
+
+    Vehicle model with Bicycle kinematics.
+
+    :inputs: 2
+    :outputs: 1
+    :states: 3
+
+    .. list-table::
+        :header-rows: 1
+
+        *   - Port type
+            - Port number
+            - Types
+            - Description
+        *   - Input
+            - 0
+            - float
+            - :math:`v`, longitudinal velocity
+        *   - Input
+            - 1
+            - float
+            - :math:`\gamma`, steer angle
+        *   - Output
+            - 0
+            - ndarray(3)
+            - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration
+
+    Bicycle kinematic model with velocity and steering inputs and vehicle configuration
+    as output.
+
+    Various limits are applied to the inputs:
+
+    * velocity limit ``speed_max``
+    * acceleration limit ``accel_max``
+    * steering limit ``steer_max``
+
+    :seealso: :class:`~roboticstoolbox.mobile.Vehicle.Bicycle` :class:`Unicycle` :class:`DiffSteer`
+    """
+
+    nin = 2
+    nout = 1
+    inlabels = ("v", "γ")
+    outlabels = ("q",)
+
+    def __init__(
+        self,
+        L=1,
+        speed_max=np.inf,
+        accel_max=np.inf,
+        steer_max=0.45 * pi,
+        x0=None,
+        **blockargs,
+    ):
+        r"""
+        :param L: Wheelbase, defaults to 1
+        :type L: float, optional
+        :param speed_max: Velocity limit, defaults to math.inf
+        :type speed_max: float, optional
+        :param accel_max: maximum acceleration, defaults to math.inf
+        :type accel_max: float, optional
+        :param steer_max: maximum steered wheel angle, defaults to math.pi*0.45
+        :type steer_max: float, optional
+        :param x0: Initial state, defaults to [0,0,0]
+        :type x0: array_like(3), optional
+        :param blockargs: |BlockOptions|
+        :type blockargs: dict
+
+        .. deprecated:: the ``vlim`` and ``slim`` keywords are deprecated,
+            use ``speed_max`` and ``steer_max`` instead.
+        """
+        # TODO: add option to model the effect of steering arms, responds to
+        #  gamma dot
+
+        vlim = blockargs.pop("vlim", None)
+        if vlim is not None:
+            warnings.warn(
+                "vlim is deprecated, use speed_max instead",
+                DeprecationWarning,
+                stacklevel=2,
+            )
+            speed_max = vlim
+
+        slim = blockargs.pop("slim", None)
+        if slim is not None:
+            warnings.warn(
+                "slim is deprecated, use steer_max instead",
+                DeprecationWarning,
+                stacklevel=2,
+            )
+            steer_max = slim
+
+        super().__init__(nstates=3, **blockargs)
+        self.type = "bicycle"
+
+        self.vehicle = mobile.Bicycle(
+            L=L, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max
+        )
+
+        if x0 is None:
+            self._x0 = np.zeros((self.nstates,))
+        else:
+            assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(
+                len(x0), self.nstates
+            )
+            self._x0 = x0
+
+        self.inport_names(("v", r"$\gamma$"))
+        self.outport_names(("q",))
+        self.state_names(("x", "y", r"$\theta$"))
+
+    def output(self, t, inports, x):
+        return [x]  # one output which is ndarray(3)
+
+    def deriv(self, t, inports, x):
+        return self.vehicle.deriv(x, inports)
+
+
+# ------------------------------------------------------------------------ #
+class Unicycle(ContinuousBlock):
+    r"""
+    :blockname:`UNICYCLE`
+
+    Vehicle model with Unicycle kinematics.
+
+    :inputs: 2
+    :outputs: 1
+    :states: 3
+
+    .. list-table::
+        :header-rows: 1
+
+        *   - Port type
+            - Port number
+            - Types
+            - Description
+        *   - Input
+            - 0
+            - float
+            - :math:`v`, longitudinal velocity
+        *   - Input
+            - 1
+            - float
+            - :math:`\omega`, turn rate
+        *   - Output
+            - 0
+            - ndarray(3)
+            - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration
+
+    Unicycle kinematic model with velocity and steering inputs and vehicle configuration
+    as output.
+
+    Various limits are applied to the inputs:
+
+    * velocity limit ``speed_max``
+    * acceleration limit ``accel_max``
+    * steering limit ``steer_max``
+
+    :seealso: :class:`~roboticstoolbox.mobile.Vehicle.Unicycle` :class:`Bicycle` :class:`DiffSteer`
+
+    """
+
+    nin = 2
+    nout = 1
+    inlabels = ("v", "ω")
+    outlabels = ("q",)
+
+    def __init__(
+        self,
+        w=1,
+        speed_max=np.inf,
+        accel_max=np.inf,
+        steer_max=np.inf,
+        # a=0,  implement this, RVC2 p111, change output and deriv
+        x0=None,
+        **blockargs,
+    ):
+        r"""
+
+        :param w: vehicle width, defaults to 1
+        :type w: float, optional
+        :param speed_max: Velocity limit, defaults to math.inf
+        :type speed_max: float, optional
+        :param accel_max: maximum acceleration, defaults to math.inf
+        :type accel_max: float, optional
+        :param steer_max: maximum turn rate :math:`\omega`, defaults to math.inf
+        :type steer_max: float, optional
+        :param x0: Inital state, defaults to [0,0,0]
+        :type x0: array_like(3), optional
+        :param blockargs: |BlockOptions|
+        :type blockargs: dict
+        """
+        super().__init__(nstates=3, **blockargs)
+        self.type = "unicycle"
+
+        if x0 is None:
+            self._x0 = np.zeros((self.nstates,))
+        else:
+            assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(
+                len(x0), self.nstates
+            )
+            self._x0 = x0
+
+        self.vehicle = mobile.Unicycle(
+            W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max
+        )
+
+        # TODO, add support for origin shift
+        #         If ``a`` is non-zero then the planar velocity of that point $x=a$
+        # can be controlled by
+
+        # .. math::
+
+        #     \begin{pmatrix} v \\ \omega \end{pmatrix} =
+        #     \begin{pmatrix}
+        #         \cos \theta & \sin \theta \\
+        #         -\frac{1}{a}\sin \theta & \frac{1}{a}\cos \theta
+        #     \end{pmatrix}\begin{pmatrix}
+        #         \dot{x} \\ \dot{y}
+        #     \end{pmatrix}
+
+    def output(self, t, inports, x):
+        return [x]
+
+    def deriv(self, t, inports, x):
+        return self.vehicle.deriv(x, inports)
+
+
+# ------------------------------------------------------------------------ #
+class DiffSteer(ContinuousBlock):
+    r"""
+    :blockname:`DIFFSTEER`
+
+    Differential steer vehicle model
+
+    :inputs: 2
+    :outputs: 1
+    :states: 3
+
+    .. list-table::
+        :header-rows: 1
+
+        *   - Port type
+            - Port number
+            - Types
+            - Description
+        *   - Input
+            - 0
+            - float
+            - :math:`\omega_L`, left-wheel angular velocity (radians/sec).
+        *   - Input
+            - 1
+            - float
+            - :math:`\omega_R`, right-wheel angular velocity (radians/sec).
+        *   - Output
+            - 0
+            - ndarray(3)
+            - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration
+
+    Differential steering kinematic model with wheel velocity inputs and vehicle
+    configuration as output.
+
+    Various limits are applied to the inputs:
+
+    * velocity limit ``speed_max``
+    * acceleration limit ``accel_max``
+    * steering limit ``steer_max``
+
+    .. note:: Wheel velocity is defined such that if both are positive the vehicle
+            moves forward.
+
+    :seealso: :class:`~roboticstoolbox.mobile.Vehicle.Diffsteer` :class:`Bicycle` :class:`Unicycle`
+    """
+
+    nin = 2
+    nout = 1
+
+    inlabels = ("ωL", "ωR")
+    outlabels = ("q",)
+
+    def __init__(
+        self,
+        w=1,
+        R=1,
+        speed_max=np.inf,
+        accel_max=np.inf,
+        steer_max=None,
+        a=0,
+        x0=None,
+        **blockargs,
+    ):
+        r"""
+        :param w: vehicle width, defaults to 1
+        :type w: float, optional
+        :param R: Wheel radius, defaults to 1
+        :type R: float, optional
+        :param speed_max: Velocity limit, defaults to 1
+        :type speed_max: float, optional
+        :param accel_max: maximum acceleration, defaults to math.inf
+        :type accel_max: float, optional
+        :param steer_max: maximum steering rate, defaults to 1
+        :type steer_max: float, optional
+        :param x0: Inital state, defaults to None
+        :type x0: array_like, optional
+        :param blockargs: |BlockOptions|
+        :type blockargs: dict
+        """
+        super().__init__(nstates=3, **blockargs)
+        self.type = "diffsteer"
+        self.R = R
+
+        if x0 is None:
+            self._x0 = np.zeros((slef.nstates,))
+        else:
+            assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(
+                len(x0), self.nstates
+            )
+            self._x0 = x0
+
+        self.vehicle = mobile.DiffSteer(
+            W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max
+        )
+
+    def output(self, t, inports, x):
+        return [x]
+
+    def deriv(self, t, inports, x):
+        return self.vehicle.deriv(x, inports)
+
+
+# ------------------------------------------------------------------------ #
+
+
+class VehiclePlot(GraphicsBlock):
+    r"""
+    :blockname:`VEHICLEPLOT`
+
+    Vehicle animation
+
+    :inputs: 1
+    :outputs: 0
+    :states: 0
+
+    .. list-table::
+        :header-rows: 1
+
+        *   - Port type
+            - Port number
+            - Types
+            - Description
+        *   - Input
+            - 0
+            - float
+            - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration
+
+    Create a vehicle animation similar to the figure below.
+
+    .. figure:: ../figs/rvc4_4.gif
+        :width: 500px
+        :alt: example of generated graphic
+
+        Example of vehicle display (animated).  The label at the top is the
+        block name.
+    """
+
+    nin = 1
+    nout = 0
+    AXES_POLICY = "subplot"
+    TIMESTAMP = True
+
+    inlabels = ("q",)
+
+    # TODO add ability to render an image instead of an outline
+
+    def __init__(
+        self,
+        animation=None,
+        path=None,
+        labels=["X", "Y"],
+        square=True,
+        init=None,
+        scale="auto",
+        shape="car",
+        size=None,
+        polyargs={},
+        **blockargs,
+    ):
+        """
+        :param animation: Graphical animation of vehicle, defaults to None
+        :type animation: VehicleAnimation subclass, optional
+        :param path: linestyle to plot path taken by vehicle, defaults to None
+        :type path: str or dict, optional
+        :param labels: axis labels (xlabel, ylabel), defaults to ["X","Y"]
+        :type labels: array_like(2) or list
+        :param square: Set aspect ratio to 1, defaults to True
+        :type square: bool, optional
+        :param init: function to initialize graphics, defaults to None
+        :type init: callable, optional
+        :param scale: scale of plot, defaults to "auto"
+        :type scale: list or str, optional
+        :param shape: shape of vehicle, passed to animation, defaults to None
+        :type shape: array_like, optional
+        :param size: size of vehicle, passed to animation, defaults to None
+        :type size: float or array_like, optional
+        :param polyargs: arguments passed to :meth:`Animation.Polygon`
+        :type polyargs: dict
+        :param blockargs: |GraphicsBlockOptions|
+        :type blockargs: dict
+
+        .. note::
+
+            - The ``init`` function is called after the axes are initialized
+              and can be used to draw application specific detail on the
+              plot. In the example below, this is the dot and star.
+            - If an ``animation`` object is not provided then a default polygon animation is
+              created.  It's size and shape can be set by the ``size`` and ``shape`` parameters, which are passed to the animation object,
+              or a dictionary of arguments ``polyargs`` which is passed to the default animation object.
+            - A dynamic trail, showing path to date can be animated if
+              the option ``path`` is set to a linestyle.
+        """
+        super().__init__(**blockargs)
+        self.xdata = []
+        self.ydata = []
+        # self.type = "vehicleplot"
+        if init is not None:
+            assert callable(init), "graphics init function must be callable"
+        self.init = init
+        self.square = square
+
+        self.pathstyle = path
+
+        if scale != "auto":
+            if len(scale) == 2:
+                scale = scale * 2
+        self.scale = scale
+        self.labels = labels
+
+        polyargs.update(
+            {k: v for k, v in [("shape", shape), ("scale", size)] if v is not None}
+        )
+
+        if animation is None:
+            animation = mobile.VehiclePolygon(**polyargs)
+        elif not isinstance(animation, mobile.VehicleAnimationBase):
+            raise TypeError("animation object must be VehicleAnimationBase subclass")
+
+        self.animation = animation
+
+    def start(self, simstate):
+        super().start(simstate)
+
+        if not self._enabled:
+            return
+
+        # create the plot
+        # super().reset()
+        # create the figures
+        assert self.fig is not None and self.ax is not None
+
+        if self.square:
+            self.ax.set_aspect("equal")
+        print("done")
+
+        self.ax.set_axisbelow(True)  # keeps grid beneath artists
+        self.ax.grid(True)
+        self.ax.set_xlabel(self.labels[0])
+        self.ax.set_ylabel(self.labels[1])
+        self.ax.set_title(self.name)
+        if self.scale != "auto":
+            self.ax.set_xlim(*self.scale[0:2])
+            self.ax.set_ylim(*self.scale[2:4])
+        if self.init is not None:
+            self.init(self.ax)
+
+        if isinstance(self.pathstyle, str):
+            (self.line,) = plt.plot(0, 0, self.pathstyle)
+        elif isinstance(self.pathstyle, dict):
+            (self.line,) = plt.plot(0, 0, **self.pathstyle)
+
+        self.animation.add()
+
+        # plt.draw()
+        # plt.show(block=False)
+
+    def step(self, t, inports):
+        if not self._enabled:
+            return
+
+        # inputs are set
+        xyt = inports[0]
+
+        # update the path line
+        self.xdata.append(xyt[0])
+        self.ydata.append(xyt[1])
+        # plt.figure(self.fig.number)
+        if self.pathstyle is not None:
+            self.line.set_data(self.xdata, self.ydata)
+
+        # update the vehicle pose
+        self.animation.update(xyt)
+
+        if isinstance(self.scale, str) and self.scale == "auto":
+            self.ax.relim()
+            self.ax.autoscale_view()
+        super().step(t, inports)

roboticstoolbox/blocks/quad_model.py

@@ -0,0 +1,132 @@
+# MDL_QUADCOPTER Dynamic parameters for a quadrotor.
+#
+# MDL_QUADCOPTER is a script creates the workspace variable quad which
+# describes the dynamic characterstics of a quadrotor flying robot.
+#
+# Properties::
+#
+# This is a structure with the following elements:
+#
+# nrotors   Number of rotors (1x1)
+# J         Flyer rotational inertia matrix (3x3)
+# h         Height of rotors above CoG (1x1)
+# d         Length of flyer arms (1x1)
+# nb        Number of blades per rotor (1x1)
+# r         Rotor radius (1x1)
+# c         Blade chord (1x1)
+# e         Flapping hinge offset (1x1)
+# Mb        Rotor blade mass (1x1)
+# Mc        Estimated hub clamp mass (1x1)
+# ec        Blade root clamp displacement (1x1)
+# Ib        Rotor blade rotational inertia (1x1)
+# Ic        Estimated root clamp inertia (1x1)
+# mb        Static blade moment (1x1)
+# Ir        Total rotor inertia (1x1)
+# Ct        Non-dim. thrust coefficient (1x1)
+# Cq        Non-dim. torque coefficient (1x1)
+# sigma     Rotor solidity ratio (1x1)
+# thetat    Blade tip angle (1x1)
+# theta0    Blade root angle (1x1)
+# theta1    Blade twist angle (1x1)
+# theta75   3/4 blade angle (1x1)
+# thetai    Blade ideal root approximation (1x1)
+# a         Lift slope gradient (1x1)
+# A         Rotor disc area (1x1)
+# gamma     Lock number (1x1)
+#
+#
+# Notes::
+# - SI units are used.
+#
+# References::
+# - Design, Construction and Control of a Large Quadrotor micro air vehicle.
+#   P.Pounds, PhD thesis,
+#   Australian National University, 2007.
+#   http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf
+# - This is a heavy lift quadrotor
+#
+
+import numpy as np
+from math import pi, sqrt, inf
+
+quadrotor = {}
+quadrotor["nrotors"] = 4  # 4 rotors
+quadrotor["g"] = 9.81  # g     Gravity
+quadrotor["rho"] = 1.184  # rho   Density of air
+quadrotor["muv"] = 1.5e-5  # muv   Viscosity of air
+
+# Airframe
+quadrotor["M"] = 4  # M    Mass
+Ixx = 0.082
+Iyy = 0.082
+Izz = 0.149  # 0.160
+quadrotor["J"] = np.diag([Ixx, Iyy, Izz])  # I   Flyer rotational inertia matrix     3x3
+
+quadrotor["h"] = -0.007  # h    Height of rotors above CoG
+quadrotor["d"] = 0.315  # d    Length of flyer arms
+
+# Rotor
+quadrotor["nb"] = 2  # b    Number of blades per rotor
+quadrotor["r"] = 0.165  # r    Rotor radius
+
+quadrotor["c"] = 0.018  # c    Blade chord
+
+quadrotor["e"] = 0.0  # e    Flapping hinge offset
+quadrotor["Mb"] = 0.005  # Mb   Rotor blade mass
+quadrotor["Mc"] = 0.010  # Mc   Estimated hub clamp mass
+quadrotor["ec"] = 0.004  # ec   Blade root clamp displacement
+quadrotor["Ib"] = (
+    quadrotor["Mb"] * (quadrotor["r"] - quadrotor["ec"]) ** 2 / 4
+)  # Ib      Rotor blade rotational inertia
+quadrotor["Ic"] = (
+    quadrotor["Mc"] * (quadrotor["ec"]) ** 2 / 4
+)  # Ic      Estimated root clamp inertia
+quadrotor["mb"] = quadrotor["g"] * (
+    quadrotor["Mc"] * quadrotor["ec"] / 2 + quadrotor["Mb"] * quadrotor["r"] / 2
+)  #   mb  Static blade moment
+quadrotor["Ir"] = quadrotor["nb"] * (
+    quadrotor["Ib"] + quadrotor["Ic"]
+)  # Ir      Total rotor inertia
+
+quadrotor["Ct"] = 0.0048  # Ct      Non-dim. thrust coefficient
+quadrotor["Cq"] = quadrotor["Ct"] * sqrt(
+    quadrotor["Ct"] / 2
+)  # Cq      Non-dim. torque coefficient
+
+quadrotor["sigma"] = (
+    quadrotor["c"] * quadrotor["nb"] / (pi * quadrotor["r"])
+)  # sigma   Rotor solidity ratio
+quadrotor["thetat"] = 6.8 * (pi / 180)  # thetat  Blade tip angle
+quadrotor["theta0"] = 14.6 * (pi / 180)  # theta0  Blade root angle
+quadrotor["theta1"] = (
+    quadrotor["thetat"] - quadrotor["theta0"]
+)  # theta1  Blade twist angle
+quadrotor["theta75"] = (
+    quadrotor["theta0"] + 0.75 * quadrotor["theta1"]
+)  # theta76 3/4 blade angle
+try:
+    quadrotor["thetai"] = quadrotor["thetat"] * (
+        quadrotor["r"] / quadrotor["e"]
+    )  # thetai  Blade ideal root approximation
+except ZeroDivisionError:
+    quadrotor["thetai"] = inf
+quadrotor["a"] = 5.5  # a       Lift slope gradient
+
+# derived constants
+quadrotor["A"] = pi * quadrotor["r"] ** 2  # A       Rotor disc area
+quadrotor["gamma"] = (
+    quadrotor["rho"]
+    * quadrotor["a"]
+    * quadrotor["c"]
+    * quadrotor["r"] ** 4
+    / (quadrotor["Ib"] + quadrotor["Ic"])
+)  # gamma   Lock number
+
+quadrotor["b"] = (
+    quadrotor["Ct"] * quadrotor["rho"] * quadrotor["A"] * quadrotor["r"] ** 2
+)  # T = b w^2
+quadrotor["k"] = (
+    quadrotor["Cq"] * quadrotor["rho"] * quadrotor["A"] * quadrotor["r"] ** 3
+)  # Q = k w^2
+
+quadrotor["verbose"] = False