Fix division by zero in arm paths with repeated configurations

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[

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()