Add RTP (Robot with Tool Positioner) Inverse Kinematics Plugin

[rjoomen]

Adds RTPInvKin, a compound inverse-kinematics solver for a robot with a tool positioner —
extra actuated joint(s) at the end of the manipulator chain, between the robot flange and the
TCP. It is the mirror image of ROPInvKin, which places the positioner joints at the base of the
chain rather than the tip.

The motivating case is a standard industrial robot with an actuated gripper or tool axis: pair
OPWInvKin for the 6-DOF arm with a 1-DOF tool joint and get analytic IK over the full 7-DOF
chain, instead of falling back to a numerical whole-chain solver like Trac-IK. Besides possible
improved speed, main advantage is that RTP+OPW will return a lot of possible solutions, instead
of only one like Trac-IK and the KDL solvers do.

How it works

RTPInvKin wraps an inner manipulator IK solver and discretises the tool chain onto a LinSpaced
sample grid. Construction consumes a tool-side forward-kinematics chain to precompute, per grid
sample, the joint values and the transform taking a tool-tip target to the corresponding
manipulator-tip target. calcInvKin() is then one transform product per sample, followed by an
inner IK dispatch only for those samples whose manipulator-tip target falls within
manipulator_reach — the rest are rejected by a squared-norm test before any IK runs. The FK chain
is not retained. All branches from all surviving samples are concatenated.

• Base link and working frame are the manipulator base.
• getTipLinkIds() reports the tool chain tip — the frame the target pose is expressed for.
• Joint order in returned solutions: manipulator joints first, then tool joints.

There are four public constructor variants: explicit vs. auto-derived manipulator_reach, crossed
with sample range taken from the tool joint limits vs. given explicitly. Auto-reach derives the
early-exit filter radius from the manipulator's base→tip chain via computeChainReachUpperBound.

All four are one-line delegations to a private init() that validates every input and populates
every member, so there is a single validation path rather than four. It throws std::runtime_error
on: a null manipulator or tool positioner, an empty solver name, an invalid scene-graph root,
either chain having other than exactly one tip link, a link missing from the scene state, a
non-positive reach, a resolution vector or sample-range row count that does not match the tool
positioner's DOF, and a malformed sample range or resolution (non-finite or inverted bounds, or a
resolution that is not finite and positive).

Two of the checks are worth calling out, because failing them is silent rather than loud:

• The tool-positioner base must be rigidly attached to the manipulator tip. init() rejects a
  base that is missing from the scene graph, disconnected from the manipulator tip, or separated
  from it by an active joint. That last case is the dangerous one — it is perfectly reachable, so
  nothing else would catch it, but sampling the tool chain would no longer produce a deterministic
  wrist target and the returned solutions would be quietly wrong.
• Auto-reach inherits the chain's own validation. The two auto-reach constructors run
  computeChainReachUpperBound over the manipulator's base→tip chain, so they additionally throw
  on anything that makes the bound underivable: a FLOATING or PLANAR joint on the path, or a
  prismatic joint that is a mimic or has missing or non-finite limits.

A combined sample-count cap is enforced too. buildSampleGrid() bounds each tool joint
individually; init() additionally bounds their product, since that sets both the size of the
precomputed tables and the number of inner IK solves per calcInvKin() call. It turns a
misconfigured discretisation into a diagnosable exception instead of a multi-gigabyte allocation.

Plugin configuration

kinematic_plugins:
  search_libraries:
    - tesseract_kinematics_factories
  inv_kin_plugins:
    rtp_manipulator:
      default: RTPInvKin
      plugins:
        RTPInvKin:
          class: RTPInvKinFactory
          config:
            manipulator_reach: 2.0        # optional; auto-derived when omitted
            tool_sample_resolution:
              - name: tool_joint
                value: 0.1
            tool_positioner:
              class: KDLFwdKinChainFactory
              config:
                base_link: tool0
                tip_link: tool_tip
            manipulator:
              class: OPWInvKinFactory
              config:
                base_link: base_link
                tip_link: tool0
                params: { ... }

Build change

Adds a tesseract::kinematics_factories in-tree ALIAS. configure_component already exports the
library under that name, so this only lets in-tree targets link it by the same name downstream
consumers use, instead of by the bare kinematics_factories target name.

Testing

rtp_kinematics_unit.cpp — 19 tests: metadata, the constructor validation matrix, independence
from arm configuration, auto-reach equivalence with an explicit reach, single- and multi-sample
roundtrips, clone() / operator= / KinematicGroup integration, factory YAML round-trip
including the auto-reach and failure paths, multi-tip rejection, the tool-base parentage checks
(active joint in between, base absent from the scene graph, base disconnected from the manipulator
tip), multi-joint tool FK correctness and solution counting, and calcInvKin's early return when
the inner IK yields nothing.

factory_utils_unit.cpp — 12 tests over all three helpers against a two-link scene graph:
parsePluginInfo class/config handling and its missing-class throw; parseSampleResolutionMap
joint-limit defaults, min/max overrides, overrides outside the joint limits, missing name/value
keys, unknown joint names, and joints whose limits member is null; and toSampleGridConfig
joint-count mismatch and output ordering.

