roboticstoolbox-python 1.4.0__py3-none-any.whl

roboticstoolbox/robot/RobotPlottingMPL.py

@@ -0,0 +1,355 @@
+"""
+MPL-specific plotting methods for robot objects.
+
+This mixin is mixed into BaseRobot.  All matplotlib / PyPlot imports are
+deferred to the first call so that importing ``roboticstoolbox`` in a
+headless or browser environment (Pyodide / JupyterLite) does not pull in
+matplotlib.
+"""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING, Any, Literal as L
+
+if TYPE_CHECKING:  # pragma: nocover
+    from roboticstoolbox.backends.PyPlot import PyPlot
+    from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
+    from roboticstoolbox.tools.types import ArrayLike
+
+
+class RobotPlottingMPLMixin:
+    """Mixin that adds MPL-specific visualisation to BaseRobot subclasses."""
+
+    # ------------------------------------------------------------------
+    # Colour helpers
+    # ------------------------------------------------------------------
+
+    def linkcolormap(self, linkcolors: list[Any] | str = "viridis"):
+        """
+        Create a colormap for robot joints.
+
+        :param linkcolors: list of colors or colormap name, defaults to ``"viridis"``
+        :returns: the color map
+
+        - ``cm = robot.linkcolormap()`` is an n-element colormap that gives a
+          unique color for every link.  The RGBA colors for link ``j`` are
+          ``cm(j)``.
+        - ``cm = robot.linkcolormap(cmap)`` as above but ``cmap`` is the name
+          of a valid matplotlib colormap.  The default, example above, is the
+          ``viridis`` colormap.
+        - ``cm = robot.linkcolormap(list of colors)`` as above but a
+          colormap is created from a list of n color names given as strings,
+          tuples or hexstrings.
+
+        .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.DH.Puma560()
+        >>> cm = robot.linkcolormap("inferno")
+        >>> print(cm(range(6))) # cm(i) is 3rd color in colormap
+        >>> cm = robot.linkcolormap(
+        ...     ['red', 'g', (0,0.5,0), '#0f8040', 'yellow', 'cyan'])
+        >>> print(cm(range(6)))
+
+        .. rubric:: Notes
+
+        - Colormaps have 4-elements: red, green, blue, alpha (RGBA)
+        - Names of supported colors and colormaps are defined in the
+          matplotlib documentation.
+
+          - `Specifying colors <https://matplotlib.org/3.1.0/tutorials/colors/colors.html>`_
+          - `Colormaps <https://matplotlib.org/3.1.0/tutorials/colors/colormaps.html>`_
+
+        """
+
+        from matplotlib import cm, colors
+
+        if isinstance(linkcolors, list) and len(linkcolors) == self.n:  # type: ignore[attr-defined]  # pragma: nocover
+            return colors.ListedColormap(linkcolors)
+        else:  # pragma: nocover
+            return cm.get_cmap(linkcolors, 6)  # type: ignore[arg-type]
+
+    # ------------------------------------------------------------------
+    # Ellipse creation
+    # ------------------------------------------------------------------
+
+    def fellipse(
+        self,
+        q: ArrayLike,
+        opt: L["trans", "rot"] = "trans",
+        unit: L["rad", "deg"] = "rad",
+        centre: L["ee"] | ArrayLike = "ee",
+        add: bool = True,
+    ) -> EllipsePlot:
+        """
+        Create a force ellipsoid object for plotting with PyPlot.
+
+        :param q: the joint configuration of the robot
+        :param opt: ``'trans'`` or ``'rot'`` — plot the translational or
+            rotational force ellipsoid
+        :param unit: ``'rad'`` or ``'deg'``
+        :param centre: centre of the ellipsoid — ``'ee'`` for the
+            end-effector or a 3-vector ``[x, y, z]`` in the world frame
+        :param add: if ``True``, add the ellipsoid to the active plot environment
+        :returns: an EllipsePlot object
+        :rtype: EllipsePlot
+
+        ``robot.fellipse(q)`` creates a force ellipsoid for the robot at
+        pose ``q``. By default the ellipsoid is centered at the end-effector.
+
+        .. rubric:: Notes
+
+        - By default the ellipsoid related to translational motion is
+          drawn.  Use ``opt='rot'`` to draw the rotational velocity ellipsoid.
+        - By default the ellipsoid is drawn at the end-effector.  The option
+          ``centre`` allows its origin to be set to the specified 3-vector,
+          or the string ``"ee"`` ensures it is drawn at the end-effector position.
+        """
+        import roboticstoolbox as rtb
+        from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
+        from spatialmath.base.argcheck import getunit
+
+        if isinstance(self, rtb.ERobot):  # pragma: nocover
+            raise NotImplementedError("ERobot fellipse not implemented yet")
+
+        q = getunit(q, unit)  # type: ignore[assignment]
+        ell = EllipsePlot(self, q, "f", opt, centre=centre)
+
+        if add:
+            self._maybe_add_ellipse_to_active_env(ell)
+
+        return ell
+
+    def vellipse(
+        self,
+        q: ArrayLike,
+        opt: L["trans", "rot"] = "trans",
+        unit: L["rad", "deg"] = "rad",
+        centre: L["ee"] | ArrayLike = "ee",
+        scale: float = 0.1,
+        add: bool = True,
+    ) -> EllipsePlot:
+        """
+        Create a velocity ellipsoid object for plotting with PyPlot.
+
+        :param q: the joint configuration of the robot
+        :param opt: ``'trans'`` or ``'rot'`` — plot the translational or
+            rotational velocity ellipsoid
+        :param unit: ``'rad'`` or ``'deg'``
+        :param centre: centre of the ellipsoid — ``'ee'`` for the
+            end-effector or a 3-vector ``[x, y, z]`` in the world frame
+        :param scale: scale factor for the ellipsoid
+        :param add: if ``True``, add the ellipsoid to the active plot environment
+        :returns: an EllipsePlot object
+        :rtype: EllipsePlot
+
+        ``robot.vellipse(q)`` creates a velocity ellipsoid for the robot at
+        pose ``q``. By default the ellipsoid is centered at the end-effector.
+
+        .. rubric:: Notes
+
+        - By default the ellipsoid related to translational motion is
+          drawn.  Use ``opt='rot'`` to draw the rotational velocity ellipsoid.
+        - By default the ellipsoid is drawn at the end-effector.  The option
+          ``centre`` allows its origin to be set to the specified 3-vector,
+          or the string ``"ee"`` ensures it is drawn at the end-effector position.
+        """
+        import roboticstoolbox as rtb
+        from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
+        from spatialmath.base.argcheck import getunit
+
+        if isinstance(self, rtb.ERobot):  # pragma: nocover
+            raise NotImplementedError("ERobot vellipse not implemented yet")
+
+        q = getunit(q, unit)  # type: ignore[assignment]
+        ell = EllipsePlot(self, q, "v", opt, centre=centre, scale=scale)
+
+        if add:
+            self._maybe_add_ellipse_to_active_env(ell)
+
+        return ell
+
+    def _maybe_add_ellipse_to_active_env(self, ellipse: EllipsePlot) -> None:
+        """Add ellipse to the most recently active PyPlot environment, if available."""
+        env = self._active_plot_env  # type: ignore[attr-defined]
+        if env is None:
+            return
+
+        if not getattr(env, "supports_ellipse", False):
+            return
+
+        try:
+            env.add(ellipse)
+        except Exception:
+            pass
+
+    # ------------------------------------------------------------------
+    # Ellipse plotting
+    # ------------------------------------------------------------------
+
+    def plot_ellipse(
+        self,
+        ellipse: EllipsePlot,
+        block: bool = True,
+        limits: ArrayLike | None = None,
+        jointaxes: bool = True,
+        eeframe: bool = True,
+        shadow: bool = True,
+        name: bool = True,
+    ) -> PyPlot:
+        """
+        Plot an ellipsoid.
+
+        :param ellipse: the ellipsoid to plot
+        :param block: block operation of the code and keep the figure open
+        :param limits: custom view limits ``[x1, x2, y1, y2, z1, z2]``; autoscales if not supplied
+        :param jointaxes: plot an arrow indicating the joint axis
+        :param eeframe: plot the end-effector coordinate frame
+        :param shadow: plot a shadow of the robot in the x-y plane
+        :param name: plot the name of the robot near its base
+        :returns: a reference to the PyPlot object controlling the matplotlib figure
+        :rtype: PyPlot
+
+        ``robot.plot_ellipse(ellipsoid)`` displays the ellipsoid.
+        """
+        from roboticstoolbox.backends.PyPlot import PyPlot
+        from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
+
+        if not isinstance(ellipse, EllipsePlot):  # pragma: nocover
+            raise TypeError(
+                "ellipse must be of type roboticstoolbox.backend.PyPlot.EllipsePlot"
+            )
+
+        env = PyPlot()
+
+        env.launch(ellipse.robot.name + " " + ellipse.name, limits=limits)
+        env.add(ellipse, jointaxes=jointaxes, eeframe=eeframe, shadow=shadow, name=name)
+
+        if block:  # pragma: nocover
+            env.hold()
+
+        return env
+
+    def plot_fellipse(
+        self,
+        q: ArrayLike | None,
+        block: bool = True,
+        fellipse: EllipsePlot | None = None,
+        limits: ArrayLike | None = None,
+        opt: L["trans", "rot"] = "trans",
+        centre: L["ee"] | ArrayLike = "ee",
+        jointaxes: bool = True,
+        eeframe: bool = True,
+        shadow: bool = True,
+        name: bool = True,
+    ) -> PyPlot:
+        """
+        Plot the force ellipsoid for a manipulator.
+
+        :param q: the joint configuration of the robot
+        :param block: block operation of the code and keep the figure open
+        :param fellipse: a pre-built force ellipsoid to plot
+        :param limits: custom view limits ``[x1, x2, y1, y2, z1, z2]``; autoscales if not supplied
+        :param opt: ``'trans'`` or ``'rot'`` — plot the translational or rotational force ellipsoid
+        :param centre: coordinates to plot the ellipse — ``[x, y, z]`` or ``"ee"``
+        :param jointaxes: plot an arrow indicating the joint axis
+        :param eeframe: plot the end-effector coordinate frame
+        :param shadow: plot a shadow of the robot in the x-y plane
+        :param name: plot the name of the robot near its base
+        :raises ValueError: if neither ``q`` nor ``fellipse`` is supplied
+        :returns: a reference to the PyPlot object controlling the matplotlib figure
+        :rtype: PyPlot
+
+        ``robot.plot_fellipse(q)`` displays the force ellipsoid for the robot
+        at pose ``q``. The plot will autoscale with an aspect ratio of 1.
+
+        ``robot.plot_fellipse(vellipse=ell)`` uses a pre-built ellipse object.
+
+        .. rubric:: Notes
+
+        - By default the ellipsoid related to translational motion is drawn.
+          Use ``opt='rot'`` to draw the rotational velocity ellipsoid.
+        - By default the ellipsoid is drawn at the origin.  Use ``centre``
+          to specify a 3-vector, or ``"ee"`` to draw at the end-effector.
+        """
+        import roboticstoolbox as rtb
+
+        if isinstance(self, rtb.ERobot):  # pragma: nocover
+            raise NotImplementedError("Ellipse Plotting of ERobot's not implemented yet")
+
+        if fellipse is None and q is not None:
+            fellipse = self.fellipse(q, opt=opt, centre=centre, add=False)
+        elif fellipse is None:
+            raise ValueError("Must specify either q or fellipse")  # pragma: nocover
+
+        return self.plot_ellipse(
+            fellipse,
+            block=block,
+            limits=limits,
+            jointaxes=jointaxes,
+            eeframe=eeframe,
+            shadow=shadow,
+            name=name,
+        )
+
+    def plot_vellipse(
+        self,
+        q: ArrayLike | None,
+        block: bool = True,
+        vellipse: EllipsePlot | None = None,
+        limits: ArrayLike | None = None,
+        opt: L["trans", "rot"] = "trans",
+        centre: L["ee"] | ArrayLike = "ee",
+        jointaxes: bool = True,
+        eeframe: bool = True,
+        shadow: bool = True,
+        name: bool = True,
+    ) -> PyPlot:
+        """
+        Plot the velocity ellipsoid for a manipulator.
+
+        :param q: the joint configuration of the robot
+        :param block: block operation of the code and keep the figure open
+        :param vellipse: a pre-built velocity ellipsoid to plot
+        :param limits: custom view limits ``[x1, x2, y1, y2, z1, z2]``; autoscales if not supplied
+        :param opt: ``'trans'`` or ``'rot'`` — plot the translational or rotational velocity ellipsoid
+        :param centre: coordinates to plot the ellipse — ``[x, y, z]`` or ``"ee"``
+        :param jointaxes: plot an arrow indicating the joint axis
+        :param eeframe: plot the end-effector coordinate frame
+        :param shadow: plot a shadow of the robot in the x-y plane
+        :param name: plot the name of the robot near its base
+        :raises ValueError: if neither ``q`` nor ``vellipse`` is supplied
+        :returns: a reference to the PyPlot object controlling the matplotlib figure
+        :rtype: PyPlot
+
+        ``robot.plot_vellipse(q)`` displays the velocity ellipsoid for the robot
+        at pose ``q``. The plot will autoscale with an aspect ratio of 1.
+
+        ``robot.plot_vellipse(vellipse=ell)`` uses a pre-built ellipse object.
+
+        .. rubric:: Notes
+
+        - By default the ellipsoid related to translational motion is drawn.
+          Use ``opt='rot'`` to draw the rotational velocity ellipsoid.
+        - By default the ellipsoid is drawn at the origin.  Use ``centre``
+          to specify a 3-vector, or ``"ee"`` to draw at the end-effector.
+        """
+        import roboticstoolbox as rtb
+
+        if isinstance(self, rtb.ERobot):  # pragma: nocover
+            raise NotImplementedError("Ellipse Plotting of ERobot's not implemented yet")
+
+        if vellipse is None and q is not None:
+            vellipse = self.vellipse(q=q, opt=opt, centre=centre, add=False)
+        elif vellipse is None:
+            raise ValueError("Must specify either q or vellipse")  # pragma: nocover
+
+        return self.plot_ellipse(
+            vellipse,
+            block=block,
+            limits=limits,
+            jointaxes=jointaxes,
+            eeframe=eeframe,
+            shadow=shadow,
+            name=name,
+        )

