roboticstoolbox-python 1.4.3__py3-none-any.whl → 1.4.4__py3-none-any.whl

roboticstoolbox/bin/rtbtool.py

@@ -202,7 +202,7 @@ def make_banner():
     from spatialmath import *
     from spatialmath.base import *
     from spatialmath.base import sym
-    from roboticstoolbox import *"
+    from roboticstoolbox import *
 
     # useful variables
     from math import pi

roboticstoolbox/ets/cpp-extensions/ik.cpp

@@ -51,8 +51,52 @@ static void _IK_loop(
 
             if (*E < tol)
             {
-                for (int i = 0; i < ets->n; i++)
-                    q(i) = std::fmod(q(i) + PI, PI_x2) - PI;
+                // Preserve translations and coordinates already within their limits.
+                // Other revolute coordinates may move only by whole turns, so the
+                // converged end-effector pose is unchanged (see IKSolver._normalise_q).
+                int j = 0;
+                for (int i = 0; i < ets->m; i++)
+                {
+                    ET *et = ets->ets[i];
+                    if (!et->isjoint)
+                        continue;
+
+                    double lower = ets->qlim_l[j];
+                    double upper = ets->qlim_h[j];
+                    if (et->axis < 3 && !(lower <= q(j) && q(j) <= upper))
+                    {
+                        // Avoid rounding a principal angle across a nearby limit.
+                        double angle = q(j);
+                        if (!(angle >= -PI && angle < PI))
+                        {
+                            // Unlike Python's %, fmod can return a negative remainder.
+                            angle = std::fmod(angle + PI, PI_x2);
+                            if (angle < 0)
+                                angle += PI_x2;
+                            angle -= PI;
+                        }
+                        if (angle < lower)
+                            angle += PI_x2 * std::ceil((lower - angle) / PI_x2);
+                        else if (angle > upper)
+                            angle -= PI_x2 * std::ceil((angle - upper) / PI_x2);
+                        if (!(lower <= angle && angle <= upper))
+                        {
+                            // Recover rounded endpoints only if whole turns
+                            // reconstruct the original coordinate exactly.
+                            for (double bound : {lower, upper})
+                            {
+                                double turns = std::round((q(j) - bound) / PI_x2);
+                                if (turns != 0 && bound + turns * PI_x2 == q(j))
+                                {
+                                    angle = bound;
+                                    break;
+                                }
+                            }
+                        }
+                        q(j) = angle;
+                    }
+                    j++;
+                }
                 *solution = reject_jl ? _check_lim(ets, q) : 1;
                 break;
             }
@@ -315,4 +359,4 @@ extern "C"
         // return q;
     }
 
-} /* extern "C" */
+} /* extern "C" */

roboticstoolbox/models/DH/UR10.py

@@ -27,6 +27,11 @@ class UR10(DHRobot):
 
     .. note::
         - SI units are used.
+        - The last link's inertia tensor has :math:`I_{xx} = 0` -- that's not
+          a bug in this model, it's what the manufacturer's own reference
+          (below) publishes. It makes the robot's inertia matrix near-singular
+          (``det(robot.inertia(q)) ~ 2e-6``), so dynamics work that needs a
+          well-conditioned inertia matrix should account for this.
 
     :References:
 

roboticstoolbox/models/URDF/Fetch.py

@@ -47,6 +47,9 @@ class Fetch(URDFRobot):
     from a URDF file.  The model describes its kinematic and graphical
     characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/models/URDF/Frankie.py

@@ -13,6 +13,9 @@ class Frankie(URDFRobot):
     from a URDF file.  The model describes its kinematic and graphical
     characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb
@@ -23,8 +26,14 @@ class Frankie(URDFRobot):
 
     - qz, zero joint angle configuration, 'L' shaped configuration
     - qr, vertical 'READY' configuration
-    - qs, arm is stretched out in the x-direction
-    - qn, arm is at a nominal non-singular configuration
+
+    Loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package
+    (real inertial data, plain-mesh collision geometry), unlike
+    :class:`~roboticstoolbox.models.URDF.Panda.Panda`'s bundled default. See
+    the wiki's `Panda models
+    <https://github.com/petercorke/robotics-toolbox-python/wiki/Panda-models>`_
+    page for why the two differ.
 
     .. codeauthor:: Jesse Haviland
     .. sectionauthor:: Peter Corke

roboticstoolbox/models/URDF/FrankieOmni.py

@@ -27,8 +27,16 @@ class FrankieOmni(Robot):
 
     - qz, zero joint angle configuration, 'L' shaped configuration
     - qr, vertical 'READY' configuration
-    - qs, arm is stretched out in the x-direction
-    - qn, arm is at a nominal non-singular configuration
+
+    The arm is loaded from the same bundled ``qut_frankie_description`` xacro
+    as :class:`~roboticstoolbox.models.URDF.Panda.Panda`'s default (no
+    inertial data), spliced by hand onto a separately-loaded
+    ``clearpath_ridgeback_description`` mobile base -- it has no
+    ``use_robot_descriptions``-style option, since the base+arm splice is
+    done manually rather than through :class:`URDFRobot`'s own loader. See
+    the wiki's `Panda models
+    <https://github.com/petercorke/robotics-toolbox-python/wiki/Panda-models>`_
+    page for the full picture.
 
     .. codeauthor:: Jesse Haviland
     .. sectionauthor:: Peter Corke

roboticstoolbox/models/URDF/Jaco.py

@@ -12,6 +12,9 @@ class Jaco(URDFRobot):
     from a URDF file.  The model describes its kinematic and graphical
     characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/models/URDF/PR2.py

@@ -12,6 +12,9 @@ class PR2(URDFRobot):
     from a URDF file.  The model describes its kinematic and graphical
     characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/models/URDF/Panda.py

@@ -23,20 +23,40 @@ class Panda(URDFRobot):
 
     - qz, zero joint angle configuration, 'L' shaped configuration
     - qr, vertical 'READY' configuration
-    - qs, arm is stretched out in the x-direction
-    - qn, arm is at a nominal non-singular configuration
+
+    :param use_robot_descriptions: if ``True``, load the Panda URDF from the
+        `robot_descriptions <https://github.com/robot-descriptions/robot_descriptions.py>`_
+        package instead of the toolbox's own bundled ``qut_frankie_description``
+        xacro (the default, ``False``). The bundled model has real collision
+        geometry (a hand-built capsule approximation) but no inertial
+        (mass/CoM/inertia) data at all -- ``rne()``/``inertia()``/``coriolis()``/
+        ``gravload()`` are all silently zero. The ``robot_descriptions`` model
+        has real inertial data, but its collision geometry is plain meshes,
+        which are roughly an order of magnitude slower to collision-check
+        against than the bundled model's capsules -- noticeable in a
+        real-time reactive-avoidance loop (see ``examples/neo.py``). See the
+        wiki's `Panda models <https://github.com/petercorke/robotics-toolbox-python/wiki/Panda-models>`_
+        page for the full comparison and rationale.
+    :type use_robot_descriptions: bool
 
     .. codeauthor:: Jesse Haviland
     .. sectionauthor:: Peter Corke
     """
 
-    def __init__(self):
-
-        super().__init__(
-            "qut_frankie_description/robots/panda_arm_hand.urdf.xacro",
-            manufacturer="Franka Emika",
-            gripper_link_index=9,
-        )
+    def __init__(self, use_robot_descriptions: bool = False):
+
+        if use_robot_descriptions:
+            super().__init__(
+                "panda",
+                manufacturer="Franka Emika",
+                gripper_link_index=9,
+            )
+        else:
+            super().__init__(
+                "qut_frankie_description/robots/panda_arm_hand.urdf.xacro",
+                manufacturer="Franka Emika",
+                gripper_link_index=9,
+            )
 
         self.grippers[0].tool = SE3(0, 0, 0.1034)
 

roboticstoolbox/models/URDF/UR10.py

@@ -12,6 +12,9 @@ class UR10(URDFRobot):
     definition from a URDF file.  The model describes its kinematic and
     graphical characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/models/URDF/UR3.py

@@ -12,6 +12,9 @@ class UR3(URDFRobot):
     definition from a URDF file.  The model describes its kinematic and
     graphical characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/models/URDF/UR5.py

@@ -13,6 +13,9 @@ class UR5(URDFRobot):
     definition from a URDF file.  The model describes its kinematic and
     graphical characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb
@@ -30,7 +33,16 @@ class UR5(URDFRobot):
 
     def __init__(self):
 
-        super().__init__("ur5", manufacturer="Universal Robotics", gripper_link_index=7)
+        # Name-based lookup, not a raw positional index: robot_descriptions
+        # 3.0.0 changed which upstream repo "ur5" resolves to, reordering
+        # links so index 7 silently pointed at a real arm joint instead of
+        # the tool attachment link (#578). tool0/ee_link/flange are stable
+        # names across the versions checked.
+        super().__init__(
+            "ur5",
+            manufacturer="Universal Robotics",
+            gripper_link_name=["tool0", "ee_link", "flange"],
+        )
 
         # for link in links:
         #     print(link)

roboticstoolbox/models/URDF/URDFRobot.py

@@ -383,10 +383,16 @@ class URDFRobot(Robot):
                 super().__init__(
                     "ur5",
                     manufacturer="Universal Robotics",
-                    gripper_link_index=7,
+                    gripper_link_name=["tool0", "ee_link"],
                 )
                 self.qz = np.zeros(6)
                 ...
+
+    ``gripper_link_name`` (a name, or a list of candidate names tried in
+    order) survives an upstream URDF reordering that would silently break a
+    raw positional ``gripper_link_index`` -- see #578. Prefer it for new
+    models; ``gripper_link_index`` remains for models where a stable,
+    well-known link name isn't available.
     """
 
     def __init__(
@@ -394,6 +400,7 @@ class URDFRobot(Robot):
         urdf_path: "str | Path",
         manufacturer: str = "",
         gripper_link_index: "int | None" = None,
+        gripper_link_name: "str | list[str] | None" = None,
         patch: "Callable[[str], str] | None" = None,
         extra_packages: "dict[str, str] | None" = None,
         **kwargs,
@@ -401,7 +408,26 @@ class URDFRobot(Robot):
         elinks, name, filepath = URDF_file(
             urdf_path, patch=patch, extra_packages=extra_packages
         )
-        if gripper_link_index is not None:
+        if gripper_link_name is not None:
+            candidates = (
+                [gripper_link_name]
+                if isinstance(gripper_link_name, str)
+                else gripper_link_name
+            )
+            for candidate in candidates:
+                match = next(
+                    (link for link in elinks if link.name == candidate), None
+                )
+                if match is not None:
+                    kwargs["gripper_links"] = match
+                    break
+            else:
+                raise ValueError(
+                    f"none of the candidate gripper link names {candidates!r} "
+                    f"were found in the parsed URDF for {name!r} (have: "
+                    f"{[link.name for link in elinks]})"
+                )
+        elif gripper_link_index is not None:
             kwargs["gripper_links"] = elinks[gripper_link_index]
         super().__init__(elinks, name=name, manufacturer=manufacturer, **kwargs)
         self._urdf_filepath = str(filepath) if filepath is not None else ""

roboticstoolbox/models/URDF/Valkyrie.py

@@ -41,6 +41,9 @@ class Valkyrie(Robot):
     from a URDF file.  The model describes its kinematic and graphical
     characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/models/URDF/YuMi.py

@@ -15,6 +15,9 @@ class YuMi(Robot):
     from a URDF file.  The model describes its kinematic and graphical
     characteristics.
 
+    The model is loaded via the `robot_descriptions
+    <https://github.com/robot-descriptions/robot_descriptions.py>`_ package.
+
     .. runblock:: pycon
 
         >>> import roboticstoolbox as rtb

roboticstoolbox/robot/DHRobot.py

@@ -1889,7 +1889,7 @@ class DHRobot(Robot):
         if base is not None:
             T = base.inv() * T
         if tool is not None:
-            T = tool.inv() * T
+            T = T * tool.inv()
 
         # q = np.zeros((6,))
         solutions = []

roboticstoolbox/robot/IK.py

@@ -308,13 +308,10 @@ class IKSolver(ABC):
             while True:
                 # Check convergence for the current q before another update.
                 if E < self.tol:
-                    # Wrap q to be within +- 180 deg
-                    # If your robot has larger than 180 deg range on a joint
-                    # this line should be modified in incorporate the extra range
-                    q = (q + np.pi) % (2 * np.pi) - np.pi
+                    self._normalise_q(ets, q)
 
                     # Check if we have violated joint limits
-                    jl_valid = self._check_jl(ets, q)
+                    jl_valid = self._check_jl(ets, q[ets.jindices])
 
                     if not jl_valid and self.joint_limits:
                         # Abandon search and try again
@@ -450,6 +447,46 @@ class IKSolver(ABC):
 
         return q
 
+    def _normalise_q(self, ets: "rtb.ETS", q: np.ndarray) -> None:
+        """
+        Choose equivalent revolute coordinates without changing prismatic joints.
+
+        :param ets: The ETS defining the joint types and limits
+        :param q: Joint coordinates, indexed by joint jindex, modified in place
+        :returns: None
+
+        Preserve coordinates already within their limits, including revolute joints
+        outside the principal interval. Otherwise prefer the principal angle, shifted
+        by whole turns towards the limits if necessary. If no equivalent angle lies
+        within the limits, the subsequent joint-limit check will reject the solution.
+        """
+        qlim = ets.qlim
+        for i, joint in enumerate(ets.joints()):
+            j = joint.jindex
+            lower, upper = qlim[:, i]
+            if not joint.isrotation or lower <= q[j] <= upper:
+                continue
+
+            # Avoid rounding a principal angle across a nearby joint limit.
+            angle = q[j]
+            if not -np.pi <= angle < np.pi:
+                angle = (angle + np.pi) % (2 * np.pi) - np.pi
+            if angle < lower:
+                angle += 2 * np.pi * np.ceil((lower - angle) / (2 * np.pi))
+            elif angle > upper:
+                angle -= 2 * np.pi * np.ceil((angle - upper) / (2 * np.pi))
+            if not lower <= angle <= upper:
+                # Argument reduction can round a limit plus whole turns just
+                # outside the closed interval. Accept that endpoint only when
+                # it reconstructs the original coordinate exactly, without an
+                # epsilon that could admit an unrelated out-of-limit angle.
+                for bound in (lower, upper):
+                    turns = np.rint((q[j] - bound) / (2 * np.pi))
+                    if turns != 0 and bound + turns * (2 * np.pi) == q[j]:
+                        angle = bound
+                        break
+            q[j] = angle
+
     def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool:
         """
         Checks if the joints are within their respective limits

roboticstoolbox/robot/Robot.py

@@ -1385,7 +1385,7 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
 
     def joint_velocity_damper(
         self,
-        q=None,
+        q: NDArray | None = None,
         ps: float = 0.05,
         pi: float = 0.1,
         n: int | None = None,
@@ -1413,16 +1413,16 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
             n = self.n
 
         if q is None:
-            q = np.copy(self.q)
+            q = 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:
+            if 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:
+            if self.qlim[1, i] - q[i] <= pi:
                 Bin[i] = gain * ((self.qlim[1, i] - q[i]) - ps) / (pi - ps)
                 Ain[i, i] = 1
 
@@ -1460,13 +1460,21 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
 
         end, start, _ = self._get_limit_links(start=start, end=end)
 
-        links, n, _ = self.get_path(start=start, end=end)
+        links, _, _ = self.get_path(start=start, end=end)
 
         q = np.array(q)
         j = 0
         Ain = None
         bin = None
 
+        def get_link_joint_index(link: Link | None) -> int:
+            curr = link
+            while curr is not None:
+                if curr.isjoint and curr.jindex is not None:
+                    return curr.jindex
+                curr = curr.parent
+            return 0
+
         def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray):
             d, wTlp, wTcp = link_col.closest_point(shape, di)
 
@@ -1491,9 +1499,10 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
                 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 = np.zeros((1, self.n))
 
-                l_Ain[0, :n_dim] = 1 * norm_h @ Je
+                start_jidx = get_link_joint_index(start)
+                l_Ain[0, start_jidx : start_jidx + n_dim] = 1 * norm_h @ Je
                 l_bin = (xi * (d - ds) / (di - ds)) + dp
             else:  # pragma nocover
                 l_Ain = None
@@ -1507,9 +1516,6 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
 
             if collision_list is None:
                 col_list = link.collision
-
-                for c in col_list:
-                    pass
             else:
                 col_list = [collision_list[j - 1]]  # pragma nocover
 
@@ -1786,28 +1792,71 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
 
         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 = []
+        # Group each joint together with every static (fixed) link rigidly
+        # attached to -- and moving with -- its own output frame: found by
+        # walking each static link's ancestry (via .parent, not flat
+        # self.links order, so this is correct under branching too) up to
+        # its nearest joint. A static link ends up in that joint's group
+        # regardless of where it sits relative to the *next* joint in the
+        # chain -- immediately after this joint, sandwiched anywhere before
+        # the next one, or trailing after the very last joint with nothing
+        # further downstream (e.g. a tool flange/mount link, such as URDF
+        # Panda's panda_link8). A static link with no joint ancestor at all
+        # (rigidly mounted on the immovable base) contributes no joint
+        # torque and is dropped.
+        #
+        # An earlier version grouped a static link with the first joint
+        # encountered scanning *forward* -- correct for a trailing run
+        # (#636), but wrong for a sandwiched static link (joint_A -> static
+        # -> joint_B): that attaches the static link's mass to joint_B's
+        # group, when it's actually rigidly welded to joint_A's output and
+        # physically independent of joint_B's angle, silently misattributing
+        # its torque contribution to the wrong joint (#483).
+        group_of_link_idx: dict[int, int] = {}
         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 = []
+                link_groups.append([i])
+                group_of_link_idx[i] = len(link_groups) - 1
+            elif link.parent is not None:
+                group_idx = group_of_link_idx.get(self.links.index(link.parent))
+                if group_idx is not None:
+                    link_groups[group_idx].append(i)
+                    group_of_link_idx[i] = group_idx
 
         # Make some intermediate variables
         for i, group in enumerate(link_groups):
             I_int = SpatialInertia()
 
+            # group[0] is always the joint itself (see the grouping loop
+            # above); everything after it in the group is a static link
+            # rigidly carried by that joint's own output frame, but
+            # link.r/link.I are each expressed in that *link's own* frame,
+            # not the joint's. Compose their fixed transforms relative to
+            # the joint and carry the CoM/inertia into the joint's frame
+            # before summing -- otherwise a static link's mass lands in
+            # I_int with the wrong moment arm relative to this joint
+            # (effectively r=0), which for a non-negligible offset silently
+            # drops its contribution to this joint's torque (#636, #483).
+            past_joint = False
+            T_from_joint = None
             for idx in group:
                 link = self.links[idx]
 
-                I_int = I_int + SpatialInertia(m=link.m, r=link.r, I=link.I)
+                if past_joint:
+                    T_from_joint = (
+                        SE3(link.A())
+                        if T_from_joint is None
+                        else T_from_joint * SE3(link.A())
+                    )
+                    r = T_from_joint.R @ np.asarray(link.r) + T_from_joint.t
+                    Ir = T_from_joint.R @ link.I @ T_from_joint.R.T
+                    I_int = I_int + SpatialInertia(m=link.m, r=r, I=Ir)
+                else:
+                    I_int = I_int + SpatialInertia(m=link.m, r=link.r, I=link.I)
 
                 if link.v is not None:
                     s.append(link.v.s)  # type: ignore[union-attr]
+                    past_joint = True
 
             I[i] = I_int
 
@@ -1832,8 +1881,8 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
         # 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
+        # to the jindex of the joint, which is always the first link in the
+        # group within link_groups
 
         for k in range(l):
             qk = q[k, :]
@@ -1842,48 +1891,43 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
 
             # forward recursion
             for j, group in enumerate(link_groups):
-                # The joint is the last link in the group
-                joint = self.links[group[-1]]
+                # group[0] is always the joint (see the grouping loop
+                # above); any static links after it in the group are
+                # rigidly carried by this joint's own output frame and are
+                # folded into I[j] directly instead, not into this
+                # kinematic frame -- v[j]/a[j] stay defined at the joint's
+                # own output, matching s[j]/vJ, which are themselves only
+                # ever expressed there.
+                joint = self.links[group[0]]
                 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())
-
+                # transform from parent(j) to j: joint.A() already
+                # incorporates any fixed transform within the joint link's
+                # own ETS (the joint variable is guaranteed to be the last
+                # element of its segment -- see this method's docstring),
+                # so no further composition is needed.
+                Xup_int = SE3(joint.A(qk[jindex]))
                 Xup[j] = Xup_int.inv()  # type: ignore[union-attr]
 
-                # The first link in the group
-                first_link = self.links[group[0]]
+                # group_of_link_idx also covers a joint whose immediate
+                # .parent is a static link, or a run of them, rather than
+                # another joint directly -- returning None (rather than
+                # raising) when that walk reaches the root with no joint
+                # ancestor at all (e.g. a static base link, such as URDF
+                # Panda's panda_link0), which is kinematically equivalent
+                # to .parent being None.
+                group_idx = (
+                    group_of_link_idx.get(self.links.index(joint.parent))
+                    if joint.parent is not None
+                    else None
+                )
 
-                if first_link.parent is None:
+                if group_idx 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]
@@ -1896,9 +1940,7 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
             # 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]
+                joint = self.links[group[0]]  # always the joint -- see above
 
                 # next line could be dot(), but fails for symbolic arguments
                 Q[k, j] = sum(f[j].A * s[j])
@@ -1913,16 +1955,26 @@ class Robot(BaseRobot[Link], RobotKinematicsMixin):
                     - joint.friction(qdk[jindex], coulomb=not symbolic)
                 )
 
-                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]
+                # See the forward recursion above: group_of_link_idx
+                # resolves through any static link(s) between this joint
+                # and its nearest joint ancestor, returning None if there
+                # isn't one (root).
+                group_idx = (
+                    group_of_link_idx.get(self.links.index(joint.parent))
+                    if joint.parent is not None
+                    else None
+                )
 
-                    f[group_idx] = f[group_idx] + Xup[j] * f[j]
+                if group_idx is not None:
+                    # Xup[j] is the child<-parent motion transform (v_child =
+                    # Xup[j] * v_parent); propagating a force the other way,
+                    # child->parent, needs the transform in the other
+                    # direction too. spatialmath's SE3 * SpatialForce applies
+                    # the coadjoint of its left operand (since
+                    # spatialmath-python 1.1.18, rai-opensource/
+                    # spatialmath-python#207), so the un-inverted transform
+                    # (Xup[j].inv(), i.e. parent<-child) is what's needed here.
+                    f[group_idx] = f[group_idx] + Xup[j].inv() * 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

roboticstoolbox/robot/RobotKinematics.py

@@ -21,6 +21,16 @@ class RobotKinematicsMixin:
 
     """
 
+    def _resolve_tool(self: KinematicsProtocol, tool: NDArray | SE3 | None) -> SE3:
+        """
+        Resolve an explicit ``tool`` argument, defaulting to the robot's
+        own :attr:`tool` (identity if never set) when not given explicitly
+        -- see :meth:`fkine`'s ``tool`` parameter.
+        """
+        if tool is None:
+            return self.tool
+        return tool if isinstance(tool, SE3) else SE3(tool, check=False)
+
     # --------------------------------------------------------------------- #
     # --------- Kinematic Methods ----------------------------------------- #
     # --------------------------------------------------------------------- #
@@ -85,9 +95,11 @@ class RobotKinematicsMixin:
             specify ``end``
         - For a robot with multiple end-effectors, the ``end`` must
             be specified.
-        - The robot's base tool transform, if set, is incorporated
-            into the result.
-        - A tool transform, if provided, is incorporated into the result.
+        - The robot's own base transform (``self.base``), if set, is
+            always incorporated into the result.
+        - The robot's own tool transform (``self.tool``), if set, is
+            incorporated into the result unless the ``tool`` parameter is
+            given explicitly, which overrides it.
         - Works from the end-effector link to the base
 
         .. rubric:: References
@@ -101,7 +113,10 @@ class RobotKinematicsMixin:
 
         return SE3(
             self.ets(start, end).fkine(
-                q, base=self._T, tool=tool, include_base=include_base
+                q,
+                base=self._T,
+                tool=self._resolve_tool(tool),
+                include_base=include_base,
             ),
             check=False,
         )
@@ -155,7 +170,7 @@ class RobotKinematicsMixin:
 
         """
 
-        return self.ets(start, end).jacob0(q, tool=tool)
+        return self.ets(start, end).jacob0(q, tool=self._resolve_tool(tool))
 
     def jacobe(
         self: KinematicsProtocol,
@@ -206,7 +221,7 @@ class RobotKinematicsMixin:
 
         """
 
-        return self.ets(start, end).jacobe(q, tool=tool)
+        return self.ets(start, end).jacobe(q, tool=self._resolve_tool(tool))
 
     @overload
     def hessian0(
@@ -306,7 +321,7 @@ class RobotKinematicsMixin:
 
         """
 
-        return self.ets(start, end).hessian0(q, J0=J0, tool=tool)
+        return self.ets(start, end).hessian0(q, J0=J0, tool=self._resolve_tool(tool))
 
     @overload
     def hessiane(
@@ -406,7 +421,7 @@ class RobotKinematicsMixin:
 
         """
 
-        return self.ets(start, end).hessiane(q, Je=Je, tool=tool)
+        return self.ets(start, end).hessiane(q, Je=Je, tool=self._resolve_tool(tool))
 
     def partial_fkine0(
         self: KinematicsProtocol,
@@ -506,7 +521,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).jacob0_analytical(
-            q, tool=tool, representation=representation
+            q, tool=self._resolve_tool(tool), representation=representation
         )
 
     # --------------------------------------------------------------------- #
@@ -526,6 +541,7 @@ class RobotKinematicsMixin:
         joint_limits: bool = True,
         k: float = 1.0,
         method: L["chan", "wampler", "sugihara"] = "chan",
+        tool: NDArray | SE3 | None = None,
     ) -> IKSolution:
         r"""
         Fast Levenberg-Marquardt Numerical Inverse Kinematics Solver
@@ -544,6 +560,9 @@ class RobotKinematicsMixin:
         :param k: Sets the gain value for the damping matrix Wn in the next iteration
         :param method: One of "chan", "sugihara" or "wampler". Defines which method is used
             to calculate the damping matrix Wn in the ``step`` method
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
         :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
             ``iterations``, ``searches`` and ``residual`` error value (``reason`` is
             always empty -- this fast C++ solver doesn't produce a granular failure
@@ -661,7 +680,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ik_LM(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,
@@ -685,6 +704,7 @@ class RobotKinematicsMixin:
         joint_limits: bool = True,
         pinv: int = True,
         pinv_damping: float = 0.0,
+        tool: NDArray | SE3 | None = None,
     ) -> IKSolution:
         r"""
         Fast numerical inverse kinematics using Newton-Raphson optimization
@@ -705,6 +725,9 @@ class RobotKinematicsMixin:
             another search up to the slimit)
         :param pinv: Use the pseudo-inverse instead of the normal matrix inverse
         :param pinv_damping: Damping factor for the pseudo-inverse
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
         :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
             ``iterations``, ``searches`` and ``residual`` error value (``reason`` is
             always empty -- this fast C++ solver doesn't produce a granular failure
@@ -782,7 +805,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ik_NR(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,
@@ -806,6 +829,7 @@ class RobotKinematicsMixin:
         joint_limits: bool = True,
         pinv: int = True,
         pinv_damping: float = 0.0,
+        tool: NDArray | SE3 | None = None,
     ) -> IKSolution:
         r"""
         Fast numerical inverse kinematics by Gauss-Newton optimization
@@ -826,6 +850,9 @@ class RobotKinematicsMixin:
             another search up to the slimit)
         :param pinv: Use the pseudo-inverse instead of the normal matrix inverse
         :param pinv_damping: Damping factor for the pseudo-inverse
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
         :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
             ``iterations``, ``searches`` and ``residual`` error value (``reason`` is
             always empty -- this fast C++ solver doesn't produce a granular failure
@@ -918,7 +945,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ik_GN(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,
@@ -947,6 +974,7 @@ class RobotKinematicsMixin:
         km: float = 0.0,
         ps: float = 0.0,
         pi: NDArray | float = 0.3,
+        tool: NDArray | SE3 | None = None,
         **kwargs,
     ):
         r"""
@@ -976,6 +1004,9 @@ class RobotKinematicsMixin:
             allowed to approach to its limit
         :param pi: The influence angle/distance (in radians or metres) in null space motion
             becomes active
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
         :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
             ``iterations``, ``searches``, ``residual`` error value, and ``reason``
             string if applicable
@@ -1096,7 +1127,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ikine_LM(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,
@@ -1130,6 +1161,7 @@ class RobotKinematicsMixin:
         km: float = 0.0,
         ps: float = 0.0,
         pi: NDArray | float = 0.3,
+        tool: NDArray | SE3 | None = None,
         **kwargs,
     ):
         r"""
@@ -1158,6 +1190,9 @@ class RobotKinematicsMixin:
             allowed to approach to its limit
         :param pi: The influence angle/distance (in radians or metres) in null space motion
             becomes active
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
 
         A method which provides functionality to perform numerical inverse kinematics (IK)
         using the Newton-Raphson method.
@@ -1222,7 +1257,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ikine_NR(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,
@@ -1255,6 +1290,7 @@ class RobotKinematicsMixin:
         km: float = 0.0,
         ps: float = 0.0,
         pi: NDArray | float = 0.3,
+        tool: NDArray | SE3 | None = None,
         **kwargs,
     ):
         r"""
@@ -1283,6 +1319,9 @@ class RobotKinematicsMixin:
             allowed to approach to its limit
         :param pi: The influence angle/distance (in radians or metres) in null space motion
             becomes active
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
 
         A method which provides functionality to perform numerical inverse kinematics (IK)
         using the Gauss-Newton method.
@@ -1362,7 +1401,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ikine_GN(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,
@@ -1396,6 +1435,7 @@ class RobotKinematicsMixin:
         km: float = 0.0,
         ps: float = 0.0,
         pi: NDArray | float = 0.3,
+        tool: NDArray | SE3 | None = None,
         **kwargs,
     ):
         r"""
@@ -1424,6 +1464,9 @@ class RobotKinematicsMixin:
             allowed to approach to its limit
         :param pi: The influence angle/distance (in radians or metres) in null space motion
             becomes active
+        :param tool: a static tool transformation matrix to apply to the
+            end of ``end``; defaults to the robot's own ``self.tool`` if
+            not given
         :raises ImportError: If the package ``qpsolvers`` is not installed
         :returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
             ``iterations``, ``searches``, ``residual`` error value, and ``reason``
@@ -1541,7 +1584,7 @@ class RobotKinematicsMixin:
         """
 
         return self.ets(start, end).ikine_QP(
-            Tep=Tep,
+            Tep=SE3(Tep, check=False) * self._resolve_tool(tool).inv(),
             q0=q0,
             ilimit=ilimit,
             slimit=slimit,

{roboticstoolbox_python-1.4.3.dist-info → roboticstoolbox_python-1.4.4.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.2
 Name: roboticstoolbox-python
-Version: 1.4.3
+Version: 1.4.4
 Summary: A Python library for robotics education and research
 Keywords: python,robotics,robotics-toolbox,kinematics,dynamics,motion-planning,trajectory-generation,jacobian,hessian,control,simulation,robot-manipulator,mobile-robot
 Author-Email: Jesse Haviland <j.haviland@qut.edu.au>, Peter Corke <rvc@petercorke.com>
@@ -39,7 +39,7 @@ Project-URL: documentation, https://petercorke.github.io/robotics-toolbox-python
 Project-URL: repository, https://github.com/petercorke/robotics-toolbox-python
 Requires-Python: >=3.10
 Requires-Dist: numpy
-Requires-Dist: spatialmath-python>=1.1.16
+Requires-Dist: spatialmath-python>=1.1.18
 Requires-Dist: spatialgeometry>=1.4.0
 Requires-Dist: pgraph-python
 Requires-Dist: scipy
@@ -543,44 +543,4 @@ Graphical visualisation via Swift is currently not supported under Windows. Howe
 
 The toolbox is incredibly useful for developing and prototyping algorithms for research, thanks to the exhaustive set of well documented and mature robotic functions exposed through clean and painless APIs. Additionally, the ease at which a user can visualize their algorithm supports a rapid prototyping paradigm.
 
-### Publication List
-
-J. Haviland, N. Sünderhauf and P. Corke, "**A Holistic Approach to Reactive Mobile Manipulation**," in _IEEE Robotics and Automation Letters_, doi: 10.1109/LRA.2022.3146554. In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
-
-[[Arxiv Paper](https://arxiv.org/abs/2109.04749)] [[IEEE Xplore](https://ieeexplore.ieee.org/abstract/document/9695298)] [[Project Website](https://jhavl.github.io/holistic/)] [[Video](https://youtu.be/-DXBQPeLIV4)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/main/roboticstoolbox/examples/holistic_mm_non_holonomic.py)]
-
-<p>
-  <a href="https://youtu.be/-DXBQPeLIV4">
-    <img src="https://raw.githubusercontent.com/petercorke/robotics-toolbox-python/main/docs/figs/holistic_youtube.png" width="560">
-  </a>
-</p>
-
-J. Haviland and P. Corke, "**NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators**," in _IEEE Robotics and Automation Letters_, doi: 10.1109/LRA.2021.3056060. In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
-
-[[Arxiv Paper](https://arxiv.org/abs/2010.08686)] [[IEEE Xplore](https://ieeexplore.ieee.org/document/9343718)] [[Project Website](https://jhavl.github.io/neo/)] [[Video](https://youtu.be/jSLPJBr8QTY)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/main/roboticstoolbox/examples/neo.py)]
-
-<p>
-  <a href="https://youtu.be/jSLPJBr8QTY">
-    <img src="https://raw.githubusercontent.com/petercorke/robotics-toolbox-python/main/docs/figs/neo_youtube.png" width="560">
-  </a>
-</p>
-
-K. He, R. Newbury, T. Tran, J. Haviland, B. Burgess-Limerick, D. Kulić, P. Corke, A. Cosgun, "**Visibility Maximization Controller for Robotic Manipulation**", in _IEEE Robotics and Automation Letters_, doi: 10.1109/LRA.2022.3188430. In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
-
-[[Arxiv Paper](https://arxiv.org/abs/2202.12557)] [[IEEE Xplore](https://ieeexplore.ieee.org/abstract/document/9815144)] [[Project Website](https://rhys-newbury.github.io/projects/vmc/)] [[Video](https://youtu.be/vobLvg4E3kM)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/main/roboticstoolbox/examples/fetch_vision.py)]
-
-<p>
-  <a href="https://youtu.be/vobLvg4E3kM">
-    <img src="https://raw.githubusercontent.com/petercorke/robotics-toolbox-python/main/docs/figs/vmc_youtube.png" width="560">
-  </a>
-</p>
-
-**A Purely-Reactive Manipulability-Maximising Motion Controller**, J. Haviland and P. Corke. In the video, the robot is controlled using the Robotics toolbox for Python.
-
-[[Paper](https://arxiv.org/abs/2002.11901)] [[Project Website](https://jhavl.github.io/mmc/)] [[Video](https://youtu.be/Vu_rcPlaADI)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/main/roboticstoolbox/examples/mmc.py)]
-
-<p>
-  <a href="https://youtu.be/Vu_rcPlaADI">
-    <img src="https://raw.githubusercontent.com/petercorke/robotics-toolbox-python/main/docs/figs/mmc_youtube.png" width="560">
-  </a>
-</p>
+It's been the implementation substrate for several published reactive-control papers, and its kinematics are a direct implementation of a two-part differential-kinematics tutorial — see the wiki's [Reactive control](https://github.com/petercorke/robotics-toolbox-python/wiki/Reactive-control) and [Inverse kinematics](https://github.com/petercorke/robotics-toolbox-python/wiki/Inverse-kinematics) pages.

{roboticstoolbox_python-1.4.3.dist-info → roboticstoolbox_python-1.4.4.dist-info}/RECORD

@@ -30,7 +30,7 @@ roboticstoolbox/backends/__init__.py,sha256=WXkKfxqjgu_LdyyvZT0qmYT5bZ9XtRJQvEn_
 roboticstoolbox/backends/swift/__init__.py,sha256=uenayFsFBt1k3565I8rxjMHgyW3mrFhLXimRF5LYPn0,7077
 roboticstoolbox/bin/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 roboticstoolbox/bin/_bintools.py,sha256=ofTQ47u8sixdJGIxMzcxHsBRtWyenSVGhCqUKV8rF5c,1903
-roboticstoolbox/bin/rtbtool.py,sha256=YIO-d9oycB2LRlHL4uaZM_swHSK9zUNkSKlCMdaceb0,12207
+roboticstoolbox/bin/rtbtool.py,sha256=9OEfJ_dRCg0jMimP3O4TdIPsi5TjepB7Pr_9mYvgcdg,12206
 roboticstoolbox/blocks/Icons/250x250/armplot.png,sha256=3bZtXa_ZaLRtzEXZSooLEBp3EPFmEIaVIUQCT3bY8-8,5918
 roboticstoolbox/blocks/Icons/250x250/bicycle.png,sha256=BEQkGUY81Bms4gvdBechZzbgZClTSOq9YqX03-XuVZk,8995
 roboticstoolbox/blocks/Icons/250x250/camera.png,sha256=CGcTB_llMoucN-rfnP6ZP_7AfE_92z6JnCdam-hDLgs,1642
@@ -468,7 +468,7 @@ roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/MatrixCwiseUnaryOps.h,sha25
 roboticstoolbox/ets/cpp-extensions/Eigen/src/plugins/ReshapedMethods.h,sha256=OYyO_yoZ3TL7X8GFrIW-IKzqs_1LbM0M96HJyZAVOi8,6915
 roboticstoolbox/ets/cpp-extensions/README.md,sha256=xLvpxXx8xbe-wElyzpODFOxairojzFyGgeeCjXYQyMQ,3666
 roboticstoolbox/ets/cpp-extensions/fknm_nb.cpp,sha256=Zent7Jp9gn-Cg6Y2nZnrgaaMwi-WBrZ9tAuXE1ybz4A,22027
-roboticstoolbox/ets/cpp-extensions/ik.cpp,sha256=guMsgJ6xYhFP9oG_8FdP8Woxp4xjRPPuIE9T94VMad4,10974
+roboticstoolbox/ets/cpp-extensions/ik.cpp,sha256=6B4cDKiVc_5WKGba-bTE_ngB7_jU9gyQXa3tGGZEku0,13132
 roboticstoolbox/ets/cpp-extensions/ik.h,sha256=OkgTgJ4tV8P47elnQCOnM76n8ru0PpkZPCQFmUn7fWY,1780
 roboticstoolbox/ets/cpp-extensions/linalg.cpp,sha256=D8c4GmITlK_2ardbeUys6SyuoTKqSYrSke0144caFpc,8905
 roboticstoolbox/ets/cpp-extensions/linalg.h,sha256=YeuiXgnFGHjmgv3GMN4q_veR3fLdnoEa00LLJk6Opos,1862
@@ -520,7 +520,7 @@ roboticstoolbox/models/DH/README.md,sha256=h8F7jSpP-6v0u4Ika5YEYluZg6qxTXQVsO6c8
 roboticstoolbox/models/DH/Sawyer.py,sha256=WUeAeQeJ7-Kw6rvJAcAIr0IwhNeFl_SPnpOdlB3xKVM,2084
 roboticstoolbox/models/DH/Stanford.py,sha256=fOLeK6oo6Zmr9-93Wd2yZJLIkDqMqm2WG6HNUHEGUtU,3908
 roboticstoolbox/models/DH/TwoLink.py,sha256=EavoSjY8waes_WehZ5TVs3EDfD2N1kMwgoJtuuPiHd4,6077
-roboticstoolbox/models/DH/UR10.py,sha256=-zzC5_LDSta9yJtX4Fc9vjEAIkFk5t8mt5JQpeaIrrs,3362
+roboticstoolbox/models/DH/UR10.py,sha256=mj-smTlQox9nIqTOqXec0gXPFjvNlMn4dMbF8Ip_XPQ,3736
 roboticstoolbox/models/DH/UR3.py,sha256=ZHq5ayfCdU2HW2yV8Yist8BC8PKR1woKWRka3pO6e34,2483
 roboticstoolbox/models/DH/UR5.py,sha256=yYxp5JABLTbQjwiDQksBeyNmjzJ5uH8_IEOqwPawpls,2534
 roboticstoolbox/models/DH/Uprighttl.py,sha256=4bTrWW7uFTyoJCb4n9ivy1V8SCPCo0XLByo_Wni6-iw,571
@@ -536,21 +536,21 @@ roboticstoolbox/models/ETS/XYPanda.py,sha256=62_oLmwss-incP-NkaDdvKF7ee0gsbNsH7k
 roboticstoolbox/models/ETS/__init__.py,sha256=GYPivBP0GXaX8nODS-3lRoppTISH4PEcXnKUNasDXB8,583
 roboticstoolbox/models/README.md,sha256=2NHhybtNIE6rIViAw6ElKUd44zPM8umpM8pB4FpKUpU,1312
 roboticstoolbox/models/URDF/AL5D.py,sha256=He4bM47R-nyO15jv0fYOtvKQV0nPcsubH9UKamt2A3Y,1192
-roboticstoolbox/models/URDF/Fetch.py,sha256=ga6BVYVVLzwVxgo_2DspLXOvySDWxilcPGCJO978sFQ,3034
-roboticstoolbox/models/URDF/Frankie.py,sha256=ht5jKMH18I9u-uK2TKSHzgh9Ko6lb0MT_909VNLIVFw,1710
-roboticstoolbox/models/URDF/FrankieOmni.py,sha256=JN_eXZDlhxc7T3zWPiegB6kmaO68LTwn1Kj-51Yd0L4,2540
-roboticstoolbox/models/URDF/Jaco.py,sha256=LWqEV9lKzqxud5X2osJx1wOu7x_WTi1VKyOAKycZQ6E,1285
+roboticstoolbox/models/URDF/Fetch.py,sha256=XZfV7wfIvy--3vTNqIq33TUdbLkFIznEjUzJJ0vaCsY,3164
+roboticstoolbox/models/URDF/Frankie.py,sha256=FvZXXdT2-6VsnphkduiisDr4cmk_pZFq7w3s6Eqi-0s,2132
+roboticstoolbox/models/URDF/FrankieOmni.py,sha256=D2TJSbxWD6LpW3_bh7uCtIFSBrE_kJqcHJdpC1NS2rc,2998
+roboticstoolbox/models/URDF/Jaco.py,sha256=vwK88sHrMp6Q6gqHgDlbn4TiXfrwrD8jvEmgfVnb4V4,1415
 roboticstoolbox/models/URDF/KinovaGen3.py,sha256=fsmxZX7kooNMqp_tUGGXJgh86GOG2OT3_shOhRIdaSQ,1580
 roboticstoolbox/models/URDF/LBR.py,sha256=uzP-7FzCpF8mIhSiT8HoKuYeISOkYS-NkmTjmUJgQmA,1806
-roboticstoolbox/models/URDF/PR2.py,sha256=kTDa2IjkBobUsKw4dX7f8TmxInex8h5GqwAnaSVUnGY,1482
-roboticstoolbox/models/URDF/Panda.py,sha256=ZF5PPfPaAbZEVW3onR2ntaT6jgW1InAO8GdSaGmG9rM,1549
+roboticstoolbox/models/URDF/PR2.py,sha256=ZEpFuOWXQW33l68EEP3QxnKs7l9lP0vHIvUJeVFDmc4,1612
+roboticstoolbox/models/URDF/Panda.py,sha256=ws8YtoudZ88L9F6O2S7oioK3w_V4RzztMMLHqHAnJcU,2752
 roboticstoolbox/models/URDF/Puma560.py,sha256=1WHxYNXkYgJ6TJ9xJpJzfkZlfHZA2N9HFS6_p1b171w,2881
-roboticstoolbox/models/URDF/UR10.py,sha256=OIw8-f3diIytx9M4FKofNOlduEDRiYTVQZdvZjY5Zo4,1073
-roboticstoolbox/models/URDF/UR3.py,sha256=T0pd-KtNQYoqmONkfMaqWKZSA6WS3Wq0JzS3NIL1lvg,1066
-roboticstoolbox/models/URDF/UR5.py,sha256=DDLzthyJ0fyylxijTZ14X_KZIYsCgLO1S6Pk5vecQZ0,1671
-roboticstoolbox/models/URDF/URDFRobot.py,sha256=cEyhWSozFhvQwJsYZ8hhGjkByzChGjHWXahTjiK6a7k,15441
-roboticstoolbox/models/URDF/Valkyrie.py,sha256=7LLgVRXFZ_NM5lksNW7jx6jugSDEJ4uylqqqfwkbPP0,4133
-roboticstoolbox/models/URDF/YuMi.py,sha256=lg6d827b4cr0tazyFnds2i4LuPGI9bw2d4QWRntAJ7A,3245
+roboticstoolbox/models/URDF/UR10.py,sha256=Excq3j1nuaqWGw67UPmAlWoXICUMsngbY1NU1Uc4MA8,1203
+roboticstoolbox/models/URDF/UR3.py,sha256=FIQTrAlqUrY8qeCkbNc2hpQPnxnv19cz_nlC1EM0JGg,1196
+roboticstoolbox/models/URDF/UR5.py,sha256=vCM373teYtNMcQXyWLuqtNqCNVoEJBgbnwa-BSeo-44,2221
+roboticstoolbox/models/URDF/URDFRobot.py,sha256=x6ItlP5oZBLXo0IZAijDyrZwZ-LiFHfFnRs5S7TLNLY,16632
+roboticstoolbox/models/URDF/Valkyrie.py,sha256=E75Sl1fG9lM-iCtxy6mo8OymW7M8qnXkc-uJSWMmvew,4263
+roboticstoolbox/models/URDF/YuMi.py,sha256=BrkpWMsOB9jFVeNVaw8MX_vAVYEBIOUrNI7gKbBMGC4,3375
 roboticstoolbox/models/URDF/__init__.py,sha256=G4PNTfYRiYekdxssVHN8v3zPUWPhbcOrffJR9u6lWDo,1599
 roboticstoolbox/models/URDF/px100.py,sha256=sEU-keutJh_KP30zQfV-n51UZoaLCD1SQgw2vf31Ipk,1240
 roboticstoolbox/models/URDF/px150.py,sha256=NzGJ0pV9pLeS5mtaMvtLJpkcwvwYcSYpQcuqPTVxdqI,1243
@@ -566,16 +566,16 @@ roboticstoolbox/models/catalog.py,sha256=R-Lc_vHoOfae-afVS1olmoPu9cq7YTDfYqtCJeM
 roboticstoolbox/robot/BaseRobot.py,sha256=OcUR9yMLDvUUTrD34FnoufoZOdbU2Lc-pRxn08g3OCk,82370
 roboticstoolbox/robot/DHFactor.py,sha256=Bf-j7IvBKRA6_k3uev_ceQRMAC79D7W0Ir7kCJt4mec,15981
 roboticstoolbox/robot/DHLink.py,sha256=_7FtGDuS_dVxFGUQx9-Vqj7EoLsMIhV9SwVDFpWBPWs,27815
-roboticstoolbox/robot/DHRobot.py,sha256=aqi-EmoC9ixwjax8ORi1REKx54E_XLu_Hrb1QSq8wdM,67041
+roboticstoolbox/robot/DHRobot.py,sha256=UTWuE7Fl2shooddEKQn2BQ6w5VHfNwClz2ljKYy093A,67041
 roboticstoolbox/robot/Dynamics.py,sha256=iCplz4L1wT2OeXuS0acO9x8h4uxsifT67PV4TViUwZw,52479
 roboticstoolbox/robot/ELink.py,sha256=sgA-1c-U0Gz6TxABSJvfauq2L0Wu_1_pGJVKfLgrGP8,525
 roboticstoolbox/robot/ERobot.py,sha256=aUnSg91PwHyIHO6ipWtnNBNLYCftKGIxgYz1qX8wghg,567
 roboticstoolbox/robot/Gripper.py,sha256=CYyXFBYbEp52-RX-b6xGWS3EH-Y2BsYdtaR8Q0yAm3s,6954
-roboticstoolbox/robot/IK.py,sha256=7CzondLtnZdETFJPC4MczdDz46Ar3n83OvJnMBauV6I,48515
+roboticstoolbox/robot/IK.py,sha256=8s_YwcU22IxcMwke9yHX-ssv5HvdtHf_2vcuknhimBg,50270
 roboticstoolbox/robot/Link.py,sha256=m98nfpv61C5Z1GDrOV_iN6gxyh9qj8URwiju9ssKXxw,46283
 roboticstoolbox/robot/PoERobot.py,sha256=mByMjb3P5FrW3Z8zyLS5waYcKBy8lpHG44g2uBUyB70,11053
-roboticstoolbox/robot/Robot.py,sha256=-5SsVItMPKAd56lxPQrXlHHbdpMn-GMKGCUy73LeELw,78647
-roboticstoolbox/robot/RobotKinematics.py,sha256=Mq4MfE3hOuJaEdWD26s9xVlIOuXyiGACSd-iDuChLHg,61869
+roboticstoolbox/robot/Robot.py,sha256=rkzRa9LTrTBIuSI0dbT1SjI44YdX5VS-1OuPm2W1Q6U,82555
+roboticstoolbox/robot/RobotKinematics.py,sha256=0K-bdowzzzPUnImSVAxRylucSAyl-dyfhP7_4hpcvrs,64448
 roboticstoolbox/robot/RobotPlottingMPL.py,sha256=tmIGievDZwtLkd0To0Of2MeAB38yvbUd8T0bF0qwLd8,14207
 roboticstoolbox/robot/RobotProto.py,sha256=os5bJSXp4ZO-ANJjaSF8_Xb-TJxbCg1Ry_obS3A6LqM,4339
 roboticstoolbox/robot/__init__.py,sha256=RUeji1GTnAL3U3mn0ae6W1lnmvC-CFlUw5U7RTQnvO8,1237
@@ -604,8 +604,8 @@ roboticstoolbox/tools/urdf/tests/data/ur5.urdf,sha256=9g0uYkRHRsWHWl6vn5lIK_JMHq
 roboticstoolbox/tools/urdf/tests/test_urdf.py,sha256=MoRP8DolFu7RXxrKkwEWJOSfVygX7UJwEv-hLZkdlKw,3445
 roboticstoolbox/tools/urdf/urdf.py,sha256=IE5EUSNjkOAZZUOkuhXKDhYRq2AuJGulIvfbsRHy7pI,58743
 roboticstoolbox/tools/urdf/utils.py,sha256=6g1uuI2u7CUaWhKavI7wDHT3a2T3ijb92JByQ1lNq6w,1483
-roboticstoolbox_python-1.4.3.dist-info/METADATA,sha256=5GYwYgEhenEQBNgrQtYgHuCprlkow1AIfxbxq_xNczE,27162
-roboticstoolbox_python-1.4.3.dist-info/WHEEL,sha256=DJBbB-IMWy7eyPvSdm5L2t-BbrULtweJWf0YC-blkl0,95
-roboticstoolbox_python-1.4.3.dist-info/entry_points.txt,sha256=m9xEI6rJsYQL0HosyeS-4TUPzxOR3kqx4vbJVVxwo7k,214
-roboticstoolbox_python-1.4.3.dist-info/licenses/LICENSE,sha256=oXNGHTPcMKl8PPZWk_4NIWGAtiaQcTgVJ1hpnTcnKFA,1062
-roboticstoolbox_python-1.4.3.dist-info/RECORD,,
+roboticstoolbox_python-1.4.4.dist-info/METADATA,sha256=VEV1p5AVn9XeJpI3VhDpvo9PYFA9mKufl8XwcQwYu1Y,24258
+roboticstoolbox_python-1.4.4.dist-info/WHEEL,sha256=DJBbB-IMWy7eyPvSdm5L2t-BbrULtweJWf0YC-blkl0,95
+roboticstoolbox_python-1.4.4.dist-info/entry_points.txt,sha256=m9xEI6rJsYQL0HosyeS-4TUPzxOR3kqx4vbJVVxwo7k,214
+roboticstoolbox_python-1.4.4.dist-info/licenses/LICENSE,sha256=oXNGHTPcMKl8PPZWk_4NIWGAtiaQcTgVJ1hpnTcnKFA,1062
+roboticstoolbox_python-1.4.4.dist-info/RECORD,,