kinematics_core_unit.cpp — 18 tests: computeChainReachUpperBound across
revolute/prismatic/continuous/branched chains and its rejection of floating, mimic, and unlimited or
non-finite prismatic joints; gatherJointLimits; buildSampleGrid including malformed input and
the sample-count cap; the runtime_error type for all four RTP constructor variants given a null
manipulator; and a regression guard on the shared test helper's default joint limits.

Benchmark

tesseract_kinematics_rtp_vs_kdl_benchmark compares RTPInvKin (OPW 6-DOF manipulator + 1-DOF
revolute tool positioner) against all three KDLInvKinChain solvers on the same 7-DOF ABB IRB2400

• tool positioner fixture. RTP is swept over a range of tool-grid resolutions; each KDL solver runs
  in a WARM and a COLD seeding variant.

The solvers produce fundamentally different solution sets — RTP enumerates every manipulator branch
at every tool sample, the KDL solvers return at most one seed-dependent solution — so no single
number decides it. Reachability dominates the comparison, which is why every solver runs over
two workloads, REACHABLE and UNREACHABLE, and no row can be mistaken for a solver's general
cost.

Representative time_per_call, 256 targets, 3 repetitions, RelWithDebInfo on one machine — treat as
indicative, not as a spec:

solver	REACHABLE	UNREACHABLE	sols/call (reachable)
RTP, tool res 200 mrad	18.1 µs	0.56 µs	246
RTP, tool res 100 mrad	34.0 µs	1.03 µs	477
RTP, tool res 50 mrad	66.8 µs	2.02 µs	947
KDLInvKinChainNR_JL WARM	36.2 µs	738 µs	0.98
KDLInvKinChainNR_JL COLD	448 µs	745 µs	0.45
KDLInvKinChainNR WARM	34.8 µs	751 µs	1.00
KDLInvKinChainNR COLD	169 µs	767 µs	0.97
KDLInvKinChainLMA WARM	62.3 µs	2.30 ms	1.00
KDLInvKinChainLMA COLD	89.6 µs	2.33 ms	0.95

On reachable targets the ordering depends on the tool resolution: at 200 mrad RTP is faster per call
than every KDL row, at 100 mrad it is level with the warm-seeded ones, and only at 50 mrad do warm
NR / NR_JL overtake it — while still returning a single solution against RTP's ~950. Per solution
RTP is ~70 ns at every resolution, against 35 µs to 1 ms for KDL.

Against cold seeds RTP is ahead everywhere, and on unreachable targets the gap is three to four
orders of magnitude: RTP rejects a sample with a squared-norm test, while the KDL solvers iterate to
their cap before reporting failure. A planner that queries near or beyond the workspace boundary
should weight the UNREACHABLE rows accordingly.

KDLInvKinChainNR and KDLInvKinChainLMA additionally do not enforce joint limits and can converge
to configurations a planner has to discard, so raw and in-limits convergence are scored separately
via an in_limits_success_rate counter. The gap is not academic: cold-seeded KDLInvKinChainNR
converges on 97% of reachable targets but only 18% of those results are within limits.

RTP is seeded with ground truth deliberately — its inner OPW solver is closed-form and ignores the
seed — and targets are generated deterministically by FK roundtrip from a seeded RNG, so reruns are
reproducible.

The binary prints a legend after the results table explaining every counter and how to pick a
solver from them. That legend sits next to the code emitting the counters and is the authoritative
description; the README deliberately does not duplicate the counter names.

Follow-up

The utils and factory_utils helpers added here are deliberately generic: they are written to
serve all three compound IK solvers, not RTP alone. REPInvKin and ROPInvKin currently carry
their own near-identical copies of the same joint-limit lookup, sample-grid construction and YAML
parsing.

Six helpers are shared: computeChainReachUpperBound, gatherJointLimits and buildSampleGrid
from utils.h, and parsePluginInfo, parseSampleResolutionMap and toSampleGridConfig from
factory_utils.h.

Migrating REP and ROP onto them is a follow-up PR, kept separate so this one stays reviewable and
the deduplication can be judged on its own merits. That work is already written and will follow once
this lands.

[codecov]

Codecov Report

❌ Patch coverage is 99.42197% with 2 lines in your changes missing coverage. Please review.
✅ Project coverage is 94.44%. Comparing base (d25d24f) to head (4ee6b43).

Files with missing lines	Patch %	Lines
kinematics/core/src/rtp_factory.cpp	97.22%	1 Missing ⚠️
kinematics/core/src/utils.cpp	98.78%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1297      +/-   ##
==========================================
+ Coverage   94.36%   94.44%   +0.08%     
==========================================
  Files         278      282       +4     
  Lines       21532    21879     +347     
==========================================
+ Hits        20318    20663     +345     
- Misses       1214     1216       +2     