roboticstoolbox/robot/RobotProto.py

@@ -0,0 +1,156 @@
+from roboticstoolbox.tools.types import ArrayLike, NDArray
+from typing import Any, Callable, Literal
+from typing_extensions import Protocol, Self
+
+from roboticstoolbox.robot.Link import Link, BaseLink
+from roboticstoolbox.robot.Gripper import Gripper
+from roboticstoolbox.ets.ETS import ETS
+from spatialmath import SE3
+
+
+class KinematicsProtocol(Protocol):
+    """Structural type contract for the RobotKinematics mixin.
+
+    Declares the attributes that mixin methods access on ``self``.  This avoids
+    circular imports while allowing type checkers to validate mixin usage.
+    """
+
+    @property
+    def _T(self) -> NDArray: ...
+
+    def ets(
+        self,
+        start: Link | Gripper | str | None = None,
+        end: Link | Gripper | str | None = None,
+    ) -> ETS: ...
+
+
+class RobotProto(Protocol):
+    """Structural type contract for the Dynamics mixin.
+
+    Declares every attribute that Dynamics mixin methods access on ``self``,
+    including methods defined within the mixin itself (which must be listed here
+    so the type checker knows they exist on ``self`` when called cross-method).
+    """
+
+    # ------------------------------------------------------------------
+    # Core robot properties
+    # ------------------------------------------------------------------
+
+    @property
+    def links(self) -> list[BaseLink]: ...
+
+    @property
+    def n(self) -> int: ...
+
+    @property
+    def q(self) -> NDArray: ...
+
+    @property
+    def name(self) -> str: ...
+
+    @name.setter
+    def name(self, new_name: str): ...
+
+    @property
+    def gravity(self) -> NDArray: ...
+
+    # ------------------------------------------------------------------
+    # Kinematics (called by dynamics methods)
+    # ------------------------------------------------------------------
+
+    def jacobe(
+        self,
+        q: ArrayLike,
+        end: str | BaseLink | Gripper | None = None,
+        start: str | BaseLink | Gripper | None = None,
+        tool: NDArray | SE3 | None = None,
+    ) -> NDArray: ...
+
+    def jacob0(
+        self,
+        q: ArrayLike,
+        end: str | BaseLink | Gripper | None = None,
+        start: str | BaseLink | Gripper | None = None,
+        tool: NDArray | SE3 | None = None,
+    ) -> NDArray: ...
+
+    def jacob0_analytical(
+        self,
+        q: ArrayLike,
+        representation: Literal["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz",
+        end: str | BaseLink | Gripper | None = None,
+        start: str | BaseLink | Gripper | None = None,
+        tool: NDArray | SE3 | None = None,
+    ) -> NDArray: ...
+
+    def jacob0_dot(
+        self,
+        q: ArrayLike,
+        qd: ArrayLike,
+        J0: NDArray | None = None,
+        representation: Literal["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None,
+    ) -> NDArray: ...
+
+    # ------------------------------------------------------------------
+    # Dynamics methods (cross-called within the Dynamics mixin)
+    # ------------------------------------------------------------------
+
+    def dynchanged(self) -> None: ...
+
+    def rne(
+        self,
+        q: NDArray,
+        qd: NDArray,
+        qdd: NDArray,
+        symbolic: bool = False,
+        gravity: ArrayLike | None = None,
+    ) -> NDArray: ...
+
+    def inertia(self, q: NDArray) -> NDArray: ...
+
+    def inertia_x(
+        self,
+        q: ArrayLike | None = None,
+        pinv: bool = False,
+        representation: Literal["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz",
+        Ji: NDArray | None = None,
+    ) -> NDArray: ...
+
+    def coriolis(self, q: ArrayLike, qd: ArrayLike) -> NDArray: ...
+
+    def gravload(
+        self,
+        q: ArrayLike | None = None,
+        gravity: ArrayLike | None = None,
+    ) -> NDArray: ...
+
+    def accel(
+        self,
+        q: ArrayLike,
+        qd: ArrayLike,
+        torque: ArrayLike,
+        gravity: ArrayLike | None = None,
+    ) -> NDArray: ...
+
+    def pay(
+        self,
+        W: ArrayLike,
+        q: NDArray | None = None,
+        J: NDArray | None = None,
+        frame: int = 1,
+    ) -> NDArray: ...
+
+    def nofriction(self, coulomb: bool = True, viscous: bool = False) -> Self: ...
+
+    def copy(self) -> Self: ...
+
+    def _fdyn(
+        self,
+        t: float,
+        x: NDArray,
+        torqfun: Callable[[Any], NDArray],
+        targs: dict,
+    ) -> NDArray: ...
+
+    def delete_rne(self) -> None: ...

roboticstoolbox/robot/__init__.py

@@ -0,0 +1,57 @@
+from roboticstoolbox.robot.Robot import BaseRobot, Robot, Robot2
+from roboticstoolbox.robot.Link import Link, Link2
+from roboticstoolbox.robot.DHRobot import SerialLink, DHRobot
+from roboticstoolbox.robot.DHLink import (
+    DHLink,
+    RevoluteDH,
+    PrismaticDH,
+    RevoluteMDH,
+    PrismaticMDH,
+)
+from roboticstoolbox.robot.PoERobot import PoELink, PoERobot, PoERevolute, PoEPrismatic
+from roboticstoolbox.robot.ERobot import ERobot, ERobot2
+
+from roboticstoolbox.robot.ELink import ELink, ELink2
+from roboticstoolbox.ets import ET, ET2, ETS, ETS2
+from roboticstoolbox.robot.Gripper import Gripper
+
+
+from roboticstoolbox.robot.IK import IKSolution, IKSolver, IK_LM, IK_NR, IK_GN, IK_QP
+
+__all__ = [
+    "Robot",
+    "Robot2",
+    "SerialLink",
+    "DHRobot",
+    "Link",
+    "DHLink",
+    "RevoluteDH",
+    "PrismaticDH",
+    "RevoluteMDH",
+    "PrismaticMDH",
+    "BaseRobot",
+    "ELink",
+    "ELink2",
+    "Link",
+    "Link2",
+    "ERobot",
+    "ERobot2",
+    "ETS",
+    "ETS2",
+    "Gripper",
+    "PoERobot",
+    "PoELink",
+    "PoEPrismatic",
+    "PoERevolute",
+    "ET",
+    "ET2",
+    "IKSolution",
+    "IKSolver",
+    "IK_LM",
+    "IK_NR",
+    "IK_GN",
+    "IK_QP",
+    # "URDFRobot",
+    # "URDF_str",
+    # "URDF_file"
+]

roboticstoolbox/robot/cpp-extensions/README.md

@@ -0,0 +1,29 @@
+# Fast recursive Newton-Euler (frne)
+
+## Synopsis
+
+nanobind C++/C extension that accelerates `DHRobot.rne()`: recursive
+Newton-Euler inverse dynamics. These files were developed for a "MEX
+version" of robot dynamics in the MATLAB version of the Robotics Toolbox
+— `ne.c` is virtually unchanged.
+
+This is one of two such extensions in the codebase — the other, **fknm**
+(`_fknm_c`, ETS forward kinematics/Jacobians/Hessians/IK), lives at
+`ets/cpp-extensions/` since it's an ETS concern, not a dynamics one. See
+that directory's README for the architecture patterns shared by both
+extensions (build/install mechanics, the Python facade + pure-Python
+fallback pattern, lazy serialization into the C++ mirror object).
+
+It's optional: `robot/frne.py` is a pure-Python facade that tries to
+import the compiled extension and transparently falls back to an
+equivalent pure-Python implementation when it's unavailable.
+
+# Files
+
+| File | Purpose |
+| ---- | ------- |
+| frne_nb.cpp | nanobind glue for `_frne_c` — binds `init`/`frne`/`delete` |
+| frne.h | struct/function declarations shared with `ne.c` |
+| ne.c | implementation of the RNE algorithm |
+| vmath.c | Simple vector/matrix library |
+| vmath.h | Simple vector/matrix library |

roboticstoolbox/robot/cpp-extensions/frne.h

@@ -0,0 +1,96 @@
+/**
+ * \file frne.h
+ * \author Peter Corke
+ * \author Jesse Haviland
+ * \brief Definitions for c file
+ *
+ */
+
+#ifndef _rne_h_
+#define _rne_h_
+
+#include    <math.h>
+#include    "vmath.h"
+
+#define TRUE    1
+#define FALSE   0
+
+/*
+ * Accessing information within a MATLAB structure is inconvenient and slow.
+ * To get around this we build our own robot and link data structures, and
+ * copy the information from the MATLAB objects once per call.  If the call
+ * is for multiple states values then our efficiency becomes very high.
+ */
+
+/* Robot kinematic convention */
+typedef
+    enum _dhtype {
+        STANDARD,
+        MODIFIED
+} DHType;
+
+/* Link joint type */
+typedef
+    enum _axistype {
+        REVOLUTE ,
+        PRISMATIC
+} Sigma;
+
+/* A robot link structure */
+typedef struct _link {
+    /**********************************************************
+     *************** kinematic parameters *********************
+     **********************************************************/
+    double  alpha;      /* link twist */
+    double  A;          /* link offset */
+    double  D;          /* link length */
+    double  theta;      /* link rotation angle */
+    double  offset;     /* link coordinate offset */
+    Sigma jointtype;      /* axis type; revolute ('R') or prismatic ('P') */
+
+    /**********************************************************
+     ***************** dynamic parameters *********************
+     **********************************************************/
+
+    /**************** of links ********************************/
+    Vect    *rbar;      /* centre of mass of link wrt link origin */
+    double  m;          /* mass of link */
+    double  *I;         /* inertia tensor of link wrt link origin */
+
+    /**************** of actuators *****************************/
+        /* these parameters are motor referenced */
+    double  Jm;         /* actuator inertia */
+    double  G;          /* gear ratio */
+    double  B;          /* actuator friction damping coefficient */
+    double  *Tc;        /* actuator Coulomb friction coeffient */
+
+    /**********************************************************
+     **************** intermediate variables ******************
+     **********************************************************/
+    Vect    r;          /* distance of ith origin from i-1th wrt ith */
+    Rot R;              /* link rotation matrix */
+    Vect    omega;      /* angular velocity */
+    Vect    omega_d;    /* angular acceleration */
+    Vect    acc;        /* acceleration */
+    Vect    abar;       /* acceleration of centre of mass */
+    Vect    f;          /* inter-link force */
+    Vect    n;          /* inter-link moment */
+} Link;
+
+/* A robot */
+typedef struct _robot {
+    int njoints;    /* number of joints */
+    Vect    *gravity;   /* gravity vector */
+    DHType  dhtype;     /* kinematic convention */
+    Link    *links;     /* the links */
+} Robot;
+
+void newton_euler (
+	Robot	*robot,		/*!< robot object  */
+	double	*tau,		/*!< returned joint torques */
+	double	*qd,		/*!< joint velocities */
+	double	*qdd,		/*!< joint accelerations */
+	double	*fext,		/*!< wrench applied to end-effector */
+	int	stride		/*!< indexing stride for qd, qdd */
+);
+#endif