From 7edf0287d973022d008cec926a8df46b59a3b8e4 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 14 Oct 2026 13:47:58 +1000 Subject: [PATCH] fix(dynamics): paycap returns the capacity, a scalar, as the MATLAB method does The method comes from the MATLAB Robotics Toolbox, where it was contributed by Bryan Moutrie (pHRIWARE). There it ends with [wM, j] = min(WM, [], 2): the largest multiple of the unit payload wrench that no joint torque limit forbids, and the joint that limits it. The Python port kept the argmin but dropped the min, so it returned one value per joint, labelled as a six element wrench, which is why it only worked for six joint robots (#728, #731). - return the scalar capacity and the limiting joint, a vector over poses for a trajectory - treat a joint that the wrench does not load, to within rounding error, as unbounded rather than as a capacity of ~1e17 - document that the result is negative if the robot cannot support itself, and that w is the wrench the payload applies to the end-effector - say in pay() that its result is -J'W, the holding torque, which is what the code and the MATLAB code do; both docstrings had the wrong sign - credit Bryan Moutrie in paycap() and pay() - tests on a planar two link arm with results worked out by hand, replacing the commented out test, which could not have shown anything useful Builds on the shape handling in #731. Co-authored-by: huxiaoyi-ovo Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/robot/Dynamics.py | 91 ++++++++++++---- tests/test_DHRobot.py | 24 ----- tests/test_paycap_capacity.py | 150 ++++++++++++++++++++++---- 3 files changed, 199 insertions(+), 66 deletions(-) diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index e7b5b1707..64e254598 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/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): diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index b2c010184..086f0394f 100644 --- a/tests/test_DHRobot.py +++ b/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 diff --git a/tests/test_paycap_capacity.py b/tests/test_paycap_capacity.py index 6035f9e52..00bcc2268 100644 --- a/tests/test_paycap_capacity.py +++ b/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 @@ def _limits(n): @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 @@ def test_paycap_rejects_zero_wrench_direction(): 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