Files with missing lines	Coverage Δ
...cs/core/include/tesseract/kinematics/rtp_inv_kin.h	100.00% <100.00%> (ø)
...nematics/core/include/tesseract/kinematics/utils.h	100.00% <ø> (ø)
kinematics/core/src/factory_utils.cpp	100.00% <100.00%> (ø)
kinematics/core/src/rtp_inv_kin.cpp	100.00% <100.00%> (ø)
kinematics/core/src/rtp_factory.cpp	97.22% <97.22%> (ø)
kinematics/core/src/utils.cpp	99.10% <98.78%> (-0.19%)	⬇️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[jf---]

hi @rjoomen super interesting... FWIW the Levenberg-Marquardt / ChainIkSolverPos_LMA I think is (quite a bit more) performant over the newton raphson solver, IIRC it converged in more challenging cases and being able to set weights for the joints is a meaningful feature, who knows something you might want to explore. doesnt come with the joint limits, but that's fairly trivial to implement.

[rjoomen]

From some preliminary benchmarks this is on par with the KDL solvers or Trac-IK for 7 joints (6 axis industrial plus gripper), but the difference is that this returns a lot of solutions. OPW can return up to 8 solutions per point, multiplied by the number of successful 7th joint samples.

[rjoomen]

This currently does simple linearly spaced sampling for the 7th joint, like ROP does. I'm working on a somewhat smarter sampling solution, so less underlying solver (OPW, in my case) calls are necessary and speed increases.

[Copilot]

Pull request overview

Adds an RTP (Robot with Tool Positioner) inverse-kinematics plugin that composes a manipulator IK solver with a tool-side FK chain by sampling the tool joints and solving the manipulator IK at each sample. This fits into the kinematics plugin ecosystem alongside ROP/REP, extending support for manipulators with additional tool joints at the tip.

Changes:

• Introduces RTPInvKin (core implementation) and RTPInvKinFactory (YAML/plugin factory) plus shared factory parsing helpers.
• Adds new kinematics utility helpers (computeChainReachUpperBound, gatherJointLimits, buildSampleGrid) and corresponding unit tests.
• Adds RTP-focused unit tests and an optional benchmark suite comparing RTP vs KDL-NR-JL.

Reviewed changes

Copilot reviewed 19 out of 19 changed files in this pull request and generated 1 comment.

Show a summary per file

File	Description
kinematics/test/rtp_kinematics_unit.cpp	New unit tests covering RTP IK behavior, validation, cloning, factory loading, and edge cases.
kinematics/test/kinematics_test_utils.h	Adds ABB scene-graph fixtures with tool positioners and a small helper for building chains in tests.
kinematics/test/kinematics_core_unit.cpp	Adds coverage for new reach upper-bound and sampling helper behaviors, plus ctor rejection regression tests.
kinematics/test/CMakeLists.txt	Registers the new RTP unit test target.
kinematics/test/benchmarks/rtp_vs_kdl_nr_jl_benchmarks.cpp	Adds a benchmark comparing RTP (OPW + sampled tool joint) vs KDL NR-JL across randomized reachable targets.
kinematics/test/benchmarks/README.md	Documents how to run/interpret the RTP vs KDL benchmark.
kinematics/test/benchmarks/CMakeLists.txt	Adds benchmark build rules for kinematics benchmarks.
kinematics/core/src/utils.cpp	Implements new kinematics utility functions used by RTP and tests.
kinematics/core/src/rtp_inv_kin.cpp	Implements the RTP inverse kinematics solver.
kinematics/core/src/rtp_factory.cpp	Implements the RTP IK plugin factory and YAML parsing/creation logic.
kinematics/core/src/factory_utils.cpp	Adds shared YAML parsing helpers for factories (plugin blocks + sample resolution maps).
kinematics/core/include/tesseract/kinematics/utils.h	Declares new utility APIs.
kinematics/core/include/tesseract/kinematics/rtp_inv_kin.h	Declares the RTP IK solver class and public constructors.
kinematics/core/include/tesseract/kinematics/rtp_factory.h	Declares the RTP IK factory.
kinematics/core/include/tesseract/kinematics/fwd.h	Adds forward declarations for RTP types.
kinematics/core/include/tesseract/kinematics/factory_utils.h	Declares shared factory YAML parsing helpers.
kinematics/core/CMakeLists.txt	Wires new sources into the core kinematics libraries and factories target.
kinematics/CMakeLists.txt	Adds conditional inclusion of the new kinematics benchmark subdirectory.
.github/workflows/clang_format.yml	Fixes quoting for workflow echo output.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[jf---]

@rjoomen I'm curious how if the IK of the rotary table and manipulator is solved in unison, kinetically is it considered a single chain?

[jf---]

@rjoomen wow, should this be added to 0.36?

[rjoomen]

Rebased onto master following the id migration. The string-based version is preserved at rjoomen/tesseract@feat/rtp-inv-kin-strings. No behavioural change — the first four commits are the same work retyped onto LinkId/JointId.

[Levi-Armstrong]

Do you know if this is duplicated anywhere? I know where have several utilities like this but maybe not exactly or should we move this to the SceneGraph class?

[Levi-Armstrong]

I think these will be better handled when we update to using the property tree / yaml schema.