Skip to content

Fix division by zero in arm paths with repeated configurations - #402

Open
llm-learner wants to merge 2 commits into
stepjam:masterfrom
llm-learner:fix/zero-length-arm-path
Open

llm-learner wants to merge 2 commits into
stepjam:masterfrom
llm-learner:fix/zero-length-arm-path

Conversation

@llm-learner

@llm-learner llm-learner commented Sep 26, 2026 •

Copy link
Copy Markdown

Problem

Repeated joint configurations give adjacent path points the same cumulative length. Both interpolation sites in ArmConfigurationPath divide by that zero-length span. The final-segment fallback can also select a duplicate segment when Reflexxes slightly overshoots, producing non-finite joint targets.

This addresses the failure reported in #383. That earlier report already suggested guarding the division; this change covers both interpolation sites and fully stationary paths.

Change

  • Use the segment start when its length is zero in both interpolators.
  • Complete a non-empty stationary path without creating a Reflexxes trajectory, while applying and exposing its final joint target.
  • Preserve path indexing and the existing formula for positive-length segments; no epsilon threshold or waypoint removal.

Validation

  • Four new regression tests cover stationary and single-point paths, duplicate leading/interior/trailing segments, final-segment overshoot, and normal/tiny positive displacements. All pass with the patch. The duplicate-segment cases reproduce the original numeric errors with NumPy errors enabled.
  • All 21 existing tests in tests.test_arms_and_configuration_paths and tests.test_grippers pass in CoppeliaSim 4.1.
  • Real before/after replay: place_cups, TRAIN episode 0, variation 1, held object mug1. Each run independently resets and replays the same 14-action prefix. The restored state, requested release action and first native planned path match exactly. That 50-point path has total joint-space length about 1.91e-6 and repeated points, including a zero-length final segment.
Original Patched
Seven NaN joint targets; release not reached Finite targets; release completes
Still holding mug1 Grasp list empty

The diagnostic guard stops the original run before NaN targets reach the simulator. The video uses real front/wrist renders with the stopped side frozen and non-real-time playback labelled. No policy inference is involved. This demonstrates the numerical fix, not completion of the whole cup-placement task (reward remains 0).

中文说明

路径中重复的关节配置会产生零长度片段,原版两处插值可能除零;Reflexxes 轻微越过终点时,末段兜底也可能命中零长度片段。本修改保护两处插值,并直接完成非空静止路径。对比视频中,原版产生 7 个 NaN 关节目标,松爪未执行;补丁版目标有限、松爪完成。这里只验证数值故障修复,不代表整个放杯任务成功。

Before / after video

Bilingual comparison / 中英双语对比(19.2 s)

pyrep_zero_length_before_after_zh_en.mp4

Download MP4 / 下载视频

Original vs patched: real front and wrist renders

The video is attached above, with a backup on a separate evidence branch. This PR contains only the code fix and regression tests.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant