diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index dd6ca6950..bb6f95db4 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -1347,6 +1347,8 @@ def accel_x( q = getmatrix(q, (None, self.n)) xd = getmatrix(xd, (None, 6)) w = getmatrix(wrench, (None, 6)) + if q.shape[0] != xd.shape[0] or q.shape[0] != w.shape[0]: + raise ValueError("q, xd and wrench must have the same number of rows") if q.shape[1] != 6: pinv = True diff --git a/tests/test_accel_x_row_counts.py b/tests/test_accel_x_row_counts.py new file mode 100644 index 000000000..0ed511a90 --- /dev/null +++ b/tests/test_accel_x_row_counts.py @@ -0,0 +1,53 @@ +"""Operational acceleration requires aligned trajectory rows.""" + +import numpy as np +import numpy.testing as npt +import pytest + +from roboticstoolbox.models.DH import Panda, Puma560 + + +@pytest.mark.parametrize("robot_type", [Puma560, Panda]) +@pytest.mark.parametrize( + "rows", [(3, 1, 3), (3, 3, 1), (1, 3, 3), (3, 2, 3), (3, 3, 2), (2, 3, 3)] +) +def test_mismatched_trajectories_raise( + robot_type: type[Puma560] | type[Panda], rows: tuple[int, int, int] +) -> None: + robot = robot_type() + pose = robot.qn if isinstance(robot, Puma560) else robot.qr + q = np.tile(pose, (rows[0], 1)) + xd = np.full((rows[1], 6), 0.01) + wrench = np.full((rows[2], 6), 0.2) + with pytest.raises(ValueError, match="same number of rows"): + robot.accel_x(q, xd, wrench) + + +@pytest.mark.parametrize("robot_type", [Puma560, Panda]) +@pytest.mark.parametrize("count", [1, 3]) +def test_matching_trajectories_match_individual_calls( + robot_type: type[Puma560] | type[Panda], count: int +) -> None: + robot = robot_type() + pose = robot.qn if isinstance(robot, Puma560) else robot.qr + q = np.tile(pose, (count, 1)) + np.arange(count)[:, None] * 0.01 + xd = np.arange(count * 6).reshape(count, 6) * 0.001 + wrench = np.arange(count * 6).reshape(count, 6) * 0.01 + saved = [value.copy() for value in (q, xd, wrench)] + expected = np.stack( + [robot.accel_x(qk, vk, wk) for qk, vk, wk in zip(q, xd, wrench)] + ) + actual = robot.accel_x(q, xd, wrench) + assert actual.shape == ((6,) if count == 1 else (count, 6)) + npt.assert_allclose( + actual, expected[0] if count == 1 else expected, atol=1e-10, rtol=1e-10 + ) + for old, current in zip(saved, (q, xd, wrench)): + npt.assert_array_equal(current, old) + + +@pytest.mark.parametrize("robot_type", [Puma560, Panda]) +def test_empty_matching_trajectories(robot_type: type[Puma560] | type[Panda]) -> None: + robot = robot_type() + result = robot.accel_x(np.empty((0, robot.n)), np.empty((0, 6)), np.empty((0, 6))) + assert result.shape == (0, 6)