fix(dynamics): paycap returns the capacity, a scalar, as the MATLAB method does

src/roboticstoolbox/robot/Dynamics.py

@@ -522,8 +522,17 @@ def pay(
         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]'.
+        Uses the formula tau = -J'W, where W = [Fx Fy Fz Mx My Mz]' is the
+        wrench that the payload applies to the end-effector.  The result is the
+        torque that the joint actuators must provide to support the payload, so
+        it adds directly to the torque from :meth:`gravload`.  For example, a
+        weight of 1 N on a robot whose base z-axis is vertical is
+        ``W = [0, 0, -1, 0, 0, 0]``, which gives a positive torque on a joint
+        that has to hold it up.
+
+        .. note:: Based on the MATLAB Robotics Toolbox method of the same name,
+            contributed by Bryan Moutrie (pHRIWARE), who also wrote
+            :meth:`paycap`.
 
         Trajectory operation:
           In the case q is nxm or J is 6xnxm then tau is nxm where each row
@@ -1438,26 +1447,61 @@ def paycap(
         frame: int = 1,
         q: ArrayLike | None = None,
     ):
-        """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.
+        r"""
+        Static payload capacity of a robot
+
+        :param w: payload wrench direction :math:`(F_x, F_y, F_z, M_x, M_y, M_z)`,
+            or (m, 6) for m configurations.  This is the wrench that the payload
+            applies to the end-effector, only its direction is used.
+        :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: ``(wmax, joint)``: the payload capacity and the zero-based
+            index of the joint that reaches its limit first
+        :rtype: float, int, or ndarray(m), ndarray(m) for m configurations
+
+        The payload is the wrench :math:`\hat{w} = w / \|w\|` scaled by a
+        positive factor, and ``wmax`` is the largest factor for which no joint
+        exceeds its torque limit, given the torque needed to support the robot
+        against gravity at that pose.  It is a magnitude in the units of the
+        wrench: newtons if ``w`` is a pure force, newton metres if a pure
+        moment.  The payload wrench at the limit is ``wmax * w / norm(w)``.
+
+        Each joint has its own limit.  For joint :math:`j`, with gravity torque
+        :math:`\tau_{B,j}` and torque :math:`\tau_{P,j}` for the unit wrench
+
+        .. math::
+
+            s_j = \frac{\tau_{max,j} - \tau_{B,j}}{\tau_{P,j}} \quad
+            (\tau_{P,j} > 0), \qquad
+            s_j = \frac{\tau_{min,j} - \tau_{B,j}}{\tau_{P,j}} \quad
+            (\tau_{P,j} < 0)
+
+        and a joint that the wrench does not load, to within rounding error,
+        has :math:`s_j = \infty`.
+        ``wmax`` is the smallest :math:`s_j` and ``joint`` the index of it.
 
         .. rubric:: Notes
 
+        - ``wmax`` is negative if gravity alone is beyond a joint's limit at
+          this pose, ie. the robot cannot support itself, let alone a payload.
         - 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.
