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 7495859f7..e31362e63 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 @@ -37,7 +37,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 @@ -513,108 +513,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:: 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 + 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: ArrayLike | None = None) -> None: """ @@ -912,6 +953,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 @@ -1433,66 +1476,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 7b13a8d22..8a960f442 100644 --- a/src/roboticstoolbox/robot/RobotProto.py +++ b/src/roboticstoolbox/robot/RobotProto.py @@ -150,14 +150,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)