Skip to content

feat: add SDK-local kinematics and guarded pose PTP - #48

Merged
yd-sl merged 3 commits into
nexform-tech:mainfrom
misaka-maples:feat/local-kinematics-safe-ptp
Nov 7, 2026
Merged

yd-sl merged 3 commits into
nexform-tech:mainfrom
misaka-maples:feat/local-kinematics-safe-ptp

Conversation

@misaka-maples

@misaka-maples misaka-maples commented Nov 6, 2026 •

Copy link
Copy Markdown
Contributor

Summary

  • add arm.kinematics.fk(), jacobian(), ik(), and move_pose() using Pinocchio and the packaged LiteArm URDF
  • keep Arm.ik() and Arm.get_tcp() as firmware-remote APIs, and keep litearm.exact_ik as the velocity-level differential IK API
  • share one URDF/Pinocchio model between local kinematics and self-collision checks; add minimum_distance(q)
  • package the URDF and collision meshes behind the optional robotics dependency group

Guarded pose PTP

arm.kinematics.move_pose() solves IK locally, then sends exactly one existing CMD_MOVE_J_SYNC (0x07) target. STM32 remains responsible for synchronized S-curve execution and joint velocity, acceleration, and jerk limits.

Before sending a motion frame it:

  • requires a fresh enabled, fault-free state outside zero-gravity and Cartesian planning
  • reads the current STM32 runtime soft limits
  • uses pose continuation (10 mm / 2 deg) and rejects IK branch jumps over 0.35 rad per segment
  • samples the actual joint line at no more than 0.02 rad
  • enforces a 0.01 rad soft-limit margin, self-collision rejection, 5 mm non-adjacent-link clearance, and sigma_min >= 0.05
  • replans once if the start changes by more than 0.03 rad, then fails closed on a second drift
  • fails before transmission on non-finite values, unavailable collision distance, or model errors

Positive motion APIs are serialized through one re-entrant lock, including home, move_js, send_mit, send_mit_all, and joint_follow. emergency_stop() and disable() deliberately bypass it.

Review follow-up

Addressed the hardware review in 18afde8:

  • completed positive-motion lock coverage and added blocking/bypass regression tests
  • changed the runnable example to poll fresh status frames until enable is confirmed, with fault and timeout handling instead of a fixed sleep
  • documented that IK residual is numerical convergence, while physical endpoint accuracy is governed by joint arrival tolerance, tracking, encoder, calibration, and firmware behavior; the example reports FK from the returned measured joint state
  • rebased onto current main (5643e47) without restoring the full test suite intentionally removed upstream

Hardware review already confirmed local FK agreement (about 7.6e-8 m position and 1.1e-7 rotation), singularity rejection, successful 2/5/10 mm guarded moves, and fail-closed behavior. Observed final TCP error was about 0.7-1.1 mm; this is recorded as informative test data, not a universal accuracy specification.

Compatibility

  • existing Arm.ik(), get_tcp(), movej(), movej_sync(), and move_p() signatures remain unchanged
  • the default serial install still depends only on pyserial; local robotics is installed with litearm-python[robotics]
  • no STM32 protocol or firmware changes are required
  • package version is bumped to 2.2.0

Verification

  • current-main smoke suite: 6 passed
  • motion serialization regression: 7 passed
  • isolated robotics suite: 43 passed, 5 subtests passed
  • SDK Pinocchio FK, rotation, and 6x7 Jacobian are checked against compiled STM32 kin.c at fixed and 20 deterministic random configurations
  • tests assert collision, clearance, singularity, limit, NaN, stale/moving start, and model failures send no motion frame
  • tests assert a successful guarded move sends one CMD_MOVE_J_SYNC and never CMD_GET_IK
  • wheel build verified the local-kinematics modules, URDF, and collision meshes are packaged

Safety boundary

This detects robot self-collision only. It does not model people, workbenches, tooling fixtures, or other external obstacles. A singular boot pose is intentionally rejected by move_pose(); operators must first leave it with a deliberately slow joint-space move in a clear workspace.

@yd-sl yd-sl left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Review — validated on a real arm

Tested commit 672ffc5 on a real 7J LiteArm (firmware Litearm1.11.1-7J), plus the offline suite.

