roboticstoolbox-python 1.4.2__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

@@ -293,43 +293,25 @@ class IKSolver(ABC):
         linalg_error = 0
 
         # Initialise variables
-        E = 0.0
+        E = np.inf
         q = q0[0]
 
         for search in range(self.slimit):
             q = q0[search].copy()
             i = 0
 
-            while i < self.ilimit:
-                i += 1
-
-                # step() reports E for q as it was *before* this iteration's
-                # update. An undamped update (GN/NR) can overshoot, so if E is
-                # already below tol we must return this pre-step q, not the
-                # mutated one step() hands back - otherwise we can report
-                # success with a q whose actual residual is far above tol.
-                q_prev = q.copy()
-
-                # Attempt a step
-                try:
-                    E, q[ets.jindices] = self.step(ets, Tep, q)
-
-                except np.linalg.LinAlgError:
-                    # Abandon search and try again
-                    linalg_error += 1
-                    break
+            # Check the initial configuration before attempting an update.
+            # An exact or already-converged q0 must not enter a solver step,
+            # which can be singular even though the requested pose is solved.
+            _, E = self.error(ets.eval(q), Tep)
 
-                # Check if we have arrived
+            while True:
+                # Check convergence for the current q before another update.
                 if E < self.tol:
-                    q = q_prev
-
-                    # 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
@@ -344,6 +326,21 @@ class IKSolver(ABC):
                             residual=E,
                             reason="Success",
                         )
+
+                if i >= self.ilimit:
+                    break
+
+                i += 1
+
+                # Attempt a step. step() reports E for the updated q.
+                try:
+                    E, q[ets.jindices] = self.step(ets, Tep, q)
+
+                except np.linalg.LinAlgError:
+                    # Abandon search and try again
+                    linalg_error += 1
+                    break
+
             total_i += i
 
         # If we make it here, then we have failed
@@ -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
@@ -709,6 +746,7 @@ class IK_NR(IKSolver):
         else:
             q[ets.jindices] += np.linalg.inv(J) @ e + qnull
 
+        _, E = self.error(ets.eval(q), Tep)
         return E, q[ets.jindices]
 
 
@@ -941,6 +979,7 @@ class IK_LM(IKSolver):
 
         q[ets.jindices] += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull
 
+        _, E = self.error(ets.eval(q), Tep)
         return E, q[ets.jindices]
 
 
@@ -1121,6 +1160,7 @@ class IK_GN(IKSolver):
         else:
             q[ets.jindices] += np.linalg.inv(J) @ e + qnull
 
+        _, E = self.error(ets.eval(q), Tep)
         return E, q[ets.jindices]
 
 
@@ -1388,6 +1428,7 @@ class IK_QP(IKSolver):
 
         q += xd[: ets.n]
 
+        _, E = self.error(ets.eval(q), Tep)
         return E, q