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