From 648d920434d355fbe90629b9b21f8dfcc2069bcb Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 22 Oct 2026 09:32:16 +1000 Subject: [PATCH 1/2] feat(dynamics): payload_capacity() replaces pay() and paycap() payload_capacity(q, tau_m, w, frame="base") is the multiple of a wrench w that each joint can support, for each configuration in q, for joint torque limits tau_m: a positive number, inf if the joint has no part in it, and nan if gravity alone is beyond the limit. The wrench is in the base frame, or "ee" for the end-effector frame. Limits are (n,) or (1,n) maxima, or (2,n) minima then maxima. pay() is removed, it is -J.T @ w and was a poor name, and paycap() is removed, which returned a tuple when its docstring said otherwise and was skipped in its test. Both were ported from the MATLAB functions of Bryan Moutrie, credited in the docstring. Co-Authored-By: Claude Sonnet 5.5 --- docs/source/arm.rst | 1 + src/roboticstoolbox/robot/Dynamics.py | 293 +++++++++++------------- src/roboticstoolbox/robot/RobotProto.py | 8 - tests/test_DHRobot.py | 67 ------ tests/test_paycap_capacity.py | 87 ------- tests/test_payload_capacity.py | 191 +++++++++++++++ 6 files changed, 330 insertions(+), 317 deletions(-) delete mode 100644 tests/test_paycap_capacity.py create mode 100644 tests/test_payload_capacity.py diff --git a/docs/source/arm.rst b/docs/source/arm.rst index 0024d882c..29b402211 100644 --- a/docs/source/arm.rst +++ b/docs/source/arm.rst @@ -134,6 +134,7 @@ The graphical :doc:`backends ` do the drawing. ~roboticstoolbox.robot.Robot.Robot.friction ~roboticstoolbox.robot.Robot.Robot.nofriction ~roboticstoolbox.robot.Robot.Robot.payload + ~roboticstoolbox.robot.Robot.Robot.payload_capacity ~roboticstoolbox.robot.Robot.Robot.perturb .. rubric:: Collision checking diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index 1e0b5b887..fe9edc1ba 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -2,7 +2,7 @@ Rigid-body dynamics functionality of the Toolbox. ``DynamicsMixin`` holds *derived* dynamics quantities -- ``accel``, -``gravload``, ``coriolis``, ``inertia``, ``itorque``, ``pay``, etc. -- +``gravload``, ``coriolis``, ``inertia``, ``itorque``, ``payload_capacity``, etc. -- implemented generically, in terms of a small set of primitives (``rne``, ``jacob0``, ...) declared abstractly by ``RobotProto``. This mixin never touches representation-specific internals (DH parameters, ETS chains, the @@ -36,7 +36,7 @@ import numpy as np from ansitable import ANSITable, Column from scipy import integrate, interpolate -from spatialmath.base import getmatrix, getvector, isscalar, verifymatrix +from spatialmath.base import getmatrix, getvector, isscalar import roboticstoolbox as rtb from roboticstoolbox.robot.RobotProto import RobotProto @@ -510,108 +510,149 @@ def accel(self: RobotProto, q: Any, qd: Any, torque: Any, gravity: Any = None) - else: return qdd - def pay( + def payload_capacity( self: RobotProto, - W: ArrayLike, - q: NDArray | None = None, - J: NDArray | None = None, - frame: int = 1, - ) -> Any: + q: ArrayLike, + tau_m: ArrayLike, + w: ArrayLike, + frame: L["base", "ee"] = "base", + ) -> NDArray: """ - Generalised joint force/torque due to a payload wrench - - :param W: A wrench vector applied at the end effector, - W = [Fx Fy Fz Mx My Mz] - :type W: ndarray(6,) - :param q: Joint coordinates - :type q: ndarray(n,) - :param J: The manipulator Jacobian (Optional, if not supplied will - use the q value). - :type J: ndarray(6,n) - :param frame: The frame in which to torques are expressed in when J - is not supplied. 0 means base frame of the robot, 1 means end- - effector frame - :returns: Joint forces/torques due to W - :rtype: ndarray(n,) - - tau = pay(W, J) Returns the generalised joint force/torques due to a - payload wrench W applied to the end-effector. Where the manipulator - Jacobian is J (6xn), and n is the number of robot joints. - - tau = pay(W, q, frame) as above but the Jacobian is calculated at pose - q in the frame given by frame which is 0 for base frame, 1 for - end-effector frame. - - Uses the formula tau = J'W, where W is a wrench vector applied at the - end effector, W = [Fx Fy Fz Mx My Mz]'. - - Trajectory operation: - In the case q is nxm or J is 6xnxm then tau is nxm where each row - is the generalised force/torque at the pose given by corresponding - row of q. - - .. rubric:: Notes - - - Wrench vector and Jacobian must be from the same reference - frame. - - Tool transforms are taken into consideration when frame=1. - - Must have a constant wrench - no trajectory support for this - yet. - + Compute the payload capacity of the manipulator + + :param q: Joint coordinates for trajectory + :type q: ndarray(m,n) where `m` is the number of trajectory points and `n` is + the number of robot joints + :param tau_m: actuator torque limits + :type tau_m: ndarray(n,), ndarray(1,n), ndarray(2,n) where `n` is the number of + robot joints + :param w: External wrench applied to the end-effector + :type w: ArrayLike(6) + :param frame: the frame in which ``w`` is expressed, ``"base"`` for the base + frame (default) and ``"ee"`` for the end-effector frame, which includes + the tool transform + :returns: Payload capacity of the manipulator + :rtype: ndarray(m,n) + + For each configuration, row of ``q``, compute the multiple of the applied external + wrench ``w`` that can be applied to the end-effector without exceeding the + actuator torque limits ``tau_m``. If ``w`` is the weight of a 1 kg mass then + the multiple is the capacity in kg. A load such as a weight is fixed in the + world, so ``w`` is in the base frame by default; use ``frame="ee"`` for a wrench + that is fixed to the end-effector, such as a force along the tool axis. This + multiple is computed for each joint and can be + + * a positive real number + * ``inf`` the joint has no role in determining the payload capacity + * ``nan`` the gravity load already exceeds the joint torque limit + + The joint with the lowest payload capacity is the one that determines the + overall payload capacity for that configuration. + + The result is a matrix of payload capacities for each joint and configuration. + + The joint force/torque limits ``tau_m`` are given as an array of shape (n,) or + (1,n) of maximum values, in which case the minimum is the negative of the + maximum, or (2,n), in which case the first row is the minimum and the second + row is the maximum. + + .. rubric:: Example + + In this example we compute the payload capacity of the Puma560 robot along a + straight line trajectory in the x-direction, moving from tightly folded to + almost straight. The end-effector is oriented with the tool frame pointing in + the x-direction, and a tool offset is added so that the load causes a moment on + the wrist. The external wrench is a downward force of 9.81 N, which is + equivalent to a 1 kg mass. The torque limits are the motor fuse limits from + Table 2.17 of "Visual Control of Robots", Corke (1996). The payload capacity, + as well as the limiting joint, is plotted as a function of the x-position of the + end-effector. The minimum payload capacity is also plotted, along with the + index of the joint that determines the payload capacity. + + .. code:: + + >>> from roboticstoolbox import models + >>> import matplotlib.pyplot as plt + >>> import numpy as np + >>> from spatialmath import SE3 + >>> puma = models.DH.Puma560() + >>> puma.tool = SE3.Tz(0.05) + >>> T = SE3.Tx(np.arange(0.35, 0.85, 0.01)) * SE3.Tz(puma.links[0].d) * SE3.Ry(90, "deg") + >>> sol = puma.ikine_a(T) + >>> w = [0,0,-9.81,0,0,0] + >>> cap = puma.payload_capacity(sol.q, [56, 97, 52,10,10,10], w) + >>> plt.plot(T.x, [np.min(c) for c in cap]) + >>> plt.plot(T.x, [np.argmin(c) for c in cap]) + + .. plot:: + + from roboticstoolbox import models + import matplotlib.pyplot as plt + import numpy as np + from spatialmath import SE3 + puma = models.DH.Puma560() + puma.tool = SE3.Tz(0.05) + T = SE3.Tx(np.arange(0.35, 0.85, 0.01)) * SE3.Tz(puma.links[0].d) * SE3.Ry(90, "deg") + sol = puma.ikine_a(T) + w = [0,0,-9.81,0,0,0] + cap = puma.payload_capacity(sol.q, [56, 97, 52,10,10,10], w) + fig, (ax1, ax2) = plt.subplots(2, 1) + ax1.plot(T.x, [np.min(c) for c in cap]) + ax1.set_ylabel("payload capacity (kg)") + ax1.grid(True) + ax2.plot(T.x, [np.argmin(c) for c in cap]) + ax2.set_xlabel("x-position of end-effector (m)") + ax2.set_ylabel("index of limiting joint") + ax2.grid(True) + + Derived from the MATLAB functions ``pay`` and ``paycap`` originally contributed + to the MATLAB Robotics Toolbox by Bryan Moutrie (@au-phiware). Ported to Python + and adapted for ``robotics-toolbox-python`` with permission from the original + author. + + .. versionadded:: 1.5.0 + + :seealso: :meth:`gravload` :meth:`payload` """ - - try: - W = np.array(getvector(W, 6)) - trajn = 0 - except ValueError: - if isinstance(W, NDArray): - trajn = W.shape[0] - verifymatrix(W, (trajn, 6)) - else: - raise ValueError("W is invalid") - - if trajn: - # A trajectory - if J is not None: - # Jacobian supplied - verifymatrix(J, (trajn, 6, self.n)) - elif q is not None: - # Use q instead - verifymatrix(q, (trajn, self.n)) - J = np.zeros((trajn, 6, self.n)) - for i in range(trajn): - if frame: - J[i, :, :] = self.jacobe(q[i, :]) - else: - J[i, :, :] = self.jacob0(q[i, :]) - else: - raise ValueError("q of J is needed for trajectory") + # each of τ_min, τ_max are (n,) arrays of joint torque limits + tau_m = np.asarray(getmatrix(tau_m, (None, self.n))) + if tau_m.shape[0] == 1: + τ_max = tau_m.squeeze() + τ_min = -τ_max else: - # Single configuration - if J is not None: - # Jacobian supplied - verifymatrix(J, (6, self.n)) - else: - # Use q instead - if q is None: - q = np.copy(self.q) - else: - q = getvector(q, self.n) + τ_min = tau_m[0].squeeze() + τ_max = tau_m[1].squeeze() - if frame: - J = self.jacobe(q) - else: - J = self.jacob0(q) + w = np.asarray(getvector(w, 6)) + q = np.asarray(getmatrix(q, (None, self.n))) - if trajn == 0: - tau = -J.T @ W + # the Jacobian that is in the frame of the wrench + if frame == "base": + jacobian = self.jacob0 + elif frame == "ee": + jacobian = self.jacobe else: - tau = np.zeros((trajn, self.n)) + raise ValueError(f"frame is {frame!r}, choose from: 'base', 'ee'") - for i in range(trajn): - tau[i, :] = -J[i, :, :].T @ W[i, :] + # The motor torque that holds the robot and a payload s * w is + # τ = τ_g + s τ_p + # where τ_g is the gravity load, and τ_p is the torque due to w, which is + # -Jᵀw since w is the wrench that the payload applies to the robot. + limits = [] + for _q in q: + τ_g = self.gravload(_q) + τ_p = -jacobian(_q).T @ w - return tau + # Pick the appropriate bound based on sign of τ_p + bound = np.where(τ_p > 0, τ_max, τ_min) + + with np.errstate(divide='ignore', invalid='ignore'): + s = np.where(abs(τ_p) > 1e-10, (bound - τ_g) / τ_p, np.inf) + # Overwrite out-of-bounds positions with NaN + s[(τ_g > τ_max) | (τ_g < τ_min)] = np.nan + limits.append(s) + + return np.array(limits) def payload(self: RobotProto, m: float, p: Any = np.zeros(3)) -> None: """ @@ -909,6 +950,8 @@ def gravload( ``robot.gravload(q, gravity=g)`` as above except the gravitational acceleration is explicitly specified as ``g``. + The return value is referenced to the load, not the motor, the gear ratio is not taken into account. + **Trajectory operation** If q is a matrix (nxm) each column is interpreted as a joint @@ -1430,66 +1473,6 @@ def itorque(self: RobotProto, q: Any, qdd: Any) -> Any: else: return taui - def paycap( - self: RobotProto, - w: NDArray, - tauR: NDArray, - frame: int = 1, - q: ArrayLike | None = None, - ) -> Any: - """Compute static payload capacity along a wrench direction. - - :param w: Nonzero payload wrench direction (6,), or (m, 6) for - m configurations. Its magnitude is ignored. - :param tauR: Joint torque limits (n, 2): maximum in column 0, - minimum in column 1. - :param frame: Wrench reference frame: 0 for base, 1 for end-effector. - :param q: Joint coordinates (n,), or (m, n) for m configurations. - Defaults to the robot's current configuration. - :returns: Tuple ``(wmax, joint)``. ``wmax`` contains one allowable - load scale per joint (n,), and ``joint`` is the zero-based index - of the limiting joint. With m configurations, their shapes are - (m, n) and (m,), respectively. The maximum load scale in the - specified direction is ``wmax.min()`` per configuration. - - .. rubric:: Notes - - - Wrench and Jacobian must use the same reference frame. - - Tool transforms are taken into consideration for ``frame=1``. - - Gravity compensation requires a robot model with valid dynamics. - """ - q = np.asarray(getmatrix(self.q if q is None else q, (None, self.n))) - w = np.asarray(getmatrix(w, (None, 6))) - if q.shape[0] != w.shape[0]: - raise ValueError("q and w must have the same number of rows") - verifymatrix(tauR, (self.n, 2)) - - w_norm = np.linalg.norm(w, axis=1) - if np.any(w_norm == 0): - raise ValueError("w must contain nonzero wrench directions") - - trajn = q.shape[0] - wmax = np.zeros((trajn, self.n)) - joint = np.zeros(trajn, dtype=int) - - for i in range(trajn): - tauB = self.gravload(q[i, :]) - tauP = self.pay(w[i, :] / w_norm[i], q=q[i, :], frame=frame) - - positive = tauP > 0 - negative = tauP < 0 - WM = np.full(self.n, np.inf) - WM[positive] = (tauR[positive, 0] - tauB[positive]) / tauP[positive] - WM[negative] = (tauR[negative, 1] - tauB[negative]) / tauP[negative] - WM[WM == -np.inf] = np.inf - - wmax[i, :] = WM - joint[i] = np.argmin(WM) - - if trajn == 1: - return wmax[0, :], joint[0] - return wmax, joint - def perturb(self: RobotProto, p: float = 0.1) -> Any: """ Perturb robot parameters diff --git a/src/roboticstoolbox/robot/RobotProto.py b/src/roboticstoolbox/robot/RobotProto.py index 6db5bdad1..4759e1ee8 100644 --- a/src/roboticstoolbox/robot/RobotProto.py +++ b/src/roboticstoolbox/robot/RobotProto.py @@ -149,14 +149,6 @@ def accel( gravity: ArrayLike | None = None, ) -> NDArray: ... - def pay( - self, - W: ArrayLike, - q: NDArray | None = None, - J: NDArray | None = None, - frame: int = 1, - ) -> NDArray: ... - def nofriction(self, coulomb: bool = True, viscous: bool = False) -> Self: ... def copy(self) -> Self: ... diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index b2c010184..74884e1d6 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -694,49 +694,6 @@ def test_nofriction(self): nt.assert_array_almost_equal(n2.links[i].Tc, L[i].Tc) @unittest.skip("payload needs fixing") - def test_pay(self): - panda = rp.models.DH.Panda() - q = [1, 2, 3, 4, 5, 6, 7] - - w = [1, 2, 3, 4, 5, 6] - - wT = np.c_[w, w, w, w].T - qT = np.c_[q, q, q, q].T - - tau = np.array([6.0241, -4.4972, -7.2160, -4.2400, 7.0215, -4.6884, -6.0000]) - - tau0 = np.array([-5.9498, 1.4604, -3.4544, 1.5026, -3.7777, -6.6578, 2.6047]) - - tauT = np.c_[tau, tau, tau, tau].T - tau0T = np.c_[tau0, tau0, tau0, tau0].T - - Je = panda.jacobe(q) - J0 = panda.jacob0(q) - - JeT = np.zeros((4, 6, 7)) - for i in range(4): - JeT[i, :, :] = Je - - panda.pay(w) - - nt.assert_array_almost_equal(panda.pay(w), tau, decimal=4) - nt.assert_array_almost_equal(panda.pay(w, frame=0), tau0, decimal=4) - - nt.assert_array_almost_equal(panda.pay(w, q=q), tau, decimal=4) - nt.assert_array_almost_equal(panda.pay(wT, q=qT), tauT, decimal=4) - nt.assert_array_almost_equal(panda.pay(wT, q=qT, frame=0), tau0T, decimal=4) - - nt.assert_array_almost_equal(panda.pay(w, J=Je), tau, decimal=4) - nt.assert_array_almost_equal(panda.pay(w, J=J0), tau0, decimal=4) - - nt.assert_array_almost_equal(panda.pay(wT, J=JeT), tauT, decimal=4) - - with self.assertRaises(ValueError): - panda.pay(wT, q) - - with self.assertRaises(TypeError): - panda.pay(wT) - def test_friction(self): l0 = rp.RevoluteDH(d=2, B=3, G=2, Tc=[2, -1]) qd = [1, 2, 3, 4] @@ -1488,30 +1445,6 @@ def test_itorque(self): # self.assertEqual(str(puma), res) - # def test_paycap(self): - # self.skipTest("error introduced with DHLink change") - # puma = rp.models.DH.Puma560() - # puma.q = puma.qn - # q = puma.qn - - # w = [1, 2, 1, 2, 1, 2] - # tauR = np.ones((6, 2)) - # tauR[:, 1] = -1 - - # res0 = [ - # 1.15865438e+00, -3.04790052e+02, -5.00870095e+01, 6.00479950e+15, - # 3.76356072e+00, 1.93649167e+00] - - # wmax0, joint = puma.paycap(w, tauR, q=q, frame=0) - # wmax1, _ = puma.paycap(np.c_[w, w], tauR, q=np.c_[q, q], frame=0) - # wmax2, _ = puma.paycap(w, tauR, frame=0) - - # nt.assert_allclose(wmax0, res0) - # self.assertEqual(joint, 1) - # nt.assert_allclose(wmax1[:, 0], res0) - # nt.assert_allclose(wmax1[:, 1], res0) - # nt.assert_allclose(wmax2, res0) - def test_jacob_dot(self): puma = rp.models.DH.Puma560() puma.q = puma.qr diff --git a/tests/test_paycap_capacity.py b/tests/test_paycap_capacity.py deleted file mode 100644 index 6035f9e52..000000000 --- a/tests/test_paycap_capacity.py +++ /dev/null @@ -1,87 +0,0 @@ -"""Regression tests for payload capacity with arbitrary joint counts.""" - -import numpy as np -import numpy.testing as npt -import pytest - -from roboticstoolbox.robot.Dynamics import DynamicsMixin - - -class _DummyRobot: - paycap = DynamicsMixin.paycap - - def __init__(self, n): - self.n = n - self.q = np.zeros(n) - self.calls = [] - - def gravload(self, q): - return np.zeros(self.n) - - def pay(self, w, q, frame): - self.calls.append((np.copy(w), np.copy(q), frame)) - return np.arange(1, self.n + 1) * w[0] - - -def _limits(n): - return np.column_stack((np.full(n, 20.0), np.full(n, -10.0))) - - -@pytest.mark.parametrize("n", [3, 6, 7]) -def test_paycap_joint_count(n): - robot = _DummyRobot(n) - capacity, limiting_joint = robot.paycap( - [2.0, 0, 0, 0, 0, 0], _limits(n), frame=0 - ) - npt.assert_allclose(capacity, 20.0 / np.arange(1, n + 1)) - npt.assert_equal(capacity.shape, (n,)) - npt.assert_equal(limiting_joint, n - 1) - npt.assert_allclose(robot.calls[0][0], [1, 0, 0, 0, 0, 0]) - npt.assert_equal(robot.calls[0][2], 0) - - -def test_paycap_negative_wrench_uses_minimum_torque_limit(): - robot = _DummyRobot(7) - capacity, limiting_joint = robot.paycap( - [-10.0, 0, 0, 0, 0, 0], _limits(7) - ) - npt.assert_allclose(capacity, 10.0 / np.arange(1, 8)) - npt.assert_equal(limiting_joint, 6) - - -def test_paycap_trajectory_uses_each_wrench_direction(): - robot = _DummyRobot(7) - q = np.stack([np.zeros(7), np.ones(7)]) - wrench = np.array([[2.0, 0, 0, 0, 0, 0], [-10.0, 0, 0, 0, 0, 0]]) - capacities, joints = robot.paycap(wrench, _limits(7), q=q) - npt.assert_equal(capacities.shape, (2, 7)) - npt.assert_allclose(capacities[0], 20.0 / np.arange(1, 8)) - npt.assert_allclose(capacities[1], 10.0 / np.arange(1, 8)) - npt.assert_array_equal(joints, [6, 6]) - npt.assert_allclose([call[1] for call in robot.calls], q) - - -def test_paycap_trajectory_row_mismatch(): - robot = _DummyRobot(7) - with pytest.raises(ValueError, match="same number of rows"): - robot.paycap(np.ones((1, 6)), _limits(7), q=np.zeros((2, 7))) - - -def test_paycap_rejects_zero_wrench_direction(): - robot = _DummyRobot(7) - with pytest.raises(ValueError, match="nonzero wrench"): - robot.paycap(np.zeros(6), _limits(7)) - - -def test_paycap_joint_unaffected_by_wrench_is_unbounded(): - robot = _DummyRobot(3) - robot.pay = lambda w, q, frame: np.array([1.0, 0.0, 2.0]) - limits = _limits(3) - limits[1, 1] = 0.0 - - with np.errstate(divide="raise", invalid="raise"): - capacities, joint = robot.paycap([1.0, 0, 0, 0, 0, 0], limits) - - npt.assert_allclose(capacities[[0, 2]], [20.0, 10.0]) - npt.assert_equal(np.isinf(capacities[1]), True) - npt.assert_equal(joint, 2) diff --git a/tests/test_payload_capacity.py b/tests/test_payload_capacity.py new file mode 100644 index 000000000..52fb68bcb --- /dev/null +++ b/tests/test_payload_capacity.py @@ -0,0 +1,191 @@ +""" +Payload capacity: the largest multiple of a wrench that the joint torque limits allow. + +The reference is physics that does not use the method: a real mass on the last link +changes the gravity load, computed by the recursive Newton-Euler dynamics, and the +payload capacity must be the mass at which a joint reaches its torque limit. +""" + +import copy + +import numpy as np +import numpy.testing as nt +import pytest +from spatialmath import SE3 + +import roboticstoolbox as rtb + +G = 9.81 +W_DOWN = np.array([0, 0, -G, 0, 0, 0]) # the weight of a 1 kg mass, applied to the robot + + +@pytest.fixture(scope="module", params=["puma", "panda"]) +def robot(request): + return rtb.models.DH.Puma560() if request.param == "puma" else rtb.models.DH.Panda() + + +def with_mass(robot, m): + """A copy of the robot with a real mass at the tool point.""" + r = copy.deepcopy(robot) + r.payload(m, robot.tool.t) + return r + + +def within(tau, tau_min, tau_max, tol=1e-9): + return bool(np.all(tau <= tau_max + tol) and np.all(tau >= tau_min - tol)) + + +def limits(n, rng, symmetric): + tau_max = rng.uniform(20, 80, n) + tau_min = -tau_max if symmetric else -rng.uniform(5, 30, n) + return tau_min, tau_max + + +class TestAgainstRealMass: + @pytest.mark.parametrize("symmetric", [True, False]) + def test_the_capacity_is_the_mass_that_reaches_a_limit(self, robot, symmetric): + rng = np.random.default_rng(1) + checked = 0 + for _ in range(30): + q = robot.random_q() + tau_min, tau_max = limits(robot.n, rng, symmetric) + cap = robot.payload_capacity(q, np.vstack([tau_min, tau_max]), W_DOWN)[0] + if np.any(np.isnan(cap)): + continue # gravity alone exceeds a limit + s = np.min(cap) # kg, since W_DOWN is the weight of 1 kg + assert within(with_mass(robot, 0.999 * s).gravload(q), tau_min, tau_max) + assert not within(with_mass(robot, 1.01 * s).gravload(q), tau_min, tau_max) + checked += 1 + assert checked > 10 + + def test_the_torque_that_a_mass_adds_is_minus_J_transpose_w(self, robot): + # the sign that the capacity relies on + for _ in range(5): + q = robot.random_q() + added = with_mass(robot, 1.0).gravload(q) - robot.gravload(q) + nt.assert_allclose(added, -robot.jacob0(q).T @ W_DOWN, atol=1e-9) + + def test_asymmetric_limits_are_used_the_right_way_round(self): + # one joint, gravity holds it at +5; a downward payload pushes the torque on up + puma = rtb.models.DH.Puma560() + q = puma.qn + tau_g = puma.gravload(q) + tau_p = -puma.jacob0(q).T @ W_DOWN + j = int(np.argmax(np.abs(tau_p))) + tau_max = np.full(6, 1e3) + tau_min = np.full(6, -1e3) + tau_max[j] = tau_g[j] + 2.0 * abs(tau_p[j]) if tau_p[j] > 0 else 1e3 + tau_min[j] = tau_g[j] - 2.0 * abs(tau_p[j]) if tau_p[j] < 0 else -1e3 + cap = puma.payload_capacity(q, np.vstack([tau_min, tau_max]), W_DOWN)[0] + assert cap[j] == pytest.approx(2.0) + + +class TestResult: + def test_values(self): + puma = rtb.models.DH.Puma560() + q = puma.qn + cap = puma.payload_capacity(q, [56, 97, 52, 10, 10, 10], W_DOWN) + assert cap.shape == (1, 6) + tau_g = puma.gravload(q) + tau_p = -puma.jacob0(q).T @ W_DOWN + tau_max = np.array([56, 97, 52, 10, 10, 10.0]) + bound = np.where(tau_p > 0, tau_max, -tau_max) + expect = np.where(np.abs(tau_p) > 1e-10, (bound - tau_g) / tau_p, np.inf) + nt.assert_allclose(cap[0], expect) + + def test_a_joint_that_the_wrench_does_not_load_is_inf(self): + puma = rtb.models.DH.Puma560() + # a force along z and a moment about x never loads joint 1 (rotation about z) at qz + cap = puma.payload_capacity(puma.qz, [56, 97, 52, 10, 10, 10], [0, 0, -G, 0, 0, 0]) + assert np.isinf(cap[0, 0]) + + def test_gravity_alone_beyond_a_limit_is_nan(self): + puma = rtb.models.DH.Puma560() + q = puma.qn + tau_g = puma.gravload(q) + j = int(np.argmax(np.abs(tau_g))) + tau_max = np.full(6, 1e3) + tau_max[j] = abs(tau_g[j]) / 2 + cap = puma.payload_capacity(q, tau_max, W_DOWN) + assert np.isnan(cap[0, j]) + assert not np.any(np.isnan(np.delete(cap[0], j))) + + def test_a_trajectory(self): + puma = rtb.models.DH.Puma560() + qt = np.array([puma.qn, puma.qr, puma.qs]) + cap = puma.payload_capacity(qt, [56, 97, 52, 10, 10, 10], W_DOWN) + assert cap.shape == (3, 6) + for k in range(3): + nt.assert_allclose( + cap[k], puma.payload_capacity(qt[k], [56, 97, 52, 10, 10, 10], W_DOWN)[0] + ) + + def test_a_single_configuration_may_be_a_vector(self): + puma = rtb.models.DH.Puma560() + cap = puma.payload_capacity(puma.qn, [56, 97, 52, 10, 10, 10], W_DOWN) + assert cap.shape == (1, 6) + + def test_the_forms_of_the_limits(self): + puma = rtb.models.DH.Puma560() + tmax = np.array([56, 97, 52, 10, 10, 10.0]) + q = puma.qn + a = puma.payload_capacity(q, tmax, W_DOWN) + b = puma.payload_capacity(q, tmax.reshape(1, 6), W_DOWN) + c = puma.payload_capacity(q, np.vstack([-tmax, tmax]), W_DOWN) + nt.assert_allclose(a, b) + nt.assert_allclose(a, c) + # a smaller minimum changes the joints that the payload loads towards it + tau_p = -puma.jacob0(q).T @ W_DOWN + d = puma.payload_capacity(q, np.vstack([-0.5 * tmax, tmax]), W_DOWN) + towards_max = tau_p > 0 + nt.assert_allclose(d[0, towards_max], a[0, towards_max]) + assert np.all(d[0, ~towards_max] <= a[0, ~towards_max] + 1e-12) or np.any( + np.isnan(d[0, ~towards_max]) + ) + + def test_wrong_shapes(self): + puma = rtb.models.DH.Puma560() + with pytest.raises(ValueError): + puma.payload_capacity(puma.qn, [1, 2, 3], W_DOWN) # 3 limits for 6 joints + with pytest.raises(ValueError): + puma.payload_capacity(puma.qn, [56, 97, 52, 10, 10, 10], [0, 0, 1]) + with pytest.raises(ValueError): + puma.payload_capacity(np.zeros((2, 5)), [56, 97, 52, 10, 10, 10], W_DOWN) + + +class TestFrame: + def test_the_default_frame_is_the_base(self): + puma = rtb.models.DH.Puma560() + tmax = [56, 97, 52, 10, 10, 10] + nt.assert_allclose( + puma.payload_capacity(puma.qn, tmax, W_DOWN), + puma.payload_capacity(puma.qn, tmax, W_DOWN, frame="base"), + ) + + def test_a_wrench_in_the_end_effector_frame_is_the_same_wrench_in_the_base(self): + puma = rtb.models.DH.Puma560() + puma.tool = SE3.Tz(0.05) + tmax = [56, 97, 52, 10, 10, 10] + q = puma.qn + w_e = np.array([1.0, -2.0, 3.0, 0.1, -0.2, 0.3]) # in the end-effector frame + R = puma.fkine(q).R + w_0 = np.r_[R @ w_e[:3], R @ w_e[3:]] # the same wrench in the base frame + nt.assert_allclose( + puma.payload_capacity(q, tmax, w_e, frame="ee"), + puma.payload_capacity(q, tmax, w_0, frame="base"), + rtol=1e-9, + equal_nan=True, + ) + + def test_the_end_effector_frame_makes_a_difference(self): + puma = rtb.models.DH.Puma560() + tmax = [56, 97, 52, 10, 10, 10] + base = puma.payload_capacity(puma.qn, tmax, W_DOWN, frame="base") + ee = puma.payload_capacity(puma.qn, tmax, W_DOWN, frame="ee") + assert not np.allclose(base, ee, equal_nan=True) + + @pytest.mark.parametrize("frame", [0, 1, 2, "world", "end", "tool", "", None]) + def test_a_bad_frame(self, frame): + puma = rtb.models.DH.Puma560() + with pytest.raises(ValueError, match="frame"): + puma.payload_capacity(puma.qn, [56, 97, 52, 10, 10, 10], W_DOWN, frame=frame) From 062da34c2962965bc5b407f24581f1a72c1bae58 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 22 Oct 2026 09:45:37 +1000 Subject: [PATCH 2/2] docs(dynamics): Examples heading of payload_capacity is plural, as everywhere else Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/robot/Dynamics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index fe9edc1ba..e816131f8 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -556,7 +556,7 @@ def payload_capacity( maximum, or (2,n), in which case the first row is the minimum and the second row is the maximum. - .. rubric:: Example + .. rubric:: Examples In this example we compute the payload capacity of the Puma560 robot along a straight line trajectory in the x-direction, moving from tightly folded to