+
+        .. versionchanged:: 1.4.5
+            ``wmax`` is the capacity, a scalar for a configuration, as in the
+            MATLAB Robotics Toolbox.  It was documented as a wrench and
+            returned one value per joint, which only had the right shape for
+            a six-joint robot.
+
+        .. note:: Based on the MATLAB Robotics Toolbox method of the same name,
+            contributed by Bryan Moutrie (pHRIWARE), who also wrote
+            :meth:`pay`.
+
+        :seealso: :meth:`pay` :meth:`gravload`
         """
         q = getmatrix(self.q if q is None else q, (None, self.n))
         w = getmatrix(w, (None, 6))
@@ -1470,25 +1514,28 @@ def paycap(
             raise ValueError("w must contain nonzero wrench directions")
 
         trajn = q.shape[0]
-        wmax = np.zeros((trajn, self.n))
+        wmax = np.zeros(trajn)
         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
+            # a joint that the wrench does not load has a torque of zero, in
+            # floating point this is ~1e-17 not 0, which would otherwise give
+            # a huge finite capacity instead of an unbounded one
+            tol = 1e-10 * max(np.abs(tauP).max(), 1.0)
+            positive = tauP > tol
+            negative = tauP < -tol
             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)
+            wmax[i] = WM[joint[i]]
 
         if trajn == 1:
-            return wmax[0, :], joint[0]
+            return wmax[0], joint[0]
         return wmax, joint
 
     def perturb(self: RobotProto, p=0.1):

tests/test_DHRobot.py

@@ -1488,30 +1488,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

tests/test_paycap_capacity.py

@@ -1,11 +1,24 @@
-"""Regression tests for payload capacity with arbitrary joint counts."""
+"""Tests for payload capacity, ``paycap``, and the payload torque, ``pay``.
+
+The first group uses a stand-in robot to test the arithmetic and the shapes,
+for any number of joints.  The second uses a planar two link arm whose
+gravity and payload torques are easy to work out by hand:
+
+- both links 1 m long, the mass of each link is 1 kg at its midpoint
+- gravity is along -y, the arm is in the x-y plane
+- the weight of a payload is a force along -y, applied at the tip
+"""
 
 import numpy as np
 import numpy.testing as npt
 import pytest
 
+import roboticstoolbox as rtb
+from roboticstoolbox import DHRobot, RevoluteDH
 from roboticstoolbox.robot.Dynamics import DynamicsMixin
 
+G = 9.81
+
 
 class _DummyRobot:
     paycap = DynamicsMixin.paycap
@@ -30,33 +43,30 @@
 @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)
+    wmax, joint = robot.paycap([2.0, 0, 0, 0, 0, 0], _limits(n), frame=0)
+    # the capacity is a scalar: the smallest of 20 / (1 .. n), at joint n - 1
+    npt.assert_allclose(wmax, 20.0 / n)
+    npt.assert_equal(np.ndim(wmax), 0)
+    npt.assert_equal(joint, n - 1)
+    # only the direction of the wrench is used
     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)
+    wmax, joint = robot.paycap([-10.0, 0, 0, 0, 0, 0], _limits(7))
+    npt.assert_allclose(wmax, 10.0 / 7)
+    npt.assert_equal(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))
+    wmax, joints = robot.paycap(wrench, _limits(7), q=q)
+    npt.assert_equal(wmax.shape, (2,))
+    npt.assert_allclose(wmax, [20.0 / 7, 10.0 / 7])
     npt.assert_array_equal(joints, [6, 6])
     npt.assert_allclose([call[1] for call in robot.calls], q)
 
@@ -73,15 +83,115 @@
         robot.paycap(np.zeros(6), _limits(7))
 
 
-def test_paycap_joint_unaffected_by_wrench_is_unbounded():
+def test_paycap_joint_unaffected_by_wrench_does_not_limit():
     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)
+        wmax, 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)
+    # joints 0 and 2 give 20 and 10, joint 1 is not loaded so is unbounded
+    npt.assert_allclose(wmax, 10.0)
     npt.assert_equal(joint, 2)
+
+
+# ------------------------------------------------------------------------- #
+# a planar two link arm, results worked out by hand
+
+
+@pytest.fixture
+def arm():
+    links = [
+        RevoluteDH(a=1, m=1, r=[-0.5, 0, 0], I=np.zeros(3)),
+        RevoluteDH(a=1, m=1, r=[-0.5, 0, 0], I=np.zeros(3)),
+    ]
+    return DHRobot(links, gravity=[0, -G, 0], name="planar2")
+
+
+# limits in the order (maximum, minimum) for each joint
+LIMITS = np.array([[30.0, -30.0], [10.0, -10.0]])
+WEIGHT = [0, -1, 0, 0, 0, 0]  # a weight, force along -y
+
+
+def test_arm_gravity_torque(arm):
+    # stretched along x: link 1 at x=0.5 and link 2 at x=1.5 load joint 1,
+    # only link 2, 0.5 m beyond it, loads joint 2
+    npt.assert_allclose(arm.gravload([0, 0]), [G * 2.0, G * 0.5])
+
+
+def test_pay_is_the_torque_that_supports_the_payload(arm):
+    # a 1 N weight at the tip, 2 m and 1 m from the joints, needs [2, 1] N m
+    # of holding torque, the same sign as the torque to support gravity
+    npt.assert_allclose(arm.pay(WEIGHT, q=[0, 0], frame=0), [2.0, 1.0])
+    # and an upward force needs the opposite
+    npt.assert_allclose(arm.pay([0, 1, 0, 0, 0, 0], q=[0, 0], frame=0), [-2.0, -1.0])
+
+
+def test_paycap_planar_arm(arm):
+    # holding torque for the payload is [2, 1] per N, gravity takes [2g, 0.5g]
+    #   joint 1: (30 - 2g) / 2 = 5.19      joint 2: (10 - 0.5g) / 1 = 5.095
+    wmax, joint = arm.paycap(WEIGHT, LIMITS, q=[0, 0], frame=0)
+    npt.assert_allclose(wmax, 10 - 0.5 * G)
+    npt.assert_equal(joint, 1)
+
+
+def test_paycap_magnitude_of_wrench_is_ignored(arm):
+    wmax, _ = arm.paycap([0, -7, 0, 0, 0, 0], LIMITS, q=[0, 0], frame=0)
+    npt.assert_allclose(wmax, 10 - 0.5 * G)
+
+
+def test_paycap_upward_force_uses_the_minimum_limits(arm):
+    # tauP = [-2, -1]: (-30 - 2g) / -2 = 24.8   (-10 - 0.5g) / -1 = 14.9
+    wmax, joint = arm.paycap([0, 1, 0, 0, 0, 0], LIMITS, q=[0, 0], frame=0)
+    npt.assert_allclose(wmax, 10 + 0.5 * G)
+    npt.assert_equal(joint, 1)
+
+
+def test_paycap_is_negative_if_the_arm_cannot_support_itself(arm):
+    # joint 1 is limited to 10 N m but gravity alone needs 2g = 19.62
+    limits = np.array([[10.0, -10.0], [10.0, -10.0]])
+    wmax, joint = arm.paycap(WEIGHT, limits, q=[0, 0], frame=0)
+    npt.assert_allclose(wmax, (10 - 2 * G) / 2)
+    assert wmax < 0
+    npt.assert_equal(joint, 0)
+
+
+def test_paycap_in_either_frame_agrees(arm):
+    # elbow up: tip at (1, 1), the end-effector x axis is along base +y, so a
+    # weight along base -y is along -x of the end-effector frame.
+    # Hand: gravity [1.5g, 0], payload [1, 0], capacity (30 - 1.5g) / 1 at
+    # joint 1, and joint 2 is not loaded at all
+    q = [0, np.pi / 2]
+    w0, j0 = arm.paycap([0, -1, 0, 0, 0, 0], LIMITS, q=q, frame=0)
+    w1, j1 = arm.paycap([-1, 0, 0, 0, 0, 0], LIMITS, q=q, frame=1)
+    npt.assert_allclose(w0, 30 - 1.5 * G)
+    npt.assert_allclose(w1, w0)
+    npt.assert_equal((j0, j1), (0, 0))
+
+
+def test_paycap_payload_along_the_arm_is_unlimited(arm):
+    # pointing straight up, a force along the arm gives no joint torque and
+    # gravity gives none either
+    wmax, _ = arm.paycap(WEIGHT, LIMITS, q=[np.pi / 2, 0], frame=0)
+    assert np.isinf(wmax)
+
+
+def test_paycap_trajectory_planar_arm(arm):
+    q = np.array([[0.0, 0.0], [0.0, np.pi / 2]])
+    w = np.array([[0, -1, 0, 0, 0, 0], [0, -1, 0, 0, 0, 0]])
+    wmax, joint = arm.paycap(w, LIMITS, q=q, frame=0)
+    npt.assert_allclose(wmax, [10 - 0.5 * G, 30 - 1.5 * G])
+    npt.assert_array_equal(joint, [1, 0])
+
+
+def test_paycap_on_a_real_model():
+    # no hand value, but it must run on a six joint model with dynamics and
+    # give a sensible scalar and joint index
+    puma = rtb.models.DH.Puma560()
+    tauR = np.column_stack((np.full(6, 100.0), np.full(6, -100.0)))
+    wmax, joint = puma.paycap([0, 0, -1, 0, 0, 0], tauR, q=puma.qn, frame=0)
+    assert np.ndim(wmax) == 0
+    assert 0 <= joint < 6
+    assert wmax > 0