What works

  • Local FK matches firmware get_tcp on hardware: 7.6e-8 m position, 1.1e-7 rotation. The packaged URDF is consistent with the board.
  • At the singular boot pose (sigma_min=0.019) move_pose() correctly refuses before sending anything (reason=near_singular). After a deliberate slow escape (movej_sync, J4 -> -1.0 rad, speed=0.1), sigma was 0.0717.
  • move_pose() increments of 2 / 5 / 10 mm all passed preflight and arrived without faults. Measured tip error 0.68 / 1.12 / 0.73 mm; sigma stayed 0.072-0.077; self-clearance 22.4 mm.
  • The fail-closed design (fresh status_seq, live runtime limits, path sampling, branch-jump guard, collision/clearance/sigma checks) behaves as described.

Findings

1. Motion lock coverage is incomplete — arm.py:2347, 2364, 2382, 2390

The PR description says "Positive motion APIs are serialized through one re-entrant lock", but home(), move_js(), send_mit() and send_mit_all() are not decorated with _serialize_positive_motion. Reproduced with the fake transport: while one thread holds _motion_serial (simulating an in-flight move_pose preflight), home() / move_js() / send_mit() still transmitted 0x2A / 0x03 / 0x04; only movej() blocked until the lock was released. A concurrent home() can therefore invalidate the read state -> preflight -> one frame atomicity that the guard relies on. Either decorate these too, or explicitly narrow the claim/docs to the discrete PTP/Cartesian entry points.

2. examples/10_local_kinematics.py:46-47 has an enable race

The example calls enable() and immediately move_pose(). The enabled flag takes 17.6-48.5 ms to appear on hardware (already documented in Arm.enable()), while _fresh_state() (kinematics.py:317) waits for only one new status frame. Reproduced deterministically by delaying the enabled bit in the fake status stream: the call raises InvalidCommandError: move_pose 要求机械臂已使能 with no motion frame sent. Poll get_state().enabled before calling move_pose().

3. Arrival accuracy is not the IK tolerance

The IK tolerance is 0.5 mm / 0.005 rad, but final accuracy is governed by the firmware arrival criterion; measured tip error was 0.7-1.1 mm. Worth stating explicitly in the move_pose() docs so callers do not expect 0.5 mm.

4. Package size (non-blocking)

The 9 STL collision meshes add ~62 MB to git and the wheel. Default install remains pyserial-only, but consider decimation or a separate data package.

Test notes

  • 764 passed, 22 skipped. One timing-sensitive failure in tests/test_cart_protocol.py::test_poll_cart_does_not_steal_a_frame_from_a_running_entry under the full suite only; it passes standalone, as a file, and on main, so it appears unrelated to this PR.
  • The cross-repo kin.c FK/Jacobian consistency test passed here.

Not covered

  • Self-collision / soft-limit / start-drift rejections were exercised offline only; no dangerous configurations were constructed on hardware.
  • External obstacles (people, bench, fixtures) are out of scope, as stated in the PR.

Overall: solid implementation and a real-hardware PASS. Items 1 and 2 should be addressed before merge.

@misaka-maples
misaka-maples force-pushed the feat/local-kinematics-safe-ptp branch from 672ffc5 to 18afde8 Compare November 7, 2026 07:03
@misaka-maples

Copy link
Copy Markdown
Contributor Author

@yd-sl 感谢真机 review,指出的两个阻塞项已在 18afde8 修复:

  • home、move_js、send_mit、send_mit_all 已纳入正向运动串行锁;同类的 joint_follow 也一并覆盖。新增测试确认持锁期间不会发运动帧,emergency_stop / disable 仍可立即发送。
  • examples/10_local_kinematics.py 不再 enable() 后立即运动,改为轮询 fresh status,确认 enabled,并处理 fault/timeout。
  • README、开发指南和 move_pose() docstring 已说明 IK residual 不等于真机到位精度,并建议用返回的实测 q 再做 FK。

同时已 rebase 到最新 main,解决了 PR 冲突且没有恢复上游已删除的旧测试套件。复测结果:smoke 6 passed、运动锁专项 7 passed、隔离 robotics 43 passed + 5 subtests。可以基于当前 head 18afde8 复测。

@yd-sl
yd-sl merged commit 33dc2c6 into nexform-tech:main Nov 7, 2026
3 checks passed
@github-actions

github-actions Bot commented Nov 7, 2026

Copy link
Copy Markdown

🎉 This PR is included in version 1.4.0 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀

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

Labels

released Shipped in a release

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants