roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/robot/Robot.py

@@ -0,0 +1,2100 @@
+#!/usr/bin/env python
+
+"""
+@author: Jesse Haviland
+@author: Peter Corke
+"""
+
+# import sys
+from os.path import splitext
+from copy import deepcopy
+from warnings import warn
+from pathlib import PurePosixPath, Path
+from typing import (
+    List,
+    TypeVar,
+    Union,
+    Dict,
+    Tuple,
+    overload,
+)
+from typing_extensions import Literal as L
+
+
+import numpy as np
+
+import spatialmath.base as smb
+from spatialmath.base.argcheck import (
+    getvector,
+    getmatrix,
+    verifymatrix,
+)
+
+from spatialgeometry import Shape, CollisionShape, Cylinder
+
+from spatialmath import (
+    SE3,
+    SE2,
+    SpatialAcceleration,
+    SpatialVelocity,
+    SpatialInertia,
+    SpatialForce,
+)
+
+import roboticstoolbox as rtb
+from roboticstoolbox.robot.BaseRobot import BaseRobot
+from roboticstoolbox.robot.RobotKinematics import RobotKinematicsMixin
+from roboticstoolbox.robot.Gripper import Gripper
+from roboticstoolbox.robot.Link import BaseLink, Link, Link2
+from roboticstoolbox.robot.ETS import ETS, ETS2
+from roboticstoolbox.tools import xacro
+from roboticstoolbox.tools import URDF
+from roboticstoolbox.tools.types import ArrayLike, NDArray
+from roboticstoolbox.tools.data import rtb_path_to_datafile
+
+# A generic type variable representing any subclass of BaseLink
+LinkType = TypeVar("LinkType", bound=BaseLink)
+
+
+# ==================================================================================== #
+# ================= Robot Class ====================================================== #
+# ==================================================================================== #
+
+
+class Robot(BaseRobot[Link], RobotKinematicsMixin):
+    _color = True
+
+    def __init__(
+        self,
+        arg: Union[List[Link], ETS, "Robot"],
+        gripper_links: Union[Link, List[Link], None] = None,
+        name: str = "",
+        manufacturer: str = "",
+        comment: str = "",
+        base: Union[NDArray, SE3, None] = None,
+        tool: Union[NDArray, SE3, None] = None,
+        gravity: ArrayLike = [0, 0, -9.81],
+        keywords: Union[List[str], Tuple[str]] = [],
+        symbolic: bool = False,
+        configs: Union[Dict[str, NDArray], None] = None,
+        check_jindex: bool = True,
+        urdf_string: Union[str, None] = None,
+        urdf_filepath: Union[Path, PurePosixPath, None] = None,
+    ):
+        # Process links
+        if isinstance(arg, Robot):
+            # We're passed a Robot, clone it
+            # We need to preserve the parent link as we copy
+
+            # Copy each link within the robot
+            links = [deepcopy(link) for link in arg.links]
+            gripper_links = []
+
+            for gripper in arg.grippers:
+                glinks = []
+                for link in gripper.links:
+                    glinks.append(deepcopy(link))
+
+                gripper_links.append(glinks[0])
+                links = links + glinks
+
+            # Sever parent connection, but save the string
+            # The constructor will piece this together for us
+            for link in links:
+                link._children = []
+                if link.parent is not None:
+                    link._parent_name = link.parent.name
+                    link._parent = None
+
+            super().__init__(links, gripper_links=gripper_links)
+
+            for i, gripper in enumerate(self.grippers):
+                gripper.tool = arg.grippers[i].tool.copy()
+
+            self._urdf_string = arg.urdf_string
+            self._urdf_filepath = arg.urdf_filepath
+        else:
+            if isinstance(arg, ETS):
+                # We're passed an ETS string
+                links = []
+                # chop it up into segments, a link frame after every joint
+                parent = None
+                for j, ets_j in enumerate(arg.split()):
+                    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
+                    ):
+                        elink.qlim = elink.v.qlim  # pragma nocover
+                    parent = elink
+                    links.append(elink)
+
+            elif smb.islistof(arg, Link):
+                links = arg
+
+            else:
+                raise TypeError("arg was invalid, must be List[Link], ETS, or Robot")
+
+            # Initialise Base Robot object
+            super().__init__(
+                links=links,
+                gripper_links=gripper_links,
+                name=name,
+                manufacturer=manufacturer,
+                comment=comment,
+                base=base,
+                tool=tool,
+                gravity=gravity,
+                keywords=keywords,
+                symbolic=symbolic,
+                configs=configs,
+                check_jindex=check_jindex,
+            )
+
+    # --------------------------------------------------------------------- #
+    # --------- Swift Methods --------------------------------------------- #
+    # --------------------------------------------------------------------- #
+
+    def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0):
+        ob = []
+
+        for link in self.links:
+            if robot_alpha > 0:
+                for gi in link.geometry:
+                    gi.set_alpha(robot_alpha)
+                    ob.append(gi.to_dict())
+            if collision_alpha > 0:
+                for gi in link.collision:
+                    gi.set_alpha(collision_alpha)
+                    ob.append(gi.to_dict())
+
+        # Do the grippers now
+        for gripper in self.grippers:
+            for link in gripper.links:
+                if robot_alpha > 0:
+                    for gi in link.geometry:
+                        gi.set_alpha(robot_alpha)
+                        ob.append(gi.to_dict())
+                if collision_alpha > 0:
+                    for gi in link.collision:
+                        gi.set_alpha(collision_alpha)
+                        ob.append(gi.to_dict())
+
+        # for o in ob:
+        #     print(o)
+
+        return ob
+
+    def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0):
+        ob = []
+
+        # Do the robot
+        for link in self.links:
+            if robot_alpha > 0:
+                for gi in link.geometry:
+                    ob.append(gi.fk_dict())
+            if collision_alpha > 0:
+                for gi in link.collision:
+                    ob.append(gi.fk_dict())
+
+        # Do the grippers now
+        for gripper in self.grippers:
+            for link in gripper.links:
+                if robot_alpha > 0:
+                    for gi in link.geometry:
+                        ob.append(gi.fk_dict())
+                if collision_alpha > 0:
+                    for gi in link.collision:
+                        ob.append(gi.fk_dict())
+
+        return ob
+
+    # --------------------------------------------------------------------- #
+    # --------- URDF Methods ---------------------------------------------- #
+    # --------------------------------------------------------------------- #
+
+    @staticmethod
+    def URDF_read(
+        file_path, tld=None, xacro_tld=None
+    ) -> Tuple[List[Link], str, str, Union[Path, PurePosixPath]]:
+        """
+        Read a URDF file as Links
+
+        File should be specified relative to ``RTBDATA/URDF/xacro``
+
+        Parameters
+        ----------
+        file_path
+            File path relative to the xacro folder
+        tld
+            A custom top-level directory which holds the xacro data,
+            defaults to None
+        xacro_tld
+            A custom top-level within the xacro data,
+            defaults to None
+
+        Returns
+        -------
+        links
+            a list of links
+        name
+            the name of the robot
+        urdf
+            a string representing the URDF
+        file_path
+            a path to the original file
+
+        Notes
+        -----
+        If ``tld`` is not supplied, filepath pointing to xacro data should
+        be directly under ``RTBDATA/URDF/xacro`` OR under ``./xacro`` relative
+        to the model file calling this method. If ``tld`` is supplied, then
+        ```file_path``` needs to be relative to ``tld``
+
+        """
+
+        # Get the path to the class that defines the robot
+        if tld is None:
+            base_path = rtb_path_to_datafile("xacro")
+        else:
+            base_path = PurePosixPath(tld)
+
+        # Add on relative path to get to the URDF or xacro file
+        # base_path = PurePath(classpath).parent.parent / 'URDF' / 'xacro'
+        file_path = base_path / PurePosixPath(file_path)
+        _, ext = splitext(file_path)
+
+        if ext == ".xacro":
+            # it's a xacro file, preprocess it
+            if xacro_tld is not None:
+                xacro_tld = base_path / PurePosixPath(xacro_tld)
+            urdf_string = xacro.main(file_path, xacro_tld)
+            try:
+                urdf = URDF.loadstr(urdf_string, file_path, base_path)
+            except BaseException as e:  # pragma nocover
+                print("error parsing URDF file", file_path)
+                raise e
+        else:  # pragma nocover
+            urdf_string = open(file_path).read()
+            urdf = URDF.loadstr(urdf_string, file_path, base_path)
+
+        if not isinstance(urdf_string, str):  # pragma nocover
+            raise ValueError("Parsing failed, did not get valid URDF string back")
+
+        return urdf.elinks, urdf.name, urdf_string, file_path
+
+    @classmethod
+    def URDF(cls, file_path: str, gripper: Union[int, str, None] = None):
+        """
+        Construct a Robot object from URDF file
+
+        Parameters
+        ----------
+        file_path
+            the path to the URDF
+        gripper
+            index or name of the gripper link(s)
+
+        Returns
+        -------
+        If ``gripper`` is specified, links from that link outward are removed
+        from the rigid-body tree and folded into a ``Gripper`` object.
+
+        """
+
+        links, name, urdf_string, urdf_filepath = Robot.URDF_read(file_path)
+
+        gripperLink: Union[Link, None] = None
+
+        if gripper is not None:
+            if isinstance(gripper, int):
+                gripperLink = links[gripper]
+            elif isinstance(gripper, str):
+                for link in links:
+                    if link.name == gripper:
+                        gripperLink = link
+                        break
+                else:  # pragma nocover
+                    raise ValueError(f"no link named {gripper}")
+            else:  # pragma nocover
+                raise TypeError("bad argument passed as gripper")
+
+        # links, name, urdf_string, urdf_filepath = Robot.URDF_read(file_path)
+
+        return cls(
+            links,
+            name=name,
+            gripper_links=gripperLink,
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+    #     # --------------------------------------------------------------------- #
+    #     # --------- Utility Methods ------------------------------------------- #
+    #     # --------------------------------------------------------------------- #
+
+    #     def showgraph(self, display_graph: bool = True, **kwargs) -> Union[None, str]:
+    #         """
+    #         Display a link transform graph in browser
+
+    #         ``robot.showgraph()`` displays a graph of the robot's link frames
+    #         and the ETS between them.  It uses GraphViz dot.
+
+    #         The nodes are:
+    #             - Base is shown as a grey square. This is the world frame origin,
+    #               but can be changed using the ``base`` attribute of the robot.
+    #             - Link frames are indicated by circles
+    #             - ETS transforms are indicated by rounded boxes
+
+    #         The edges are:
+    #             - an arrow if `jtype` is False or the joint is fixed
+    #             - an arrow with a round head if `jtype` is True and the joint is
+    #               revolute
+    #             - an arrow with a box head if `jtype` is True and the joint is
+    #               prismatic
+
+    #         Edge labels or nodes in blue have a fixed transformation to the
+    #         preceding link.
+
+    #         Parameters
+    #         ----------
+    #         display_graph
+    #             Open the graph in a browser if True. Otherwise will return the
+    #             file path
+    #         etsbox
+    #             Put the link ETS in a box, otherwise an edge label
+    #         jtype
+    #             Arrowhead to node indicates revolute or prismatic type
+    #         static
+    #             Show static joints in blue and bold
+
+    #         Examples
+    #         --------
+    #         >>> import roboticstoolbox as rtb
+    #         >>> panda = rtb.models.URDF.Panda()
+    #         >>> panda.showgraph()
+
+    #         .. image:: ../figs/panda-graph.svg
+    #             :width: 600
+
+    #         See Also
+    #         --------
+    #         :func:`dotfile`
+
+    #         """
+
+    #         # Lazy import
+    #         import tempfile
+    #         import subprocess
+    #         import webbrowser
+
+    #         # create the temporary dotfile
+    #         dotfile = tempfile.TemporaryFile(mode="w")
+    #         self.dotfile(dotfile, **kwargs)
+
+    #         # rewind the dot file, create PDF file in the filesystem, run dot
+    #         dotfile.seek(0)
+    #         pdffile = tempfile.NamedTemporaryFile(suffix=".pdf", delete=False)
+    #         subprocess.run("dot -Tpdf", shell=True, stdin=dotfile, stdout=pdffile)
+
+    #         # open the PDF file in browser (hopefully portable), then cleanup
+    #         if display_graph:  # pragma nocover
+    #             webbrowser.open(f"file://{pdffile.name}")
+    #         else:
+    #             return pdffile.name
+
+    #     def dotfile(
+    #         self,
+    #         filename: Union[str, IO[str]],
+    #         etsbox: bool = False,
+    #         ets: L["full", "brief"] = "full",
+    #         jtype: bool = False,
+    #         static: bool = True,
+    #     ):
+    #         """
+    #         Write a link transform graph as a GraphViz dot file
+
+    #         The file can be processed using dot:
+    #             % dot -Tpng -o out.png dotfile.dot
+
+    #         The nodes are:
+    #             - Base is shown as a grey square.  This is the world frame origin,
+    #               but can be changed using the ``base`` attribute of the robot.
+    #             - Link frames are indicated by circles
+    #             - ETS transforms are indicated by rounded boxes
+
+    #         The edges are:
+    #             - an arrow if `jtype` is False or the joint is fixed
+    #             - an arrow with a round head if `jtype` is True and the joint is
+    #               revolute
+    #             - an arrow with a box head if `jtype` is True and the joint is
+    #               prismatic
+
+    #         Edge labels or nodes in blue have a fixed transformation to the
+    #         preceding link.
+
+    #         Note
+    #         ----
+    #         If ``filename`` is a file object then the file will *not*
+    #             be closed after the GraphViz model is written.
+
+    #         Parameters
+    #         ----------
+    #         file
+    #             Name of file to write to
+    #         etsbox
+    #             Put the link ETS in a box, otherwise an edge label
+    #         ets
+    #             Display the full ets with "full" or a brief version with "brief"
+    #         jtype
+    #             Arrowhead to node indicates revolute or prismatic type
+    #         static
+    #             Show static joints in blue and bold
+
+    #         See Also
+    #         --------
+    #         :func:`showgraph`
+
+    #         """
+
+    #         if isinstance(filename, str):
+    #             file = open(filename, "w")
+    #         else:
+    #             file = filename
+
+    #         header = r"""digraph G {
+    # graph [rankdir=LR];
+    # """
+
+    #         def draw_edge(link, etsbox, jtype, static):
+    #             # draw the edge
+    #             if jtype:
+    #                 if link.isprismatic:
+    #                     edge_options = 'arrowhead="box", arrowtail="inv", dir="both"'
+    #                 elif link.isrevolute:
+    #                     edge_options = 'arrowhead="dot", arrowtail="inv", dir="both"'
+    #                 else:
+    #                     edge_options = 'arrowhead="normal"'
+    #             else:
+    #                 edge_options = 'arrowhead="normal"'
+
+    #             if link.parent is None:
+    #                 parent = "BASE"
+    #             else:
+    #                 parent = link.parent.name
+
+    #             if etsbox:
+    #                 # put the ets fragment in a box
+    #                 if not link.isjoint and static:
+    #                     node_options = ', fontcolor="blue"'
+    #                 else:
+    #                     node_options = ""
+
+    #                 try:
+    #                     file.write(
+    #                         '  {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format(
+    #                             link.name,
+    #                             link.ets.__str__(q=f"q{link.jindex}"),
+    #                             node_options,
+    #                         )
+    #                     )
+    #                 except UnicodeEncodeError:  # pragma nocover
+    #                     file.write(
+    #                         '  {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format(
+    #                             link.name,
+    #                             link.ets.__str__(q=f"q{link.jindex}")
+    #                             .encode("ascii", "ignore")
+    #                             .decode("ascii"),
+    #                             node_options,
+    #                         )
+    #                     )
+
+    #                 file.write("  {} -> {}_ets;\n".format(parent, link.name))
+    #                 file.write(
+    #                     "  {}_ets -> {} [{}];\n".format(link.name, link.name, edge_options)
+    #                 )
+    #             else:
+    #                 # put the ets fragment as an edge label
+    #                 if not link.isjoint and static:
+    #                     edge_options += 'fontcolor="blue"'
+    #                 if ets == "full":
+    #                     estr = link.ets.__str__(q=f"q{link.jindex}")
+    #                 elif ets == "brief":
+    #                     if link.jindex is None:
+    #                         estr = ""
+    #                     else:
+    #                         estr = f"...q{link.jindex}"
+    #                 else:
+    #                     return
+    #                 try:
+    #                     file.write(
+    #                         '  {} -> {} [label="{}", {}];\n'.format(
+    #                             parent,
+    #                             link.name,
+    #                             estr,
+    #                             edge_options,
+    #                         )
+    #                     )
+    #                 except UnicodeEncodeError:  # pragma nocover
+    #                     file.write(
+    #                         '  {} -> {} [label="{}", {}];\n'.format(
+    #                             parent,
+    #                             link.name,
+    #                             estr.encode("ascii", "ignore").decode("ascii"),
+    #                             edge_options,
+    #                         )
+    #                     )
+
+    #         file.write(header)
+
+    #         # add the base link
+    #         file.write("  BASE [shape=square, style=filled, fillcolor=gray]\n")
+
+    #         # add the links
+    #         for link in self:
+    #             # draw the link frame node (circle) or ee node (doublecircle)
+    #             if link in self.ee_links:
+    #                 # end-effector
+    #                 node_options = 'shape="doublecircle", color="blue", fontcolor="blue"'
+    #             else:
+    #                 node_options = 'shape="circle"'
+
+    #             file.write("  {} [{}];\n".format(link.name, node_options))
+
+    #             draw_edge(link, etsbox, jtype, static)
+
+    #         for gripper in self.grippers:
+    #             for link in gripper.links:
+    #                 file.write("  {} [shape=cds];\n".format(link.name))
+    #                 draw_edge(link, etsbox, jtype, static)
+
+    #         file.write("}\n")
+
+    #         if isinstance(filename, str):
+    #             file.close()  # noqa
+
+    # --------------------------------------------------------------------- #
+    # --------- Kinematic Methods ----------------------------------------- #
+    # --------------------------------------------------------------------- #
+
+    @property
+    def reach(self) -> float:
+        r"""
+        Reach of the robot
+
+        A conservative estimate of the reach of the robot. It is computed as
+        the sum of the translational ETs that define the link transform.
+
+        Note
+        ----
+        Computed on the first access. If kinematic parameters
+        subsequently change this will not be reflected.
+
+        Returns
+        -------
+        reach
+            Maximum reach of the robot
+
+        Notes
+        -----
+        - Probably an overestimate of reach
+        - Used by numerical inverse kinematics to scale translational
+          error.
+        - For a prismatic joint, uses ``qlim`` if it is set
+
+        """
+
+        # TODO
+        # This should be a start, end method and compute the reach based on the
+        # given ets. Then use an lru_cache to speed up return
+
+        if self._reach is None:
+            d_all = []
+            for link in self.ee_links:
+                d = 0
+                while True:
+                    for et in link.ets:
+                        if et.istranslation:
+                            if et.isjoint:
+                                # the length of a prismatic joint depends on the
+                                # joint limits.  They might be set in the ET
+                                # or in the Link depending on how the robot
+                                # was constructed
+                                if link.qlim is not None:
+                                    d += max(link.qlim)
+                                elif et.qlim is not None:  # pragma nocover
+                                    d += max(et.qlim)
+                            else:
+                                d += abs(et.eta)
+                    link = link.parent
+                    if link is None or isinstance(link, str):
+                        d_all.append(d)
+                        break
+
+            self._reach = max(d_all)
+        return self._reach
+
+    def fkine_all(self, q: ArrayLike) -> SE3:
+        """
+        Compute the pose of every link frame
+
+        ``T = robot.fkine_all(q)`` is  an SE3 instance with ``robot.nlinks +
+        1`` values:
+
+        - ``T[0]`` is the base transform
+        - ``T[i]`` is the pose of link whose ``number`` is ``i``
+
+        Parameters
+        ----------
+        q
+            The joint configuration
+
+        Returns
+        -------
+        fkine_all
+            Pose of all links
+
+        References
+        ----------
+        - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
+          Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
+
+        """  # noqa
+
+        q = getvector(q)
+
+        Tbase = SE3(self.base)  # add base, also sets the type
+
+        linkframes = Tbase.__class__.Alloc(self.nlinks + 1)
+        linkframes[0] = Tbase
+
+        def recurse(Tall, Tparent, q, link):
+            # if joint??
+            T = Tparent
+            while True:
+                T *= SE3(link.A(q[link.jindex]))
+
+                Tall[link.number] = T
+
+                if link.nchildren == 0:
+                    # no children
+                    return
+                elif link.nchildren == 1:
+                    # one child
+                    if link in self.ee_links:  # pragma nocover
+                        # this link is an end-effector, go no further
+                        return
+                    link = link.children[0]
+                    continue
+                else:
+                    # multiple children
+                    for child in link.children:
+                        recurse(Tall, T, q, child)
+                    return
+
+        recurse(linkframes, Tbase, q, self.links[0])
+
+        return linkframes
+
+    @overload
+    def manipulability(
+        self,
+        q: ArrayLike = ...,
+        J: None = None,
+        end: Union[str, Link, Gripper, None] = None,
+        start: Union[str, Link, Gripper, None] = None,
+        method: L[
+            "yoshikawa", "asada", "minsingular", "invcondition"  # noqa
+        ] = "yoshikawa",
+        axes: Union[L["all", "trans", "rot"], List[bool]] = "all",  # noqa
+        **kwargs,
+    ) -> Union[float, NDArray]:  # pragma nocover
+        ...
+
+    @overload
+    def manipulability(
+        self,
+        q: None = None,
+        J: NDArray = ...,
+        end: Union[str, Link, Gripper, None] = None,
+        start: Union[str, Link, Gripper, None] = None,
+        method: L[
+            "yoshikawa", "asada", "minsingular", "invcondition"  # noqa
+        ] = "yoshikawa",
+        axes: Union[L["all", "trans", "rot"], List[bool]] = "all",  # noqa
+        **kwargs,
+    ) -> Union[float, NDArray]:  # pragma nocover
+        ...
+
+    def manipulability(
+        self,
+        q=None,
+        J=None,
+        end: Union[str, Link, Gripper, None] = None,
+        start: Union[str, Link, Gripper, None] = None,
+        method: L[
+            "yoshikawa", "asada", "minsingular", "invcondition"  # noqa
+        ] = "yoshikawa",
+        axes: Union[L["all", "trans", "rot"], List[bool]] = "all",  # noqa
+        **kwargs,
+    ):
+        """
+        Manipulability measure
+
+        ``manipulability(q)`` is the scalar manipulability index
+        for the robot at the joint configuration ``q``.  It indicates
+        dexterity, that is, how well conditioned the robot is for motion
+        with respect to the 6 degrees of Cartesian motion.  The values is
+        zero if the robot is at a singularity.
+
+        Parameters
+        ----------
+        q
+            Joint coordinates, one of J or q required
+        J
+            Jacobian in base frame if already computed, one of J or
+            q required
+        method
+            method to use, "yoshikawa" (default), "invcondition",
+            "minsingular"  or "asada"
+        axes
+            Task space axes to consider: "all" [default],
+            "trans", or "rot"
+
+        Returns
+        -------
+        manipulability
+
+        Synopsis
+        --------
+
+        Various measures are supported:
+
+        | Measure           |       Description                               |
+        | ``"yoshikawa"``   | Volume of the velocity ellipsoid, *distance*    |
+        |                   | from singularity [Yoshikawa85]_                 |
+        | ``"invcondition"``| Inverse condition number of Jacobian, isotropy  |
+        |                   | of the velocity ellipsoid [Klein87]_            |
+        | ``"minsingular"`` | Minimum singular value of the Jacobian,         |
+        |                   | *distance*  from singularity [Klein87]_         |
+        | ``"asada"``       | Isotropy of the task-space acceleration         |
+        |                   | ellipsoid which is a function of the Cartesian  |
+        |                   | inertia matrix which depends on the inertial    |
+        |                   | parameters [Asada83]_                           |
+
+        **Trajectory operation**:
+
+        If ``q`` is a matrix (m,n) then the result (m,) is a vector of
+        manipulability indices for each joint configuration specified by a row
+        of ``q``.
+
+        Notes
+        -----
+        - Invokes the ``jacob0`` method of the robot if ``J`` is not passed
+        - The "all" option includes rotational and translational
+            dexterity, but this involves adding different units. It can be
+            more useful to look at the translational and rotational
+            manipulability separately.
+        - Examples in the RVC book (1st edition) can be replicated by
+            using the "all" option
+        - Asada's measure requires inertial a robot model with inertial
+            parameters.
+
+        References
+        ----------
+        .. [Yoshikawa85] Manipulability of Robotic Mechanisms. Yoshikawa T.,
+                The International Journal of Robotics Research.
+                1985;4(2):3-9. doi:10.1177/027836498500400201
+        .. [Asada83] A geometrical representation of manipulator dynamics and
+                its application to arm design, H. Asada,
+                Journal of Dynamic Systems, Measurement, and Control,
+                vol. 105, p. 131, 1983.
+        .. [Klein87] Dexterity Measures for the Design and Control of
+                Kinematically Redundant Manipulators. Klein CA, Blaho BE.
+                The International Journal of Robotics Research.
+                1987;6(2):72-83. doi:10.1177/027836498700600206
+        - Robotics, Vision & Control, Chap 8, P. Corke, Springer 2011.
+
+
+        .. versionchanged:: 1.0.3
+            Removed 'both' option for axes, added a custom list option.
+
+        """
+
+        ets = self.ets(end, start)
+
+        axes_list: List[bool] = []
+
+        if isinstance(axes, list):
+            axes_list = axes
+        elif axes == "all":
+            axes_list = [True, True, True, True, True, True]
+        elif axes.startswith("trans"):
+            axes_list = [True, True, True, False, False, False]
+        elif axes.startswith("rot"):
+            axes_list = [False, False, False, True, True, True]
+        elif axes == "both":
+            return (
+                self.manipulability(
+                    q=q, J=J, end=end, start=start, method=method, axes="trans"
+                ),
+                self.manipulability(
+                    q=q, J=J, end=end, start=start, method=method, axes="rot"
+                ),
+            )
+        else:
+            raise ValueError("axes must be all, trans, rot or both")
+
+        def yoshikawa(robot, J, q, axes_list):
+            J = J[axes_list, :]
+            if J.shape[0] == J.shape[1]:
+                # simplified case for square matrix
+                return abs(np.linalg.det(J))
+            else:
+                m2 = np.linalg.det(J @ J.T)
+                return np.sqrt(abs(m2))
+
+        def condition(robot, J, q, axes_list):
+            J = J[axes_list, :]
+
+            # return 1/cond(J)
+            return 1 / np.linalg.cond(J)
+
+        def minsingular(robot, J, q, axes_list):
+            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):
+            # dof = np.sum(axes_list)
+            if np.linalg.matrix_rank(J) < 6:
+                return 0
+            Ji = np.linalg.pinv(J)
+            Mx = Ji.T @ robot.inertia(q) @ Ji
+            d = np.where(axes_list)[0]
+            Mx = Mx[d]
+            Mx = Mx[:, d.tolist()]
+            e, _ = np.linalg.eig(Mx)
+            return np.min(e) / np.max(e)
+
+        # choose the handler function
+        if method.lower().startswith("yoshi"):
+            mfunc = yoshikawa
+        elif method.lower().startswith("invc"):
+            mfunc = condition
+        elif method.lower().startswith("mins"):
+            mfunc = minsingular
+        elif method.lower().startswith("asa"):
+            mfunc = asada
+        else:
+            raise ValueError("Invalid method chosen")
+
+        # Calculate manipulability based on supplied Jacobian
+        if J is not None:
+            w = [mfunc(self, J, q, axes_list)]
+
+        # Otherwise use the q vector/matrix
+        else:
+            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])
+
+            for k, qk in enumerate(q):
+                Jk = ets.jacob0(qk)
+                w[k] = mfunc(self, Jk, qk, axes_list)
+
+        if len(w) == 1:
+            return w[0]
+        else:
+            return w
+
+    def jtraj(
+        self,
+        T1: Union[NDArray, SE3],
+        T2: Union[NDArray, SE3],
+        t: Union[NDArray, int],
+        **kwargs,
+    ):
+        """
+        Joint-space trajectory between SE(3) poses
+
+        The initial and final poses are mapped to joint space using inverse
+        kinematics:
+
+        - if the object has an analytic solution ``ikine_a`` that will be used,
+        - otherwise the general numerical algorithm ``ikine_lm`` will be used.
+
+        ``traj = obot.jtraj(T1, T2, t)`` is a trajectory object whose
+        attribute ``traj.q`` is a row-wise joint-space trajectory.
+
+        Parameters
+        ----------
+        T1
+            initial end-effector pose
+        T2
+            final end-effector pose
+        t
+            time vector or number of steps
+        kwargs
+            arguments passed to the IK solver
+
+        Returns
+        -------
+        trajectory
+
+        """  # noqa
+
+        if hasattr(self, "ikine_a"):
+            ik = self.ikine_a  # type: ignore
+        else:
+            ik = self.ikine_LM
+
+        q1 = ik(T1, **kwargs)
+        q2 = ik(T2, **kwargs)
+
+        return rtb.jtraj(q1.q, q2.q, t)
+
+    @overload
+    def jacob0_dot(
+        self,
+        q: ArrayLike,
+        qd: ArrayLike,
+        J0: None = None,
+        representation: Union[
+            L["rpy/xyz", "rpy/zyx", "eul", "exp"], None  # noqa
+        ] = None,
+    ) -> NDArray:  # pragma no cover
+        ...
+
+    @overload
+    def jacob0_dot(
+        self,
+        q: None,
+        qd: ArrayLike,
+        J0: NDArray = ...,
+        representation: Union[
+            L["rpy/xyz", "rpy/zyx", "eul", "exp"], None  # noqa
+        ] = None,
+    ) -> NDArray:  # pragma no cover
+        ...
+
+    def jacob0_dot(
+        self,
+        q,
+        qd: ArrayLike,
+        J0=None,
+        representation: Union[
+            L["rpy/xyz", "rpy/zyx", "eul", "exp"], None  # noqa
+        ] = None,
+    ):
+        r"""
+        Derivative of Jacobian
+
+        ``robot.jacob_dot(q, qd)`` computes the rate of change of the
+        Jacobian elements
+
+        .. math::
+
+            \dmat{J} = \frac{d \mat{J}}{d \vec{q}} \frac{d \vec{q}}{dt}
+
+        where the first term is the rank-3 Hessian.
+
+        Parameters
+        ----------
+        q
+        The joint configuration of the robot
+        qd
+            The joint velocity of the robot
+        J0
+            Jacobian in {0} frame
+        representation
+            angular representation
+
+        Returns
+        -------
+        jdot
+            The derivative of the manipulator Jacobian
+
+        Synopsis
+        --------
+
+        If ``J0`` is already calculated for the joint
+        coordinates ``q`` it can be passed in to to save computation time.
+
+        It is computed as the mode-3 product of the Hessian tensor and the
+        velocity vector.
+
+        The derivative of an analytical Jacobian can be obtained by setting
+        ``representation`` as
+
+        .. list-table::
+           :header-rows: 1
+
+           * - ``representation``
+             - Rotational representation
+           * - ``'rpy/xyz'``
+             - RPY angular rates in XYZ order
+           * - ``'rpy/zyx'``
+             - RPY angular rates in ZYX order
+           * - ``'eul'``
+             - Euler angular rates in ZYZ order
+           * - ``'exp'``
+             - exponential coordinate rates
+
+
+        References
+        ----------
+        - Kinematic Derivatives using the Elementary Transform
+            Sequence, J. Haviland and P. Corke
+
+        See Also
+        --------
+        :func:`jacob0`
+        :func:`hessian0`
+
+        """
+
+        qd = np.array(qd)
+
+        if representation is None:
+            if J0 is None:
+                J0 = self.jacob0(q)
+            H = self.hessian0(q, J0=J0)
+
+        else:
+            # # determine analytic rotation
+            # T = self.fkine(q).A
+            # gamma = smb.r2x(smb.t2r(T), representation=representation)
+
+            # # get transformation angular velocity to analytic velocity
+            # Ai = smb.rotvelxform(
+            #     gamma, representation=representation, inverse=True, full=True
+            # )
+
+            # # get analytic rate from joint rates
+            # omega = J0[3:, :] @ qd
+            # gamma_dot = Ai[3:, 3:] @ omega
+            # Ai_dot = smb.rotvelxform_inv_dot(gamma, gamma_dot, full=True)
+            # Ai_dot = sp.linalg.block_diag(np.zeros((3, 3)), Ai_dot)
+
+            # Jd = Ai_dot @ J0 + Ai @ Jd
+
+            # not actually sure this can be written in closed form
+
+            H = smb.numhess(
+                lambda q: self.jacob0_analytical(q, representation=representation), q
+            )
+
+            # Jd = Ai @ Jd
+
+            # return Jd
+
+        return np.tensordot(H, qd, (0, 0))
+
+    @overload
+    def jacobm(
+        self,
+        q: ArrayLike = ...,
+        J: None = None,
+        H: None = None,
+        end: Union[str, Link, Gripper, None] = None,
+        start: Union[str, Link, Gripper, None] = None,
+        axes: Union[L["all", "trans", "rot"], List[bool]] = "all",  # noqa
+    ) -> NDArray:  # pragma no cover
+        ...
+
+    @overload
+    def jacobm(
+        self,
+        q: None = None,
+        J: NDArray = ...,
+        H: NDArray = ...,
+        end: Union[str, Link, Gripper, None] = None,
+        start: Union[str, Link, Gripper, None] = None,
+        axes: Union[L["all", "trans", "rot"], List[bool]] = "all",  # noqa
+    ) -> NDArray:  # pragma no cover
+        ...
+
+    def jacobm(
+        self,
+        q=None,
+        J=None,
+        H=None,
+        end: Union[str, Link, Gripper, None] = None,
+        start: Union[str, Link, Gripper, None] = None,
+        axes: Union[L["all", "trans", "rot"], List[bool]] = "all",  # noqa
+    ) -> NDArray:
+        r"""
+        The manipulability Jacobian
+
+        This measure relates the rate of change of the manipulability to the
+        joint velocities of the robot. One of J or q is required. Supply J
+        and H if already calculated to save computation time
+
+        Parameters
+        ----------
+        q
+            The joint angles/configuration of the robot (Optional,
+            if not supplied will use the stored q values).
+        J
+            The manipulator Jacobian in any frame
+        H
+            The manipulator Hessian in any frame
+        end
+            the final link or Gripper which the Hessian represents
+        start
+            the first link which the Hessian represents
+
+        Returns
+        -------
+        jacobm
+            The manipulability Jacobian
+
+        Synopsis
+        --------
+        Yoshikawa's manipulability measure
+
+        .. math::
+
+            m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}
+
+        This method returns its Jacobian with respect to configuration
+
+        .. math::
+
+            \frac{\partial m(\vec{q})}{\partial \vec{q}}
+
+        References
+        ----------
+        - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
+          Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
+        - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
+          Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
+
+        """  # noqa
+
+        end, start, _ = self._get_limit_links(end, start)
+
+        #
+        if not isinstance(axes, list):
+            if axes.startswith("all"):
+                axes = [True, True, True, True, True, True]
+            elif axes.startswith("trans"):
+                axes = [True, True, True, False, False, False]
+            elif axes.startswith("rot"):
+                axes = [False, False, False, True, True, True]
+            else:
+                raise ValueError("axes must be all, trans or rot")
+
+        if J is None:
+            if q is None:
+                q = np.copy(self.q)
+            else:
+                q = getvector(q, self.n)
+
+            J = self.jacob0(q, start=start, end=end)
+        else:
+            verifymatrix(J, (6, self.n))
+
+        n = J.shape[1]
+
+        if H is None:
+            H = self.hessian0(J0=J, start=start, end=end)
+        # else:
+        #     verifymatrix(H, (6, self.n, self.n))
+        elif not isinstance(H, np.ndarray):
+            raise TypeError("Hessian must be numpy array of shape 6xnxn")
+        elif H.shape != (6, self.n, self.n):
+            raise ValueError("Hessian must be numpy array of shape 6xnxn")
+
+        manipulability = self.manipulability(
+            q,
+            J=J,
+            start=start,
+            end=end,
+            axes=axes,  # type: ignore
+        )
+
+        J = J[axes, :]  # type: ignore
+        H = H[:, axes, :]  # type: ignore
+
+        b = np.linalg.inv(J @ np.transpose(J))
+        Jm = np.zeros((n, 1))
+
+        for i in range(n):
+            c = J @ np.transpose(H[i, :, :])
+            Jm[i, 0] = manipulability * np.transpose(c.flatten("F")) @ b.flatten("F")
+
+        return Jm
+
+    # --------------------------------------------------------------------- #
+    # --------- Collision Methods ----------------------------------------- #
+    # --------------------------------------------------------------------- #
+
+    def closest_point(
+        self, q: ArrayLike, shape: Shape, inf_dist: float = 1.0, skip: bool = False
+    ) -> Tuple[
+        Union[int, None],
+        Union[NDArray, None],
+        Union[NDArray, None],
+    ]:
+        """
+        Find the closest point between robot and shape
+
+        ``closest_point(shape, inf_dist)`` returns the minimum euclidean
+        distance between this robot and shape, provided it is less than
+        inf_dist. It will also return the points on self and shape in the
+        world frame which connect the line of length distance between the
+        shapes. If the distance is negative then the shapes are collided.
+
+        Parameters
+        ----------
+        shape
+            The shape to compare distance to
+        inf_dist
+            The minimum distance within which to consider
+            the shape
+        skip
+            Skip setting all shape transforms based on q, use this
+            option if using this method in conjuction with Swift to save time
+
+        Returns
+        -------
+        d
+            distance between the robot and shape
+        p1
+            [x, y, z] point on the robot (in the world frame)
+        p2
+            [x, y, z] point on the shape (in the world frame)
+
+        """
+
+        if not skip:
+            self._update_link_tf(q)
+            self._propogate_scene_tree()
+            shape._propogate_scene_tree()
+
+        d = 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:
+                d = td
+                p1 = tp1
+                p2 = tp2
+
+        if d == 10000:
+            d = None
+
+        return d, p1, p2
+
+    def iscollided(self, q, shape: Shape, skip: bool = False) -> bool:
+        """
+        Check if the robot is in collision with a shape
+
+        ``iscollided(shape)`` checks if this robot and shape have collided
+
+        Parameters
+        ----------
+        shape
+            The shape to compare distance to
+        skip
+            Skip setting all shape transforms based on q, use this
+            option if using this method in conjuction with Swift to save time
+
+        Returns
+        -------
+        iscollided
+            True if shapes have collided
+
+        """
+
+        if not skip:
+            self._update_link_tf(q)
+            self._propogate_scene_tree()
+            shape._propogate_scene_tree()
+
+        for link in self.links:
+            if link.iscollided(shape, skip=True):
+                return True
+
+        if isinstance(self, rtb.Robot):
+            for gripper in self.grippers:
+                for link in gripper.links:
+                    if link.iscollided(shape, skip=True):
+                        return True
+
+        return False
+
+    def collided(self, q, shape: Shape, skip: bool = False) -> bool:
+        """
+        Check if the robot is in collision with a shape
+
+        ``collided(shape)`` checks if this robot and shape have collided
+
+        Parameters
+        ----------
+        shape
+            The shape to compare distance to
+        skip
+            Skip setting all shape transforms based on q, use this
+            option if using this method in conjuction with Swift to save time
+
+        Returns
+        -------
+        collided
+            True if shapes have collided
+
+        """
+
+        warn("method collided is deprecated, use iscollided instead", FutureWarning)
+        return self.iscollided(q, shape, skip=skip)
+
+    # --------------------------------------------------------------------- #
+    # --------- Constraint Methods ---------------------------------------- #
+    # --------------------------------------------------------------------- #
+
+    def joint_velocity_damper(
+        self,
+        q=None,
+        ps: float = 0.05,
+        pi: float = 0.1,
+        n: Union[int, None] = None,
+        gain: float = 1.0,
+    ) -> Tuple[NDArray, NDArray]:
+        """
+        Compute the joint velocity damper for QP motion control
+
+        Formulates an inequality contraint which, when optimised for will
+        make it impossible for the robot to run into joint limits. Requires
+        the joint limits of the robot to be specified. See examples/mmc.py
+        for use case
+
+        Parameters
+        ----------
+        ps
+            The minimum angle (in radians) in which the joint is
+            allowed to approach to its limit
+        pi
+            The influence angle (in radians) in which the velocity
+            damper becomes active
+        n
+            The number of joints to consider. Defaults to all joints
+        gain
+            The gain for the velocity damper
+
+        Returns
+        -------
+        Ain
+            A (6,) vector inequality contraint for an optisator
+        Bin
+            b (6,) vector inequality contraint for an optisator
+
+        """
+
+        if n is None:
+            n = self.n
+
+        if q is None:
+            q = np.copy(self.q)
+
+        Ain = np.zeros((n, n))
+        Bin = np.zeros(n)
+
+        for i in range(n):
+            if self.q[i] - self.qlim[0, i] <= pi:
+                Bin[i] = -gain * (((self.qlim[0, i] - q[i]) + ps) / (pi - ps))
+                Ain[i, i] = -1
+            if self.qlim[1, i] - self.q[i] <= pi:
+                Bin[i] = gain * ((self.qlim[1, i] - q[i]) - ps) / (pi - ps)
+                Ain[i, i] = 1
+
+        return Ain, Bin
+
+    def link_collision_damper(
+        self,
+        shape: CollisionShape,
+        q: ArrayLike,
+        di: float = 0.3,
+        ds: float = 0.05,
+        xi: float = 1.0,
+        end: Union[Link, None] = None,
+        start: Union[Link, None] = None,
+        collision_list: Union[List[Shape], None] = None,
+    ):
+        """
+        Compute a collision constrain for QP motion control
+
+        Formulates an inequality contraint which, when optimised for will
+        make it impossible for the robot to run into a collision. Requires
+        See examples/neo.py for use case
+
+        Parameters
+        ----------
+        ds
+            The minimum distance in which a joint is allowed to
+            approach the collision object shape
+        di
+            The influence distance in which the velocity
+            damper becomes active
+        xi
+            The gain for the velocity damper
+        end
+            The end link of the robot to consider
+        start
+            The start link of the robot to consider
+        collision_list
+            A list of shapes to consider for collision
+
+        Returns
+        -------
+        Ain
+            A (6,) vector inequality contraint for an optisator
+        Bin
+            b (6,) vector inequality contraint for an optisator
+
+        """
+
+        end, start, _ = self._get_limit_links(start=start, end=end)
+
+        links, n, _ = self.get_path(start=start, end=end)
+
+        q = np.array(q)
+        j = 0
+        Ain = None
+        bin = None
+
+        def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray):
+            d, wTlp, wTcp = link_col.closest_point(shape, di)
+
+            if d is not None and wTlp is not None and wTcp is not None:
+                lpTcp = -wTlp + wTcp
+
+                norm = lpTcp / d
+                norm_h = np.expand_dims(
+                    np.concatenate((norm, [0.0, 0.0, 0.0])),
+                    axis=0,  # type: ignore
+                )
+
+                # tool = (self.fkine(q, end=link).inv() * SE3(wTlp)).A[:3, 3]
+
+                # Je = self.jacob0(q, end=link, tool=tool)
+                # Je[:3, :] = self._T[:3, :3] @ Je[:3, :]
+
+                # n_dim = Je.shape[1]
+                # dp = norm_h @ shape.v
+                # l_Ain = zeros((1, self.n))
+
+                Je = self.jacobe(q, start=start, end=link, tool=link_col.T)
+                n_dim = Je.shape[1]
+                dp = norm_h @ shape.v
+                l_Ain = np.zeros((1, n))
+
+                l_Ain[0, :n_dim] = 1 * norm_h @ Je
+                l_bin = (xi * (d - ds) / (di - ds)) + dp
+            else:  # pragma nocover
+                l_Ain = None
+                l_bin = None
+
+            return l_Ain, l_bin
+
+        for link in links:
+            if link.isjoint:
+                j += 1
+
+            if collision_list is None:
+                col_list = link.collision
+
+                for c in col_list:
+                    pass
+            else:
+                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
+
+                if l_Ain is not None and l_bin is not None:
+                    if Ain is None:
+                        Ain = l_Ain
+                    else:
+                        Ain = np.concatenate((Ain, l_Ain))
+
+                    if bin is None:
+                        bin = np.array(l_bin)
+                    else:
+                        bin = np.concatenate((bin, l_bin))
+
+        return Ain, bin
+
+    def vision_collision_damper(
+        self,
+        shape: CollisionShape,
+        camera: Union["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
+        """
+        Compute a vision collision constrain for QP motion control
+
+        Formulates an inequality contraint which, when optimised for will
+        make it impossible for the robot to run into a line of sight.
+        See examples/fetch_vision.py for use case
+
+        Parameters
+        ----------
+        camera
+            The camera link, either as a robotic link or SE3
+            pose
+        camera_n
+            Degrees of freedom of the camera link
+        ds
+            The minimum distance in which a joint is allowed to
+            approach the collision object shape
+        di
+            The influence distance in which the velocity
+            damper becomes active
+        xi
+            The gain for the velocity damper
+        end
+            The end link of the robot to consider
+        start
+            The start link of the robot to consider
+        collision_list
+            A list of shapes to consider for collision
+
+        Returns
+        -------
+        Ain
+            A (6,) vector inequality contraint for an optisator
+        Bin
+            b (6,) vector inequality contraint for an optisator
+        """
+
+        end, start, _ = self._get_limit_links(start=start, end=end)
+
+        links, n, _ = self.get_path(start=start, end=end)
+
+        q = np.array(q)
+        j = 0
+        Ain = None
+        bin = None
+
+        def rotation_between_vectors(a, b):
+            a = a / np.linalg.norm(a)
+            b = b / np.linalg.norm(b)
+
+            angle = np.arccos(np.dot(a, b))
+            axis = np.cross(a, b)
+
+            return SE3.AngleAxis(angle, axis)
+
+        if isinstance(camera, rtb.BaseRobot):
+            wTcp = camera.fkine(camera.q).A[:3, 3]
+        elif isinstance(camera, SE3):
+            wTcp = camera.t
+        else:
+            raise TypeError("Camera must be a robotic link or SE3 pose")
+
+        wTtp = shape.T[:3, -1]
+
+        # Create line of sight object
+        los_mid = SE3((wTcp + wTtp) / 2)
+        los_orientation = rotation_between_vectors(
+            np.array([0.0, 0.0, 1.0]),
+            wTcp - wTtp,  # type: ignore
+        )
+
+        los = Cylinder(
+            radius=0.001,
+            length=np.linalg.norm(wTcp - wTtp),  # type: ignore
+            base=(los_mid * los_orientation),
+        )
+
+        def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray):
+            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
+
+                tool = SE3(
+                    (np.linalg.inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3]
+                )
+
+                Je = self.jacob0(q, end=link, tool=tool.A)
+                Je[:3, :] = self._T[:3, :3] @ Je[:3, :]
+                n_dim = Je.shape[1]
+
+                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
+
+                    dpc = norm_h @ Jv
+                    dpc = np.concatenate(
+                        (
+                            dpc[0, :-camera_n],
+                            np.zeros(self.n - (camera.n - camera_n)),
+                            dpc[0, -camera_n:],
+                        )
+                    )
+                else:
+                    dpc = np.zeros((1, self.n + camera_n))
+
+                dpt = norm_h @ shape.v
+                dpt *= np.linalg.norm(wTvp - wTcp) / los.length  # type: ignore
+
+                l_Ain = np.zeros((1, self.n + camera_n))
+                l_Ain[0, :n_dim] = norm_h @ Je
+                l_Ain -= dpc
+                l_bin = (xi * (d - ds) / (di - ds)) + dpt
+            else:
+                l_Ain = None
+                l_bin = None
+
+            return l_Ain, l_bin
+
+        for link in links:
+            if link.isjoint:
+                j += 1
+
+            if collision_list is None:
+                col_list = link.collision
+            else:
+                col_list = collision_list[j - 1]
+
+            for link_col in col_list:
+                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:
+                        Ain = l_Ain
+                    else:
+                        Ain = np.concatenate((Ain, l_Ain))
+
+                    if bin is None:
+                        bin = np.array(l_bin)
+                    else:
+                        bin = np.concatenate((bin, l_bin))
+
+        return Ain, bin
+
+    # --------------------------------------------------------------------- #
+    # --------- Dynamics Methods ------------------------------------------ #
+    # --------------------------------------------------------------------- #
+
+    def rne(
+        self,
+        q: NDArray,
+        qd: NDArray,
+        qdd: NDArray,
+        symbolic: bool = False,
+        gravity: Union[ArrayLike, None] = None,
+    ):
+        """
+        Compute inverse dynamics via recursive Newton-Euler formulation
+
+        ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is
+        the number of robot joints.  The result has shape (n,).
+
+        ``rne_dh(q, qd, qdd)`` where the arguments have shape (m,n) where n
+        is the number of robot joints and where m is the number of steps in
+        the joint trajectory.  The result has shape (m,n).
+
+        ``rne_dh(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] with
+        shape (3n,), and the result has shape (n,).
+
+        ``rne_dh(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] with
+        shape (m,3n) and the result has shape (m,n).
+
+        Parameters
+        ----------
+        q
+            Joint coordinates
+        qd
+            Joint velocity
+        qdd
+            Joint acceleration
+        symbolic
+            If True, supports symbolic expressions
+        gravity
+            Gravitational acceleration, defaults to attribute
+            of self
+
+        Returns
+        -------
+        tau
+            Joint force/torques
+
+        Notes
+        -----
+        - This version supports symbolic model parameters
+        - Verified against MATLAB code
+
+        """
+
+        n = self.n
+        # n = len(self.links)
+
+        # allocate intermediate variables
+        Xup = SE3.Alloc(n)
+
+        v = SpatialVelocity.Alloc(n)
+        a = SpatialAcceleration.Alloc(n)
+        f = SpatialForce.Alloc(n)
+        I = SpatialInertia.Alloc(n)  # noqa
+        s = []  # joint motion subspace
+
+        # Handle trajectory case
+        q = getmatrix(q, (None, None))
+        qd = getmatrix(qd, (None, None))
+        qdd = getmatrix(qdd, (None, None))
+        l, _ = q.shape
+
+        if symbolic:  # pragma: nocover
+            Q = np.empty((l, n), dtype="O")  # joint torque/force
+        else:
+            Q = np.empty((l, n))  # joint torque/force
+
+        link_groups: List[List[int]] = []
+
+        # Group links together based on whether they are joints or not
+        # Static links are grouped with the first joint encountered
+        current_group = []
+        for i, link in enumerate(self.links):
+            current_group.append(i)
+
+            # Break after adding the first link
+            if link.isjoint:
+                link_groups.append(current_group)
+                current_group = []
+
+        # Make some intermediate variables
+        for i, group in enumerate(link_groups):
+            I_int = SpatialInertia()
+
+            for idx in group:
+                link = self.links[idx]
+
+                I_int = I_int + SpatialInertia(m=link.m, r=link.r)
+
+                if link.v is not None:
+                    s.append(link.v.s)
+
+            I[i] = I_int
+
+        if gravity is None:
+            a_grav = -SpatialAcceleration(self.gravity)
+        else:  # pragma nocover
+            a_grav = -SpatialAcceleration(gravity)
+
+        # For the following, v, a, f, I, s, Xup are all lists of length n
+        # where the indices correspond to the index of the group within
+        # link_groups
+        # As always, q, qd, qdd are lists of length n, where indices correspond
+        # to the jindex of the joint, which will be the last link in the group
+        # within link_groups
+
+        for k in range(l):
+            qk = q[k, :]
+            qdk = qd[k, :]
+            qddk = qdd[k, :]
+
+            # forward recursion
+            for j, group in enumerate(link_groups):
+                # The joint is the last link in the group
+                joint = self.links[group[-1]]
+                jindex = joint.jindex
+
+                vJ = SpatialVelocity(s[j] * qdk[jindex])
+
+                # transform from parent(j) to j
+                # Xup_int = SE3()
+                first_element = True
+                for idx in group:
+                    link = self.links[idx]
+
+                    if link.isjoint and link.jindex is not None:
+                        if first_element:
+                            Xup_int = SE3(link.A(qk[link.jindex]))
+                            first_element = False
+                        else:
+                            Xup_int = Xup_int * SE3(link.A(qk[link.jindex]))
+                    else:
+                        if first_element:
+                            Xup_int = SE3(link.A())
+                            first_element = False
+                        else:
+                            Xup_int = Xup_int * SE3(link.A())
+
+                Xup[j] = Xup_int.inv()
+
+                # The first link in the group
+                first_link = self.links[group[0]]
+
+                if first_link.parent is None:
+                    v[j] = vJ
+                    a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[jindex])
+                else:
+                    # The index of `link`s parent within self.links
+                    parent_idx = self.links.index(first_link.parent)
+
+                    # The index of the group that the parent link is in
+                    group_idx = [
+                        i for i, group in enumerate(link_groups) if parent_idx in group
+                    ][0]
+
+                    v[j] = Xup[j] * v[group_idx] + vJ
+                    a[j] = (
+                        Xup[j] * a[group_idx]
+                        + SpatialAcceleration(s[j] * qddk[jindex])
+                        + v[j] @ vJ
+                    )
+
+                f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
+
+            # Backward recursion
+            for j in reversed(range(n)):
+                group = link_groups[j]
+                joint = self.links[group[-1]]
+                first_link = self.links[group[0]]
+                # link = self.links[j]
+
+                # next line could be dot(), but fails for symbolic arguments
+                Q[k, j] = sum(f[j].A * s[j])
+
+                if first_link.parent is not None:
+                    # The index of `link`s parent within self.links
+                    parent_idx = self.links.index(first_link.parent)
+
+                    # The index of the group that the parent link is in
+                    group_idx = [
+                        i for i, group in enumerate(link_groups) if parent_idx in group
+                    ][0]
+
+                    f[group_idx] = f[group_idx] + Xup[j] * f[j]
+
+        # The current Q has the length equal to the number of links within the robot
+        # rather than the number of joints. We need to remove the static links
+        # from the Q array
+        # joint_idx = [i for i, link in enumerate(self.links) if link.isjoint]
+
+        if l == 1:
+            return Q[0]
+        else:  # pragma nocover
+            return Q
+
+
+# ============================================================================= #
+# ================= Robot2 Class ============================================== #
+# ============================================================================= #
+
+
+class Robot2(BaseRobot[Link2]):
+    def __init__(self, arg, **kwargs):
+        if isinstance(arg, ETS2):
+            # we're passed an ETS string
+            links = []
+            # chop it up into segments, a link frame after every joint
+            parent = None
+            for j, ets_j in enumerate(arg.split()):
+                elink = Link2(ETS2(ets_j), parent=parent, name=f"link{j:d}")
+                parent = elink
+                if (
+                    elink.qlim is None
+                    and elink.v is not None
+                    and elink.v.qlim is not None
+                ):  # pragma nocover
+                    elink.qlim = elink.v.qlim
+                links.append(elink)
+
+        elif smb.islistof(arg, Link2):
+            links = arg
+
+        else:  # pragma nocover
+            raise TypeError("constructor argument must be ETS2 or list of Link2")
+
+        super().__init__(links, **kwargs)
+
+        # Should just set it to None
+        self.base = SE2()  # override superclass
+
+    @property
+    def base(self) -> SE2:
+        """
+        Get/set robot base transform (Robot superclass)
+
+        ``robot.base`` is the robot base transform
+
+        Returns
+        -------
+        base
+            robot tool transform
+
+        - ``robot.base = ...`` checks and sets the robot base transform
+
+        Notes
+        -----
+        - The private attribute ``_base`` will be None in the case of
+            no base transform, but this property will return ``SE3()`` which
+            is an identity matrix.
+        """
+        if self._base is None:  # pragma nocover
+            self._base = SE2()
+
+        # return a copy, otherwise somebody with
+        # reference to the base can change it
+        return self._base.copy()
+
+    @base.setter
+    def base(self, T):
+        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):
+        return self.ets(start, end).jacob0(q)
+
+    def jacobe(self, q, start=None, end=None):
+        return self.ets(start, end).jacobe(q)
+
+    def fkine(self, q, end=None, start=None):
+        return self.ets(start, end).fkine(q)
+
+    @property
+    def reach(self) -> float:
+        r"""
+        Reach of the robot
+
+        A conservative estimate of the reach of the robot. It is computed as
+        the sum of the translational ETs that define the link transform.
+
+        Note
+        ----
+        Computed on the first access. If kinematic parameters
+        subsequently change this will not be reflected.
+
+        Returns
+        -------
+        reach
+            Maximum reach of the robot
+
+        Notes
+        -----
+        - Probably an overestimate of reach
+        - Used by numerical inverse kinematics to scale translational
+          error.
+        - For a prismatic joint, uses ``qlim`` if it is set
+
+        """
+
+        # TODO
+        # This should be a start, end method and compute the reach based on the
+        # given ets. Then use an lru_cache to speed up return
+
+        if self._reach is None:
+            d_all = []
+            for link in self.ee_links:
+                d = 0
+                while True:
+                    for et in link.ets:
+                        if et.istranslation:
+                            if et.isjoint:
+                                # the length of a prismatic joint depends on the
+                                # joint limits.  They might be set in the ET
+                                # or in the Link depending on how the robot
+                                # was constructed
+                                if link.qlim is not None:
+                                    d += max(link.qlim)
+                                elif et.qlim is not None:  # pragma nocover
+                                    d += max(et.qlim)
+                            else:
+                                d += abs(et.eta)
+                    link = link.parent
+                    if link is None or isinstance(link, str):
+                        d_all.append(d)
+                        break
+
+            self._reach = max(d_all)
+        return self._reach
+
+    def fkine_all(self, q: ArrayLike) -> SE2:
+        """
+        Compute the pose of every link frame
+
+        ``T = robot.fkine_all(q)`` is  an SE3 instance with ``robot.nlinks +
+        1`` values:
+
+        - ``T[0]`` is the base transform
+        - ``T[i]`` is the pose of link whose ``number`` is ``i``
+
+        Parameters
+        ----------
+        q
+            The joint configuration
+
+        Returns
+        -------
+        fkine_all
+            Pose of all links
+
+        References
+        ----------
+        - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
+          Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
+
+        """  # noqa
+
+        q = getvector(q)
+
+        Tbase = SE2(self.base)  # add base, also sets the type
+
+        linkframes = Tbase.__class__.Alloc(self.nlinks + 1)
+        linkframes[0] = Tbase
+
+        def recurse(Tall, Tparent, q, link):
+            # if joint??
+            T = Tparent
+            while True:
+                T *= SE2(link.A(q[link.jindex]))
+
+                Tall[link.number] = T
+
+                if link.nchildren == 0:
+                    # no children
+                    return
+                elif link.nchildren == 1:
+                    # one child
+                    if link in self.ee_links:  # pragma nocover
+                        # this link is an end-effector, go no further
+                        return
+                    link = link.children[0]
+                    continue
+                else:
+                    # multiple children
+                    for child in link.children:
+                        recurse(Tall, T, q, child)
+                    return
+
+        recurse(linkframes, Tbase, q, self.links[0])
+
+        return linkframes