From 5652d670de0dce10ba779656100e4c7c4f692cce Mon Sep 17 00:00:00 2001 From: wenchao <1369404136@qq.com> Date: Sat, 26 Sep 2026 13:31:40 +0800 Subject: [PATCH 1/2] Handle zero-length segments in arm configuration paths --- .../configuration_paths/arm_configuration_path.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/pyrep/robots/configuration_paths/arm_configuration_path.py b/pyrep/robots/configuration_paths/arm_configuration_path.py index cbd0dc0..ba018a2 100644 --- a/pyrep/robots/configuration_paths/arm_configuration_path.py +++ b/pyrep/robots/configuration_paths/arm_configuration_path.py @@ -47,6 +47,13 @@ def step(self) -> bool: if self._path_done: raise RuntimeError('This path has already been completed. ' 'If you want to re-run, then call set_to_start.') + # A stationary path needs no trajectory generation (e.g. gripper-only + # actions). Still expose and apply the final joint target to callers. + if len(self) > 0 and self._get_path_point_lengths()[-1] == 0: + self._joint_position_action = self._path_points[-self._num_joints:].copy() + self._arm.set_joint_target_positions(self._joint_position_action) + self._path_done = True + return True if self._rml_handle is None: self._rml_handle = self._get_rml_handle() done = self._step_motion() == 1 @@ -131,7 +138,8 @@ def _get_rml_handle(self) -> int: pos = pos_vel_accel[0] for i in range(len(lengths)-1): if lengths[i] <= pos <= lengths[i + 1]: - t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i]) + span = lengths[i + 1] - lengths[i] + t = (pos - lengths[i]) / span if span > 0 else 0. # For each joint offset = len(self._arm.joints) * i p1 = self._path_points[ @@ -169,7 +177,8 @@ def _step_motion(self) -> int: for i in range(len(lengths) - 1): # Always execute the last point as pos might overshoot if lengths[i] <= pos <= lengths[i + 1] or i == len(lengths) - 2: - t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i]) + span = lengths[i + 1] - lengths[i] + t = (pos - lengths[i]) / span if span > 0 else 0. # For each joint offset = len(self._arm.joints) * i p1 = self._path_points[ From 2063f68a1a2ee6ac919dfe633b935e542797e514 Mon Sep 17 00:00:00 2001 From: wenchao <1369404136@qq.com> Date: Sat, 26 Sep 2026 13:33:37 +0800 Subject: [PATCH 2/2] Add regression tests for degenerate arm paths --- .../test_arm_configuration_path_degenerate.py | 85 +++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 tests/test_arm_configuration_path_degenerate.py diff --git a/tests/test_arm_configuration_path_degenerate.py b/tests/test_arm_configuration_path_degenerate.py new file mode 100644 index 0000000..99ebc2b --- /dev/null +++ b/tests/test_arm_configuration_path_degenerate.py @@ -0,0 +1,85 @@ +"""No simulator launch needed: exercise degenerate path interpolation.""" +import unittest +from unittest.mock import Mock, patch + +import numpy as np + +from pyrep.robots.configuration_paths.arm_configuration_path import ( + ArmConfigurationPath, sim) + + +class TestDegenerateArmConfigurationPath(unittest.TestCase): + + def make_path(self, points): + arm = Mock() + arm.joints = [Mock(), Mock()] + arm._num_joints = 2 + arm.get_joint_count.return_value = 2 + arm.get_joint_upper_velocity_limits.return_value = [100., 100.] + arm.max_velocity = arm.max_acceleration = arm.max_jerk = 1. + return ArmConfigurationPath(arm, np.asarray(points).flatten()) + + def test_stationary_path_completes_without_reflexxes(self): + for count in (1, 2, 50): + with self.subTest(points=count): + path = self.make_path([[.1, -.2]] * count) + with patch.object(sim, 'simRMLPos') as create: + with np.errstate(all='raise'): + self.assertTrue(path.step()) + create.assert_not_called() + self.assertEqual(len(path), count) + np.testing.assert_array_equal( + path.get_executed_joint_position_action(), [.1, -.2]) + np.testing.assert_array_equal( + path._arm.set_joint_target_positions.call_args[0][0], + [.1, -.2]) + with self.assertRaises(RuntimeError): + path.step() + path.set_to_start() + self.assertTrue(path.step()) + + def test_single_point_slice_can_be_stepped(self): + path = self.make_path([[0., 0.], [1., 0.]])[1] + with patch.object(sim, 'simRMLPos') as create: + self.assertTrue(path.step()) + create.assert_not_called() + np.testing.assert_array_equal( + path.get_executed_joint_position_action(), [1., 0.]) + + def test_duplicate_segments_are_finite_in_both_interpolators(self): + cases = [ + ([[0., 0.], [0., 0.], [1., 0.]], 0., [0., 0.]), + ([[0., 0.], [1., 0.], [1., 0.], [2., 0.]], 1., [1., 0.]), + ([[0., 0.], [1., 0.], [1., 0.]], 1., [1., 0.]), + # Reflexxes can slightly overshoot; the last segment may be zero. + ([[0., 0.], [1., 0.], [1., 0.]], 1.0001, [1., 0.]), + ] + for points, position, target in cases: + with self.subTest(points=points, position=position): + path = self.make_path(points) + with patch.object(sim, 'simGetSimulationTimeStep', return_value=.05), \ + patch.object(sim, 'simRMLPos', return_value=42), \ + patch.object(sim, 'simRMLStep', return_value=(1, [position, 0., 0.])), \ + patch.object(sim, 'simRMLRemove'): + with np.errstate(all='raise'): + self.assertEqual(path._get_rml_handle(), 42) + path._rml_handle = 42 + self.assertEqual(path._step_motion(), 1) + np.testing.assert_allclose( + path.get_executed_joint_position_action(), target) + + def test_nonzero_segment_interpolation_is_unchanged(self): + for endpoint in (1., 1e-12): + with self.subTest(endpoint=endpoint): + path = self.make_path([[0., 0.], [endpoint, 0.]]) + path._rml_handle = 42 + with patch.object(sim, 'simGetSimulationTimeStep', return_value=.05), \ + patch.object(sim, 'simRMLStep', return_value=(0, [endpoint / 2, 0., 0.])): + with np.errstate(all='raise'): + self.assertEqual(path._step_motion(), 0) + np.testing.assert_array_equal( + path.get_executed_joint_position_action(), [endpoint / 2, 0.]) + + +if __name__ == '__main__': + unittest.main()