From c48c944df330669df6570ba0be182c6ab80c1815 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 12:37:22 +0000 Subject: [PATCH 1/9] feat(planning): add SelfCollision (P4) Joint-to-joint capsules per link from the FrameChain. Every non-allowed pair is checked with PrimitiveDistance::CapsuleCapsule; adjacent links are allowed by default through a symmetric bit matrix. Reports the minimum signed distance, the attaining pair and a margin-based collision flag. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 15 +-- doc/planning/README.md | 9 +- doc/planning/SelfCollision.md | 90 +++++++++++++ roadmap/README.md | 2 +- roadmap/planning/SelfCollision/explanation.md | 30 ----- .../planning/SelfCollision/implementation.md | 83 ------------ roadmap/planning/SelfCollision/tests.md | 43 ------- robotics/planning/CMakeLists.txt | 3 + robotics/planning/SelfCollision.cpp | 8 ++ robotics/planning/SelfCollision.hpp | 119 ++++++++++++++++++ robotics/planning/test/CMakeLists.txt | 1 + robotics/planning/test/TestSelfCollision.cpp | 106 ++++++++++++++++ 13 files changed, 339 insertions(+), 172 deletions(-) create mode 100644 doc/planning/SelfCollision.md delete mode 100644 roadmap/planning/SelfCollision/explanation.md delete mode 100644 roadmap/planning/SelfCollision/implementation.md delete mode 100644 roadmap/planning/SelfCollision/tests.md create mode 100644 robotics/planning/SelfCollision.cpp create mode 100644 robotics/planning/SelfCollision.hpp create mode 100644 robotics/planning/test/TestSelfCollision.cpp diff --git a/README.md b/README.md index c508b11..332e7dc 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. |--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| | [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration, Generic Joint Link | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | | [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | diff --git a/ROADMAP.md b/ROADMAP.md index a6825e5..4c3dd69 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -401,20 +401,15 @@ Cross-family building blocks that several of the families below depend on. ## Motion planning and collision -| # | Component | Target module | Depends on | Difficulty | -|----|----------------------------------------------------------|---------------|------------|------------| -| P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | -| P4 | Self-collision checking for a link chain (capsule model) | `planning` | P1, M30 | ★★★☆☆ | -| P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | -| P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | +| # | Component | Target module | Depends on | Difficulty | +|----|---------------------------------------------------|---------------|------------|------------| +| P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | +| P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | +| P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | **P3. Grid search.** A* and Dijkstra on 2-D/3-D grids with per-cell costs from a callable, an indexed binary heap with decrease-key, 16-bit indices on small grids and the octile heuristic (no heap allocation). - *Algorithm / paper:* P. Hart, N. Nilsson, B. Raphael, *IEEE Trans. Systems Science and Cybernetics*, 4(2), 1968. -**P4. Self-collision checking.** Joint-to-joint capsule per link from the M30 frame chain, pairwise P1 distances with an allowed-collision matrix (adjacent links by default), minimum distance and pair. -- *Algorithm / paper:* Ericson (2004), Ch. 4–5. -- *Reuses / builds on:* P1, M30. - **P5. GJK + EPA.** Distance between convex shapes (sphere, capsule, box, bounded hull) from support functions, run on cores plus margins; penetration depth and normal by a bounded EPA, robust to tied supports. - *Algorithm / paper:* E. Gilbert, D. Johnson, S. Keerthi, *IEEE J. Robotics and Automation*, 4(2), 1988; G. van den Bergen, *Collision Detection in Interactive 3D Environments* (2003). diff --git a/doc/planning/README.md b/doc/planning/README.md index 00f3bee..87dee0f 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -4,7 +4,8 @@ Geometric queries and planners for collision-free robot motion. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------| -| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | -| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | +| Algorithm | Description | +|----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------| +| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | +| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | +| [Self-Collision Checking](SelfCollision.md) | Joint-to-joint capsules per link, pairwise signed distances with an allowed-collision matrix and a safety margin — $O(n^2)$ | diff --git a/doc/planning/SelfCollision.md b/doc/planning/SelfCollision.md new file mode 100644 index 0000000..dd42a87 --- /dev/null +++ b/doc/planning/SelfCollision.md @@ -0,0 +1,90 @@ +# Self-Collision Checking + +## Overview & Motivation + +A manipulator can hit itself: a folded elbow drives the forearm into the base, or the wrist swings back into the upper arm. Planners must reject such configurations, controllers must slow down before reaching them, and safety monitors must catch them in real time. + +The standard real-time approach wraps each link in a **capsule**, a cylinder with hemispherical caps, i.e. a sphere swept along a segment. Each capsule runs between consecutive joint origins, and every relevant pair of capsules is checked with a closed-form distance. +The result is a **signed minimum distance** plus the pair that attains it. It serves as a binary collision test, as a safety margin for sampling planners, and as the input of a repulsive term for reactive control. + +## Mathematical Theory + +### Link Model + +From the frame chain (joint origins $o_i$ in the base frame and the tool point $t$), link $i$ is the capsule + +$$\mathcal{C}_i = \left\{ x : \operatorname{dist}\left(x, [o_i, o_{i+1}]\right) \le r_i \right\}, \qquad o_n \equiv t$$ + +with a radius $r_i$ chosen to enclose the link's geometry. + +### Pairwise Distance + +For capsules $i$ and $j$, the signed distance is the segment–segment distance minus both radii: + +$$d_{ij} = \min_{s,u \in [0,1]} \left\| o_i + s(o_{i+1} - o_i) - o_j - u(o_{j+1} - o_j) \right\| - r_i - r_j$$ + +A negative value is a penetration. It is exact while the core segments are apart and a lower bound on the depth once they cross. The arm's self-clearance is + +$$d_{\min} = \min_{(i,j) \notin \mathcal{A}} d_{ij}$$ + +where the **allowed set** $\mathcal{A}$ excludes pairs that need no check. + +### Allowed Pairs + +Adjacent links share a joint, so their capsules always touch, and they are allowed by default. Further pairs belong in $\mathcal{A}$ when joint limits keep them permanently apart, or when the coarse model makes them permanently overlap (a housing over its neighbour). +A permanent false positive would otherwise hide every real collision behind it. The allowed set is a symmetric bit matrix, so membership is a single test. + +### Margin + +The configuration is flagged as colliding when $d_{\min} < m$. A positive margin $m$ absorbs model error, sensor latency and the motion between checks. At tool speed $v$ and control period $\Delta t$, use at least $m \ge v\,\Delta t$. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|------------|----------|--------|---------------------------------------------------------------------| +| $n$ links | $O(n^2)$ | $O(n)$ | $n(n-1)/2$ pairs minus allowed ones; one segment–segment solve each | +| 7-link arm | 15 pairs | — | All non-adjacent pairs; constant work per pair | + +## Step-by-Step Walkthrough + +A planar four-link arm has link lengths $(0.4, 0.8, 0.7, 0.5)$ m and radii $(0.10, 0.08, 0.06, 0.05)$ m. Adjacent pairs are allowed, which leaves three checked pairs: $(0,2)$, $(0,3)$ and $(1,3)$. + +1. Straight arm: links 1 and 3 are collinear, with a $0.7$ m gap between their segments. Subtracting the radii gives $d = 0.7 - 0.08 - 0.05 = 0.57$ m. That is the closest pair and the arm is clear. +2. Folded at $q = (0, 2.4, 2.4, 0.5)$: link 2 doubles back over link 0. Their segments come within $0.141905$ m, below the summed radii of $0.16$ m, so $d = -0.018$ m and the arm collides. +3. Allowing $(0, 2)$, for instance because the real geometry clears there, reports the next-closest pair, $(0, 3)$, at $+0.053$ m. +4. At $q = (0, 2.6, 2.2, 1.0)$ the clearance is $0.088$ m: no collision at zero margin, but a $0.1$ m safety margin flags it. + +## Pitfalls & Edge Cases + +- **Coarse geometry**: joint-to-joint capsules miss offset parts such as elbow housings, cables and tools. Enlarge the radius, or add extra frames that model those parts as separate capsules. +- **Over-allowing**: every allowed pair is a blind spot. Justify each one with joint limits or permanent overlap, never just to silence a warning. +- **Crossing cores**: once two core segments intersect, the penetration depth is only a lower bound and the closest points are arbitrary. Treat any negative value as a collision rather than trusting its magnitude. +- **Tunnelling between checks**: a fast arm can pass through a thin link between two control cycles. Use a margin that covers the motion per period, or swept capsules for continuous checking. +- **Degenerate links**: coincident joint origins make a capsule a sphere. The segment distance handles it without special cases. + +## Variants & Generalizations + +- **Bounding-volume hierarchies**: sphere trees or OBB trees per link give tighter fits for complex geometry, with a broad phase on the capsules. +- **Convex hulls with GJK/EPA**: exact distances for convex link meshes at higher cost. +- **Swept volumes**: continuous collision checking along a motion segment instead of at sampled configurations. +- **Distance gradients**: the closest-point direction yields a repulsive velocity or a constraint Jacobian for optimization-based control. +- **Learned or precomputed self-collision maps**: offline sampling of the configuration space to build fast classifiers for high-DoF robots. + +## Applications + +- Validity checking inside sampling-based planners such as RRT-Connect. +- Real-time self-collision avoidance for teleoperated and hand-guided arms. +- Safety monitors that stop or slow the robot before contact. +- Repulsive terms in potential-field or null-space controllers that keep links apart. + +## Connections to Other Algorithms + +- [Primitive Distance Queries](PrimitiveDistance.md): the capsule–capsule distance evaluated for each pair. +- [Chain Pose Kinematics](../kinematics/ChainPoseKinematics.md): supplies the joint origins and tool point that define the capsules. +- [Artificial Potential Fields](PotentialField.md): consumes the minimum distance and direction as a repulsive input. + +## References & Further Reading + +- Ericson, C. (2004). *Real-Time Collision Detection*. Morgan Kaufmann. Chapters 4 and 5. +- Sucan, I.A., Moll, M. and Kavraki, L.E. (2012). "The Open Motion Planning Library." *IEEE Robotics & Automation Magazine*, 19(4), 72–82. +- Schulman, J. et al. (2014). "Motion planning with sequential convex optimization and convex collision checking." *International Journal of Robotics Research*, 33(9), 1251–1270. diff --git a/roadmap/README.md b/roadmap/README.md index 69c36d9..cac5fa4 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -101,7 +101,7 @@ prerequisites. `PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) ### `planning` -`GridSearch` (P3) · `SelfCollision` (P4) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) +`GridSearch` (P3) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) ### `localization` `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) diff --git a/roadmap/planning/SelfCollision/explanation.md b/roadmap/planning/SelfCollision/explanation.md deleted file mode 100644 index b33422b..0000000 --- a/roadmap/planning/SelfCollision/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# Self-Collision Checking — Overview - -## What it is -A test of whether a robot arm is about to hit itself. Each link is wrapped in a capsule (a cylinder with -rounded ends) between its joints, and the distance between every pair of links that could touch is -computed. The answer is the smallest gap, which pair it belongs to, and whether it is below a safety -margin. - -## Why it matters (embedded) -Arms with many joints can fold into themselves, under teleoperation, during redundancy resolution, or along -sampled plans. Checking this in every control cycle protects the hardware. Distances rather than yes/no -answers also let controllers slow down or steer away before contact. - -## How it works (intuition) -The link capsules come straight from the arm's joint positions, which forward kinematics already computes. -Neighbouring links always touch at their shared joint, so they are skipped; so are pairs that can never meet -or that the coarse model would always flag. The remaining pairs are measured with the exact -capsule–capsule distance, and the minimum over them is reported along with the offending pair. - -## Key parameters -- **link radii** — capsule thickness per link. -- **allowed pairs** — pairs never checked (adjacent links by default). -- **margin** — the clearance treated as a collision. - -## Reference -C. Ericson, *Real-Time Collision Detection* (2004), Ch. 4–5; S. Quinlan, "Real-time modification of -collision-free paths," PhD thesis, Stanford University (1994), Ch. 3. - -## See also -`PrimitiveDistance` (P1), `ChainPoseKinematics` (M30, the frame chain), `RrtConnect` (P6), `PotentialField` (P2). diff --git a/roadmap/planning/SelfCollision/implementation.md b/roadmap/planning/SelfCollision/implementation.md deleted file mode 100644 index 45f3d18..0000000 --- a/roadmap/planning/SelfCollision/implementation.md +++ /dev/null @@ -1,83 +0,0 @@ -# Self-Collision Checking for a Link Chain (Capsule Model) — Implementation Pseudocode - -> Roadmap ref: #P4 (★★★☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) - -Distance-based self-collision checking for a serial arm. Link `i` is a capsule around the segment from -joint `i`'s origin to joint `i+1`'s (the last one ends at the tool point), all taken from M30's -`FrameChain`. Every pair not marked allowed is checked with P1's capsule–capsule distance; adjacent links -are allowed by default because they share a joint. The result is the minimum signed distance and the pair -that attains it, which serves as a binary collision test, a safety margin for P6 and a repulsive input -for P2. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct SelfCollisionResult: - T minimumDistance # signed (P1); +∞ when every pair is allowed - std::array pair # links attaining it (i < j) - bool colliding # minimumDistance < margin - std::size_t checkedPairs - -template # static_assert(NumLinks ≥ 2 and NumLinks ≤ 32) -class SelfCollision: - std::array radii - std::array allowed # bit j of allowed[i] ⇒ pair (i, j) skipped; kept symmetric -``` - -## Interface - -```cpp -explicit SelfCollision(const std::array& radii) # allows (i, i+1) for every i -void Allow(std::size_t i, std::size_t j); void Disallow(std::size_t i, std::size_t j) -bool IsAllowed(std::size_t i, std::size_t j) const -SelfCollisionResult Check(const kinematics::FrameChain& frames, T margin) const # hot path -Capsule LinkCapsule(const kinematics::FrameChain& frames, std::size_t link) const -``` - -## Algorithm (pseudocode) - -```text -function LinkCapsule(frames, i): - end = (i + 1 < N) ? frames.jointOrigins[i + 1] : frames.tool.translation - return { [frames.jointOrigins[i], end], radii[i] } - -function Check(frames, margin): # OPTIMIZE_FOR_SPEED - best = { +∞, (0, 0), false, 0 } - for i in [0, N), j in (i, N): - if IsAllowed(i, j): continue - d = PrimitiveDistance::CapsuleCapsule(LinkCapsule(frames, i), LinkCapsule(frames, j)).distance # P1 - best.checkedPairs += 1 - if d < best.minimumDistance: best.minimumDistance = d; best.pair = (i, j) - best.colliding = best.minimumDistance < margin - return best -``` - -## Complexity & memory - -- `O(N²)` capsule pairs (`N(N−1)/2` minus allowed pairs). A 7-link arm has 15 non-adjacent pairs, each one - segment–segment solve. -- Memory: `N` radii and `N` 32-bit masks. No heap. - -## Numerical / embedded notes - -- **Allowed pairs.** Beyond adjacent links, allow pairs that can never meet because of joint limits, or - that always overlap in the capsule model (a wrist housing over the forearm). Then a real self-collision - never hides behind a permanent false positive. -- **Margin.** Checking against a positive margin (a few centimetres) instead of 0 absorbs model error and - the motion between control cycles; at speed `v` and period `Δt`, use at least `v·Δt`. -- **Coarse model.** Capsules on the joint-to-joint segments under-represent offset links (elbow housings, - tools). Add radius, or model those parts as extra links in the frame chain. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/planning/SelfCollision.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Check`, and - `extern template class SelfCollision;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/planning/SelfCollision.cpp` → the same instantiations. -- Test: `robotics/planning/test/TestSelfCollision.cpp` -- Doc: `doc/planning/SelfCollision.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.planning_test`. -- Depends on: P1 (`PrimitiveDistance`, `Capsule`), M30 (`FrameChain`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/SelfCollision/tests.md b/roadmap/planning/SelfCollision/tests.md deleted file mode 100644 index e24c9c5..0000000 --- a/roadmap/planning/SelfCollision/tests.md +++ /dev/null @@ -1,43 +0,0 @@ -# Self-Collision Checking — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSelfCollision : public ::testing::Test: - SelfCollision model{ { 0.1, 0.08, 0.06, 0.05 } } - # Planar(q): FrameChain of a planar arm with z-axis joints and link lengths (0.4, 0.8, 0.7, 0.5): - # jointOrigins[0] = 0, jointOrigins[k+1] = jointOrigins[k] + L_k·(cos θ_k, sin θ_k, 0), θ_k = q₀ + … + q_k; - # tool.translation = end of link 3 -# each case below is a TEST_F(TestSelfCollision, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -straight_arm_is_clear: - Assert: Check(Planar(0, 0, 0, 0), 0) ⇒ minimumDistance ≈ 0.57, pair (1, 3), colliding false, checkedPairs == 3 -folded_arm_collides_between_links_0_and_2: - Assert: Check(Planar(0, 2.4, 2.4, 0.5), 0) ⇒ minimumDistance ≈ −0.018095, pair (0, 2), colliding true -allowing_a_pair_reports_the_next_closest: - Arrange: Allow(0, 2) - Assert: Check(Planar(0, 2.4, 2.4, 0.5), 0) ⇒ minimumDistance ≈ 0.052945, pair (0, 3), colliding false, checkedPairs == 2 -margin_turns_proximity_into_a_collision: - Assert: Check(Planar(0, 2.6, 2.2, 1.0), 0) ⇒ 0.088331, not colliding; Check(…, 0.1) ⇒ colliding -links_1_and_3_can_also_collide: - Assert: Check(Planar(0.5, −1.0, −2.5, −2.0), 0) ⇒ minimumDistance ≈ −0.13, pair (1, 3), colliding true -adjacent_pairs_are_allowed_by_default: - Assert: IsAllowed(0, 1) and IsAllowed(2, 3); not IsAllowed(0, 2); after Disallow(0, 1), checkedPairs == 4 -``` - -## Reference vectors - -- `Planar(0, 2.4, 2.4, 0.5)` joint origins: `(0,0,0), (0.4,0,0), (−0.189915, 0.540371, 0), (−0.128666, −0.156945, 0)`, - tool `(0.148521, −0.573078, 0)`. -- Straight arm: links 1 and 3 are 0.7 apart along the axis; minus radii `0.08 + 0.05` ⇒ 0.57. - -## Edge cases - -- Every pair allowed ⇒ `minimumDistance = +∞`, `checkedPairs = 0`, not colliding. -- Zero-length link (coincident joint origins) ⇒ a sphere capsule; P1's degenerate branch handles it. diff --git a/robotics/planning/CMakeLists.txt b/robotics/planning/CMakeLists.txt index f33f168..befdac1 100644 --- a/robotics/planning/CMakeLists.txt +++ b/robotics/planning/CMakeLists.txt @@ -8,16 +8,19 @@ target_include_directories(robotics.planning ${ROBOTICS_VISIBILITY} target_link_libraries(robotics.planning ${ROBOTICS_VISIBILITY} infra.util numerical.math + robotics.kinematics ) target_sources(robotics.planning PRIVATE PotentialField.hpp PrimitiveDistance.hpp + SelfCollision.hpp ) robotics_add_coverage_sources(robotics.planning PotentialField.cpp PrimitiveDistance.cpp + SelfCollision.cpp ) add_subdirectory(test) diff --git a/robotics/planning/SelfCollision.cpp b/robotics/planning/SelfCollision.cpp new file mode 100644 index 0000000..ff3a64d --- /dev/null +++ b/robotics/planning/SelfCollision.cpp @@ -0,0 +1,8 @@ +#include "robotics/planning/SelfCollision.hpp" + +namespace planning +{ + template class SelfCollision; + template class SelfCollision; + template class SelfCollision; +} diff --git a/robotics/planning/SelfCollision.hpp b/robotics/planning/SelfCollision.hpp new file mode 100644 index 0000000..4e63a62 --- /dev/null +++ b/robotics/planning/SelfCollision.hpp @@ -0,0 +1,119 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include "robotics/planning/PrimitiveDistance.hpp" +#include +#include +#include +#include +#include + +namespace planning +{ + template + struct SelfCollisionResult + { + static_assert(std::is_floating_point_v, "SelfCollisionResult supports floating-point types"); + + T minimumDistance{ std::numeric_limits::infinity() }; + std::array pair{}; + bool colliding{ false }; + std::size_t checkedPairs{ 0 }; + }; + + template + class SelfCollision + { + static_assert(std::is_floating_point_v, "SelfCollision supports floating-point types"); + static_assert(NumLinks >= 2 && NumLinks <= 32, "SelfCollision supports 2 to 32 links"); + + public: + using Frames = kinematics::FrameChain; + + explicit SelfCollision(const std::array& radii); + + void Allow(std::size_t i, std::size_t j); + void Disallow(std::size_t i, std::size_t j); + bool IsAllowed(std::size_t i, std::size_t j) const; + + OPTIMIZE_FOR_SPEED SelfCollisionResult Check(const Frames& frames, T margin) const; + Capsule LinkCapsule(const Frames& frames, std::size_t link) const; + + private: + static std::uint32_t Bit(std::size_t index); + + std::array radii; + std::array allowed{}; + }; + + template + SelfCollision::SelfCollision(const std::array& radii) + : radii{ radii } + { + for (std::size_t i = 0; i + 1 < NumLinks; ++i) + Allow(i, i + 1); + } + + template + void SelfCollision::Allow(std::size_t i, std::size_t j) + { + allowed[i] |= Bit(j); + allowed[j] |= Bit(i); + } + + template + void SelfCollision::Disallow(std::size_t i, std::size_t j) + { + allowed[i] &= ~Bit(j); + allowed[j] &= ~Bit(i); + } + + template + bool SelfCollision::IsAllowed(std::size_t i, std::size_t j) const + { + return (allowed[i] & Bit(j)) != 0; + } + + template + OPTIMIZE_FOR_SPEED SelfCollisionResult SelfCollision::Check(const Frames& frames, T margin) const + { + SelfCollisionResult result{}; + for (std::size_t i = 0; i < NumLinks; ++i) + for (std::size_t j = i + 1; j < NumLinks; ++j) + { + if (IsAllowed(i, j)) + continue; + + const T distance{ PrimitiveDistance::CapsuleCapsule(LinkCapsule(frames, i), LinkCapsule(frames, j)).distance }; + ++result.checkedPairs; + if (distance < result.minimumDistance) + { + result.minimumDistance = distance; + result.pair = { static_cast(i), static_cast(j) }; + } + } + + result.colliding = result.minimumDistance < margin; + return result; + } + + template + Capsule SelfCollision::LinkCapsule(const Frames& frames, std::size_t link) const + { + const auto& end{ link + 1 < NumLinks ? frames.jointOrigins[link + 1] : frames.tool.translation }; + return Capsule{ Segment{ frames.jointOrigins[link], end }, radii[link] }; + } + + template + std::uint32_t SelfCollision::Bit(std::size_t index) + { + return std::uint32_t{ 1 } << index; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class SelfCollision; + extern template class SelfCollision; + extern template class SelfCollision; +#endif +} diff --git a/robotics/planning/test/CMakeLists.txt b/robotics/planning/test/CMakeLists.txt index 47028c6..51efb2a 100644 --- a/robotics/planning/test/CMakeLists.txt +++ b/robotics/planning/test/CMakeLists.txt @@ -10,4 +10,5 @@ target_link_libraries(robotics.planning_test PUBLIC target_sources(robotics.planning_test PRIVATE TestPotentialField.cpp TestPrimitiveDistance.cpp + TestSelfCollision.cpp ) diff --git a/robotics/planning/test/TestSelfCollision.cpp b/robotics/planning/test/TestSelfCollision.cpp new file mode 100644 index 0000000..89626c9 --- /dev/null +++ b/robotics/planning/test/TestSelfCollision.cpp @@ -0,0 +1,106 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/planning/SelfCollision.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Frames = kinematics::FrameChain; + + constexpr std::array lengths{ 0.4f, 0.8f, 0.7f, 0.5f }; + + Frames Planar(float q0, float q1, float q2, float q3) + { + const std::array q{ q0, q1, q2, q3 }; + Frames frames{}; + Vector3 origin{}; + float theta{ 0.0f }; + for (std::size_t k = 0; k < 4; ++k) + { + frames.jointOrigins[k] = origin; + frames.jointAxes[k] = Vector3{ 0.0f, 0.0f, 1.0f }; + theta += q[k]; + origin = origin + Vector3{ std::cos(theta), std::sin(theta), 0.0f } * lengths[k]; + } + frames.tool.translation = origin; + return frames; + } + + class TestSelfCollision + : public ::testing::Test + { + protected: + void ExpectPair(const planning::SelfCollisionResult& result, std::uint8_t first, std::uint8_t second) const + { + EXPECT_EQ(result.pair[0], first); + EXPECT_EQ(result.pair[1], second); + } + + planning::SelfCollision model{ { 0.1f, 0.08f, 0.06f, 0.05f } }; + }; +} + +TEST_F(TestSelfCollision, straight_arm_is_clear) +{ + const auto result{ model.Check(Planar(0.0f, 0.0f, 0.0f, 0.0f), 0.0f) }; + + EXPECT_NEAR(result.minimumDistance, 0.57f, math::Tolerance()); + ExpectPair(result, 1, 3); + EXPECT_FALSE(result.colliding); + EXPECT_EQ(result.checkedPairs, 3u); +} + +TEST_F(TestSelfCollision, folded_arm_collides_between_links_0_and_2) +{ + const auto result{ model.Check(Planar(0.0f, 2.4f, 2.4f, 0.5f), 0.0f) }; + + EXPECT_NEAR(result.minimumDistance, -0.018095f, math::Tolerance()); + ExpectPair(result, 0, 2); + EXPECT_TRUE(result.colliding); +} + +TEST_F(TestSelfCollision, allowing_a_pair_reports_the_next_closest) +{ + model.Allow(0, 2); + + const auto result{ model.Check(Planar(0.0f, 2.4f, 2.4f, 0.5f), 0.0f) }; + + EXPECT_NEAR(result.minimumDistance, 0.052945f, math::Tolerance()); + ExpectPair(result, 0, 3); + EXPECT_FALSE(result.colliding); + EXPECT_EQ(result.checkedPairs, 2u); +} + +TEST_F(TestSelfCollision, margin_turns_proximity_into_a_collision) +{ + const auto frames{ Planar(0.0f, 2.6f, 2.2f, 1.0f) }; + + const auto unguarded{ model.Check(frames, 0.0f) }; + const auto guarded{ model.Check(frames, 0.1f) }; + + EXPECT_NEAR(unguarded.minimumDistance, 0.088331f, math::Tolerance()); + EXPECT_FALSE(unguarded.colliding); + EXPECT_TRUE(guarded.colliding); +} + +TEST_F(TestSelfCollision, links_1_and_3_can_also_collide) +{ + const auto result{ model.Check(Planar(0.5f, -1.0f, -2.5f, -2.0f), 0.0f) }; + + EXPECT_NEAR(result.minimumDistance, -0.13f, math::Tolerance()); + ExpectPair(result, 1, 3); + EXPECT_TRUE(result.colliding); +} + +TEST_F(TestSelfCollision, adjacent_pairs_are_allowed_by_default) +{ + EXPECT_TRUE(model.IsAllowed(0, 1)); + EXPECT_TRUE(model.IsAllowed(2, 3)); + EXPECT_FALSE(model.IsAllowed(0, 2)); + + model.Disallow(0, 1); + + EXPECT_EQ(model.Check(Planar(0.0f, 0.0f, 0.0f, 0.0f), 0.0f).checkedPairs, 4u); +} From da79aeb96fc38743e52a9f02dfb3aef6e22211b3 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 12:39:28 +0000 Subject: [PATCH 2/9] feat(kinematics): add ProductOfExponentials (M15) Screw-axis forward kinematics (base-frame screws, linear-first), the space Jacobian through running adjoints, conversion to the geometric Jacobian, and a FrameChain so GeometricJacobian applies unchanged. Pitched screws are rejected at construction. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +- ROADMAP.md | 4 - doc/kinematics/ProductOfExponentials.md | 94 +++++++++++ doc/kinematics/README.md | 1 + roadmap/README.md | 2 +- .../ProductOfExponentials/explanation.md | 35 ----- .../ProductOfExponentials/implementation.md | 93 ----------- .../kinematics/ProductOfExponentials/tests.md | 58 ------- robotics/kinematics/CMakeLists.txt | 2 + robotics/kinematics/ProductOfExponentials.cpp | 8 + robotics/kinematics/ProductOfExponentials.hpp | 147 ++++++++++++++++++ robotics/kinematics/test/CMakeLists.txt | 1 + .../test/TestProductOfExponentials.cpp | 143 +++++++++++++++++ 13 files changed, 404 insertions(+), 198 deletions(-) create mode 100644 doc/kinematics/ProductOfExponentials.md delete mode 100644 roadmap/kinematics/ProductOfExponentials/explanation.md delete mode 100644 roadmap/kinematics/ProductOfExponentials/implementation.md delete mode 100644 roadmap/kinematics/ProductOfExponentials/tests.md create mode 100644 robotics/kinematics/ProductOfExponentials.cpp create mode 100644 robotics/kinematics/ProductOfExponentials.hpp create mode 100644 robotics/kinematics/test/TestProductOfExponentials.cpp diff --git a/README.md b/README.md index 332e7dc..1684665 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration, Generic Joint Link | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | +| Category | Description | +|--------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration, Generic Joint Link | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 4c3dd69..9ca1011 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -120,7 +120,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | -| M15 | Product-of-Exponentials forward kinematics | `kinematics` | NT53 | ★★★☆☆ | | M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | | M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | | M31 | Coriolis matrix and inertial-parameter regressor | `dynamics` | M28 | ★★★☆☆ | @@ -147,9 +146,6 @@ retired here. - *Algorithm / paper:* S. R. Buss (2004); Nakamura & Hanafusa (1986); Chiaverini (1997). - *Reuses / builds on:* upstream `optimization::LevenbergMarquardt` (NT64) with a manipulability damping policy. -**M15. Product-of-Exponentials forward kinematics.** Screw-theory FK and the space Jacobian, with conversion to the geometric Jacobian. -- *Algorithm / paper:* Lynch & Park, *Modern Robotics*, Ch. 4. - **M16. Momentum-based collision-detection observer.** External joint-torque estimate from the generalized-momentum residual, using `Cᵀq̇`. - *Algorithm / paper:* A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, *IROS*, 2006. diff --git a/doc/kinematics/ProductOfExponentials.md b/doc/kinematics/ProductOfExponentials.md new file mode 100644 index 0000000..31e2cf7 --- /dev/null +++ b/doc/kinematics/ProductOfExponentials.md @@ -0,0 +1,94 @@ +# Product of Exponentials + +## Overview & Motivation + +The **product-of-exponentials (PoE)** formula describes a serial arm by one **screw axis** per joint, all written once in the base frame at the home configuration, plus the tool pose at home. Denavit–Hartenberg needs per-link frames placed by convention. PoE needs none: every joint is just a line in space (revolute) or a direction (prismatic), read directly off a drawing or a CAD model. + +Forward kinematics becomes a product of matrix exponentials, the screw axes give the **space Jacobian** with almost no extra work, and the formulation degrades gracefully where DH is ill-conditioned (nearly parallel axes). It is the language of modern robotics texts (Lynch and Park, Murray–Li–Sastry) and the basis of screw-theoretic calibration and dynamics. + +## Mathematical Theory + +### Screw Axes + +A joint's screw axis $S = (v;\omega)$ uses the linear-first ordering: + +- revolute about the unit axis $\omega$ through a point $a$: $S = (-\omega \times a;\ \omega)$, zero pitch; +- prismatic along the unit direction $v$: $S = (v;\ 0)$. + +### Forward Kinematics + +With $M$ the tool pose at $q = 0$: + +$$T(q) = e^{[S_1]q_1}\, e^{[S_2]q_2} \cdots e^{[S_n]q_n}\, M$$ + +Each factor is the rigid motion produced by moving joint $i$ alone by $q_i$, expressed in the base frame. The order matters: the outer joints' motions are applied first in space, which is what the product encodes. + +### Space Jacobian + +The current screw of joint $i$ is its home screw carried by the motion of all joints before it: + +$$J_{s,i}(q) = \mathrm{Ad}_{e^{[S_1]q_1} \cdots e^{[S_{i-1}]q_{i-1}}}\, S_i, \qquad \mathrm{Ad}_{(R,p)} \begin{bmatrix} v \\ \omega \end{bmatrix} = \begin{bmatrix} R v + p \times R\omega \\ R\omega \end{bmatrix}$$ + +$J_s \dot{q}$ is the spatial twist $(v_s;\omega)$. Its linear part is the velocity of the body point that currently coincides with the **base origin**, not the velocity of the tool. + +### Geometric Jacobian + +The geometric Jacobian uses the tool point's velocity. Shifting the reference point from the base origin to the tool point $p$ gives, column by column: + +$$J_{\text{geo}} = \begin{bmatrix} \mathbf{I} & -[p]_\times \\ 0 & \mathbf{I} \end{bmatrix} J_s, \qquad (v;\omega) \mapsto (v + \omega \times p;\ \omega)$$ + +### Frame Chain + +From each current screw, a revolute joint's axis is $\omega/\|\omega\|$, and the point on it closest to the base origin is $(\omega \times v)/\|\omega\|^2$. A prismatic joint's direction is $v$. These form the same frame chain as link-by-link or DH descriptions, so the shared geometric Jacobian routine applies unchanged. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|--------------------|--------|--------|------------------------------------------------------------| +| Forward kinematics | $O(n)$ | $O(1)$ | One SE(3) exponential and one product per joint | +| Space Jacobian | $O(n)$ | $O(n)$ | One adjoint–twist product per joint on the running product | +| Frame chain | $O(n)$ | $O(n)$ | Same pass, plus one cross product per revolute joint | + +## Step-by-Step Walkthrough + +A planar two-link arm with unit links has joints about $\hat{z}$ through $(0,0,0)$ and $(1,0,0)$. Its screws are $S_1 = (0;\hat{z})$ and $S_2 = (-\hat{z} \times (1,0,0);\hat{z}) = ((0,-1,0);\hat{z})$, with home $M = (\mathbf{I}, (2,0,0))$. + +1. $q = (0, \pi/2)$: the first factor is the identity, and the second rotates the tool by $90°$ about the axis through $(1,0,0)$, carrying $(2,0,0)$ to $(1,1,0)$. +2. $q = (0.4, -0.7)$: the space Jacobian's first column is $S_1 = (0;\hat{z})$. The second is $S_2$ rotated by $0.4$ rad about $\hat{z}$, which is $((0.389, -0.921, 0);\hat{z})$. Its linear part is the velocity of the base origin's coincident body point. +3. Shifting to the tool point $p = (1.876, 0.094, 0)$ adds $\omega \times p$ to each linear part. That gives the geometric columns $(-0.094, 1.876, 0)$ and $(0.296, 0.955, 0)$, the familiar planar-arm Jacobian. +4. For a UR5 with the Lynch–Park screws, the home tool position is $(L_1 + L_2,\ W_1 + W_2,\ H_1 - H_2) = (0.817, 0.191, -0.006)$ m. + +## Pitfalls & Edge Cases + +- **Space versus geometric Jacobian**: using $J_s$ where the tool-point Jacobian is expected silently gives the wrong linear velocity everywhere except at the base origin. Convert first. +- **Ordering of the twist**: this formulation is linear-first $(v;\omega)$; Lynch–Park write $(\omega; v)$. Swapping the blocks is the most common porting error. +- **Unit axes**: a non-unit $\omega$ scales the joint angle. Author unit axes so $q$ is a true angle. +- **Pitched screws**: screws with $\omega \cdot v \ne 0$ (helical joints) have no point-and-axis frame-chain representation and are rejected at construction. +- **Home pose consistency**: $M$ must be the tool pose with every joint at zero, consistent with how the screws were measured. Mixing conventions (e.g. a joint offset in $q$) shifts every result. + +## Variants & Generalizations + +- **Body form**: $T = M e^{[B_1]q_1} \cdots e^{[B_n]q_n}$ with $B_i = \mathrm{Ad}_{M^{-1}} S_i$, giving the body Jacobian directly. +- **PoE calibration**: identifying screw-axis and home-pose errors avoids the singularities of DH-parameter calibration. +- **Screw-based dynamics**: Lie-group formulations of the recursive Newton–Euler algorithm use the same screws. +- **Closed chains**: loop-closure constraints written as products of exponentials equal to the identity. + +## Applications + +- Modelling arms directly from CAD joint axes without assigning DH frames. +- Kinematic calibration with well-conditioned parameter identification. +- Velocity kinematics for task-space control, through conversion to the geometric Jacobian. +- Teaching and analysis of screw theory and spatial velocities. + +## Connections to Other Algorithms + +- [Geometric Jacobian](GeometricJacobian.md): consumes the frame chain produced here, and equals the converted space Jacobian. +- [Denavit-Hartenberg Parameters](DenavitHartenberg.md): an alternative description producing the same poses and frame chain. +- [Chain Pose Kinematics](ChainPoseKinematics.md): the link-by-link alternative. +- The SE(3) exponential and adjoint come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Brockett, R.W. (1984). "Robotic manipulators and the product of exponentials formula." *Mathematical Theory of Networks and Systems*, Springer, 120–129. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapters 4 and 5. +- Murray, R.M., Li, Z. and Sastry, S.S. (1994). *A Mathematical Introduction to Robotic Manipulation*. CRC Press. Chapter 3. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index 1517dd9..9307c0b 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -11,5 +11,6 @@ Geometry and motion algorithms for serial kinematic chains. | [Denavit-Hartenberg Parameters](DenavitHartenberg.md) | Standard and modified DH tables to tool pose and frame chain, with constant joint offsets — $O(n)$ | | [Geometric Jacobian](GeometricJacobian.md) | $6 \times n$ Jacobian, bias acceleration $\dot{J}\dot{q}$ and statics $\tau = J^T w$ from any frame chain — $O(n)$ | | [Manipulability Index](ManipulabilityIndex.md) | Yoshikawa index, velocity-ellipsoid axes and condition number per unit-consistent Jacobian block — singularity monitor | +| [Product of Exponentials](ProductOfExponentials.md) | Screw-axis forward kinematics, space Jacobian and its conversion to the geometric Jacobian — $O(n)$ | | [Redundancy Resolution](RedundancyResolution.md) | Damped pseudo-inverse primary task plus exact null-space projection of a secondary objective for redundant arms | | [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | diff --git a/roadmap/README.md b/roadmap/README.md index cac5fa4..7c3fcb4 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -89,7 +89,7 @@ prerequisites. ## Index ### `kinematics` -`PoseInverseKinematics` (M13) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) +`PoseInverseKinematics` (M13) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CoriolisMatrixAndRegressor` (M31) diff --git a/roadmap/kinematics/ProductOfExponentials/explanation.md b/roadmap/kinematics/ProductOfExponentials/explanation.md deleted file mode 100644 index d397e5e..0000000 --- a/roadmap/kinematics/ProductOfExponentials/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Product of Exponentials — Overview - -## What it is -A forward-kinematics formula built from screw theory: `T = e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M`. Each joint is a -**screw axis** `Sᵢ` written once in the base frame; moving the joint by `qᵢ` applies the exponential of -that screw, and `M` is the tool's pose when every joint is at zero. - -## Why it matters (embedded) -PoE describes an arm with one home pose and one screw per joint — all in the *base* frame — instead of -a chain of intermediate DH frames. There is nothing to line up between links, so it is less -error-prone to author from a CAD model, and the same screws give the Jacobian and the frame chain in -the same pass. With an SE(3) type already in the library it is the most direct FK to write. - -## How it works (intuition) -A screw axis packages "rotate about this line while sliding along it" into one 6-vector: the unit -direction `ω` of the line and the velocity `v = −ω × a` that a point at the base origin would have when -the body spins about the line through `a`. Its exponential is the finite rigid motion produced by -riding that screw for `qᵢ`. Forward kinematics starts at the home pose and multiplies in each joint's -motion. Carrying each screw through the motions of the joints before it gives the "space Jacobian", -whose linear part is the velocity of the point at the base origin; shifting that reference point to -the tool gives the geometric Jacobian used everywhere else in the library. - -## Key parameters -- **screw axes `Sᵢ = (vᵢ; ωᵢ)`** — one per joint, base frame, at `q = 0`. -- **home configuration `M`** — the tool pose at `q = 0`. - -## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 4–5 (PoE and the space Jacobian; the book -orders screws `(ω; v)`, this library `(v; ω)`); R. W. Brockett, "Robotic Manipulators and the Product -of Exponentials Formula," 1984. - -## See also -`math::SE3Transform` (NT53, `Exp` and `Adjoint`), `ChainPoseKinematics` (M30, `FrameChain`), -`GeometricJacobian` (M8), `DenavitHartenberg` (M7, the frame-based alternative), `MatrixExponential` -([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), test cross-check). diff --git a/roadmap/kinematics/ProductOfExponentials/implementation.md b/roadmap/kinematics/ProductOfExponentials/implementation.md deleted file mode 100644 index c49227b..0000000 --- a/roadmap/kinematics/ProductOfExponentials/implementation.md +++ /dev/null @@ -1,93 +0,0 @@ -# Product of Exponentials (Screw-Theory FK) — Implementation Pseudocode - -> Roadmap ref: #M15 (Tier 3) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Screws follow the library convention (NT53): `S = (v; ω)`, linear part first. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class ProductOfExponentials: - std::array, N> screws # space-frame screw axes Sᵢ = (vᵢ; ωᵢ) at q = 0 - SE3Transform home # M: tool pose at q = 0 -``` - -Screw construction (asserted at construction): -- revolute about unit `ω` through point `a`: `S = (−ω × a; ω)` (pitch 0 ⇒ `ω·v = 0`); -- prismatic along unit `v`: `S = (v; 0)`. - -## Interface - -```cpp -ProductOfExponentials(const std::array, N>& screws, const SE3Transform& home) -SE3Transform Compute(const JointVector& q) const # tool pose; hot path -Matrix SpaceJacobian(const JointVector& q) const # Lynch–Park space Jacobian, (v; ω) rows -FrameChain Frames(const JointVector& q) const # M30 frame chain for M8 -static Matrix SpaceToGeometric(const Matrix& Js, const Vector3& pTool) -``` - -## Algorithm (pseudocode) - -```text -function Compute(q): # OPTIMIZE_FOR_SPEED - A = Identity - for i in 0..N-1: A = A * SE3Transform::Exp(screws[i], q[i]) # NT53 - return A * home # e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M - -function SpaceJacobian(q): - A = Identity - for i in 0..N-1: - column i = A.Adjoint() · screws[i] # Ad(e^{[S₁]q₁}···e^{[Sᵢ₋₁]qᵢ₋₁})·Sᵢ, Ad = [[R, Skew(p)R],[0, R]] - A = A * SE3Transform::Exp(screws[i], q[i]) - return Js - -function Frames(q): # same loop, one pass - A = Identity - for i in 0..N-1: - (v; ω) = A.Adjoint() · screws[i] # current screw of joint i in the base frame - if ‖ω‖ > 0: jointAxes[i] = ω/‖ω‖; jointOrigins[i] = (ω × v)/‖ω‖²; jointTypes[i] = Revolute - # = foot of the perpendicular from the base origin to the axis - else: jointAxes[i] = v; jointOrigins[i] = A.p; jointTypes[i] = Prismatic - A = A * SE3Transform::Exp(screws[i], q[i]) - frames.tool = A * home - return frames - -function SpaceToGeometric(Js, pTool): # J_geo = [[I, −Skew(pTool)], [0, I]] · J_space - for each column (v; ω): (v − Skew(pTool)·ω ; ω) = (v + ω × pTool ; ω) -``` - -## Complexity & memory - -- `Compute`: `O(N)` — one `Exp` and one compose per joint. -- `SpaceJacobian` / `Frames`: `O(N)` — one adjoint–vector product per joint, reusing the running product. -- Memory: `N` screws + `M` + one accumulator; stack only. - -## Numerical / embedded notes - -- **Which Jacobian:** the space Jacobian's linear part is the velocity of the (virtual) body point that - currently coincides with the **base origin**, not of the tool. The geometric Jacobian (M8) — tool-point - velocity, used by M11/M13/M14 and the controllers — is `J_geo = [[I, −Skew(p_tool)],[0, I]]·J_space`, - equal column by column to `GeometricJacobian::Compute(Frames(q))`. -- No per-link frames: every screw is written once in the base frame (the main advantage over DH). -- `math::SE3Transform::Exp` handles the prismatic (`ω = 0`) branch and folds a non-unit `ω` into the angle; - still author unit `ω` so `qᵢ` is a true angle. Pitched screws (`ω·v ≠ 0`) cannot be expressed as a - `FrameChain` and are rejected at construction. -- Body form (`T = M·e^{[B₁]q₁}···`, `Bᵢ = Ad(M⁻¹)·Sᵢ`) is not needed: `J_body = Ad(T⁻¹)·J_space`. -- Upstream `math::MatrixExponential` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)) - on the 4×4 `[S]θ` is a valid cross-check of `Exp` in tests, not a runtime dependency. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/ProductOfExponentials.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ProductOfExponentials;` / `` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ProductOfExponentials.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestProductOfExponentials.cpp` -- Doc: `doc/kinematics/ProductOfExponentials.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestProductOfExponentials.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform::Exp`, `Adjoint`), M30 (`FrameChain`), M8 (`GeometricJacobian`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ProductOfExponentials/tests.md b/roadmap/kinematics/ProductOfExponentials/tests.md deleted file mode 100644 index 57937ef..0000000 --- a/roadmap/kinematics/ProductOfExponentials/tests.md +++ /dev/null @@ -1,58 +0,0 @@ -# Product of Exponentials — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestProductOfExponentials : public ::testing::Test: - # 2-link unit planar arm: revolute about ẑ through (0,0,0) and through (1,0,0); screws (v; ω) - std::array, 2> screws{ ((0,0,0); ẑ), ((0,−1,0); ẑ) } - ProductOfExponentials poe{ screws, SE3Transform{ I, (2, 0, 0) } } - # UR5 (Lynch–Park Ex. 4.5, lengths W1=0.109 W2=0.082 L1=0.425 L2=0.392 H1=0.089 H2=0.095), (v; ω): - # ((0,0,0);ẑ) ((−H1,0,0);ŷ) ((−H1,0,L1);ŷ) ((−H1,0,L1+L2);ŷ) ((−W1,L1+L2,0);−ẑ) ((H2−H1,0,L1+L2);ŷ) - # M = { [[−1,0,0],[0,0,1],[0,1,0]], (L1+L2, W1+W2, H1−H2) } - ProductOfExponentials ur5{ ... } -# each case below is a TEST_F(TestProductOfExponentials, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -zero_config_returns_home: - Assert: poe.Compute({0, 0}) ≈ home (tip (2, 0, 0)); ur5.Compute(0).p ≈ (0.817, 0.191, −0.006) -single_joint_rotation: - Assert: poe.Compute({π/2, 0}).p ≈ (0, 2, 0) -elbow_rotation: - Assert: poe.Compute({0, π/2}).p ≈ (1, 1, 0) -agrees_with_denavit_hartenberg: - Assert: poe.Compute(q) ≈ DH 2-link unit arm Forward(q) for q ∈ {(0.4,−0.7), (1.3,2.1)} -space_jacobian_linear_part_is_base_origin_velocity: - Arrange: q = (0.4, −0.7) - Assert: SpaceJacobian(q) = [[0, 0.389418], [0, −0.921061], [0,0], [0,0], [0,0], [1,1]] -space_to_geometric_matches_frame_chain_jacobian: - Arrange: ur5, q = (0.3,−0.5,0.8,−0.2,0.6,1.1) - Assert: SpaceToGeometric(SpaceJacobian(q), Compute(q).p) ≈ GeometricJacobian::Compute(Frames(q)) -geometric_jacobian_matches_finite_difference: - Arrange: ur5, q as above, q̇ = (0.2,−0.4,0.3,0.5,−0.1,0.7), ε = 1e-3 - Assert: J_geo·q̇ ≈ (Δp; Log(ΔR)) / ε (tol 1e-3) -frames_place_joint_origins_on_the_axes: - Assert: poe.Frames({0.4, −0.7}).jointOrigins ≈ {(0,0,0), (0.921061, 0.389418, 0)}, axes = ẑ -prismatic_screw_translates: - Arrange: ProductOfExponentials{ { ((1,0,0); 0) }, Identity } - Assert: Compute({0.3}).p ≈ (0.3, 0, 0); Frames type Prismatic, axis x̂ -``` - -## Reference vectors - -- Planar arm: `q = (π/2, 0)` ⇒ `(0, 2, 0)`; `q = (0, π/2)` ⇒ `(1, 1, 0)`; `q = (0.4, −0.7)` ⇒ - `(1.876397, 0.093898, 0)`, `J_geo = [[−0.093898, 0.295520], [1.876397, 0.955336], 0, 0, 0, [1, 1]]`. -- UR5 at the test `q`: tool `p = (0.696820, 0.400489, 0.077764)`; - `J_space·q̇ = (−0.170343, −0.064298, 0.868026; 0.096307, 1.053237, 0.260041)`, - `J_geo·q̇ = (−0.192582, 0.109415, 0.172680; 0.096307, 1.053237, 0.260041)`. - -## Edge cases - -- `q = 0` ⇒ `Exp` returns identity, `Compute = M`. -- Prismatic screw (`ω = 0`) ⇒ pure translation branch of `Exp`. -- Pitched screw (`ω·v ≠ 0`) ⇒ rejected at construction (`really_assert`). diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index 5cac13d..19f1848 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -21,6 +21,7 @@ target_sources(robotics.kinematics PRIVATE InverseKinematics.hpp JacobianProvider.hpp ManipulabilityIndex.hpp + ProductOfExponentials.hpp RedundancyResolution.hpp ) @@ -31,6 +32,7 @@ robotics_add_coverage_sources(robotics.kinematics GeometricJacobian.cpp InverseKinematics.cpp ManipulabilityIndex.cpp + ProductOfExponentials.cpp RedundancyResolution.cpp ) diff --git a/robotics/kinematics/ProductOfExponentials.cpp b/robotics/kinematics/ProductOfExponentials.cpp new file mode 100644 index 0000000..8515e50 --- /dev/null +++ b/robotics/kinematics/ProductOfExponentials.cpp @@ -0,0 +1,8 @@ +#include "robotics/kinematics/ProductOfExponentials.hpp" + +namespace kinematics +{ + template class ProductOfExponentials; + template class ProductOfExponentials; + template class ProductOfExponentials; +} diff --git a/robotics/kinematics/ProductOfExponentials.hpp b/robotics/kinematics/ProductOfExponentials.hpp new file mode 100644 index 0000000..584d257 --- /dev/null +++ b/robotics/kinematics/ProductOfExponentials.hpp @@ -0,0 +1,147 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include +#include +#include + +namespace kinematics +{ + template + class ProductOfExponentials + { + static_assert(std::is_floating_point_v, "ProductOfExponentials supports floating-point types"); + static_assert(N > 0, "Number of joints must be positive"); + + public: + using JointVector = math::Vector; + using Screw = math::Vector6; + using Vector3 = math::Vector3; + using Transform = math::SE3Transform; + using Jacobian = math::Matrix; + + ProductOfExponentials(const std::array& screws, const Transform& home); + + OPTIMIZE_FOR_SPEED Transform Compute(const JointVector& q) const; + Jacobian SpaceJacobian(const JointVector& q) const; + FrameChain Frames(const JointVector& q) const; + static Jacobian SpaceToGeometric(const Jacobian& space, const Vector3& toolPoint); + + private: + static constexpr T pitchTolerance{ static_cast(1e-5) }; + + static Vector3 Linear(const Screw& screw); + static Vector3 Angular(const Screw& screw); + static void PlaceJoint(FrameChain& frames, std::size_t joint, const Screw& current, const Vector3& framePosition); + + std::array screws; + Transform home; + }; + + template + ProductOfExponentials::ProductOfExponentials(const std::array& screws, const Transform& home) + : screws{ screws } + , home{ home } + { + for (const auto& screw : screws) + really_assert(math::Abs(math::DotProduct(Linear(screw), Angular(screw))) < pitchTolerance); + } + + template + OPTIMIZE_FOR_SPEED typename ProductOfExponentials::Transform ProductOfExponentials::Compute(const JointVector& q) const + { + Transform product{}; + for (std::size_t i = 0; i < N; ++i) + product = product * Transform::Exp(screws[i], q[i]); + + return product * home; + } + + template + typename ProductOfExponentials::Jacobian ProductOfExponentials::SpaceJacobian(const JointVector& q) const + { + Jacobian jacobian{}; + Transform product{}; + for (std::size_t i = 0; i < N; ++i) + { + jacobian.SetBlock(product.TransformTwist(screws[i]), 0, i); + product = product * Transform::Exp(screws[i], q[i]); + } + + return jacobian; + } + + template + FrameChain ProductOfExponentials::Frames(const JointVector& q) const + { + FrameChain frames{}; + Transform product{}; + for (std::size_t i = 0; i < N; ++i) + { + PlaceJoint(frames, i, product.TransformTwist(screws[i]), product.translation); + product = product * Transform::Exp(screws[i], q[i]); + } + + frames.tool = product * home; + return frames; + } + + template + typename ProductOfExponentials::Jacobian ProductOfExponentials::SpaceToGeometric(const Jacobian& space, const Vector3& toolPoint) + { + Jacobian geometric{ space }; + for (std::size_t i = 0; i < N; ++i) + { + const Screw column{ space.GetColumn(i) }; + geometric.SetBlock(Vector3{ Linear(column) + math::CrossProduct(Angular(column), toolPoint) }, 0, i); + } + + return geometric; + } + + template + typename ProductOfExponentials::Vector3 ProductOfExponentials::Linear(const Screw& screw) + { + return screw.template GetBlock<3, 1>(0, 0); + } + + template + typename ProductOfExponentials::Vector3 ProductOfExponentials::Angular(const Screw& screw) + { + return screw.template GetBlock<3, 1>(3, 0); + } + + template + void ProductOfExponentials::PlaceJoint(FrameChain& frames, std::size_t joint, const Screw& current, const Vector3& framePosition) + { + const Vector3 omega{ Angular(current) }; + const Vector3 v{ Linear(current) }; + const T omegaSquared{ math::DotProduct(omega, omega) }; + + if (omegaSquared > pitchTolerance) + { + frames.jointAxes[joint] = omega * (T{ 1 } / math::Sqrt(omegaSquared)); + frames.jointOrigins[joint] = math::CrossProduct(omega, v) * (T{ 1 } / omegaSquared); + frames.jointTypes[joint] = dynamics::JointType::Revolute; + } + else + { + frames.jointAxes[joint] = v * (T{ 1 } / math::VectorNorm(v)); + frames.jointOrigins[joint] = framePosition; + frames.jointTypes[joint] = dynamics::JointType::Prismatic; + } + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ProductOfExponentials; + extern template class ProductOfExponentials; + extern template class ProductOfExponentials; +#endif +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index a3f5bc2..291625d 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -15,5 +15,6 @@ target_sources(robotics.kinematics_test PRIVATE TestGeometricJacobian.cpp TestInverseKinematics.cpp TestManipulabilityIndex.cpp + TestProductOfExponentials.cpp TestRedundancyResolution.cpp ) diff --git a/robotics/kinematics/test/TestProductOfExponentials.cpp b/robotics/kinematics/test/TestProductOfExponentials.cpp new file mode 100644 index 0000000..cc7e0f3 --- /dev/null +++ b/robotics/kinematics/test/TestProductOfExponentials.cpp @@ -0,0 +1,143 @@ +#include "numerical/math/SE3Transform.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/GeometricJacobian.hpp" +#include "robotics/kinematics/ProductOfExponentials.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Screw = math::Vector6; + using Transform = math::SE3Transform; + using Joints2 = math::Vector; + using Joints6 = math::Vector; + + constexpr float pi{ std::numbers::pi_v }; + constexpr float w1{ 0.109f }; + constexpr float w2{ 0.082f }; + constexpr float l1{ 0.425f }; + constexpr float l2{ 0.392f }; + constexpr float h1{ 0.089f }; + constexpr float h2{ 0.095f }; + + Screw MakeScrew(const Vector3& linear, const Vector3& angular) + { + Screw screw{}; + screw.SetBlock(linear, 0, 0); + screw.SetBlock(angular, 3, 0); + return screw; + } + + class TestProductOfExponentials + : public ::testing::Test + { + protected: + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], math::Tolerance()); + } + + template + void ExpectNear(const math::Matrix& actual, const math::Matrix& expected) const + { + for (std::size_t r = 0; r < 6; ++r) + for (std::size_t c = 0; c < N; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), math::Tolerance()); + } + + Vector3 x{ 1.0f, 0.0f, 0.0f }; + Vector3 y{ 0.0f, 1.0f, 0.0f }; + Vector3 z{ 0.0f, 0.0f, 1.0f }; + kinematics::ProductOfExponentials poe{ { MakeScrew(Vector3{}, z), MakeScrew(Vector3{ 0.0f, -1.0f, 0.0f }, z) }, Transform{ Matrix3::Identity(), Vector3{ 2.0f, 0.0f, 0.0f } } }; + kinematics::ProductOfExponentials ur5{ { MakeScrew(Vector3{}, z), MakeScrew(Vector3{ -h1, 0.0f, 0.0f }, y), MakeScrew(Vector3{ -h1, 0.0f, l1 }, y), + MakeScrew(Vector3{ -h1, 0.0f, l1 + l2 }, y), MakeScrew(Vector3{ -w1, l1 + l2, 0.0f }, Vector3{ 0.0f, 0.0f, -1.0f }), + MakeScrew(Vector3{ h2 - h1, 0.0f, l1 + l2 }, y) }, + Transform{ Matrix3{ { -1.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 1.0f }, { 0.0f, 1.0f, 0.0f } }, Vector3{ l1 + l2, w1 + w2, h1 - h2 } } }; + Joints6 ur5Q{ 0.3f, -0.5f, 0.8f, -0.2f, 0.6f, 1.1f }; + }; +} + +TEST_F(TestProductOfExponentials, zero_config_returns_home) +{ + const auto pose{ poe.Compute(Joints2{}) }; + + ExpectNear(pose.translation, Vector3{ 2.0f, 0.0f, 0.0f }); + ExpectNear(Vector3{ pose.rotation * x }, x); + ExpectNear(ur5.Compute(Joints6{}).translation, Vector3{ 0.817f, 0.191f, -0.006f }); +} + +TEST_F(TestProductOfExponentials, single_joint_rotation) +{ + ExpectNear(poe.Compute(Joints2{ pi / 2.0f, 0.0f }).translation, Vector3{ 0.0f, 2.0f, 0.0f }); +} + +TEST_F(TestProductOfExponentials, elbow_rotation) +{ + ExpectNear(poe.Compute(Joints2{ 0.0f, pi / 2.0f }).translation, Vector3{ 1.0f, 1.0f, 0.0f }); +} + +TEST_F(TestProductOfExponentials, agrees_with_denavit_hartenberg) +{ + const kinematics::DenavitHartenberg dh{ { kinematics::DhLink{ 1.0f, 0.0f, 0.0f, 0.0f }, kinematics::DhLink{ 1.0f, 0.0f, 0.0f, 0.0f } }, kinematics::DhConvention::Standard }; + + for (const auto& q : { Joints2{ 0.4f, -0.7f }, Joints2{ 1.3f, 2.1f } }) + { + const auto fromScrews{ poe.Compute(q) }; + const auto fromDh{ dh.Forward(q) }; + ExpectNear(fromScrews.translation, fromDh.translation); + ExpectNear(Vector3{ fromScrews.rotation * x }, Vector3{ fromDh.rotation * x }); + } +} + +TEST_F(TestProductOfExponentials, space_jacobian_linear_part_is_base_origin_velocity) +{ + const math::Matrix expected{ { 0.0f, 0.389418f }, { 0.0f, -0.921061f }, { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 1.0f, 1.0f } }; + + ExpectNear(poe.SpaceJacobian(Joints2{ 0.4f, -0.7f }), expected); +} + +TEST_F(TestProductOfExponentials, space_to_geometric_matches_frame_chain_jacobian) +{ + const auto geometric{ decltype(ur5)::SpaceToGeometric(ur5.SpaceJacobian(ur5Q), ur5.Compute(ur5Q).translation) }; + + ExpectNear(geometric, kinematics::GeometricJacobian::Compute(ur5.Frames(ur5Q))); +} + +TEST_F(TestProductOfExponentials, geometric_jacobian_matches_finite_difference) +{ + const Joints6 qDot{ 0.2f, -0.4f, 0.3f, 0.5f, -0.1f, 0.7f }; + constexpr float epsilon{ 1e-3f }; + + const auto twist{ math::Vector6{ decltype(ur5)::SpaceToGeometric(ur5.SpaceJacobian(ur5Q), ur5.Compute(ur5Q).translation) * qDot } }; + + const auto ahead{ ur5.Compute(ur5Q + qDot * epsilon) }; + const auto behind{ ur5.Compute(ur5Q - qDot * epsilon) }; + ExpectNear(ur5.Compute(ur5Q).translation, Vector3{ 0.696820f, 0.400489f, 0.077764f }); + ExpectNear(twist.GetBlock<3, 1>(0, 0), Vector3{ (ahead.translation - behind.translation) * (0.5f / epsilon) }); + ExpectNear(twist.GetBlock<3, 1>(3, 0), Vector3{ Transform::LogRotation(Matrix3{ ahead.rotation * behind.rotation.Transpose() }) * (0.5f / epsilon) }); +} + +TEST_F(TestProductOfExponentials, frames_place_joint_origins_on_the_axes) +{ + const auto frames{ poe.Frames(Joints2{ 0.4f, -0.7f }) }; + + ExpectNear(frames.jointOrigins[0], Vector3{}); + ExpectNear(frames.jointOrigins[1], Vector3{ 0.921061f, 0.389418f, 0.0f }); + ExpectNear(frames.jointAxes[0], z); + ExpectNear(frames.jointAxes[1], z); +} + +TEST_F(TestProductOfExponentials, prismatic_screw_translates) +{ + const kinematics::ProductOfExponentials slider{ { MakeScrew(x, Vector3{}) }, Transform::Identity() }; + + const auto frames{ slider.Frames(math::Vector{ 0.3f }) }; + + ExpectNear(slider.Compute(math::Vector{ 0.3f }).translation, Vector3{ 0.3f, 0.0f, 0.0f }); + EXPECT_EQ(frames.jointTypes[0], dynamics::JointType::Prismatic); + ExpectNear(frames.jointAxes[0], x); +} From 77795d7c21d6204a31444f026221f13ae52c4d1e Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 12:44:21 +0000 Subject: [PATCH 3/9] feat(dynamics): add Coriolis matrix and inertial regressor (M31) A modified RNEA computes M qdd_r + C(q, qd) qd_r + g with the Christoffel-consistent C (Mdot - 2C skew-symmetric). CoriolisMatrix builds C column by column and also returns C^T qd. ChainInertialRegressor implements the new InertialRegressor interface with the 10-parameter-per-link regressor Y. The spatial motion/force algebra is added to the shared SpatialInertia.hpp. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +- ROADMAP.md | 4 - doc/dynamics/CoriolisMatrixAndRegressor.md | 107 +++++++++++++ doc/dynamics/README.md | 21 +-- roadmap/README.md | 2 +- .../implementation.md | 2 +- .../CoriolisMatrixAndRegressor/explanation.md | 37 ----- .../implementation.md | 100 ------------ .../CoriolisMatrixAndRegressor/tests.md | 47 ------ robotics/dynamics/CMakeLists.txt | 7 + robotics/dynamics/ChainInertialRegressor.cpp | 7 + robotics/dynamics/ChainInertialRegressor.hpp | 150 ++++++++++++++++++ robotics/dynamics/CoriolisMatrix.cpp | 7 + robotics/dynamics/CoriolisMatrix.hpp | 53 +++++++ robotics/dynamics/InertialRegressor.hpp | 23 +++ .../dynamics/ModifiedRecursiveNewtonEuler.cpp | 7 + .../dynamics/ModifiedRecursiveNewtonEuler.hpp | 138 ++++++++++++++++ robotics/dynamics/SpatialInertia.hpp | 55 +++++++ robotics/dynamics/test/CMakeLists.txt | 2 + .../test/TestChainInertialRegressor.cpp | 69 ++++++++ robotics/dynamics/test/TestCoriolisMatrix.cpp | 124 +++++++++++++++ 21 files changed, 769 insertions(+), 207 deletions(-) create mode 100644 doc/dynamics/CoriolisMatrixAndRegressor.md delete mode 100644 roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md delete mode 100644 roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md delete mode 100644 roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md create mode 100644 robotics/dynamics/ChainInertialRegressor.cpp create mode 100644 robotics/dynamics/ChainInertialRegressor.hpp create mode 100644 robotics/dynamics/CoriolisMatrix.cpp create mode 100644 robotics/dynamics/CoriolisMatrix.hpp create mode 100644 robotics/dynamics/InertialRegressor.hpp create mode 100644 robotics/dynamics/ModifiedRecursiveNewtonEuler.cpp create mode 100644 robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp create mode 100644 robotics/dynamics/test/TestChainInertialRegressor.cpp create mode 100644 robotics/dynamics/test/TestCoriolisMatrix.cpp diff --git a/README.md b/README.md index 1684665..cd71aa4 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration, Generic Joint Link | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | +| Category | Description | +|--------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Forward-Dynamics Integration, Generic Joint Link | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 9ca1011..7b8aed8 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -122,7 +122,6 @@ existing `dynamics/` convention. | M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | | M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | | M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | -| M31 | Coriolis matrix and inertial-parameter regressor | `dynamics` | M28 | ★★★☆☆ | | M22 | Dynamic (base-parameter) identification | `dynamics` | M31, NT63 | ★★★☆☆ | | M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | @@ -152,9 +151,6 @@ retired here. **M17. Impedance control.** Stiffness/damping impedance via `Jᵀ` (no force sensor), and inertia-shaping impedance with measured contact wrench via the task-space inertia. - *Algorithm / paper:* N. Hogan, *ASME J. Dyn. Sys. Meas. Control*, 1985. -**M31. Coriolis matrix and inertial regressor.** Modified RNEA giving the Christoffel `C(q,q̇)q̇_r` (skew-symmetric `Ṁ − 2C`), `Cᵀq̇`, and the 10-parameter-per-link regressor. -- *Algorithm / paper:* Echeandia & Wensing (2021); Niemeyer & Slotine (1991); Atkeson, An, Hollerbach (1986). - **M22. Dynamic (base-parameter) identification.** Regressor rows streamed into a rank-revealing least-squares solve; the kept directions are the base parameters. - *Algorithm / paper:* Atkeson, An, Hollerbach (1986); Gautier & Khalil (1990). - *Reuses / builds on:* M31; upstream `estimators::IncrementalQrLeastSquares` (NT63). diff --git a/doc/dynamics/CoriolisMatrixAndRegressor.md b/doc/dynamics/CoriolisMatrixAndRegressor.md new file mode 100644 index 0000000..695785d --- /dev/null +++ b/doc/dynamics/CoriolisMatrixAndRegressor.md @@ -0,0 +1,107 @@ +# Coriolis Matrix and Inertial Regressor + +## Overview & Motivation + +The recursive Newton–Euler algorithm returns the Coriolis and centrifugal **vector** $C(q,\dot{q})\dot{q}$, enough for inverse dynamics and simulation. Several advanced algorithms need more: + +- **passivity-based adaptive control** needs $C(q,\dot{q})\dot{q}_r$ for a reference velocity $\dot{q}_r \ne \dot{q}$; +- **momentum-based collision observers** need $C^T(q,\dot{q})\dot{q}$; +- **identification and adaptive control** need the dynamics written **linearly in the inertial parameters**, $Y(q,\dot{q},\dot{q}_r,\ddot{q}_r)\,\pi$. + +All three come from one modified recursive pass. It uses a particular factorization of the Coriolis terms, the **Christoffel** form, for which $\dot{M} - 2C$ is skew-symmetric. That is the passivity property every energy-based proof relies on. + +## Mathematical Theory + +### Christoffel Coriolis Matrix + +Many matrices $C$ satisfy the same product $C\dot{q}$. The Christoffel choice is + +$$C_{ij} = \sum_k \Gamma_{ijk}\,\dot{q}_k, \qquad \Gamma_{ijk} = \tfrac12\left(\frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i}\right)$$ + +and it is the unique choice that makes $N = \dot{M} - 2C$ skew-symmetric, so that $x^T N x = 0$ for every $x$. + +### Modified Recursion + +In spatial notation (motion vectors $(\omega; v)$, force vectors $(n; f)$, link frames), each link $i$ has the propagated quantities + +$$\begin{aligned} +v_i &= X_i v_{i-1} + S_i \dot{q}_i, \qquad v_{r,i} = X_i v_{r,i-1} + S_i \dot{q}_{r,i} \\ +a_i &= X_i a_{i-1} + S_i \ddot{q}_{r,i} + (v_i \times S_i)\,\dot{q}_{r,i} +\end{aligned}$$ + +with gravity entering as the base acceleration $a_{-1} = (0; -g)$. The body wrench uses a symmetric Coriolis operator: + +$$f_i = I_i a_i + \tfrac12\left( v_i \times^{*} I_i v_{r,i} - I_i (v_i \times v_{r,i}) + v_{r,i} \times^{*} I_i v_i \right)$$ + +Wrenches are then accumulated tip to base, and each joint torque is the projection onto the joint's motion subspace: $\tau_i = S_i^T f_i$, plus the armature term. The result is + +$$\tau = M(q)\,\ddot{q}_r + C(q,\dot{q})\,\dot{q}_r + g(q)$$ + +with $C$ in the Christoffel form (Echeandia and Wensing, 2021). For $\dot{q}_r = \dot{q}$ and $\ddot{q}_r = \ddot{q}$ the operator collapses to $v \times^{*} I v$ and the recursion is ordinary inverse dynamics. + +### Coriolis Matrix and Its Transpose + +Column $j$ of $C(q,\dot{q})$ is the recursion with $\dot{q}_r = e_j$, $\ddot{q}_r = 0$ and no gravity. Then $C^T\dot{q}$ follows by a matrix–vector product. + +### Inertial Regressor + +Each link contributes ten parameters about its joint origin: mass $m$, first moment $m c$, and the six independent entries of the rotational inertia $I_O = \bar{I} + m [c]_\times^T [c]_\times$. The spatial inertia acting on a motion $(\omega; u)$ is linear in them: + +$$I\,(\omega; u) = A(\omega, u)\,\pi_i, \qquad A = \begin{bmatrix} 0 & -[u]_\times & L(\omega) \\ u & [\omega]_\times & 0 \end{bmatrix}$$ + +where $L(\omega)$ arranges $\omega$ so that $L(\omega)\,(I_{xx}, I_{xy}, I_{xz}, I_{yy}, I_{yz}, I_{zz})^T = I_O\,\omega$. Substituting $A$ into the body wrench gives a $6 \times 10$ block per link. +Carrying it towards the base and projecting on each ancestor joint fills the regressor $Y$, with $Y\pi = M\ddot{q}_r + C\dot{q}_r + g$. $Y$ depends only on geometry and motion, never on the inertial parameters. The armature is not one of the ten link parameters. + +## Complexity Analysis + +| Quantity | Time | Space | Notes | +|--------------------------------|----------|----------|-----------------------------------------------------------| +| Modified inverse dynamics | $O(n)$ | $O(n)$ | About twice an ordinary recursive Newton–Euler pass | +| Coriolis matrix $C$ | $O(n^2)$ | $O(n^2)$ | One modified pass per column | +| $C^T\dot{q}$ | $O(n^2)$ | $O(n^2)$ | Via the full matrix | +| Regressor $Y$ ($n \times 10n$) | $O(n^2)$ | $O(n^2)$ | One $6 \times 10$ block per link carried to its ancestors | + +## Step-by-Step Walkthrough + +A planar two-link arm of uniform rods, with $h = m_2 l_1 \tfrac{l_2}{2} \sin q_2$: + +1. The Christoffel matrix is $C = \begin{bmatrix} -h\dot{q}_2 & -h(\dot{q}_1 + \dot{q}_2) \\ h\dot{q}_1 & 0 \end{bmatrix}$. +2. Its product with $\dot{q}$ is $[-h(2\dot{q}_1\dot{q}_2 + \dot{q}_2^2),\ h\dot{q}_1^2]$, the familiar Coriolis and centrifugal vector. +3. The mass matrix varies only through $M_{11}$ and $M_{12}$, so $\dot{M}_{11} = -2h\dot{q}_2$ and $\dot{M}_{12} = -h\dot{q}_2$. Then $N = \dot{M} - 2C$ has $N_{12} = h(2\dot{q}_1 + \dot{q}_2) = -N_{21}$ and zero diagonal: skew-symmetric, as required. +4. For a spatial three-link arm the column-by-column construction matches finite-difference Christoffel symbols of the composite-rigid-body mass matrix to about $10^{-5}$ in single precision. + +## Pitfalls & Edge Cases + +- **Wrong factorization**: reconstructing $C$ from the vector $C\dot{q}$, or using a convenient factorization from symbolic tools, generally breaks the skew-symmetry, and with it the stability proofs of passivity-based controllers and observers. +- **Parameter frame**: the regressor's rotational inertia is about the **joint origin**, not the centre of mass. Mixing the two silently corrupts identification. +- **Identifiability**: the full $10n$-parameter vector is not identifiable from joint torques. Some parameters never appear and others only in combinations. Reduce to base parameters before estimation. +- **Armature and friction**: neither is among the ten link parameters. Extend the parameter vector with per-joint armature and friction terms when identifying real arms. +- **Cost of the full matrix**: forming $C$ is $O(n^2)$. When only $C\dot{q}_r$ is needed, call the modified recursion directly. + +## Variants & Generalizations + +- **Base parameters**: QR decomposition with column pivoting of a stacked regressor yields a minimal identifiable set. +- **Floating-base regressors**: the same construction applied to a free-floating base, for legged robots and humanoids. +- **Energy regressors**: the kinetic plus potential energy is linear in the same parameters, giving filtered identification without accelerations. +- **Symbolic Coriolis matrices**: closed forms for specific arms, which are exact but must be checked for the Christoffel property. + +## Applications + +- Slotine–Li adaptive control, which needs $C(q,\dot{q})\dot{q}_r$ and $Y$. +- Generalized-momentum collision detection, which needs $C^T\dot{q}$. +- Offline and recursive identification of inertial parameters from measured torques. +- Passivity analysis and energy-based controller design. + +## Connections to Other Algorithms + +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): the special case $\dot{q}_r = \dot{q}$, and the cross-check for $C\dot{q}$. +- [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md): its mass matrix defines the Christoffel symbols this construction reproduces, and its spatial inertia is shared. +- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): uses the same spatial algebra and frame transforms. +- [Chain Dynamics Model](ChainDynamicsModel.md): supplies $M$ and $g$ beside these Coriolis terms. + +## References & Further Reading + +- Echeandia, S. and Wensing, P.M. (2021). "Numerical methods to compute the Coriolis matrix and Christoffel symbols for rigid-body systems." *ASME Journal of Computational and Nonlinear Dynamics*, 16(9). +- Niemeyer, G. and Slotine, J.-J.E. (1991). "Performance in adaptive manipulator control." *International Journal of Robotics Research*, 10(2), 149–161. +- Atkeson, C.G., An, C.H. and Hollerbach, J.M. (1986). "Estimation of inertial parameters of manipulator loads and links." *International Journal of Robotics Research*, 5(3), 101–119. +- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapters 2 and 5. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index a90298a..f85eea2 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -4,13 +4,14 @@ Rigid-body dynamics algorithms for robotics and mechanical systems. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------| -| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | -| [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | -| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | -| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | -| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | -| [Generic Joint Link](JointLink.md) | Revolute or prismatic joints with limits and armature through one motion-subspace abstraction | -| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | -| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation, optionally with an external tool wrench | +| Algorithm | Description | +|-------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------| +| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | +| [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | +| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | +| [Coriolis Matrix and Inertial Regressor](CoriolisMatrixAndRegressor.md) | Modified RNEA for $C(q,\dot{q})\dot{q}_r$ in Christoffel form, $C^T\dot{q}$, and the 10-parameter-per-link regressor $Y\pi$ | +| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | +| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | +| [Generic Joint Link](JointLink.md) | Revolute or prismatic joints with limits and armature through one motion-subspace abstraction | +| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | +| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation, optionally with an external tool wrench | diff --git a/roadmap/README.md b/roadmap/README.md index 7c3fcb4..456c025 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,7 +92,7 @@ prerequisites. `PoseInverseKinematics` (M13) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` -`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CoriolisMatrixAndRegressor` (M31) +`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) ### `trajectory` `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md index f67bc96..b586623 100644 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md @@ -65,7 +65,7 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - **The proof needs `Ṁ − 2C` skew-symmetric**, so `Y` must use the *Christoffel-consistent* `C(q,q̇)` applied to the reference velocity, `C(q,q̇)q̇r`. Plain RNEA (`ChainDynamicsModel`, M29) only returns the vector `C(q,q̇)q̇` for a single velocity and cannot provide it; use the regressor of - `roadmap/dynamics/CoriolisMatrixAndRegressor` (M31). + the shipped `robotics/dynamics/ChainInertialRegressor.hpp` and `CoriolisMatrix.hpp` (M31). - Add a **projection** (or dead-zone) so `aHat` stays in a physically valid set (positive masses) and does not drift under sensor noise or unmodelled dynamics. - `Λ` sets the error-manifold bandwidth, `Kd` damps `s`, `Γ` sets adaptation speed — too large `Γ` diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md deleted file mode 100644 index ce5cbd9..0000000 --- a/roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md +++ /dev/null @@ -1,37 +0,0 @@ -# Coriolis Matrix and Inertial Regressor — Overview - -## What it is -Two views of the same rigid-body dynamics. The **Coriolis matrix** `C(q,q̇)` is the matrix whose -product with the joint velocity gives the centrifugal and Coriolis torques — chosen in the specific -(Christoffel) form that makes `Ṁ − 2C` skew-symmetric. The **inertial regressor** `Y` rewrites the -torques as a known matrix of motion terms times an unknown vector of inertial parameters (masses, -first moments, inertia-tensor entries). - -## Why it matters (embedded) -Adaptive controllers learn payloads online by adjusting the parameter vector, which only works with -the regressor form and with a skew-symmetric `Ṁ − 2C`. Collision-detection observers need `Cᵀq̇`. -Offline identification of a real robot's inertial parameters is a least-squares fit on the -regressor. All of these fall out of one modified recursive pass, at a few times the cost of plain -inverse dynamics. - -## How it works (intuition) -Run the recursive Newton-Euler sweep with two velocities side by side — the actual joint velocity and -a "reference" one. The actual velocity decides how the bodies rotate; the reference velocity is what -those rotating bodies are asked to carry. Splitting each body's gyroscopic term symmetrically between -the two produces the Coriolis matrix with the passivity property. Because every body's contribution -is linear in its ten inertial parameters, replacing the numbers by the columns that multiply them -gives the regressor. - -## Key parameters -- **Reference velocity/acceleration** — the Slotine–Li reference motion (or the actual motion). -- **Parameter vector** — ten numbers per link, inertia taken about the joint origin. - -## Reference -S. Echeandia, P. M. Wensing, "Numerical Methods to Compute the Coriolis Matrix and Christoffel -Symbols for Rigid-Body Systems," *J. Computational and Nonlinear Dynamics*, 16(9), 2021; -G. Niemeyer, J.-J. Slotine, "Performance in Adaptive Manipulator Control," *Int. J. Robotics -Research*, 10(2), 1991; C. Atkeson, C. An, J. Hollerbach, *Int. J. Robotics Research*, 5(3), 1986. - -## See also -`SlotineLiAdaptiveControl` (M20), `MomentumObserver` (M16), `DynamicParameterIdentification` (M22), -`CompositeRigidBodyAlgorithm` (M28), `RecursiveNewtonEuler`. diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md deleted file mode 100644 index f48e066..0000000 --- a/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md +++ /dev/null @@ -1,100 +0,0 @@ -# Coriolis Matrix and Inertial Regressor — Implementation Pseudocode - -> Roadmap ref: #M31 (Tier 3) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Plain RNEA only yields the Coriolis *vector* `C(q,q̇)q̇`. Passivity-based control (Slotine–Li, M20) -needs `C(q,q̇)q̇_r` for a reference velocity `q̇_r ≠ q̇`, the momentum observer (M16) needs `Cᵀ(q,q̇)q̇`, -and identification (M22) plus adaptive control need the dynamics written linearly in the inertial -parameters, `Y·π`. One modified RNEA pass provides all three, with the **Christoffel-consistent** -`C` (so `Ṁ − 2C` is skew-symmetric). - -## Data structures - -```cpp -# internal spatial algebra uses Featherstone ordering, motion (ω; v), force (n; f), link frames, -# exactly like the shipped ArticulatedBodyAlgorithm; nothing here is exposed as a public 6-vector. - -template # static_assert(std::is_floating_point_v); instantiated for float -class ModifiedRecursiveNewtonEuler: # stateless - JointVector InverseDynamics(links, q, q̇, q̇r, q̈r, gravity) const # = M q̈r + C(q,q̇) q̇r + g - -template -class CoriolisMatrix: # stateless - SquareMatrix Compute(links, q, q̇) const # Christoffel C(q, q̇) - JointVector TransposeTimesVelocity(links, q, q̇) const # Cᵀ(q, q̇) q̇ - -template -class InertialRegressor: # interface, protected ~InertialRegressor() = default - virtual Matrix Compute(q, q̇, q̇r, q̈r) const = 0 # Y with Y·π = M q̈r + C q̇r + g - -template -class ChainInertialRegressor : public InertialRegressor: - const LinkArray& links; Vector3 gravity - static Vector Parameters(const LinkArray& links) - # per link π_i = (m, m·c_x, m·c_y, m·c_z, I_xx, I_xy, I_xz, I_yy, I_yz, I_zz), - # I = inertia about the JOINT origin: I_c + m·Skew(c)·Skew(c)ᵀ (link frame) -``` - -## Algorithm (pseudocode) - -```cpp -# helpers (6-vectors in (ω; v) / (n; f) ordering, 6×6 in 3×3 blocks): -# crm(v) = [[ω×, 0], [v×, ω×]] crf(v) = −crm(v)ᵀ = [[ω×, v×], [0, ω×]] -# barcrf(h) for force h = (n; f): barcrf(h)·w = crf(w)·h ⇒ barcrf(h) = [[−n×, −f×], [−f×, 0]] -# X_i = link-i-from-parent motion transform (same as ABA); S_i = (z_i; 0) -# bodyC(I, v) = ½·( crf(v)·I − I·crm(v) + barcrf(I·v) ) # skew-compatible body Coriolis - -function InverseDynamics(links, q, q̇, q̇r, q̈r, g): # OPTIMIZE_FOR_SPEED - v_−1 = vr_−1 = 0; a_−1 = (0; −g) # gravity as base acceleration - for i in 0..N−1: - v_i = X_i·v_{i−1} + S_i·q̇[i] - vr_i = X_i·vr_{i−1} + S_i·q̇r[i] - a_i = X_i·a_{i−1} + S_i·q̈r[i] + crm(v_i)·S_i·q̇r[i] # J_i q̈r + J̇_i q̇r (J̇ built with q̇) - f_i = I_i·a_i + bodyC(I_i, v_i)·vr_i - for i = N−1 .. 0: - τ[i] = S_iᵀ·f_i - if i > 0: f_{i−1} += X_iᵀ·f_i - return τ - -function CoriolisMatrix.Compute(links, q, q̇): # O(N²): one pass per column - for j: column j = InverseDynamics(links, q, q̇, e_j, 0, g = 0) -function TransposeTimesVelocity(links, q, q̇): return Transpose(Compute(links, q, q̇))·q̇ - -# regressor: bodyC and I·v are linear in π_i, via A(v)·π_i = I·v with v = (ω; u): -# A(v) = [[ 0₃ₓ₁, −u×, L(ω) ], -# [ u, ω×, 0₃ₓ₆ ]], L(ω) = [[ωx, ωy, ωz, 0, 0, 0], [0, ωx, 0, ωy, ωz, 0], [0, 0, ωx, 0, ωy, ωz]] -function ChainInertialRegressor.Compute(q, q̇, q̇r, q̈r): # O(N²) - forward pass for v_i, vr_i, a_i as above - for i in 0..N−1: - K = A(a_i) + ½·( crf(v_i)·A(vr_i) − A(crm(v_i)·vr_i) + crf(vr_i)·A(v_i) ) # 6×10 - for j = i .. 0: # carry towards the base - Y[j, 10i .. 10i+9] = S_jᵀ·K - if j > 0: K = X_jᵀ·K - return Y -``` - -## Complexity & memory - -- `InverseDynamics`: `O(N)`, about twice an RNEA pass. -- `CoriolisMatrix::Compute` and the regressor: `O(N²)`. -- Memory: `N` spatial velocities/accelerations and one `6×10` block; stack only; no recursion. - -## Numerical / embedded notes - -- With `q̇r = q̇, q̈r = q̈` the pass reduces to ordinary inverse dynamics — test against RNEA. -- `C` is the Christoffel form: `C(q,q̇)q̇` equals the RNEA bias and `Ṁ − 2C` is skew-symmetric; both - were verified numerically against finite-difference Christoffel symbols (residual ~1e-11 in double). -- The 10-parameter-per-link vector is not identifiable as a whole; M22 reduces it to base parameters. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Headers: `robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp`, `CoriolisMatrix.hpp`, - `InertialRegressor.hpp` (interface), `ChainInertialRegressor.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on the passes, and `extern template` - declarations for `` and `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: matching `.cpp` files with the same instantiations. -- Tests: `robotics/dynamics/test/TestCoriolisMatrix.cpp`, `TestChainInertialRegressor.cpp` -- Doc: `doc/dynamics/CoriolisMatrixAndRegressor.md` (per `doc/TEMPLATE.md`) -- Shares the spatial helpers factored out for M28 (CRBA) and the shipped ABA. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md deleted file mode 100644 index a10675d..0000000 --- a/roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md +++ /dev/null @@ -1,47 +0,0 @@ -# Coriolis Matrix and Inertial Regressor — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestCoriolisMatrix : public ::testing::Test: - # 3-link chain: skewed unit axes, full SPD inertia tensors, off-axis CoM, arbitrary offsets - std::array, 3> chain{ ... } - CoriolisMatrix coriolis - ModifiedRecursiveNewtonEuler modified - RecursiveNewtonEuler rnea - CompositeRigidBodyAlgorithm crba -# each case below is a TEST_F(TestCoriolisMatrix, ) (regressor cases in TestChainInertialRegressor) -``` - -## Test cases (Arrange / Act / Assert) - -```text -reduces_to_rnea_when_reference_equals_actual: - Assert: modified.InverseDynamics(q, q̇, q̇, q̈, g) ≈ rnea.InverseDynamics(q, q̇, q̈, g) -coriolis_times_velocity_equals_rnea_bias: - Assert: C(q, q̇)·q̇ ≈ rnea.InverseDynamics(q, q̇, 0, 0) -matches_christoffel_symbols_of_the_mass_matrix: - Arrange: Γ_ijk = ½(∂M_ij/∂q_k + ∂M_ik/∂q_j − ∂M_jk/∂q_i) by central differences of crba.Compute - Assert: C_ij ≈ Σ_k Γ_ijk q̇_k -mass_matrix_derivative_minus_twice_coriolis_is_skew: - Assert: N = Ṁ − 2C (Ṁ by finite difference along q̇) satisfies N + Nᵀ ≈ 0 -two_link_planar_closed_form: - Assert: C = [[−h q̇2, −h(q̇1 + q̇2)], [h q̇1, 0]], h = m2 l1 lc2 sin q2 (uniform rods, lc2 = l2/2) -regressor_times_parameters_reproduces_dynamics: - Arrange: π = ChainInertialRegressor::Parameters(chain) - Assert: Y(q, q̇, q̇r, q̈r)·π ≈ modified.InverseDynamics(q, q̇, q̇r, q̈r, g) -regressor_is_independent_of_parameters: - Arrange: second chain with different masses/inertias but the same geometry - Assert: Y is identical for both chains (only π differs) -``` - -## Reference vectors - -- Planar 2R with uniform rods: `C` as above; `C q̇ = [−h(2q̇1q̇2 + q̇2²), h q̇1²]`. - -## Edge cases - -- `q̇ = 0` ⇒ `C = 0`. -- Single link ⇒ `C = 0` for any `q̇` (no velocity coupling about a fixed axis). diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index 5f023f7..7d92732 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -14,12 +14,16 @@ target_link_libraries(robotics.dynamics ${ROBOTICS_VISIBILITY} target_sources(robotics.dynamics PRIVATE ArticulatedBodyAlgorithm.hpp ChainDynamicsModel.hpp + ChainInertialRegressor.hpp CompositeRigidBodyAlgorithm.hpp + CoriolisMatrix.hpp EulerLagrangeDynamics.hpp EulerLagrangeSolver.hpp ForwardDynamicsIntegrator.hpp + InertialRegressor.hpp InverseDynamicsModel.hpp JointLink.hpp + ModifiedRecursiveNewtonEuler.hpp NewtonEulerBody.hpp NewtonEulerSolver.hpp RecursiveNewtonEuler.hpp @@ -30,10 +34,13 @@ target_sources(robotics.dynamics PRIVATE robotics_add_coverage_sources(robotics.dynamics ArticulatedBodyAlgorithm.cpp ChainDynamicsModel.cpp + ChainInertialRegressor.cpp CompositeRigidBodyAlgorithm.cpp + CoriolisMatrix.cpp EulerLagrangeSolver.cpp ForwardDynamicsIntegrator.cpp JointLink.cpp + ModifiedRecursiveNewtonEuler.cpp NewtonEulerSolver.cpp RecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/ChainInertialRegressor.cpp b/robotics/dynamics/ChainInertialRegressor.cpp new file mode 100644 index 0000000..1f783a6 --- /dev/null +++ b/robotics/dynamics/ChainInertialRegressor.cpp @@ -0,0 +1,7 @@ +#include "robotics/dynamics/ChainInertialRegressor.hpp" + +namespace dynamics +{ + template class ChainInertialRegressor; + template class ChainInertialRegressor; +} diff --git a/robotics/dynamics/ChainInertialRegressor.hpp b/robotics/dynamics/ChainInertialRegressor.hpp new file mode 100644 index 0000000..fa6ae34 --- /dev/null +++ b/robotics/dynamics/ChainInertialRegressor.hpp @@ -0,0 +1,150 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/InertialRegressor.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp" +#include "robotics/dynamics/SpatialInertia.hpp" +#include +#include +#include + +namespace dynamics +{ + template + class ChainInertialRegressor + : public InertialRegressor + { + static_assert(std::is_floating_point_v, "ChainInertialRegressor supports floating-point types"); + + public: + static constexpr std::size_t ParametersPerLink{ 10 }; + + using Vector3 = math::Vector; + using JointVector = math::Vector; + using Regressor = math::Matrix; + using ParameterVector = math::Vector; + using LinkArray = std::array, NumLinks>; + + ChainInertialRegressor(const LinkArray& links, const Vector3& gravity); + + OPTIMIZE_FOR_SPEED Regressor Compute(const JointVector& q, const JointVector& qDot, const JointVector& qDotReference, const JointVector& qDDotReference) const override; + + static ParameterVector Parameters(const LinkArray& links); + + private: + using Columns = std::array, ParametersPerLink>; + using Kinematics = ModifiedRecursiveNewtonEuler; + + static Columns ParameterColumns(const SpatialMotion& motion); + static Columns LinkColumns(const ReferenceMotion& motion, std::size_t link); + void Project(Regressor& regressor, Columns columns, const ReferenceMotion& motion, std::size_t link) const; + + const LinkArray& links; + Vector3 gravity; + }; + + template + ChainInertialRegressor::ChainInertialRegressor(const LinkArray& links, const Vector3& gravity) + : links{ links } + , gravity{ gravity } + {} + + template + OPTIMIZE_FOR_SPEED typename ChainInertialRegressor::Regressor ChainInertialRegressor::Compute(const JointVector& q, const JointVector& qDot, + const JointVector& qDotReference, const JointVector& qDDotReference) const + { + const auto motion{ Kinematics::ComputeReferenceMotion(links, q, qDot, qDotReference, qDDotReference, gravity) }; + + Regressor regressor{}; + for (std::size_t i = 0; i < NumLinks; ++i) + Project(regressor, LinkColumns(motion, i), motion, i); + + return regressor; + } + + template + typename ChainInertialRegressor::ParameterVector ChainInertialRegressor::Parameters(const LinkArray& links) + { + ParameterVector parameters{}; + for (std::size_t i = 0; i < NumLinks; ++i) + { + const auto& link{ links[i] }; + const auto aboutJoint{ RigidBodyInertia(link).rot }; + const Vector3 firstMoment{ link.jointToCoM * link.mass }; + const std::array values{ link.mass, firstMoment[0], firstMoment[1], firstMoment[2], + aboutJoint.at(0, 0), aboutJoint.at(0, 1), aboutJoint.at(0, 2), aboutJoint.at(1, 1), aboutJoint.at(1, 2), aboutJoint.at(2, 2) }; + for (std::size_t k = 0; k < ParametersPerLink; ++k) + parameters[ParametersPerLink * i + k] = values[k]; + } + + return parameters; + } + + template + typename ChainInertialRegressor::Columns ChainInertialRegressor::ParameterColumns(const SpatialMotion& motion) + { + const Vector3& w{ motion.angular }; + const Vector3& u{ motion.linear }; + const std::array axes{ Vector3{ T{ 1 }, T{}, T{} }, Vector3{ T{}, T{ 1 }, T{} }, Vector3{ T{}, T{}, T{ 1 } } }; + + Columns columns{}; + columns[0] = SpatialWrench{ u, Vector3{} }; + for (std::size_t k = 0; k < 3; ++k) + columns[1 + k] = SpatialWrench{ math::CrossProduct(w, axes[k]), math::CrossProduct(axes[k], u) }; + + columns[4] = SpatialWrench{ Vector3{}, Vector3{ w[0], T{}, T{} } }; + columns[5] = SpatialWrench{ Vector3{}, Vector3{ w[1], w[0], T{} } }; + columns[6] = SpatialWrench{ Vector3{}, Vector3{ w[2], T{}, w[0] } }; + columns[7] = SpatialWrench{ Vector3{}, Vector3{ T{}, w[1], T{} } }; + columns[8] = SpatialWrench{ Vector3{}, Vector3{ T{}, w[2], w[1] } }; + columns[9] = SpatialWrench{ Vector3{}, Vector3{ T{}, T{}, w[2] } }; + return columns; + } + + template + typename ChainInertialRegressor::Columns ChainInertialRegressor::LinkColumns(const ReferenceMotion& motion, std::size_t link) + { + const auto& v{ motion.velocity[link] }; + const auto& vr{ motion.referenceVelocity[link] }; + const auto inertial{ ParameterColumns(motion.acceleration[link]) }; + const auto ofReference{ ParameterColumns(vr) }; + const auto ofCross{ ParameterColumns(MotionCross(v, vr)) }; + const auto ofVelocity{ ParameterColumns(v) }; + const T half{ T{ 0.5f } }; + + Columns columns{}; + for (std::size_t k = 0; k < ParametersPerLink; ++k) + { + const auto first{ ForceCross(v, ofReference[k]) }; + const auto third{ ForceCross(vr, ofVelocity[k]) }; + columns[k] = SpatialWrench{ inertial[k].force + (first.force - ofCross[k].force + third.force) * half, + inertial[k].torque + (first.torque - ofCross[k].torque + third.torque) * half }; + } + + return columns; + } + + template + void ChainInertialRegressor::Project(Regressor& regressor, Columns columns, const ReferenceMotion& motion, std::size_t link) const + { + for (std::size_t j = link + 1; j > 0; --j) + { + const std::size_t joint{ j - 1 }; + const auto subspace{ Kinematics::MotionSubspace(links[joint]) }; + for (std::size_t k = 0; k < ParametersPerLink; ++k) + { + regressor.at(joint, ParametersPerLink * link + k) = math::DotProduct(subspace.angular, columns[k].torque) + math::DotProduct(subspace.linear, columns[k].force); + if (joint > 0) + columns[k] = TransformWrenchToParent(columns[k], motion.rotation[joint], motion.offset[joint]); + } + } + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ChainInertialRegressor; + extern template class ChainInertialRegressor; +#endif +} diff --git a/robotics/dynamics/CoriolisMatrix.cpp b/robotics/dynamics/CoriolisMatrix.cpp new file mode 100644 index 0000000..1306765 --- /dev/null +++ b/robotics/dynamics/CoriolisMatrix.cpp @@ -0,0 +1,7 @@ +#include "robotics/dynamics/CoriolisMatrix.hpp" + +namespace dynamics +{ + template class CoriolisMatrix; + template class CoriolisMatrix; +} diff --git a/robotics/dynamics/CoriolisMatrix.hpp b/robotics/dynamics/CoriolisMatrix.hpp new file mode 100644 index 0000000..62f0691 --- /dev/null +++ b/robotics/dynamics/CoriolisMatrix.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp" +#include +#include +#include + +namespace dynamics +{ + template + class CoriolisMatrix + { + static_assert(std::is_floating_point_v, "CoriolisMatrix supports floating-point types"); + + public: + using JointVector = math::Vector; + using Matrix = math::SquareMatrix; + using LinkArray = std::array, NumLinks>; + + Matrix Compute(const LinkArray& links, const JointVector& q, const JointVector& qDot) const; + JointVector TransposeTimesVelocity(const LinkArray& links, const JointVector& q, const JointVector& qDot) const; + + private: + [[no_unique_address]] ModifiedRecursiveNewtonEuler modified; + }; + + template + typename CoriolisMatrix::Matrix CoriolisMatrix::Compute(const LinkArray& links, const JointVector& q, const JointVector& qDot) const + { + Matrix coriolis{}; + for (std::size_t j = 0; j < NumLinks; ++j) + { + JointVector unit{}; + unit[j] = T{ 1 }; + coriolis.SetBlock(modified.InverseDynamics(links, q, qDot, unit, JointVector{}, math::Vector{}), 0, j); + } + + return coriolis; + } + + template + typename CoriolisMatrix::JointVector CoriolisMatrix::TransposeTimesVelocity(const LinkArray& links, const JointVector& q, const JointVector& qDot) const + { + return Compute(links, q, qDot).Transpose() * qDot; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class CoriolisMatrix; + extern template class CoriolisMatrix; +#endif +} diff --git a/robotics/dynamics/InertialRegressor.hpp b/robotics/dynamics/InertialRegressor.hpp new file mode 100644 index 0000000..b4250c1 --- /dev/null +++ b/robotics/dynamics/InertialRegressor.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace dynamics +{ + template + class InertialRegressor + { + static_assert(std::is_floating_point_v, "InertialRegressor supports floating-point types"); + + public: + using JointVector = math::Vector; + using Regressor = math::Matrix; + + virtual Regressor Compute(const JointVector& q, const JointVector& qDot, const JointVector& qDotReference, const JointVector& qDDotReference) const = 0; + + protected: + ~InertialRegressor() = default; + }; +} diff --git a/robotics/dynamics/ModifiedRecursiveNewtonEuler.cpp b/robotics/dynamics/ModifiedRecursiveNewtonEuler.cpp new file mode 100644 index 0000000..a3757ce --- /dev/null +++ b/robotics/dynamics/ModifiedRecursiveNewtonEuler.cpp @@ -0,0 +1,7 @@ +#include "robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp" + +namespace dynamics +{ + template class ModifiedRecursiveNewtonEuler; + template class ModifiedRecursiveNewtonEuler; +} diff --git a/robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp b/robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp new file mode 100644 index 0000000..c536e7b --- /dev/null +++ b/robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp @@ -0,0 +1,138 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/dynamics/SpatialInertia.hpp" +#include +#include +#include +#include + +namespace dynamics +{ + template + struct ReferenceMotion + { + static_assert(std::is_floating_point_v, "ReferenceMotion supports floating-point types"); + + std::array, NumLinks> velocity{}; + std::array, NumLinks> referenceVelocity{}; + std::array, NumLinks> acceleration{}; + std::array, NumLinks> rotation{}; + std::array, NumLinks> offset{}; + }; + + template + class ModifiedRecursiveNewtonEuler + { + static_assert(std::is_floating_point_v, "ModifiedRecursiveNewtonEuler supports floating-point types"); + static_assert(NumLinks > 0, "Number of links must be positive"); + + public: + using Vector3 = math::Vector; + using JointVector = math::Vector; + using LinkArray = std::array, NumLinks>; + using Motion = ReferenceMotion; + + OPTIMIZE_FOR_SPEED JointVector InverseDynamics(const LinkArray& links, const JointVector& q, const JointVector& qDot, + const JointVector& qDotReference, const JointVector& qDDotReference, const Vector3& gravity) const; + + static Motion ComputeReferenceMotion(const LinkArray& links, const JointVector& q, const JointVector& qDot, + const JointVector& qDotReference, const JointVector& qDDotReference, const Vector3& gravity); + static SpatialMotion MotionSubspace(const JointLink& link); + + private: + static SpatialWrench BodyWrench(const SpatialInertia& inertia, const Motion& motion, std::size_t link); + }; + + template + OPTIMIZE_FOR_SPEED typename ModifiedRecursiveNewtonEuler::JointVector ModifiedRecursiveNewtonEuler::InverseDynamics(const LinkArray& links, + const JointVector& q, const JointVector& qDot, const JointVector& qDotReference, const JointVector& qDDotReference, const Vector3& gravity) const + { + const auto motion{ ComputeReferenceMotion(links, q, qDot, qDotReference, qDDotReference, gravity) }; + + std::array, NumLinks> wrench{}; + for (std::size_t i = 0; i < NumLinks; ++i) + wrench[i] = BodyWrench(RigidBodyInertia(links[i]), motion, i); + + JointVector tau{}; + for (std::size_t j = NumLinks; j > 0; --j) + { + const std::size_t i{ j - 1 }; + const auto subspace{ MotionSubspace(links[i]) }; + tau[i] = math::DotProduct(subspace.angular, wrench[i].torque) + math::DotProduct(subspace.linear, wrench[i].force) + links[i].armature * qDDotReference[i]; + + if (i > 0) + { + const auto parent{ TransformWrenchToParent(wrench[i], motion.rotation[i], motion.offset[i]) }; + wrench[i - 1].force = wrench[i - 1].force + parent.force; + wrench[i - 1].torque = wrench[i - 1].torque + parent.torque; + } + } + + return tau; + } + + template + typename ModifiedRecursiveNewtonEuler::Motion ModifiedRecursiveNewtonEuler::ComputeReferenceMotion(const LinkArray& links, + const JointVector& q, const JointVector& qDot, const JointVector& qDotReference, const JointVector& qDDotReference, const Vector3& gravity) + { + Motion motion{}; + SpatialMotion velocity{}; + SpatialMotion referenceVelocity{}; + SpatialMotion acceleration{ Vector3{}, gravity * T{ -1 } }; + + for (std::size_t i = 0; i < NumLinks; ++i) + { + assert(HasUnitJointAxis(links[i])); + const auto transform{ links[i].JointTransform(q[i]) }; + const auto subspace{ MotionSubspace(links[i]) }; + motion.rotation[i] = transform.rotation; + motion.offset[i] = transform.translation; + + velocity = TransformMotionToChild(velocity, transform.rotation, transform.translation); + velocity += subspace * qDot[i]; + referenceVelocity = TransformMotionToChild(referenceVelocity, transform.rotation, transform.translation); + referenceVelocity += subspace * qDotReference[i]; + acceleration = TransformMotionToChild(acceleration, transform.rotation, transform.translation); + acceleration += subspace * qDDotReference[i]; + acceleration += MotionCross(velocity, subspace) * qDotReference[i]; + + motion.velocity[i] = velocity; + motion.referenceVelocity[i] = referenceVelocity; + motion.acceleration[i] = acceleration; + } + + return motion; + } + + template + SpatialMotion ModifiedRecursiveNewtonEuler::MotionSubspace(const JointLink& link) + { + return SpatialMotion{ link.AngularAxis(), link.LinearAxis() }; + } + + template + SpatialWrench ModifiedRecursiveNewtonEuler::BodyWrench(const SpatialInertia& inertia, const Motion& motion, std::size_t link) + { + const auto& v{ motion.velocity[link] }; + const auto& vr{ motion.referenceVelocity[link] }; + const auto inertial{ Apply(inertia, motion.acceleration[link]) }; + const auto first{ ForceCross(v, Apply(inertia, vr)) }; + const auto second{ Apply(inertia, MotionCross(v, vr)) }; + const auto third{ ForceCross(vr, Apply(inertia, v)) }; + const T half{ T{ 0.5f } }; + + return SpatialWrench{ + inertial.force + (first.force - second.force + third.force) * half, + inertial.torque + (first.torque - second.torque + third.torque) * half + }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ModifiedRecursiveNewtonEuler; + extern template class ModifiedRecursiveNewtonEuler; +#endif +} diff --git a/robotics/dynamics/SpatialInertia.hpp b/robotics/dynamics/SpatialInertia.hpp index 77d64b1..4e3e476 100644 --- a/robotics/dynamics/SpatialInertia.hpp +++ b/robotics/dynamics/SpatialInertia.hpp @@ -34,6 +34,61 @@ namespace dynamics math::Vector torque; }; + template + struct SpatialMotion + { + static_assert(std::is_floating_point_v, "SpatialMotion supports floating-point types"); + + math::Vector angular; + math::Vector linear; + + SpatialMotion& operator+=(const SpatialMotion& other) + { + angular = angular + other.angular; + linear = linear + other.linear; + return *this; + } + + SpatialMotion operator*(T scale) const + { + return SpatialMotion{ angular * scale, linear * scale }; + } + }; + + template + SpatialMotion MotionCross(const SpatialMotion& velocity, const SpatialMotion& motion) + { + return SpatialMotion{ + math::CrossProduct(velocity.angular, motion.angular), + math::CrossProduct(velocity.linear, motion.angular) + math::CrossProduct(velocity.angular, motion.linear) + }; + } + + template + SpatialWrench ForceCross(const SpatialMotion& velocity, const SpatialWrench& wrench) + { + return SpatialWrench{ + math::CrossProduct(velocity.angular, wrench.force), + math::CrossProduct(velocity.angular, wrench.torque) + math::CrossProduct(velocity.linear, wrench.force) + }; + } + + template + SpatialWrench Apply(const SpatialInertia& inertia, const SpatialMotion& motion) + { + return SpatialWrench{ + inertia.cross.Transpose() * motion.angular + inertia.lin * motion.linear, + inertia.rot * motion.angular + inertia.cross * motion.linear + }; + } + + template + SpatialMotion TransformMotionToChild(const SpatialMotion& motion, const math::SquareMatrix& R, const math::Vector& parentToJoint) + { + const auto Rt{ R.Transpose() }; + return SpatialMotion{ Rt * motion.angular, Rt * (motion.linear + math::CrossProduct(motion.angular, parentToJoint)) }; + } + template SpatialInertia RigidBodyInertia(const JointLink& link) { diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index 14fa6d8..ee37563 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -11,7 +11,9 @@ target_link_libraries(robotics.dynamics_test PUBLIC target_sources(robotics.dynamics_test PRIVATE TestArticulatedBodyAlgorithm.cpp TestChainDynamicsModel.cpp + TestChainInertialRegressor.cpp TestCompositeRigidBodyAlgorithm.cpp + TestCoriolisMatrix.cpp TestEulerLagrangeSolver.cpp TestForwardDynamicsIntegrator.cpp TestJointLink.cpp diff --git a/robotics/dynamics/test/TestChainInertialRegressor.cpp b/robotics/dynamics/test/TestChainInertialRegressor.cpp new file mode 100644 index 0000000..36acab4 --- /dev/null +++ b/robotics/dynamics/test/TestChainInertialRegressor.cpp @@ -0,0 +1,69 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/ChainInertialRegressor.hpp" +#include "robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector; + using Matrix3 = math::SquareMatrix; + using Joints = math::Vector; + using Link = dynamics::JointLink; + using Regressor = dynamics::ChainInertialRegressor; + + const Vector3 gravity{ 0.0f, 0.0f, -9.81f }; + + Vector3 Normalized(float x, float y, float z) + { + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Vector3{ x / norm, y / norm, z / norm }; + } + + class TestChainInertialRegressor + : public ::testing::Test + { + protected: + std::array chain{ + Link{ 1.2f, Matrix3{ { 0.020f, 0.001f, -0.002f }, { 0.001f, 0.030f, 0.003f }, { -0.002f, 0.003f, 0.025f } }, Vector3{ 0.0f, 0.0f, 1.0f }, Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.05f, 0.02f, 0.1f } }, + Link{ 0.9f, Matrix3{ { 0.015f, -0.002f, 0.001f }, { -0.002f, 0.020f, 0.0f }, { 0.001f, 0.0f, 0.012f } }, Normalized(0.0f, 1.0f, 1.0f), Vector3{ 0.1f, 0.0f, 0.2f }, Vector3{ 0.15f, -0.03f, 0.02f } }, + Link{ 0.6f, Matrix3{ { 0.008f, 0.0f, 0.001f }, { 0.0f, 0.010f, -0.001f }, { 0.001f, -0.001f, 0.006f } }, Normalized(1.0f, 0.0f, 1.0f), Vector3{ 0.3f, 0.05f, 0.0f }, Vector3{ 0.1f, 0.04f, -0.05f } } + }; + Joints q{ 0.4f, -0.7f, 1.1f }; + Joints qDot{ 0.9f, -1.3f, 0.6f }; + Joints qDotReference{ 0.5f, 0.8f, -1.1f }; + Joints qDDotReference{ 1.5f, -2.0f, 0.7f }; + }; +} + +TEST_F(TestChainInertialRegressor, regressor_times_parameters_reproduces_dynamics) +{ + const Regressor regressor{ chain, gravity }; + + const auto Y{ regressor.Compute(q, qDot, qDotReference, qDDotReference) }; + const auto expected{ dynamics::ModifiedRecursiveNewtonEuler{}.InverseDynamics(chain, q, qDot, qDotReference, qDDotReference, gravity) }; + + const Joints reproduced{ Y * Regressor::Parameters(chain) }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(reproduced[i], expected[i], math::Tolerance()); +} + +TEST_F(TestChainInertialRegressor, regressor_is_independent_of_parameters) +{ + auto heavier{ chain }; + for (auto& link : heavier) + { + link.mass *= 2.5f; + link.inertia = link.inertia * 1.7f; + link.jointToCoM = link.jointToCoM * 0.6f; + } + const Regressor original{ chain, gravity }; + const Regressor modified{ heavier, gravity }; + + const auto first{ original.Compute(q, qDot, qDotReference, qDDotReference) }; + const auto second{ modified.Compute(q, qDot, qDotReference, qDDotReference) }; + + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 30; ++c) + EXPECT_EQ(first.at(r, c), second.at(r, c)); +} diff --git a/robotics/dynamics/test/TestCoriolisMatrix.cpp b/robotics/dynamics/test/TestCoriolisMatrix.cpp new file mode 100644 index 0000000..4f75cb1 --- /dev/null +++ b/robotics/dynamics/test/TestCoriolisMatrix.cpp @@ -0,0 +1,124 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" +#include "robotics/dynamics/CoriolisMatrix.hpp" +#include "robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp" +#include "robotics/dynamics/RecursiveNewtonEuler.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector; + using Matrix3 = math::SquareMatrix; + using Joints = math::Vector; + using Link = dynamics::JointLink; + + const Vector3 gravity{ 0.0f, 0.0f, -9.81f }; + constexpr float finiteDifferenceTolerance{ 1e-4f }; + + Vector3 Normalized(float x, float y, float z) + { + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Vector3{ x / norm, y / norm, z / norm }; + } + + class TestCoriolisMatrix + : public ::testing::Test + { + protected: + Matrix3 MassMatrixRate(const Joints& position, const Joints& velocity) const + { + constexpr float step{ 1e-3f }; + const auto ahead{ crba.Compute(chain, position + velocity * step) }; + const auto behind{ crba.Compute(chain, position - velocity * step) }; + return Matrix3{ (ahead - behind) * (0.5f / step) }; + } + + std::array chain{ + Link{ 1.2f, Matrix3{ { 0.020f, 0.001f, -0.002f }, { 0.001f, 0.030f, 0.003f }, { -0.002f, 0.003f, 0.025f } }, Vector3{ 0.0f, 0.0f, 1.0f }, Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.05f, 0.02f, 0.1f } }, + Link{ 0.9f, Matrix3{ { 0.015f, -0.002f, 0.001f }, { -0.002f, 0.020f, 0.0f }, { 0.001f, 0.0f, 0.012f } }, Normalized(0.0f, 1.0f, 1.0f), Vector3{ 0.1f, 0.0f, 0.2f }, Vector3{ 0.15f, -0.03f, 0.02f } }, + Link{ 0.6f, Matrix3{ { 0.008f, 0.0f, 0.001f }, { 0.0f, 0.010f, -0.001f }, { 0.001f, -0.001f, 0.006f } }, Normalized(1.0f, 0.0f, 1.0f), Vector3{ 0.3f, 0.05f, 0.0f }, Vector3{ 0.1f, 0.04f, -0.05f } } + }; + dynamics::CoriolisMatrix coriolis; + dynamics::ModifiedRecursiveNewtonEuler modified; + dynamics::RecursiveNewtonEuler rnea; + dynamics::CompositeRigidBodyAlgorithm crba; + Joints q{ 0.4f, -0.7f, 1.1f }; + Joints qDot{ 0.9f, -1.3f, 0.6f }; + }; +} + +TEST_F(TestCoriolisMatrix, reduces_to_rnea_when_reference_equals_actual) +{ + const Joints qDDot{ 1.5f, -2.0f, 0.7f }; + + const auto fromModified{ modified.InverseDynamics(chain, q, qDot, qDot, qDDot, gravity) }; + const auto fromRnea{ rnea.InverseDynamics(chain, q, qDot, qDDot, gravity) }; + + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(fromModified[i], fromRnea[i], math::Tolerance()); +} + +TEST_F(TestCoriolisMatrix, coriolis_times_velocity_equals_rnea_bias) +{ + const auto product{ Joints{ coriolis.Compute(chain, q, qDot) * qDot } }; + const auto bias{ rnea.InverseDynamics(chain, q, qDot, Joints{}, Vector3{}) }; + + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(product[i], bias[i], math::Tolerance()); +} + +TEST_F(TestCoriolisMatrix, matches_christoffel_symbols_of_the_mass_matrix) +{ + constexpr float step{ 1e-3f }; + std::array derivative{}; + for (std::size_t k = 0; k < 3; ++k) + { + Joints unit{}; + unit[k] = step; + derivative[k] = Matrix3{ (crba.Compute(chain, q + unit) - crba.Compute(chain, q - unit)) * (0.5f / step) }; + } + + const auto C{ coriolis.Compute(chain, q, qDot) }; + + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + { + float expected{ 0.0f }; + for (std::size_t k = 0; k < 3; ++k) + expected += 0.5f * (derivative[k].at(i, j) + derivative[j].at(i, k) - derivative[i].at(j, k)) * qDot[k]; + EXPECT_NEAR(C.at(i, j), expected, finiteDifferenceTolerance); + } +} + +TEST_F(TestCoriolisMatrix, mass_matrix_derivative_minus_twice_coriolis_is_skew) +{ + const Matrix3 n{ MassMatrixRate(q, qDot) - coriolis.Compute(chain, q, qDot) * 2.0f }; + + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + EXPECT_NEAR(n.at(i, j) + n.at(j, i), 0.0f, finiteDifferenceTolerance); +} + +TEST_F(TestCoriolisMatrix, two_link_planar_closed_form) +{ + const float m2{ 0.7f }; + const float l1{ 0.6f }; + const float l2{ 0.45f }; + const auto rod{ [](float mass, float length, const Vector3& parentToJoint) + { + const float inertia{ mass * length * length / 12.0f }; + return Link{ mass, Matrix3{ { 0.0f, 0.0f, 0.0f }, { 0.0f, inertia, 0.0f }, { 0.0f, 0.0f, inertia } }, Vector3{ 0.0f, 0.0f, 1.0f }, parentToJoint, Vector3{ length / 2.0f, 0.0f, 0.0f } }; + } }; + const std::array planar{ rod(1.2f, l1, Vector3{}), rod(m2, l2, Vector3{ l1, 0.0f, 0.0f }) }; + const math::Vector position{ 0.3f, -0.8f }; + const math::Vector velocity{ 1.1f, -0.6f }; + const float h{ m2 * l1 * (l2 / 2.0f) * std::sin(position[1]) }; + + const auto C{ dynamics::CoriolisMatrix{}.Compute(planar, position, velocity) }; + + EXPECT_NEAR(C.at(0, 0), -h * velocity[1], math::Tolerance()); + EXPECT_NEAR(C.at(0, 1), -h * (velocity[0] + velocity[1]), math::Tolerance()); + EXPECT_NEAR(C.at(1, 0), h * velocity[0], math::Tolerance()); + EXPECT_NEAR(C.at(1, 1), 0.0f, math::Tolerance()); +} From ec48168b2ddc259da10324dbd1ecef19318a5676 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 12:49:23 +0000 Subject: [PATCH 4/9] feat(dynamics): add MomentumObserver (M16) Generalized-momentum residual on the M29 model and the M31 C^T q-dot product: a first-order estimate of the external joint torque with per-joint collision thresholds. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +- ROADMAP.md | 4 - doc/dynamics/MomentumObserver.md | 100 ++++++++++++++ doc/dynamics/README.md | 23 ++-- roadmap/README.md | 2 +- .../dynamics/MomentumObserver/explanation.md | 31 ----- .../MomentumObserver/implementation.md | 67 --------- roadmap/dynamics/MomentumObserver/tests.md | 42 ------ robotics/dynamics/CMakeLists.txt | 2 + robotics/dynamics/MomentumObserver.cpp | 7 + robotics/dynamics/MomentumObserver.hpp | 98 +++++++++++++ robotics/dynamics/test/CMakeLists.txt | 1 + .../dynamics/test/TestMomentumObserver.cpp | 129 ++++++++++++++++++ 13 files changed, 357 insertions(+), 163 deletions(-) create mode 100644 doc/dynamics/MomentumObserver.md delete mode 100644 roadmap/dynamics/MomentumObserver/explanation.md delete mode 100644 roadmap/dynamics/MomentumObserver/implementation.md delete mode 100644 roadmap/dynamics/MomentumObserver/tests.md create mode 100644 robotics/dynamics/MomentumObserver.cpp create mode 100644 robotics/dynamics/MomentumObserver.hpp create mode 100644 robotics/dynamics/test/TestMomentumObserver.cpp diff --git a/README.md b/README.md index cd71aa4..24dca60 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Forward-Dynamics Integration, Generic Joint Link | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | +| Category | Description | +|--------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 7b8aed8..d7dab9e 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -120,7 +120,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | -| M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | | M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | | M22 | Dynamic (base-parameter) identification | `dynamics` | M31, NT63 | ★★★☆☆ | | M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | @@ -145,9 +144,6 @@ retired here. - *Algorithm / paper:* S. R. Buss (2004); Nakamura & Hanafusa (1986); Chiaverini (1997). - *Reuses / builds on:* upstream `optimization::LevenbergMarquardt` (NT64) with a manipulability damping policy. -**M16. Momentum-based collision-detection observer.** External joint-torque estimate from the generalized-momentum residual, using `Cᵀq̇`. -- *Algorithm / paper:* A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, *IROS*, 2006. - **M17. Impedance control.** Stiffness/damping impedance via `Jᵀ` (no force sensor), and inertia-shaping impedance with measured contact wrench via the task-space inertia. - *Algorithm / paper:* N. Hogan, *ASME J. Dyn. Sys. Meas. Control*, 1985. diff --git a/doc/dynamics/MomentumObserver.md b/doc/dynamics/MomentumObserver.md new file mode 100644 index 0000000..1f18922 --- /dev/null +++ b/doc/dynamics/MomentumObserver.md @@ -0,0 +1,100 @@ +# Generalized-Momentum Observer + +## Overview & Motivation + +A collaborative robot must notice a collision within milliseconds, from the signals it already has: the commanded joint torques and the measured joint positions and velocities. Joint-torque sensors are expensive, and differentiating velocities to estimate accelerations amplifies noise until the estimate is useless. + +The **generalized-momentum observer** sidesteps both. It tracks the momentum $p = M(q)\dot{q}$, which changes only because of known torques and the unknown external torque. The mismatch between the measured momentum and the integral of everything the model explains converges to the external torque with a first-order lag. +The result is a **residual** per joint, an estimate of the contact torque that serves for detection, isolation of the touched link, and reaction strategies. + +## Mathematical Theory + +### Momentum Dynamics + +With $\tau_{\text{ext}}$ the joint torque produced by contact, the manipulator dynamics are + +$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau_{\text{ext}}$$ + +Differentiating $p = M\dot{q}$ and using the Christoffel property $\dot{M} = C + C^T$ gives + +$$\dot{p} = \tau + C^T(q,\dot{q})\,\dot{q} - g(q) + \tau_{\text{ext}}$$ + +No acceleration appears. This is why the observer needs $C^T\dot{q}$, not the Coriolis vector $C\dot{q}$ that the recursive Newton–Euler algorithm returns. + +### Residual + +With $\beta(q,\dot{q}) = g(q) - C^T(q,\dot{q})\dot{q}$ and a diagonal gain $K_O$, the residual is + +$$r(t) = K_O \left( p(t) - p(0) - \int_0^t \left(\tau - \beta + r\right) ds \right)$$ + +Differentiating shows $\dot{r} = K_O(\tau_{\text{ext}} - r)$: each component is a first-order low-pass of the external torque with bandwidth $K_{O,i}$. For a step $\tau_{\text{ext}}$ applied at $t_0$, + +$$r(t) = \tau_{\text{ext}} \left(1 - e^{-K_O (t - t_0)}\right)$$ + +### Discrete Update + +At each control period $\Delta t$ the integral is advanced by forward Euler: + +$$\begin{aligned} +I_{k+1} &= I_k + (\tau_k - \beta_{k+1} + r_k)\,\Delta t \\ +r_{k+1} &= K_O \left( M(q_{k+1})\dot{q}_{k+1} - p_0 - I_{k+1} \right) +\end{aligned}$$ + +It is stable while $K_O \Delta t \ll 1$. The discretization leaves an $O(\Delta t)$ residual during collision-free motion, which grows with joint speed. + +### Detection + +A collision is declared when any joint exceeds its threshold, $|r_i| > \bar{r}_i$. Thresholds sit above the largest residual seen in collision-free runs, which reflects model error, friction and discretization. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------|----------|----------|--------------------------------------------------| +| `Update` | $O(n^2)$ | $O(n^2)$ | Mass matrix and $C^T\dot{q}$ dominate | +| `Reset` | $O(n^2)$ | $O(n)$ | One mass-matrix product | +| `Collision` | $O(n)$ | $O(1)$ | One comparison per joint | +| Observer state | — | $O(n)$ | Initial momentum, integral and residual | + +## Step-by-Step Walkthrough + +A planar two-link arm of uniform rods (masses $2$ and $1.5$ kg, lengths $1$ and $0.8$ m) is simulated at $\Delta t = 1$ ms with gain $K_O = 50\ \text{s}^{-1}$, so $K_O\Delta t = 0.05$. + +1. **Free motion.** Gravity-compensated sinusoidal torques drive the arm for one second. The residual stays below $10^{-2}$ N·m on both joints. +2. **Step contact.** At $t = 0.2$ s a torque of $0.8$ N·m is applied to joint 2. After one time constant ($20$ ms), $r_2 \approx 0.8(1 - e^{-1}) = 0.506$ N·m. After $0.3$ s it has settled at $0.8$ N·m, and $r_1$ stays near zero. +3. **Detection.** With thresholds $(0.5, 0.5)$ N·m, the flag is clear before the contact and set once the residual settles. +4. **Why $C^T\dot{q}$.** At $\dot{q} = (3, -4)$ rad/s, $C^T\dot{q}$ and $C\dot{q}$ differ by several N·m. An observer built on $C\dot{q}$ would report that difference as a phantom contact, while the correct observer's residual stays a few percent of it. + +## Pitfalls & Edge Cases + +- **Wrong Coriolis term**: substituting the recursive Newton–Euler vector $C\dot{q}$ for $C^T\dot{q}$ produces a residual proportional to the velocity squared during free motion. +- **Friction**: if friction is not part of $M$, $C$ and $g$, it appears as a false external torque. Subtract a friction model from the commanded torque first. +- **Commanded versus applied torque**: motor saturation, gear losses and current-loop lag make the applied torque differ from the command. Use the measured motor current where available. +- **Gain and noise**: higher gains detect faster but pass more model error and velocity noise. Keep $K_O \Delta t \ll 1$ for the forward-Euler update. +- **Initialization**: call `Reset` with the current state before the first update. Starting from a stale momentum produces a transient that decays with time constant $1/K_O$. +- **Zero gain**: the residual is identically zero, which disables detection. + +## Variants & Generalizations + +- **Higher-order observers**: second-order filters on the momentum residual trade a little delay for better noise rejection. +- **Energy observer**: a scalar residual on the total energy, cheaper but unable to tell which joint was hit. +- **Collision isolation and identification**: the residual pattern locates the touched link and, with the contact Jacobian, the contact force. +- **Sliding-mode and Kalman variants**: robustness to model error, or explicit noise models, at higher cost. + +## Applications + +- Collision detection and safe reaction (stop, retract, comply) on collaborative arms. +- Contact-force estimation for hand guiding and kinesthetic teaching without force sensors. +- Monitoring of payload changes and model drift during operation. + +## Connections to Other Algorithms + +- [Coriolis Matrix and Inertial Regressor](CoriolisMatrixAndRegressor.md): supplies $C^T\dot{q}$. +- [Chain Dynamics Model](ChainDynamicsModel.md): supplies $M(q)$ and $g(q)$. +- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): simulates the plant in the tests. +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): its external-wrench overload maps an estimated contact wrench to joint torques. + +## References & Further Reading + +- De Luca, A., Albu-Schäffer, A., Haddadin, S. and Hirzinger, G. (2006). "Collision detection and safe reaction with the DLR-III lightweight manipulator arm." *IEEE/RSJ International Conference on Intelligent Robots and Systems*, 1623–1630. +- Haddadin, S., De Luca, A. and Albu-Schäffer, A. (2017). "Robot collisions: a survey on detection, isolation, and identification." *IEEE Transactions on Robotics*, 33(6), 1292–1312. +- De Luca, A. and Mattone, R. (2005). "Sensorless robot collision detection and hybrid force/motion control." *IEEE International Conference on Robotics and Automation*, 999–1004. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index f85eea2..4e5b108 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -4,14 +4,15 @@ Rigid-body dynamics algorithms for robotics and mechanical systems. ## Algorithms -| Algorithm | Description | -|-------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------| -| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | -| [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | -| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | -| [Coriolis Matrix and Inertial Regressor](CoriolisMatrixAndRegressor.md) | Modified RNEA for $C(q,\dot{q})\dot{q}_r$ in Christoffel form, $C^T\dot{q}$, and the 10-parameter-per-link regressor $Y\pi$ | -| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | -| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | -| [Generic Joint Link](JointLink.md) | Revolute or prismatic joints with limits and armature through one motion-subspace abstraction | -| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | -| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation, optionally with an external tool wrench | +| Algorithm | Description | +|-------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------| +| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | +| [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | +| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | +| [Coriolis Matrix and Inertial Regressor](CoriolisMatrixAndRegressor.md) | Modified RNEA for $C(q,\dot{q})\dot{q}_r$ in Christoffel form, $C^T\dot{q}$, and the 10-parameter-per-link regressor $Y\pi$ | +| [Generalized-Momentum Observer](MomentumObserver.md) | Sensorless collision detection: a first-order estimate of the external joint torque from commanded torque, $M(q)$, $g(q)$ and $C^T\dot{q}$ | +| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | +| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | +| [Generic Joint Link](JointLink.md) | Revolute or prismatic joints with limits and armature through one motion-subspace abstraction | +| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | +| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation, optionally with an external tool wrench | diff --git a/roadmap/README.md b/roadmap/README.md index 456c025..27d68e7 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,7 +92,7 @@ prerequisites. `PoseInverseKinematics` (M13) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` -`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) +`DynamicParameterIdentification` (M22) ### `trajectory` `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/dynamics/MomentumObserver/explanation.md b/roadmap/dynamics/MomentumObserver/explanation.md deleted file mode 100644 index 2bc813f..0000000 --- a/roadmap/dynamics/MomentumObserver/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Generalized-Momentum Observer — Overview - -## What it is -A disturbance observer that estimates the torque an unexpected contact applies to each joint, using -only the motor command and the measured joint positions and velocities. - -## Why it matters (embedded) -Collaborative robots must stop or react within milliseconds of hitting something or someone. The -observer needs no joint-torque sensors and no differentiated velocities, runs at the control rate -and turns a model the controller already has into a safety signal. - -## How it works (intuition) -The robot's generalized momentum `M(q)q̇` changes only because of applied torques, gravity, a -velocity-dependent term, and any external torque. The observer integrates everything it knows and -compares the result with the momentum it actually measures; the mismatch, fed back through a gain, -converges to the external torque with a first-order lag. Using momentum instead of acceleration is -what avoids differentiating noisy velocities. - -## Key parameters -- **Observer gain** — bandwidth of the estimate (detection speed versus noise). -- **Thresholds** — per-joint residual levels that declare a collision. - -## Reference -A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, "Collision Detection and Safe Reaction with -the DLR-III Lightweight Manipulator Arm," *IEEE/RSJ IROS*, 2006; S. Haddadin, A. De Luca, -A. Albu-Schäffer, "Robot Collisions: A Survey on Detection, Isolation, and Identification," *IEEE -Trans. Robotics*, 33(6), 2017. - -## See also -`ChainDynamicsModel` (M29), `CoriolisMatrixAndRegressor` (M31), upstream `FrictionCompensation` (NT57), -`RneaExternalWrench` (M32). diff --git a/roadmap/dynamics/MomentumObserver/implementation.md b/roadmap/dynamics/MomentumObserver/implementation.md deleted file mode 100644 index 4aa782c..0000000 --- a/roadmap/dynamics/MomentumObserver/implementation.md +++ /dev/null @@ -1,67 +0,0 @@ -# Generalized-Momentum Observer (Collision Detection) — Implementation Pseudocode - -> Roadmap ref: #M16 (Tier 3) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Estimates the external joint torque from commanded torque and joint state only — no joint-torque -sensors and no acceleration measurement — so contacts and collisions can be detected on-line. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class MomentumObserver: - const EulerLagrangeDynamics& model # M(q), g(q) — typically ChainDynamicsModel (M29) - const CoriolisMatrix& coriolis # Cᵀ(q, q̇) q̇ (M31); the RNEA vector C q̇ is NOT enough - const LinkArray& links - math::Vector gain # K_O (diagonal), 1/s - math::Vector initialMomentum, integral, residual # STATE -``` - -## Interface - -```text -MomentumObserver(model, coriolis, links, gain) -void Reset(const JointVector& q, const JointVector& qDot) # p(0) -const JointVector& Update(const JointVector& q, const JointVector& qDot, - const JointVector& tauCommanded, T dt) # hot path -bool Collision(const JointVector& threshold) const # |r_i| > threshold_i -``` - -## Algorithm (pseudocode) - -```text -# dynamics: ṗ = τ + Cᵀ(q,q̇) q̇ − g(q) + τ_ext, p = M(q) q̇ (uses Ṁ = C + Cᵀ) -function Reset(q, q̇): - initialMomentum = M(q)·q̇; integral = 0; residual = 0 - -function Update(q, q̇, τ, dt): # OPTIMIZE_FOR_SPEED - β = g(q) − coriolis.TransposeTimesVelocity(links, q, q̇) - integral = integral + (τ − β + residual)·dt - residual = gain ⊙ (M(q)·q̇ − initialMomentum − integral) - return residual # ṙ = K_O (τ_ext − r): first-order estimate of τ_ext -``` - -## Complexity & memory - -- `Update`: `O(N²)` (mass matrix and `Cᵀq̇`); memory `O(N)` state + `O(N²)` scratch; no heap. - -## Numerical / embedded notes - -- Subtract a friction model (NT57) from `τ` if friction is not part of `M, C, g`; otherwise friction - shows up as a (false) external torque. -- The residual is a first-order low-pass of `τ_ext` with bandwidth `K_O`; larger gains detect faster - but amplify model error and velocity noise. Thresholds are set above the residual seen in - collision-free runs. -- Forward-Euler integration of the observer at the control rate is standard; keep `K_O·dt ≪ 1`. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/dynamics/MomentumObserver.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Update`, `extern template class MomentumObserver;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/MomentumObserver.cpp` → the same instantiations. -- Test: `robotics/dynamics/test/TestMomentumObserver.cpp` -- Doc: `doc/dynamics/MomentumObserver.md` (per `doc/TEMPLATE.md`) -- Depends on: M28/M29 (M, g), M31 (`Cᵀq̇`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/MomentumObserver/tests.md b/roadmap/dynamics/MomentumObserver/tests.md deleted file mode 100644 index 3e1db3a..0000000 --- a/roadmap/dynamics/MomentumObserver/tests.md +++ /dev/null @@ -1,42 +0,0 @@ -# Generalized-Momentum Observer — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestMomentumObserver : public ::testing::Test: - # 2-link uniform rods about ŷ, gravity on; plant simulated with ABA at dt = 1 ms - ChainDynamicsModel model{ ... } - CoriolisMatrix coriolis - MomentumObserver observer{ model, coriolis, links, gain = (50, 50) } -# each case below is a TEST_F(TestMomentumObserver, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -free_motion_keeps_residual_near_zero: - Arrange: gravity-compensated motion with commanded torque, no external torque, 1 s - Assert: max |r| stays below 1e-2 N·m -step_external_torque_is_recovered_with_first_order_response: - Arrange: τ_ext = (0, 0.8) N·m applied to the plant from t = 0.2 s - Assert: r_2(0.2 + 1/K) ≈ 0.8·(1 − e^{−1}) and r_2 → 0.8 -collision_flag_uses_per_joint_thresholds: - Assert: Collision((0.5, 0.5)) is false before and true after the step settles -reset_restarts_from_current_momentum: - Arrange: run with non-zero velocity, Reset(q, q̇) - Assert: residual == 0 immediately after -uses_coriolis_transpose_not_the_rnea_vector: - Arrange: fast motion where Cᵀq̇ ≠ C q̇ - Assert: residual stays near zero (a C q̇ implementation drifts — documents why M31 is required) -``` - -## Reference vectors - -- First-order response: `r(t) = τ_ext (1 − e^{−K_O (t − t0)})`. - -## Edge cases - -- `gain = 0` ⇒ residual identically zero. -- `dt` large relative to `1/K_O` ⇒ document instability; the test uses `K_O·dt = 0.05`. diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index 7d92732..acd4b9d 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -24,6 +24,7 @@ target_sources(robotics.dynamics PRIVATE InverseDynamicsModel.hpp JointLink.hpp ModifiedRecursiveNewtonEuler.hpp + MomentumObserver.hpp NewtonEulerBody.hpp NewtonEulerSolver.hpp RecursiveNewtonEuler.hpp @@ -41,6 +42,7 @@ robotics_add_coverage_sources(robotics.dynamics ForwardDynamicsIntegrator.cpp JointLink.cpp ModifiedRecursiveNewtonEuler.cpp + MomentumObserver.cpp NewtonEulerSolver.cpp RecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/MomentumObserver.cpp b/robotics/dynamics/MomentumObserver.cpp new file mode 100644 index 0000000..5a43308 --- /dev/null +++ b/robotics/dynamics/MomentumObserver.cpp @@ -0,0 +1,7 @@ +#include "robotics/dynamics/MomentumObserver.hpp" + +namespace dynamics +{ + template class MomentumObserver; + template class MomentumObserver; +} diff --git a/robotics/dynamics/MomentumObserver.hpp b/robotics/dynamics/MomentumObserver.hpp new file mode 100644 index 0000000..8681912 --- /dev/null +++ b/robotics/dynamics/MomentumObserver.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/CoriolisMatrix.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include +#include +#include + +namespace dynamics +{ + template + class MomentumObserver + { + static_assert(std::is_floating_point_v, "MomentumObserver supports floating-point types"); + + public: + using JointVector = math::Vector; + using LinkArray = std::array, Dof>; + + MomentumObserver(const EulerLagrangeDynamics& model, const CoriolisMatrix& coriolis, const LinkArray& links, const JointVector& gain); + + void Reset(const JointVector& q, const JointVector& qDot); + OPTIMIZE_FOR_SPEED const JointVector& Update(const JointVector& q, const JointVector& qDot, const JointVector& tauCommanded, T dt); + bool Collision(const JointVector& threshold) const; + const JointVector& Residual() const; + + private: + JointVector Momentum(const JointVector& q, const JointVector& qDot) const; + + const EulerLagrangeDynamics& model; + const CoriolisMatrix& coriolis; + const LinkArray& links; + JointVector gain; + JointVector initialMomentum{}; + JointVector integral{}; + JointVector residual{}; + }; + + template + MomentumObserver::MomentumObserver(const EulerLagrangeDynamics& model, const CoriolisMatrix& coriolis, const LinkArray& links, const JointVector& gain) + : model{ model } + , coriolis{ coriolis } + , links{ links } + , gain{ gain } + {} + + template + void MomentumObserver::Reset(const JointVector& q, const JointVector& qDot) + { + initialMomentum = Momentum(q, qDot); + integral = JointVector{}; + residual = JointVector{}; + } + + template + OPTIMIZE_FOR_SPEED const typename MomentumObserver::JointVector& MomentumObserver::Update(const JointVector& q, const JointVector& qDot, const JointVector& tauCommanded, T dt) + { + const JointVector beta{ model.ComputeGravityTerms(q) - coriolis.TransposeTimesVelocity(links, q, qDot) }; + integral = integral + JointVector{ tauCommanded - beta + residual } * dt; + + const JointVector unfiltered{ Momentum(q, qDot) - initialMomentum - integral }; + for (std::size_t i = 0; i < Dof; ++i) + residual[i] = gain[i] * unfiltered[i]; + + return residual; + } + + template + bool MomentumObserver::Collision(const JointVector& threshold) const + { + for (std::size_t i = 0; i < Dof; ++i) + if (math::Abs(residual[i]) > threshold[i]) + return true; + + return false; + } + + template + const typename MomentumObserver::JointVector& MomentumObserver::Residual() const + { + return residual; + } + + template + typename MomentumObserver::JointVector MomentumObserver::Momentum(const JointVector& q, const JointVector& qDot) const + { + return model.ComputeMassMatrix(q) * qDot; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class MomentumObserver; + extern template class MomentumObserver; +#endif +} diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index ee37563..cbea500 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(robotics.dynamics_test PRIVATE TestEulerLagrangeSolver.cpp TestForwardDynamicsIntegrator.cpp TestJointLink.cpp + TestMomentumObserver.cpp TestNewtonEulerSolver.cpp TestRecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/test/TestMomentumObserver.cpp b/robotics/dynamics/test/TestMomentumObserver.cpp new file mode 100644 index 0000000..8818392 --- /dev/null +++ b/robotics/dynamics/test/TestMomentumObserver.cpp @@ -0,0 +1,129 @@ +#include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" +#include "robotics/dynamics/ChainDynamicsModel.hpp" +#include "robotics/dynamics/CoriolisMatrix.hpp" +#include "robotics/dynamics/MomentumObserver.hpp" +#include +#include +#include + +namespace +{ + using Vector2 = math::Vector; + using Vector3 = math::Vector; + using Link = dynamics::JointLink; + + constexpr float dt{ 1e-3f }; + const Vector3 gravity{ 0.0f, 0.0f, -9.81f }; + + Link MakeRod(float mass, float length, const Vector3& parentToJoint) + { + const float inertia{ mass * length * length / 12.0f }; + return Link{ mass, math::SquareMatrix{ { 0.0f, 0.0f, 0.0f }, { 0.0f, inertia, 0.0f }, { 0.0f, 0.0f, inertia } }, Vector3{ 0.0f, 1.0f, 0.0f }, parentToJoint, + Vector3{ length / 2.0f, 0.0f, 0.0f } }; + } + + class TestMomentumObserver + : public ::testing::Test + { + protected: + void SetUp() override + { + observer.Reset(q, qDot); + } + + Vector2 Command(float time, float amplitude) const + { + return model.ComputeGravityTerms(q) + Vector2{ amplitude * std::sin(3.0f * time), -amplitude * std::cos(4.0f * time) }; + } + + void Step(const Vector2& tau, const Vector2& external) + { + const auto acceleration{ aba.ForwardDynamics(links, q, qDot, Vector2{ tau + external }, gravity) }; + qDot = qDot + acceleration * dt; + q = q + qDot * dt; + observer.Update(q, qDot, tau, dt); + } + + float MaxResidualOverFreeMotion(std::size_t steps, float amplitude) + { + float worst{ 0.0f }; + for (std::size_t k = 0; k < steps; ++k) + { + Step(Command(static_cast(k) * dt, amplitude), Vector2{}); + worst = std::max({ worst, std::abs(observer.Residual()[0]), std::abs(observer.Residual()[1]) }); + } + return worst; + } + + std::array links{ MakeRod(2.0f, 1.0f, Vector3{}), MakeRod(1.5f, 0.8f, Vector3{ 1.0f, 0.0f, 0.0f }) }; + dynamics::ChainDynamicsModel model{ links, gravity }; + dynamics::CoriolisMatrix coriolis; + dynamics::ArticulatedBodyAlgorithm aba; + dynamics::MomentumObserver observer{ model, coriolis, links, Vector2{ 50.0f, 50.0f } }; + Vector2 q{ 0.4f, -0.9f }; + Vector2 qDot{}; + Vector2 threshold{ 0.5f, 0.5f }; + }; +} + +TEST_F(TestMomentumObserver, free_motion_keeps_residual_near_zero) +{ + EXPECT_LT(MaxResidualOverFreeMotion(1000, 1.0f), 1e-2f); +} + +TEST_F(TestMomentumObserver, step_external_torque_is_recovered_with_first_order_response) +{ + const Vector2 external{ 0.0f, 0.8f }; + for (std::size_t k = 0; k < 200; ++k) + Step(Command(static_cast(k) * dt, 0.0f), Vector2{}); + + for (std::size_t k = 0; k < 20; ++k) + Step(Command(0.0f, 0.0f), external); + const float afterOneTimeConstant{ observer.Residual()[1] }; + for (std::size_t k = 0; k < 300; ++k) + Step(Command(0.0f, 0.0f), external); + + EXPECT_NEAR(afterOneTimeConstant, 0.8f * (1.0f - std::exp(-1.0f)), 0.02f); + EXPECT_NEAR(observer.Residual()[1], 0.8f, 0.01f); + EXPECT_NEAR(observer.Residual()[0], 0.0f, 0.01f); +} + +TEST_F(TestMomentumObserver, collision_flag_uses_per_joint_thresholds) +{ + for (std::size_t k = 0; k < 200; ++k) + Step(Command(static_cast(k) * dt, 0.0f), Vector2{}); + const bool before{ observer.Collision(threshold) }; + + for (std::size_t k = 0; k < 300; ++k) + Step(Command(0.0f, 0.0f), Vector2{ 0.0f, 0.8f }); + + EXPECT_FALSE(before); + EXPECT_TRUE(observer.Collision(threshold)); +} + +TEST_F(TestMomentumObserver, reset_restarts_from_current_momentum) +{ + for (std::size_t k = 0; k < 300; ++k) + Step(Command(static_cast(k) * dt, 1.0f), Vector2{ 0.3f, -0.2f }); + ASSERT_GT(std::abs(observer.Residual()[0]), 0.1f); + + observer.Reset(q, qDot); + + EXPECT_EQ(observer.Residual()[0], 0.0f); + EXPECT_EQ(observer.Residual()[1], 0.0f); +} + +TEST_F(TestMomentumObserver, uses_coriolis_transpose_not_the_rnea_vector) +{ + const Vector2 fastVelocity{ 3.0f, -4.0f }; + const auto C{ coriolis.Compute(links, q, fastVelocity) }; + const Vector2 transposed{ C.Transpose() * fastVelocity }; + const Vector2 direct{ C * fastVelocity }; + const float drift{ std::abs(transposed[0] - direct[0]) + std::abs(transposed[1] - direct[1]) }; + ASSERT_GT(drift, 1.0f); + + qDot = fastVelocity; + observer.Reset(q, qDot); + + EXPECT_LT(MaxResidualOverFreeMotion(1000, 0.0f), 0.05f * drift); +} From f176e36592f4688e33bc4d40c0ac03a41f0f993b Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 12:53:59 +0000 Subject: [PATCH 5/9] feat(dynamics): add DynamicParameterIdentification (M22) Streams M31 regressor rows into the upstream rank-revealing incremental QR least squares and reports the base-parameter estimate, its rank and the identifiable directions. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +- ROADMAP.md | 5 - .../DynamicParameterIdentification.md | 98 +++++++++++ doc/dynamics/README.md | 1 + roadmap/README.md | 3 - .../explanation.md | 33 ---- .../implementation.md | 80 --------- .../DynamicParameterIdentification/tests.md | 44 ----- robotics/dynamics/CMakeLists.txt | 3 + .../DynamicParameterIdentification.cpp | 6 + .../DynamicParameterIdentification.hpp | 83 +++++++++ robotics/dynamics/test/CMakeLists.txt | 1 + .../TestDynamicParameterIdentification.cpp | 158 ++++++++++++++++++ 13 files changed, 357 insertions(+), 172 deletions(-) create mode 100644 doc/dynamics/DynamicParameterIdentification.md delete mode 100644 roadmap/dynamics/DynamicParameterIdentification/explanation.md delete mode 100644 roadmap/dynamics/DynamicParameterIdentification/implementation.md delete mode 100644 roadmap/dynamics/DynamicParameterIdentification/tests.md create mode 100644 robotics/dynamics/DynamicParameterIdentification.cpp create mode 100644 robotics/dynamics/DynamicParameterIdentification.hpp create mode 100644 robotics/dynamics/test/TestDynamicParameterIdentification.cpp diff --git a/README.md b/README.md index 24dca60..91f0d8d 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | +| Category | Description | +|--------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index d7dab9e..ee27939 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -121,7 +121,6 @@ existing `dynamics/` convention. |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | | M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | -| M22 | Dynamic (base-parameter) identification | `dynamics` | M31, NT63 | ★★★☆☆ | | M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | @@ -147,10 +146,6 @@ retired here. **M17. Impedance control.** Stiffness/damping impedance via `Jᵀ` (no force sensor), and inertia-shaping impedance with measured contact wrench via the task-space inertia. - *Algorithm / paper:* N. Hogan, *ASME J. Dyn. Sys. Meas. Control*, 1985. -**M22. Dynamic (base-parameter) identification.** Regressor rows streamed into a rank-revealing least-squares solve; the kept directions are the base parameters. -- *Algorithm / paper:* Atkeson, An, Hollerbach (1986); Gautier & Khalil (1990). -- *Reuses / builds on:* M31; upstream `estimators::IncrementalQrLeastSquares` (NT63). - ### Tier 4 — Advanced ★★★★☆ **M18. Operational-space control.** Task-space inertia `Λ = (J M⁻¹ Jᵀ)⁻¹`, `J̇q̇` compensation, full joint-space gravity/Coriolis compensation and a dynamically-consistent null space. diff --git a/doc/dynamics/DynamicParameterIdentification.md b/doc/dynamics/DynamicParameterIdentification.md new file mode 100644 index 0000000..d39e21b --- /dev/null +++ b/doc/dynamics/DynamicParameterIdentification.md @@ -0,0 +1,98 @@ +# Dynamic Parameter Identification + +## Overview & Motivation + +Model-based control, collision observers and simulators all need the arm's inertial parameters: masses, centre-of-mass positions and inertia tensors. CAD values ignore cables, motors, gearboxes and paint, and they are often wrong by tens of percent. +**Dynamic identification** estimates them from data recorded on the real robot: joint positions, velocities, accelerations and torques along an exciting trajectory. + +The rigid-body model is **linear in the inertial parameters**, so identification reduces to linear least squares. The difficulty is structural. Some parameters never affect the joint torques, and others act only in fixed combinations. +Only the **base parameters**, a minimal set of identifiable linear combinations, can be recovered. The solver must reveal that rank and report which combinations it identified. + +## Mathematical Theory + +### Linear Parametrization + +With the inertial regressor $Y$ (ten parameters per link: mass, first moment, rotational inertia about the joint origin), + +$$\tau = Y(q,\dot{q},\ddot{q})\,\pi, \qquad Y \in \mathbb{R}^{n \times 10n}$$ + +Stacking $N$ samples gives the overdetermined system + +$$\underbrace{\begin{bmatrix} Y_1 \\ \vdots \\ Y_N \end{bmatrix}}_{W} \pi = \underbrace{\begin{bmatrix} \tau_1 \\ \vdots \\ \tau_N \end{bmatrix}}_{b}$$ + +### Rank Deficiency and Base Parameters + +$W$ never has full column rank. Some columns are identically zero, for example inertia components about axes perpendicular to a planar arm's joints. Others are linear combinations of the rest: the mass of link 2 enters link 1's dynamics only through $m_2 l_1$ and $m_2 l_1^2$, which merge with link 1's first moment and inertia. +The identifiable subspace is the row space of $W$. Its dimension, the number of base parameters, is far below $10n$. + +### Streaming Rank-Revealing Least Squares + +Samples are folded into an upper-triangular factor $R$ by Givens rotations, so memory stays at $P^2 + P$ scalars ($P = 10n$) regardless of $N$: + +$$W = Q R, \qquad Q^T b = (d;\ e)$$ + +At the end, a singular value decomposition $R = U \Sigma V^T$ reveals the rank: singular values below $\epsilon\,\sigma_{\max}$ are treated as zero. The minimum-norm estimate is + +$$\hat{\pi} = \sum_{\sigma_i > \epsilon\sigma_{\max}} v_i \frac{u_i^T d}{\sigma_i}$$ + +The first $r$ rows of $V^T$ span the identifiable combinations. For each of them, $v_i^T \hat{\pi} = v_i^T \pi_{\text{true}}$ on noiseless data, while components outside that span are set to zero. + +### Validation + +The estimate is validated by predicting torques $\hat{\tau} = Y\hat{\pi}$ on held-out data. Predictions are exact even though $\hat{\pi}$ differs from $\pi_{\text{true}}$, because the difference lies in the null space of every regressor. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------|------------|-----------|----------------------------------------------------------| +| `AddSample` | $O(n P^2)$ | $O(P^2)$ | One regressor evaluation and $n$ Givens-row updates | +| `Solve` | $O(P^3)$ | $O(P^2)$ | One $P \times P$ singular value decomposition | +| `Predict` | $O(n P)$ | $O(nP)$ | One regressor evaluation and product | +| Storage | — | $P^2 + P$ | Independent of the number of samples; 6 DoF: about 14 KB | + +## Step-by-Step Walkthrough + +A planar two-link arm of uniform rods (masses $2$ and $1.5$ kg, lengths $1$ and $0.8$ m) follows a three-harmonic Fourier trajectory, sampled 200 times over one period. Torques come from the recursive Newton–Euler algorithm on the true parameters. + +1. **Joint axes horizontal.** Gravity acts in the plane of motion. The stacked regressor has rank 6, matching the classical base set $ZZ_1^R = ZZ_1 + m_2 l_1^2$, $ZZ_2$, $MX_1^R = MX_1 + m_2 l_1$, $MY_1$, $MX_2$ and $MY_2$, with $Z$ along the joint axes. +2. **Joint axes vertical.** Gravity is parallel to the axes, so link 1's first moments no longer act: rank 4 ($ZZ_1^R$, $ZZ_2$, $MX_2$, $MY_2$). +3. **True values.** For the horizontal arm, $ZZ_1^R = 2/3 + 1.5 = 2.167$ kg·m², $MX_1^R = 1.0 + 1.5 = 2.5$ kg·m, $ZZ_2 = 0.32$ kg·m² and $MX_2 = 0.6$ kg·m. The projections of $\hat{\pi}$ onto the identified directions match those of $\pi_{\text{true}}$ to about $10^{-3}$ of $\|\pi\|$. +4. **Prediction.** On held-out samples at other times, $Y\hat{\pi}$ reproduces the torques to $10^{-3}$ relative. + +## Pitfalls & Edge Cases + +- **Poor excitation**: a slow or low-amplitude trajectory leaves directions nearly unobservable, so the rank collapses or noise dominates. Optimize the trajectory (e.g. a finite Fourier series) to minimize the condition number of the identified part. +- **Mixed units**: regressor columns mix masses, first moments and inertias. Scale them before accumulating so that a relative rank tolerance means the same thing for every column. +- **Differentiated signals**: velocities and accelerations from numerical differentiation are noisy. Filter $q$, $\dot{q}$, $\ddot{q}$ and $\tau$ with the same zero-phase low-pass filter offline. +- **Friction and armature**: unmodelled friction biases the estimate. Append friction columns ($\operatorname{sign}\dot{q}_i$, $\dot{q}_i$) and armature columns to the regressor. +- **Physical consistency**: the least-squares estimate may imply a negative mass or an invalid inertia tensor. Enforcing consistency needs a constrained (semidefinite) fit. +- **No data**: `Solve` returns no result before the first sample or after `Reset`. + +## Variants & Generalizations + +- **Weighted least squares**: weight each joint by its torque noise level. +- **Physically consistent identification**: semidefinite programming with linear-matrix-inequality constraints on each link's pseudo-inertia. +- **Energy and power models**: integrated forms that avoid accelerations. +- **Closed-loop identification**: instrumental-variable methods that remove the bias caused by feedback. +- **Payload identification**: the same formulation restricted to the last link's ten parameters. + +## Applications + +- Commissioning of model-based controllers (computed torque, passivity-based, impedance). +- Calibration of collision observers, whose thresholds depend on model accuracy. +- Accurate simulation and digital twins. +- Payload estimation after a tool change. + +## Connections to Other Algorithms + +- [Coriolis Matrix and Inertial Regressor](CoriolisMatrixAndRegressor.md): supplies the regressor $Y$. +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): generates reference torques for validation. +- [Generalized-Momentum Observer](MomentumObserver.md): benefits directly from identified parameters. +- The streaming rank-revealing least squares comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Atkeson, C.G., An, C.H. and Hollerbach, J.M. (1986). "Estimation of inertial parameters of manipulator loads and links." *International Journal of Robotics Research*, 5(3), 101–119. +- Gautier, M. and Khalil, W. (1990). "Direct calculation of minimum set of inertial parameters of serial robots." *IEEE Transactions on Robotics and Automation*, 6(3), 368–373. +- Swevers, J., Ganseman, C., Tükel, D.B., De Schutter, J. and Van Brussel, H. (1997). "Optimal robot excitation and identification." *IEEE Transactions on Robotics and Automation*, 13(5), 730–740. +- Wensing, P.M., Kim, S. and Slotine, J.-J.E. (2018). "Linear matrix inequalities for physically consistent inertial parameter identification." *IEEE Robotics and Automation Letters*, 3(1), 60–67. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index 4e5b108..8207dbf 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -10,6 +10,7 @@ Rigid-body dynamics algorithms for robotics and mechanical systems. | [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | | [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | | [Coriolis Matrix and Inertial Regressor](CoriolisMatrixAndRegressor.md) | Modified RNEA for $C(q,\dot{q})\dot{q}_r$ in Christoffel form, $C^T\dot{q}$, and the 10-parameter-per-link regressor $Y\pi$ | +| [Dynamic Parameter Identification](DynamicParameterIdentification.md) | Base-parameter estimation from measured torques with streaming rank-revealing least squares on the inertial regressor | | [Generalized-Momentum Observer](MomentumObserver.md) | Sensorless collision detection: a first-order estimate of the external joint torque from commanded torque, $M(q)$, $g(q)$ and $C^T\dot{q}$ | | [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | | [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | diff --git a/roadmap/README.md b/roadmap/README.md index 27d68e7..a93c2b6 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -91,9 +91,6 @@ prerequisites. ### `kinematics` `PoseInverseKinematics` (M13) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) -### `dynamics` -`DynamicParameterIdentification` (M22) - ### `trajectory` `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/dynamics/DynamicParameterIdentification/explanation.md b/roadmap/dynamics/DynamicParameterIdentification/explanation.md deleted file mode 100644 index 8695895..0000000 --- a/roadmap/dynamics/DynamicParameterIdentification/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Dynamic Parameter Identification — Overview - -## What it is -A procedure that estimates a robot's inertial parameters (masses, centers of mass, inertia tensors) -from torque and motion measurements, by exploiting the fact that rigid-body dynamics are linear in -those parameters. - -## Why it matters (embedded) -Model-based control (gravity compensation, computed torque, impedance, collision detection) is only -as good as the model. CAD data rarely matches the real arm with cables, motors and grippers. -Identifying the parameters on the real robot — during commissioning or after a tool change — makes -every model-based controller more accurate. - -## How it works (intuition) -For each recorded sample the dynamics give one linear equation per joint in the unknown parameters. -Stacking many samples gives an over-determined least-squares problem. Some parameters never affect -the torques (or only in fixed combinations), so the problem is rank-deficient; a rank-revealing -solution keeps only the identifiable combinations, the "base parameters". The numerical toolbox's -incremental QR least squares processes samples one at a time with orthogonal rotations, which keeps memory -fixed and numerics well-conditioned; this item supplies the regressor rows. - -## Key parameters -- **Excitation trajectory** — decides how well-conditioned the problem is. -- **Rank tolerance** — which singular values count as identifiable. - -## Reference -C. Atkeson, C. An, J. Hollerbach, "Estimation of Inertial Parameters of Manipulator Loads and Links," -*Int. J. Robotics Research*, 5(3), 1986; M. Gautier, W. Khalil, "Direct Calculation of Minimum Set of -Inertial Parameters of Serial Robots," *IEEE Trans. Robotics and Automation*, 6(3), 1990. - -## See also -`CoriolisMatrixAndRegressor` (M31), `SlotineLiAdaptiveControl` (M20, on-line counterpart), upstream -`IncrementalQrLeastSquares` (NT63, the solver) and `FrictionCompensation` (NT57). diff --git a/roadmap/dynamics/DynamicParameterIdentification/implementation.md b/roadmap/dynamics/DynamicParameterIdentification/implementation.md deleted file mode 100644 index 0791493..0000000 --- a/roadmap/dynamics/DynamicParameterIdentification/implementation.md +++ /dev/null @@ -1,80 +0,0 @@ -# Dynamic (Base-Parameter) Identification — Implementation Pseudocode - -> Roadmap ref: #M22 (Tier 3) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Estimates the inertial parameters of a real arm from measured joint positions, velocities, -accelerations and torques recorded along an exciting trajectory. The rigid-body model is linear in -the parameters, `τ = Y(q, q̇, q̈)·π` (M31 regressor with `q̇r = q̇`, `q̈r = q̈`), so this is a linear -least-squares problem — but only certain linear combinations of `π` (the **base parameters**) are -identifiable, so the solver must handle a rank-deficient regressor. The streaming, rank-revealing least -squares is upstream `estimators::IncrementalQrLeastSquares` (NT63); this item feeds it the regressor and -interprets the result. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class DynamicParameterIdentification: - static constexpr std::size_t P = 10 * Dof # full parameter count (M31 ordering) - const InertialRegressor& regressor # ChainInertialRegressor (M31) - estimators::IncrementalQrLeastSquares leastSquares # NT63 (STATE: P×P factor, no samples stored) - -struct IdentificationResult: - math::Vector parameters # minimum-norm solution (identifiable projection of π) - std::size_t rank # number of base parameters - math::Matrix baseDirections # first `rank` rows span the identifiable combinations - T residualRms -``` - -## Interface - -```cpp -void AddSample(const JointVector& q, const JointVector& qDot, const JointVector& qDDot, - const JointVector& tauMeasured) # streaming, O(Dof·P²) -std::optional Solve(T relativeTolerance) const # nullopt if no samples -JointVector Predict(const IdentificationResult&, q, q̇, q̈) const # Y·π̂ for validation -void Reset() -``` - -## Algorithm (pseudocode) - -```text -function AddSample(q, q̇, q̈, τ): - leastSquares.AddRows(regressor.Compute(q, q̇, q̇, q̈), τ) # Dof rows of the P-column regressor - -function Solve(tol): - r = leastSquares.Solve(tol); if !r: return nullopt - return { r.parameters, r.rank, r.rowSpace, r.residualRms } # rowSpace rows < rank = base directions - -function Predict(result, q, q̇, q̈): - return regressor.Compute(q, q̇, q̇, q̈) · result.parameters -``` - -## Complexity & memory - -- `AddSample`: `O(Dof·P²)` Givens rotations; `Solve`: one `P×P` SVD (upstream). -- Memory: `P² + P` scalars regardless of the number of samples (e.g. 6-DOF: `P = 60`, about 14 KB in - `float`); no heap. This is an offline / commissioning routine, not an ISR path. - -## Numerical / embedded notes - -- Excitation matters more than the solver: use a periodic (finite Fourier series) trajectory designed - to minimise the condition number of the stacked regressor; report `σ_max/σ_min` of the kept part. -- Filter `q̇`, `q̈` and `τ` with the same zero-phase low-pass filter (offline) before `AddSample`. -- The regressor columns mix units (mass, first and second moments): scale them before accumulating so the - relative rank tolerance is meaningful. -- The minimum-norm estimate is not guaranteed physically consistent (positive mass, valid inertia); - enforcing that needs a constrained (LMI) fit — out of scope, noted for future work. -- Friction (NT57) parameters can be appended to the regressor as extra columns (`sign(q̇)`, `q̇`). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/dynamics/DynamicParameterIdentification.hpp` — `#pragma once`, - `extern template class DynamicParameterIdentification;` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/DynamicParameterIdentification.cpp` → the same instantiation. -- Test: `robotics/dynamics/test/TestDynamicParameterIdentification.cpp` -- Doc: `doc/dynamics/DynamicParameterIdentification.md` (per `doc/TEMPLATE.md`) -- Depends on: M31 (regressor); NT63 (`estimators::IncrementalQrLeastSquares`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/DynamicParameterIdentification/tests.md b/roadmap/dynamics/DynamicParameterIdentification/tests.md deleted file mode 100644 index 5d1282e..0000000 --- a/roadmap/dynamics/DynamicParameterIdentification/tests.md +++ /dev/null @@ -1,44 +0,0 @@ -# Dynamic Parameter Identification — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestDynamicParameterIdentification : public ::testing::Test: - # planar 2R uniform rods; "true" links generate noiseless torques with RNEA - ChainInertialRegressor regressor{ links, gravity } - DynamicParameterIdentification identification{ regressor } - # excitation: 200 samples of q = Σ Fourier terms, q̇, q̈ analytic -# each case below is a TEST_F(TestDynamicParameterIdentification, ) -``` - -## Test cases (Arrange / Act / Assert) - -The streaming least-squares mechanics (order independence, block rows, minimum norm) are tested upstream -with `estimators::IncrementalQrLeastSquares` (NT63); these cases cover the robot model. - -```cpp -base_parameter_count_with_gravity_is_six: - Arrange: joint axes horizontal (gravity acts) - Assert: Solve().rank == 6 -base_parameter_count_without_gravity_is_four: - Arrange: joint axes parallel to gravity - Assert: Solve().rank == 4 -noiseless_data_reproduces_torques: - Assert: Predict(result, q, q̇, q̈) ≈ τ on held-out samples (≈ 1e-3 relative) -identifiable_combinations_match_the_true_parameters: - Assert: baseDirections·π̂ ≈ baseDirections·π_true -no_samples_returns_nullopt: - Assert: Solve(tol) == std::nullopt after Reset() -``` - -## Reference vectors - -- Planar 2R: 6 base parameters with gravity in the plane of motion, 4 without (e.g. `ZZ1R, ZZ2, - MX2, MY2` plus `MX1R, MY1R` when gravity acts) — rank checked numerically. - -## Edge cases - -- Non-exciting data (constant pose) ⇒ rank drops; residual still finite. -- Additive torque noise ⇒ bounded bias; RMS residual ≈ noise level. diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index acd4b9d..7043788 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -7,6 +7,7 @@ target_include_directories(robotics.dynamics ${ROBOTICS_VISIBILITY} target_link_libraries(robotics.dynamics ${ROBOTICS_VISIBILITY} infra.util + numerical.estimators numerical.math numerical.solver ) @@ -17,6 +18,7 @@ target_sources(robotics.dynamics PRIVATE ChainInertialRegressor.hpp CompositeRigidBodyAlgorithm.hpp CoriolisMatrix.hpp + DynamicParameterIdentification.hpp EulerLagrangeDynamics.hpp EulerLagrangeSolver.hpp ForwardDynamicsIntegrator.hpp @@ -38,6 +40,7 @@ robotics_add_coverage_sources(robotics.dynamics ChainInertialRegressor.cpp CompositeRigidBodyAlgorithm.cpp CoriolisMatrix.cpp + DynamicParameterIdentification.cpp EulerLagrangeSolver.cpp ForwardDynamicsIntegrator.cpp JointLink.cpp diff --git a/robotics/dynamics/DynamicParameterIdentification.cpp b/robotics/dynamics/DynamicParameterIdentification.cpp new file mode 100644 index 0000000..ec5f2c2 --- /dev/null +++ b/robotics/dynamics/DynamicParameterIdentification.cpp @@ -0,0 +1,6 @@ +#include "robotics/dynamics/DynamicParameterIdentification.hpp" + +namespace dynamics +{ + template class DynamicParameterIdentification; +} diff --git a/robotics/dynamics/DynamicParameterIdentification.hpp b/robotics/dynamics/DynamicParameterIdentification.hpp new file mode 100644 index 0000000..ade9728 --- /dev/null +++ b/robotics/dynamics/DynamicParameterIdentification.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include "numerical/estimators/offline/IncrementalQrLeastSquares.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/InertialRegressor.hpp" +#include +#include +#include + +namespace dynamics +{ + template + struct IdentificationResult + { + static_assert(std::is_floating_point_v, "IdentificationResult supports floating-point types"); + + math::Vector parameters{}; + std::size_t rank{ 0 }; + math::SquareMatrix baseDirections{}; + T residualRms{}; + }; + + template + class DynamicParameterIdentification + { + static_assert(std::is_floating_point_v, "DynamicParameterIdentification supports floating-point types"); + + public: + static constexpr std::size_t ParameterCount{ 10 * Dof }; + + using JointVector = math::Vector; + using Regressor = InertialRegressor; + using Result = IdentificationResult; + + explicit DynamicParameterIdentification(const Regressor& regressor); + + void AddSample(const JointVector& q, const JointVector& qDot, const JointVector& qDDot, const JointVector& tauMeasured); + std::optional Solve(T relativeTolerance) const; + JointVector Predict(const Result& result, const JointVector& q, const JointVector& qDot, const JointVector& qDDot) const; + void Reset(); + + private: + const Regressor& regressor; + estimators::IncrementalQrLeastSquares leastSquares; + }; + + template + DynamicParameterIdentification::DynamicParameterIdentification(const Regressor& regressor) + : regressor{ regressor } + {} + + template + void DynamicParameterIdentification::AddSample(const JointVector& q, const JointVector& qDot, const JointVector& qDDot, const JointVector& tauMeasured) + { + leastSquares.AddRows(regressor.Compute(q, qDot, qDot, qDDot), tauMeasured); + } + + template + auto DynamicParameterIdentification::Solve(T relativeTolerance) const -> std::optional + { + const auto solution{ leastSquares.Solve(relativeTolerance) }; + if (!solution) + return std::nullopt; + + return Result{ solution->parameters, solution->rank, solution->rowSpace, solution->residualRms }; + } + + template + auto DynamicParameterIdentification::Predict(const Result& result, const JointVector& q, const JointVector& qDot, const JointVector& qDDot) const -> JointVector + { + return regressor.Compute(q, qDot, qDot, qDDot) * result.parameters; + } + + template + void DynamicParameterIdentification::Reset() + { + leastSquares.Reset(); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DynamicParameterIdentification; +#endif +} diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index cbea500..92b8253 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -14,6 +14,7 @@ target_sources(robotics.dynamics_test PRIVATE TestChainInertialRegressor.cpp TestCompositeRigidBodyAlgorithm.cpp TestCoriolisMatrix.cpp + TestDynamicParameterIdentification.cpp TestEulerLagrangeSolver.cpp TestForwardDynamicsIntegrator.cpp TestJointLink.cpp diff --git a/robotics/dynamics/test/TestDynamicParameterIdentification.cpp b/robotics/dynamics/test/TestDynamicParameterIdentification.cpp new file mode 100644 index 0000000..be032dc --- /dev/null +++ b/robotics/dynamics/test/TestDynamicParameterIdentification.cpp @@ -0,0 +1,158 @@ +#include "robotics/dynamics/ChainInertialRegressor.hpp" +#include "robotics/dynamics/DynamicParameterIdentification.hpp" +#include "robotics/dynamics/RecursiveNewtonEuler.hpp" +#include +#include +#include + +namespace +{ + using Vector2 = math::Vector; + using Vector3 = math::Vector; + using Link = dynamics::JointLink; + using Links = std::array; + using Regressor = dynamics::ChainInertialRegressor; + using Identification = dynamics::DynamicParameterIdentification; + + constexpr float samplePeriod{ 0.01f }; + constexpr float baseFrequency{ std::numbers::pi_v }; + const Vector3 gravity{ 0.0f, 0.0f, -9.81f }; + + struct Motion + { + Vector2 q; + Vector2 qDot; + Vector2 qDDot; + }; + + Links MakeArm(const Vector3& axis) + { + const auto rod{ [&axis](float mass, float length, const Vector3& parentToJoint) + { + const float inertia{ mass * length * length / 12.0f }; + return Link{ mass, math::SquareMatrix{ { 0.0f, 0.0f, 0.0f }, { 0.0f, inertia, 0.0f }, { 0.0f, 0.0f, inertia } }, axis, parentToJoint, Vector3{ length / 2.0f, 0.0f, 0.0f } }; + } }; + return Links{ rod(2.0f, 1.0f, Vector3{}), rod(1.5f, 0.8f, Vector3{ 1.0f, 0.0f, 0.0f }) }; + } + + Motion Excitation(float time) + { + constexpr std::array, 2> amplitude{ { { 0.8f, 0.4f, 0.2f }, { 1.0f, 0.5f, 0.3f } } }; + constexpr std::array, 2> phase{ { { 0.0f, 1.1f, 2.3f }, { 0.7f, 2.9f, 0.4f } } }; + constexpr std::array offset{ 0.3f, -0.6f }; + + Motion motion{}; + for (std::size_t joint = 0; joint < 2; ++joint) + { + motion.q[joint] = offset[joint]; + for (std::size_t k = 0; k < 3; ++k) + { + const float frequency{ baseFrequency * static_cast(k + 1) }; + const float angle{ frequency * time + phase[joint][k] }; + motion.q[joint] += amplitude[joint][k] * std::sin(angle); + motion.qDot[joint] += amplitude[joint][k] * frequency * std::cos(angle); + motion.qDDot[joint] -= amplitude[joint][k] * frequency * frequency * std::sin(angle); + } + } + return motion; + } + + class TestDynamicParameterIdentification + : public ::testing::Test + { + protected: + Vector2 Torque(const Links& links, const Motion& motion) const + { + return rnea.InverseDynamics(links, motion.q, motion.qDot, motion.qDDot, gravity); + } + + void Excite(Identification& target, const Links& links) const + { + for (std::size_t k = 0; k < 200; ++k) + { + const auto motion{ Excitation(static_cast(k) * samplePeriod) }; + target.AddSample(motion.q, motion.qDot, motion.qDDot, Torque(links, motion)); + } + } + + dynamics::RecursiveNewtonEuler rnea; + Links horizontal{ MakeArm(Vector3{ 0.0f, 1.0f, 0.0f }) }; + Links vertical{ MakeArm(Vector3{ 0.0f, 0.0f, 1.0f }) }; + Regressor regressor{ horizontal, gravity }; + Regressor verticalRegressor{ vertical, gravity }; + Identification identification{ regressor }; + float tolerance{ 1e-4f }; + }; +} + +TEST_F(TestDynamicParameterIdentification, base_parameter_count_with_gravity_is_six) +{ + Excite(identification, horizontal); + + const auto result{ identification.Solve(tolerance) }; + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->rank, std::size_t{ 6 }); +} + +TEST_F(TestDynamicParameterIdentification, base_parameter_count_without_gravity_is_four) +{ + Identification withoutGravity{ verticalRegressor }; + Excite(withoutGravity, vertical); + + const auto result{ withoutGravity.Solve(tolerance) }; + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->rank, std::size_t{ 4 }); +} + +TEST_F(TestDynamicParameterIdentification, noiseless_data_reproduces_torques) +{ + Excite(identification, horizontal); + const auto result{ identification.Solve(tolerance) }; + ASSERT_TRUE(result.has_value()); + + for (std::size_t k = 0; k < 20; ++k) + { + const auto motion{ Excitation(0.0137f + static_cast(k) * 0.093f) }; + const auto expected{ Torque(horizontal, motion) }; + const auto predicted{ identification.Predict(*result, motion.q, motion.qDot, motion.qDDot) }; + const float scale{ std::abs(expected[0]) + std::abs(expected[1]) }; + for (std::size_t i = 0; i < 2; ++i) + EXPECT_NEAR(predicted[i], expected[i], 1e-3f * scale); + } + EXPECT_LT(result->residualRms, 1e-2f); +} + +TEST_F(TestDynamicParameterIdentification, identifiable_combinations_match_the_true_parameters) +{ + Excite(identification, horizontal); + const auto result{ identification.Solve(tolerance) }; + ASSERT_TRUE(result.has_value()); + + const auto truth{ Regressor::Parameters(horizontal) }; + float truthNorm{ 0.0f }; + for (std::size_t c = 0; c < Identification::ParameterCount; ++c) + truthNorm += truth[c] * truth[c]; + + for (std::size_t r = 0; r < result->rank; ++r) + { + float estimated{ 0.0f }; + float expected{ 0.0f }; + for (std::size_t c = 0; c < Identification::ParameterCount; ++c) + { + estimated += result->baseDirections.at(r, c) * result->parameters[c]; + expected += result->baseDirections.at(r, c) * truth[c]; + } + EXPECT_NEAR(estimated, expected, 1e-3f * std::sqrt(truthNorm)); + } +} + +TEST_F(TestDynamicParameterIdentification, no_samples_returns_nullopt) +{ + Excite(identification, horizontal); + + identification.Reset(); + + EXPECT_FALSE(identification.Solve(tolerance).has_value()); +} From 8141e78b4b30266b970c5ec91e4544a508154fb7 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 12:59:02 +0000 Subject: [PATCH 6/9] feat(kinematics): add PoseInverseKinematics (M13) Full 6-DoF pose IK on any JacobianProvider: upstream Levenberg-Marquardt on the weighted SE(3) log-map error, manipulability-adaptive damping, step clamping and joint-limit projection. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 29 +-- doc/kinematics/PoseInverseKinematics.md | 103 +++++++++ doc/kinematics/README.md | 21 +- roadmap/README.md | 2 +- .../PoseInverseKinematics/explanation.md | 41 ---- .../PoseInverseKinematics/implementation.md | 116 ---------- .../kinematics/PoseInverseKinematics/tests.md | 73 ------ robotics/kinematics/CMakeLists.txt | 3 + robotics/kinematics/PoseInverseKinematics.cpp | 11 + robotics/kinematics/PoseInverseKinematics.hpp | 201 ++++++++++++++++ robotics/kinematics/test/CMakeLists.txt | 1 + .../test/TestPoseInverseKinematics.cpp | 216 ++++++++++++++++++ 13 files changed, 560 insertions(+), 259 deletions(-) create mode 100644 doc/kinematics/PoseInverseKinematics.md delete mode 100644 roadmap/kinematics/PoseInverseKinematics/explanation.md delete mode 100644 roadmap/kinematics/PoseInverseKinematics/implementation.md delete mode 100644 roadmap/kinematics/PoseInverseKinematics/tests.md create mode 100644 robotics/kinematics/PoseInverseKinematics.cpp create mode 100644 robotics/kinematics/PoseInverseKinematics.hpp create mode 100644 robotics/kinematics/test/TestPoseInverseKinematics.cpp diff --git a/README.md b/README.md index 91f0d8d..23d8840 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | Category | Description | |--------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares), Pose Inverse Kinematics | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | | [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | diff --git a/ROADMAP.md b/ROADMAP.md index ee27939..17d2fd3 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -117,19 +117,18 @@ existing `dynamics/` convention. ### Manipulator list (by priority) -| # | Component | Target module | Depends on | Difficulty | -|-----|-----------------------------------------------------------------|---------------------------|----------------|------------| -| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | -| M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | -| M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | -| M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | -| M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | -| M21 | Analytical IK for ortho-parallel 6R arms with a spherical wrist | `kinematics` | NT53, NT54 | ★★★★☆ | -| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | NT53, NT64 | ★★★★☆ | -| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14, W1 | ★★★★☆ | -| M25 | Cable-driven tension distribution | `controllers/manipulator` | NT53, NT65 | ★★★★☆ | -| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | NT53 | ★★★★☆ | -| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | +| # | Component | Target module | Depends on | Difficulty | +|-----|-----------------------------------------------------------------|---------------------------|-------------|------------| +| M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | +| M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | +| M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | +| M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | +| M21 | Analytical IK for ortho-parallel 6R arms with a spherical wrist | `kinematics` | NT53, NT54 | ★★★★☆ | +| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | NT53, NT64 | ★★★★☆ | +| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14, W1 | ★★★★☆ | +| M25 | Cable-driven tension distribution | `controllers/manipulator` | NT53, NT65 | ★★★★☆ | +| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | NT53 | ★★★★☆ | +| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | M2 (polynomial profile), M3 (trapezoidal profile), M4 (friction compensation), M6 (SE(3)) and M9 (S-curve profile) moved to numerical-toolbox as NT55, NT56, NT57, NT53 and NT60; their numbers are @@ -139,10 +138,6 @@ retired here. ### Tier 3 — Moderate ★★★☆☆ -**M13. Full 6-DOF pose IK.** Damped least squares on the SE(3) log-map error with position/rotation weighting, adaptive damping, step and joint-limit clamping. -- *Algorithm / paper:* S. R. Buss (2004); Nakamura & Hanafusa (1986); Chiaverini (1997). -- *Reuses / builds on:* upstream `optimization::LevenbergMarquardt` (NT64) with a manipulability damping policy. - **M17. Impedance control.** Stiffness/damping impedance via `Jᵀ` (no force sensor), and inertia-shaping impedance with measured contact wrench via the task-space inertia. - *Algorithm / paper:* N. Hogan, *ASME J. Dyn. Sys. Meas. Control*, 1985. diff --git a/doc/kinematics/PoseInverseKinematics.md b/doc/kinematics/PoseInverseKinematics.md new file mode 100644 index 0000000..1fd194f --- /dev/null +++ b/doc/kinematics/PoseInverseKinematics.md @@ -0,0 +1,103 @@ +# Pose Inverse Kinematics + +## Overview & Motivation + +Position-only inverse kinematics places the tool point but ignores how the tool is oriented. Grasping, welding, screwing and camera pointing all need the full **pose**: position and orientation together, six constraints for a six-joint arm. + +This solver finds joint angles that reach a target pose. It works on any arm that exposes a geometric Jacobian and a tool pose (link chain, Denavit–Hartenberg or product of exponentials). It iterates damped least-squares steps on a properly weighted log-map pose error. +Damping adapts to the arm's manipulability, so iterations are fast Newton steps away from singularities and stay bounded near them. Step clamping and joint limits keep every iterate safe to command. + +## Mathematical Theory + +### Pose Error + +For a target $T^* = (R^*, p^*)$ and the current tool pose $T(q) = (R, p)$, the error is + +$$e = \begin{bmatrix} p^* - p \\ \log\left(R^* R^T\right)^\vee \end{bmatrix}$$ + +The rotation part is the true log map: a rotation vector $\phi\hat{n}$ with $\phi \le \pi$, taken the short way round. The vector part of an error quaternion, $2\sin(\phi/2)\hat{n}$, is only a small-angle approximation; at $\phi = 1$ rad it is already $4\%$ short. + +### Weighting + +Position errors are in metres and rotation errors in radians. A **characteristic length** $\rho$, of the order of the arm's reach or tool length, makes them commensurate: + +$$W = \operatorname{diag}(1, 1, 1, \rho, \rho, \rho)$$ + +The stopping test is $\|We\| < \epsilon$, and the step is computed on the weighted Jacobian $WJ$. + +### Damped Least-Squares Step + +Each iteration solves + +$$\Delta q = J^T W \left(W J J^T W + \lambda^2 I\right)^{-1} W e$$ + +then clamps $\|\Delta q\| \le \Delta_{\max}$ and projects $q + \Delta q$ onto the joint limits. With $\lambda = 0$ and an invertible $J$, the weights cancel and the step is Newton's $J^{-1}e$. + +**Damping does not bias the answer.** At a fixed point, $(WJ)^T y = 0$ with $WJ$ of full row rank forces $y = 0$ and hence $e = 0$. For a reachable, non-singular target the iteration therefore converges to the exact pose, and damping only slows it from quadratic to linear convergence. + +### Adaptive Damping + +With the weighted manipulability $w = \sqrt{\det(WJJ^TW)}$ and a threshold $w_0$ (Nakamura–Hanafusa, Chiaverini): + +$$\lambda^2 = \begin{cases} \lambda_0^2 \left(1 - (w/w_0)^2\right) & w < w_0 \\ 0 & w \ge w_0 \end{cases}$$ + +Damping is zero away from singularities and rises smoothly to $\lambda_0$ as $w \to 0$. With $w_0 = 0$, the damping is the constant $\lambda_0$. + +## Complexity Analysis + +| Step | Time | Space | Notes | +|------------------------|--------------|---------|----------------------------------------------------------| +| Tool pose and Jacobian | $O(n)$ | $O(n)$ | Supplied by the arm model | +| Weighted normal matrix | $O(36n)$ | $O(36)$ | $WJ\,(WJ)^T$, $6 \times 6$ | +| Damped solve | $O(6^3)$ | $O(36)$ | One LU factorization | +| Adaptive damping | $O(36n+6^3)$ | $O(36)$ | One more normal matrix and LU determinant when $w_0 > 0$ | +| Final error | $O(n)$ | $O(1)$ | One extra tool pose after the last update | + +## Step-by-Step Walkthrough + +A PUMA-560 (standard DH) with target pose $T^* = T(q^\ast)$, $q^\ast = (0.3, -0.4, 0.5, 0.6, -0.7, 0.8)$, has the tool at $p = (0.402, -0.033, 0.264)$ m. The solver starts from $q^\ast + 0.1$ with $\rho = 0.3$ m. + +1. **Constant damping $\lambda_0 = 0.05$**: $\|We\| < 10^{-4}$ after 3 iterations. +2. **Heavier damping $\lambda_0 = 0.2$**: 14 iterations to $10^{-5}$ instead of 4, but the same joint angles to within $10^{-3}$. Damping costs speed, not accuracy. +3. **One weighted step**: with $J = I$, $\rho = 0.1$ and $\lambda = 0.1$, the error $(0.1, 0, 0, 0, 0, 0.1)$ gives $\Delta q_i = w_i^2 e_i / (w_i^2 + \lambda^2)$, i.e. $0.0990$ on the position row and $0.05$ on the rotation row. +4. **Wrist singularity**: starting at $\theta_5 = 0$, where the wrist axes align, adaptive damping with $w_0 = 10^{-3}$ and $\Delta_{\max} = 0.2$ converges in 8 iterations. +5. **Unreachable target**: moving the target to $(3, 0, 0)$ m, beyond the arm's reach, stops at the iteration limit with a finite configuration and a residual of about $3.8$. + +## Pitfalls & Edge Cases + +- **Approximate orientation errors**: quaternion-vector or Euler-angle differences distort large rotations and can stall convergence. Use the log map. +- **Unweighted error**: mixing metres and radians unscaled makes the tolerance and the damping meaningless for one of them. Pick $\rho$ from the arm's dimensions. +- **Antipodal orientations**: at $\phi = \pi$ the rotation axis is ambiguous. Either sign is valid, and the iteration picks one. +- **Joint limits**: clamping is a projected iteration. A target that needs a joint beyond its limit ends with a non-zero residual rather than a violation. +- **Redundant arms**: with seven or more joints the same code returns the minimum-norm step, which drifts in the null space. Add a null-space objective to steer it. +- **Warm start**: seed $q_0$ from the previous control cycle. Far-away seeds may converge to a different solution branch (elbow up or down, wrist flipped). + +## Variants & Generalizations + +- **Levenberg–Marquardt with trust region**: adapt $\lambda$ from the actual versus predicted error reduction. +- **Selectively damped least squares**: damp each singular direction separately. +- **Quadratic-programming IK**: handle joint limits, velocity limits and collision constraints as inequalities. +- **Analytical solutions**: closed forms for arms with spherical wrists, used to seed or replace the iteration. + +## Applications + +- Converting Cartesian waypoints into joint targets for motion planning. +- Task-space teleoperation and jogging. +- Grasp and tool-placement queries during planning. +- Calibration routines that drive the tool to measured poses. + +## Connections to Other Algorithms + +- [Inverse Kinematics](InverseKinematics.md): the position-only damped least-squares solver. +- [Geometric Jacobian](GeometricJacobian.md): supplies the Jacobian through the shared provider interface. +- [Denavit-Hartenberg Parameters](DenavitHartenberg.md): the PUMA-560 model used in the walkthrough. +- [Manipulability Index](ManipulabilityIndex.md): the measure driving the adaptive damping. +- [Redundancy Resolution](RedundancyResolution.md): steers the null-space motion of redundant arms. +- The pose error and the Levenberg–Marquardt iteration come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Nakamura, Y. and Hanafusa, H. (1986). "Inverse kinematic solutions with singularity robustness for robot manipulator control." *ASME Journal of Dynamic Systems, Measurement, and Control*, 108(3), 163–171. +- Wampler, C.W. (1986). "Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods." *IEEE Transactions on Systems, Man, and Cybernetics*, 16(1), 93–101. +- Chiaverini, S. (1997). "Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators." *IEEE Transactions on Robotics and Automation*, 13(3), 398–410. +- Buss, S.R. (2004). "Introduction to inverse kinematics with Jacobian transpose, pseudoinverse and damped least squares methods." Technical report, University of California, San Diego. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index 9307c0b..fde5367 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -4,13 +4,14 @@ Geometry and motion algorithms for serial kinematic chains. ## Algorithms -| Algorithm | Description | -|-------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------| -| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions and the tool point from joint angles for serial chains — $O(n)$ complexity | -| [Chain Pose Kinematics](ChainPoseKinematics.md) | Full tool pose in SE(3) plus every joint origin and axis in the base frame (the frame chain) — $O(n)$ | -| [Denavit-Hartenberg Parameters](DenavitHartenberg.md) | Standard and modified DH tables to tool pose and frame chain, with constant joint offsets — $O(n)$ | -| [Geometric Jacobian](GeometricJacobian.md) | $6 \times n$ Jacobian, bias acceleration $\dot{J}\dot{q}$ and statics $\tau = J^T w$ from any frame chain — $O(n)$ | -| [Manipulability Index](ManipulabilityIndex.md) | Yoshikawa index, velocity-ellipsoid axes and condition number per unit-consistent Jacobian block — singularity monitor | -| [Product of Exponentials](ProductOfExponentials.md) | Screw-axis forward kinematics, space Jacobian and its conversion to the geometric Jacobian — $O(n)$ | -| [Redundancy Resolution](RedundancyResolution.md) | Damped pseudo-inverse primary task plus exact null-space projection of a secondary objective for redundant arms | -| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | +| Algorithm | Description | +|-------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------| +| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions and the tool point from joint angles for serial chains — $O(n)$ complexity | +| [Chain Pose Kinematics](ChainPoseKinematics.md) | Full tool pose in SE(3) plus every joint origin and axis in the base frame (the frame chain) — $O(n)$ | +| [Denavit-Hartenberg Parameters](DenavitHartenberg.md) | Standard and modified DH tables to tool pose and frame chain, with constant joint offsets — $O(n)$ | +| [Geometric Jacobian](GeometricJacobian.md) | $6 \times n$ Jacobian, bias acceleration $\dot{J}\dot{q}$ and statics $\tau = J^T w$ from any frame chain — $O(n)$ | +| [Manipulability Index](ManipulabilityIndex.md) | Yoshikawa index, velocity-ellipsoid axes and condition number per unit-consistent Jacobian block — singularity monitor | +| [Product of Exponentials](ProductOfExponentials.md) | Screw-axis forward kinematics, space Jacobian and its conversion to the geometric Jacobian — $O(n)$ | +| [Redundancy Resolution](RedundancyResolution.md) | Damped pseudo-inverse primary task plus exact null-space projection of a secondary objective for redundant arms | +| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | +| [Pose Inverse Kinematics](PoseInverseKinematics.md) | Full 6-DoF pose IK: damped least squares on the weighted log-map pose error with manipulability-adaptive damping, step clamping and joint limits | diff --git a/roadmap/README.md b/roadmap/README.md index a93c2b6..fd9d835 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -89,7 +89,7 @@ prerequisites. ## Index ### `kinematics` -`PoseInverseKinematics` (M13) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) +`AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `trajectory` `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/kinematics/PoseInverseKinematics/explanation.md b/roadmap/kinematics/PoseInverseKinematics/explanation.md deleted file mode 100644 index a9b47d4..0000000 --- a/roadmap/kinematics/PoseInverseKinematics/explanation.md +++ /dev/null @@ -1,41 +0,0 @@ -# Pose Inverse Kinematics — Overview - -## What it is -Given a desired **pose** — position *and* orientation — of the tool, find joint values that reach it. -It extends position-only IK to all six task dimensions by driving a 6-vector error (three for -translation, three for rotation) to zero with weighted, damped least-squares Newton steps. - -## Why it matters (embedded) -Real tasks are pose tasks: point a welding torch, align a gripper, hold a camera level. A numerically -robust solver that survives singularities without commanding huge joint jumps, respects joint limits -and runs a bounded number of iterations is what makes an arm safe to drive from a real-time loop, -warm-started from the previous cycle. - -## How it works (intuition) -Each step linearizes the arm with the Jacobian and asks "what joint change best cancels the current -pose error?" The plain least-squares answer explodes near singularities; adding `λ²I` inside the -inverse keeps the step bounded. Because the method *iterates*, damping does not leave an error -behind: whenever the target is reachable and the arm is not singular, the only fixed point is zero -error — damping just makes each step shorter. Adaptive damping switches it on only when the -manipulability drops, so steps are full Newton steps elsewhere. Two details keep the error honest: -rotation is measured with the true logarithm of the relative rotation (always the short way, exact -angle), and radians are converted to metres with a characteristic length so that position and -orientation are weighed fairly in both the step and the stopping test. - -## Key parameters -- **damping λ₀ and threshold w₀** — maximum damping and the manipulability below which it engages. -- **characteristic length ρ** — how many metres one radian of orientation error is worth. -- **tolerance, max iterations, max step, joint limits** — stopping rule and safety bounds. -- **seed `q0`** — warm-starting from the last cycle keeps iteration counts low. - -## Reference -Y. Nakamura, H. Hanafusa, "Inverse Kinematic Solutions with Singularity Robustness for Robot -Manipulator Control," *ASME J. Dyn. Sys. Meas. Control*, 108, 1986; S. Chiaverini, "Singularity-Robust -Task-Priority Redundancy Resolution," *IEEE Trans. Robotics and Automation*, 13(3), 1997; -S. R. Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped -Least Squares methods," 2004. - -## See also -`JacobianProvider` / `GeometricJacobian` (M8, the linearization), `math::SE3Transform::PoseError` (NT53, the -orientation error), `ManipulabilityIndex` (M11), `AnalyticalIkOpw` (M21, the closed-form alternative -for ortho-parallel 6R arms), upstream `LevenbergMarquardt` (NT64, the damped iteration). diff --git a/roadmap/kinematics/PoseInverseKinematics/implementation.md b/roadmap/kinematics/PoseInverseKinematics/implementation.md deleted file mode 100644 index 7a63f62..0000000 --- a/roadmap/kinematics/PoseInverseKinematics/implementation.md +++ /dev/null @@ -1,116 +0,0 @@ -# Pose Inverse Kinematics (Weighted Damped Least Squares) — Implementation Pseudocode - -> Roadmap ref: #M13 (Tier 3) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Model-agnostic: the arm is a `JacobianProvider` (M8), so the same solver serves the link -chain (`ChainTaskJacobian`, M30/M8), DH (`DhTaskJacobian`, M7) and PoE (M15). The iteration itself is -upstream `optimization::LevenbergMarquardt` (NT64); this item supplies the IK problem — weighted log-map -pose residual, weighted Jacobian, joint-limit retraction — and the singularity-robust damping policy. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct PoseIkConfig: - T damping # λ₀ - T manipulabilityThreshold # w₀; 0 ⇒ constant damping λ₀ (adaptive otherwise) - T characteristicLength # ρ (m): W = diag(1,1,1, ρ,ρ,ρ) makes rad commensurate with m - T tolerance # on ‖W·e‖ (m) - T maxStep # Δmax on ‖Δq‖; +∞ disables - std::size_t maxIterations - JointVector lowerLimits, upperLimits # ∓∞ disables - -template -struct PoseIkResult: - JointVector q - T finalError # ‖W·PoseError(target, ToolPose(q))‖ at the returned q - std::size_t iterations - bool converged - -template # implements optimization::LeastSquaresProblem -class PoseIkProblem: - const JacobianProvider& arm; math::SE3Transform target; T ρ; JointVector lower, upper - -template # implements optimization::DampingPolicy -class ManipulabilityDamping: - T λ₀, w₀ # w₀ = 0 ⇒ constant λ₀² - -template -class PoseInverseKinematics: - const JacobianProvider& arm # Jacobian(q), ToolPose(q); injected - PoseIkConfig config -``` - -## Interface - -```text -PoseInverseKinematics(const JacobianProvider& arm, const PoseIkConfig& config) -PoseIkResult Solve(const SE3Transform& target, const JointVector& q0) const # hot path -``` - -## Algorithm (pseudocode) - -```text -# --- the IK problem handed to the upstream solver (residual r = −W·e, so δ = Jᵀ W (W J Jᵀ W + λ²I)⁻¹ W e) --- -function PoseIkProblem::Residual(q): - e = math::SE3Transform::PoseError(target, arm.ToolPose(q)) # (Δp; true log-map rotation vector), NT53 - return −(e.linear; ρ·e.angular) -function PoseIkProblem::Jacobian(q): return arm.Jacobian(q) with rows 3–5 scaled by ρ # W·J -function PoseIkProblem::Retract(q, δ): return clamp(q + δ, lower, upper) # projected iteration - -function ManipulabilityDamping::DampingSquared(JW): # Nakamura–Hanafusa / Chiaverini adaptive form - if w₀ == 0: return λ₀² - A0 = JW·JWᵀ # 6×6 - w = LuDecomposition(A0) singular ? 0 : sqrt(max(0, Determinant(A0))) # weighted manipulability - # (≡ 0 when Dof < 6 or near-singular ⇒ λ = λ₀) - return w < w₀ ? λ₀²·(1 − (w / w₀)²) : 0 - -function Solve(target, q0): # OPTIMIZE_FOR_SPEED - problem = PoseIkProblem{ arm, target, ρ, lowerLimits, upperLimits } - damping = ManipulabilityDamping{ λ₀, w₀ } - r = optimization::LevenbergMarquardt{ problem, damping, { tolerance, maxStep, maxIterations } }.Solve(q0) - return { r.x, r.residualNorm, r.iterations, r.converged } # residual recomputed at the returned q -``` - -## Complexity & memory - -- Per iteration: one `ToolPose` + `Jacobian` (`O(Dof)`), `O(36·Dof)` for `JW·JWᵀ`, `O(6³)` LU (in the - upstream wide-branch step), plus one more `JW·JWᵀ` and 6×6 LU in the damping policy when `w₀ > 0`. -- One extra `ToolPose` after the loop for the final error (upstream). -- Memory: one `6×Dof` Jacobian and a `6×6` system; stack only. - -## Numerical / embedded notes - -- **Damping does not bias the answer:** at a fixed point `JWᵀ·y = 0` with `JW` of full row rank forces - `y = 0`, hence `e = 0`. For a reachable, non-singular target iterative DLS converges to the exact pose; - `λ` only slows convergence (linear instead of quadratic). The residual is non-zero only for - unreachable targets, at singularities (DLS returns the damped least-squares compromise) or when a - joint limit is active. -- **Units:** position error is in m, rotation error in rad. `ρ` (≈ the arm's reach or tool length) converts - radians to metres in both the stopping test and the step. With `λ = 0` and invertible `J`, `W` cancels - (`Δq = J⁻¹e`); it matters for damping, redundancy and unreachable targets. -- **Orientation error** is `math::SE3Transform::PoseError` — the true log map, `|eR| ≤ π`, short way round. - `2·vec(error quaternion)` equals `2 sin(φ/2)·n̂`, which is only a small-angle approximation of `φ·n̂` - (0.959 vs 1 rad at φ = 1 rad) and must not be used. -- **Adaptive damping:** `λ = 0` away from singularities (fast Newton convergence), rising smoothly to - `λ₀` as `w → 0`. Step clamping bounds the joint jump per iteration; joint limits are enforced by - clamping (projected iteration). -- Upstream `LuDecomposition` reports singular below a relative pivot of `1e-6`; keep `λ₀² ≫ 1e-6·‖W·J‖²` - (assert `λ₀ > 0`) so the damped system never trips it — a tripped `A0` simply means `w = 0`, `λ = λ₀`. -- Seed `q0` from the previous control cycle for warm-started, few-iteration convergence. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/PoseInverseKinematics.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class PoseInverseKinematics;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/PoseInverseKinematics.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestPoseInverseKinematics.cpp` -- Doc: `doc/kinematics/PoseInverseKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestPoseInverseKinematics.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform::PoseError`), NT64 (`optimization::LevenbergMarquardt`), - M8 (`JacobianProvider`); upstream `solvers::LuDecomposition` (damping policy). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/PoseInverseKinematics/tests.md b/roadmap/kinematics/PoseInverseKinematics/tests.md deleted file mode 100644 index 687268b..0000000 --- a/roadmap/kinematics/PoseInverseKinematics/tests.md +++ /dev/null @@ -1,73 +0,0 @@ -# Pose Inverse Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPoseInverseKinematics : public ::testing::Test: - DenavitHartenberg puma{ pumaTable, Standard } # PUMA-560, see M7 tests - DhTaskJacobian arm{ puma } - StrictMock> mock # scripted J and ToolPose - PoseIkConfig config{ λ₀ = 0.05, w₀ = 0, ρ = 0.3, tol = 1e-4, maxStep = ∞, 100, ∓∞ } - JointVector qTrue{ 0.3, −0.4, 0.5, 0.6, −0.7, 0.8 } -# each case below is a TEST_F(TestPoseInverseKinematics, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -round_trip_reachable_pose: - Arrange: target = puma.Forward(qTrue); q0 = qTrue + 0.1 - Act: r = PoseInverseKinematics{ arm, config }.Solve(target, q0) - Assert: r.converged; r.iterations ≤ 10; ‖W·PoseError(target, puma.Forward(r.q))‖ < 1e-4 -damping_slows_but_does_not_bias: - Arrange: λ₀ = 0.2, tol = 1e-5, maxIterations = 200 - Assert: r.converged; ‖r.q − qTrue‖∞ < 1e-3; r.iterations > the λ₀ = 0.05 count -zero_error_is_immediate: - Assert: Solve(puma.Forward(q0), q0) → iterations == 0, converged -final_error_is_measured_after_the_last_update: - Arrange: maxIterations = 1, λ₀ = 0, ρ = 1; target = { Rz(0.1), (0.1, 0, 0) } - mock.ToolPose → Identity (1st call), { Rz(0.08), (0.08, 0, 0) } (2nd call); mock.Jacobian → I₆ - Assert: r.q ≈ q0 + (0.1,0,0,0,0,0.1); r.finalError ≈ 0.028284 (not 0.141421); converged == false -rotation_rows_are_weighted_in_the_step: - Arrange: maxIterations = 1, λ₀ = 0.1, ρ = 0.1, target = { Rz(0.1), (0.1, 0, 0) }; - mock.ToolPose → Identity (twice); mock.Jacobian → I₆ - Assert: r.q − q0 ≈ (0.0990099, 0, 0, 0, 0, 0.05) [= wᵢ²/(wᵢ² + λ²)·eᵢ]; r.finalError ≈ 0.100499 -adaptive_damping_vanishes_away_from_singularity: - Arrange: w₀ = 0.5, λ₀ = 0.1, ρ = 1, maxIterations = 1; mock.Jacobian → I₆ (w = 1) - Assert: Δq == e exactly (λ = 0) -adaptive_damping_engages_near_singularity: - Arrange: as above, mock.Jacobian → diag(1,1,1,1,1,0.1) (w = 0.1 ⇒ λ² = 0.0096), e = 0.01·𝟙 - Assert: Δq ≈ (0.0099049 ×5, 0.0510204) -step_is_clamped: - Arrange: maxStep = 0.1, mock J = I₆, e = (0.3, 0.4, 0, 0, 0, 0) - Assert: Δq ≈ (0.06, 0.08, 0, 0, 0, 0) -joint_limits_are_enforced: - Arrange: upperLimits[0] = q0[0] + 0.05, mock J = I₆, e = (0.3, 0, 0, 0, 0, 0) - Assert: r.q[0] == upperLimits[0] -damping_survives_wrist_singularity: - Arrange: q0 = (0.3, −0.4, 0.5, 0.6, 0, 0.8) (θ₅ = 0: wrist axes aligned, σ_min ≈ 2e-9); - target = puma.Forward((0.3, −0.4, 0.5, 0.9, 0.3, 0.5)); w₀ = 1e-3, maxStep = 0.2 - Assert: r.converged; every r.q component finite -unreachable_target_fails_gracefully: - Arrange: target.p = (3, 0, 0) (beyond reach ≈ 0.9 m) - Assert: !r.converged; r.finalError > tol; q finite -``` - -## Reference vectors - -- PUMA-560 at `qTrue`: tool `p = (0.402407, −0.032586, 0.263519)`; from `qTrue + 0.1` (double): - `λ = 0.05` reaches `1e-4` in 3 iterations; `λ = 0` / `0.05` / `0.2` reach `‖W e‖ < 1e-12` in 4 / 12 / - 64 iterations, all to `qTrue` within `1e-11` (damping is unbiased). -- Weighted manipulability `√det(W J Jᵀ W)` at `qTrue` with `ρ = 0.3` is `1.09e-3` — pick `w₀` per arm - on that scale. Wrist-singular case above converges in 8 iterations (double). -- Stale-error scenario on the real arm: initial `‖W e‖ = 0.136739`, after one step `0.011815` — the - result must report the latter. -- `+190°` about `ẑ` ⇒ rotation vector `−2.967060·ẑ` (short way, via NT53). - -## Edge cases - -- Antipodal orientation (180°) ⇒ `PoseError` axis ambiguous (either sign valid, NT53). -- Target at the reach boundary ⇒ slow but bounded convergence (adaptive damping engages). -- `Dof = 7` ⇒ same code; the minimum-norm step drifts in the null space — use M14 to steer it. diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index 19f1848..5c8e963 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -8,6 +8,7 @@ target_include_directories(robotics.kinematics ${ROBOTICS_VISIBILITY} target_link_libraries(robotics.kinematics ${ROBOTICS_VISIBILITY} robotics.dynamics numerical.math + numerical.optimization numerical.solver ) @@ -21,6 +22,7 @@ target_sources(robotics.kinematics PRIVATE InverseKinematics.hpp JacobianProvider.hpp ManipulabilityIndex.hpp + PoseInverseKinematics.hpp ProductOfExponentials.hpp RedundancyResolution.hpp ) @@ -32,6 +34,7 @@ robotics_add_coverage_sources(robotics.kinematics GeometricJacobian.cpp InverseKinematics.cpp ManipulabilityIndex.cpp + PoseInverseKinematics.cpp ProductOfExponentials.cpp RedundancyResolution.cpp ) diff --git a/robotics/kinematics/PoseInverseKinematics.cpp b/robotics/kinematics/PoseInverseKinematics.cpp new file mode 100644 index 0000000..27cec31 --- /dev/null +++ b/robotics/kinematics/PoseInverseKinematics.cpp @@ -0,0 +1,11 @@ +#include "robotics/kinematics/PoseInverseKinematics.hpp" + +namespace kinematics +{ + template class PoseIkProblem; + template class PoseIkProblem; + template class ManipulabilityDamping; + template class ManipulabilityDamping; + template class PoseInverseKinematics; + template class PoseInverseKinematics; +} diff --git a/robotics/kinematics/PoseInverseKinematics.hpp b/robotics/kinematics/PoseInverseKinematics.hpp new file mode 100644 index 0000000..b81d1f3 --- /dev/null +++ b/robotics/kinematics/PoseInverseKinematics.hpp @@ -0,0 +1,201 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "numerical/optimization/DampingPolicy.hpp" +#include "numerical/optimization/LeastSquaresProblem.hpp" +#include "numerical/optimization/LevenbergMarquardt.hpp" +#include "numerical/solvers/LuDecomposition.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include +#include +#include + +namespace kinematics +{ + template + struct PoseIkConfig + { + static_assert(std::is_floating_point_v, "PoseIkConfig supports floating-point types"); + + using JointVector = math::Vector; + + static JointVector Filled(T value) + { + JointVector filled{}; + for (std::size_t i = 0; i < Dof; ++i) + filled[i] = value; + return filled; + } + + T damping{}; + T manipulabilityThreshold{}; + T characteristicLength{ 1 }; + T tolerance{}; + T maxStep{ std::numeric_limits::infinity() }; + std::size_t maxIterations{}; + JointVector lowerLimits{ Filled(-std::numeric_limits::infinity()) }; + JointVector upperLimits{ Filled(std::numeric_limits::infinity()) }; + }; + + template + struct PoseIkResult + { + static_assert(std::is_floating_point_v, "PoseIkResult supports floating-point types"); + + math::Vector q{}; + T finalError{}; + std::size_t iterations{}; + bool converged{ false }; + }; + + template + class PoseIkProblem + : public optimization::LeastSquaresProblem, 6, Dof> + { + static_assert(std::is_floating_point_v, "PoseIkProblem supports floating-point types"); + + public: + using JointVector = math::Vector; + using Arm = JacobianProvider; + + PoseIkProblem(const Arm& arm, const math::SE3Transform& target, T characteristicLength, const JointVector& lowerLimits, const JointVector& upperLimits); + + math::Vector Residual(const JointVector& q) const override; + math::Matrix Jacobian(const JointVector& q) const override; + JointVector Retract(const JointVector& q, const JointVector& delta) const override; + + private: + const Arm& arm; + math::SE3Transform target; + T characteristicLength; + JointVector lowerLimits; + JointVector upperLimits; + }; + + template + class ManipulabilityDamping + : public optimization::DampingPolicy + { + static_assert(std::is_floating_point_v, "ManipulabilityDamping supports floating-point types"); + + public: + ManipulabilityDamping(T damping, T threshold); + + T DampingSquared(const math::Matrix& jacobian) const override; + + private: + T damping; + T threshold; + }; + + template + class PoseInverseKinematics + { + static_assert(std::is_floating_point_v, "PoseInverseKinematics supports floating-point types"); + + public: + using JointVector = math::Vector; + using Arm = JacobianProvider; + using Config = PoseIkConfig; + using Result = PoseIkResult; + + PoseInverseKinematics(const Arm& arm, const Config& config); + + OPTIMIZE_FOR_SPEED Result Solve(const math::SE3Transform& target, const JointVector& q0) const; + + private: + const Arm& arm; + Config config; + }; + + template + PoseIkProblem::PoseIkProblem(const Arm& arm, const math::SE3Transform& target, T characteristicLength, const JointVector& lowerLimits, const JointVector& upperLimits) + : arm{ arm } + , target{ target } + , characteristicLength{ characteristicLength } + , lowerLimits{ lowerLimits } + , upperLimits{ upperLimits } + {} + + template + math::Vector PoseIkProblem::Residual(const JointVector& q) const + { + const auto error{ math::SE3Transform::PoseError(target, arm.ToolPose(q)) }; + + math::Vector residual{}; + for (std::size_t i = 0; i < 6; ++i) + residual[i] = -(i < 3 ? error[i] : characteristicLength * error[i]); + return residual; + } + + template + math::Matrix PoseIkProblem::Jacobian(const JointVector& q) const + { + auto weighted{ arm.Jacobian(q) }; + for (std::size_t r = 3; r < 6; ++r) + for (std::size_t c = 0; c < Dof; ++c) + weighted.at(r, c) *= characteristicLength; + return weighted; + } + + template + auto PoseIkProblem::Retract(const JointVector& q, const JointVector& delta) const -> JointVector + { + JointVector next{}; + for (std::size_t i = 0; i < Dof; ++i) + next[i] = std::min(std::max(q[i] + delta[i], lowerLimits[i]), upperLimits[i]); + return next; + } + + template + ManipulabilityDamping::ManipulabilityDamping(T damping, T threshold) + : damping{ damping } + , threshold{ threshold } + {} + + template + T ManipulabilityDamping::DampingSquared(const math::Matrix& jacobian) const + { + const T maximum{ damping * damping }; + if (threshold == T{}) + return maximum; + + solvers::LuDecomposition lu; + const T manipulability{ lu.Decompose(jacobian * jacobian.Transpose()) ? math::Sqrt(std::max(T{}, lu.Determinant())) : T{} }; + if (manipulability >= threshold) + return T{}; + + const T ratio{ manipulability / threshold }; + return maximum * (T{ 1 } - ratio * ratio); + } + + template + PoseInverseKinematics::PoseInverseKinematics(const Arm& arm, const Config& config) + : arm{ arm } + , config{ config } + {} + + template + OPTIMIZE_FOR_SPEED auto PoseInverseKinematics::Solve(const math::SE3Transform& target, const JointVector& q0) const -> Result + { + const PoseIkProblem problem{ arm, target, config.characteristicLength, config.lowerLimits, config.upperLimits }; + const ManipulabilityDamping policy{ config.damping, config.manipulabilityThreshold }; + const optimization::LevenbergMarquardt solver{ problem, policy, { config.tolerance, config.maxStep, config.maxIterations } }; + + const auto solution{ solver.Solve(q0) }; + return Result{ solution.x, solution.residualNorm, solution.iterations, solution.converged }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PoseIkProblem; + extern template class PoseIkProblem; + extern template class ManipulabilityDamping; + extern template class ManipulabilityDamping; + extern template class PoseInverseKinematics; + extern template class PoseInverseKinematics; +#endif +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index 291625d..157c043 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -15,6 +15,7 @@ target_sources(robotics.kinematics_test PRIVATE TestGeometricJacobian.cpp TestInverseKinematics.cpp TestManipulabilityIndex.cpp + TestPoseInverseKinematics.cpp TestProductOfExponentials.cpp TestRedundancyResolution.cpp ) diff --git a/robotics/kinematics/test/TestPoseInverseKinematics.cpp b/robotics/kinematics/test/TestPoseInverseKinematics.cpp new file mode 100644 index 0000000..1835b9b --- /dev/null +++ b/robotics/kinematics/test/TestPoseInverseKinematics.cpp @@ -0,0 +1,216 @@ +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/DhTaskJacobian.hpp" +#include "robotics/kinematics/PoseInverseKinematics.hpp" +#include "robotics/kinematics/test_doubles/MockJacobianProvider.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using ::testing::_; + using ::testing::Return; + using Vector3 = math::Vector3; + using Joints = math::Vector; + using Jacobian = math::Matrix; + using Transform = math::SE3Transform; + using DhLink = kinematics::DhLink; + using Ik = kinematics::PoseInverseKinematics; + + constexpr float pi{ std::numbers::pi_v }; + constexpr auto revolute{ dynamics::JointType::Revolute }; + + Transform Pose(const Vector3& rotationVector, const Vector3& translation) + { + math::Vector6 twist{}; + twist.SetBlock(rotationVector, 3, 0); + return Transform{ Transform::Exp(twist, 1.0f).rotation, translation }; + } + + Ik::Config MakeConfig() + { + Ik::Config config{}; + config.damping = 0.05f; + config.characteristicLength = 0.3f; + config.tolerance = 1e-4f; + config.maxIterations = 100; + return config; + } + + class TestPoseInverseKinematics + : public ::testing::Test + { + protected: + float WeightedError(const Transform& target, const Joints& q) const + { + const auto error{ Transform::PoseError(target, puma.Forward(q)) }; + float sum{ 0.0f }; + for (std::size_t i = 0; i < 6; ++i) + { + const float weighted{ i < 3 ? error[i] : config.characteristicLength * error[i] }; + sum += weighted * weighted; + } + return std::sqrt(sum); + } + + Ik::Result SolveWithMock(const Transform& target, const Transform& firstPose, const Transform& secondPose, const Jacobian& jacobian) + { + config.maxIterations = 1; + EXPECT_CALL(mock, ToolPose(_)).WillOnce(Return(firstPose)).WillOnce(Return(secondPose)); + EXPECT_CALL(mock, Jacobian(_)).WillOnce(Return(jacobian)); + return Ik{ mock, config }.Solve(target, qTrue); + } + + void ExpectStep(const Ik::Result& result, const Joints& expected) const + { + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(result.q[i] - qTrue[i], expected[i], 1e-6f); + } + + static void ExpectFinite(const Joints& q) + { + for (std::size_t i = 0; i < 6; ++i) + EXPECT_TRUE(std::isfinite(q[i])); + } + + kinematics::DenavitHartenberg puma{ { DhLink{ 0.0f, pi / 2.0f, 0.0f, 0.0f, revolute }, DhLink{ 0.4318f, 0.0f, 0.0f, 0.0f, revolute }, + DhLink{ 0.0203f, -pi / 2.0f, 0.15005f, 0.0f, revolute }, DhLink{ 0.0f, pi / 2.0f, 0.4318f, 0.0f, revolute }, + DhLink{ 0.0f, -pi / 2.0f, 0.0f, 0.0f, revolute }, DhLink{ 0.0f, 0.0f, 0.0f, 0.0f, revolute } }, + kinematics::DhConvention::Standard }; + kinematics::DhTaskJacobian arm{ puma }; + ::testing::StrictMock> mock; + Ik::Config config{ MakeConfig() }; + Joints qTrue{ 0.3f, -0.4f, 0.5f, 0.6f, -0.7f, 0.8f }; + Joints qNear{ 0.4f, -0.3f, 0.6f, 0.7f, -0.6f, 0.9f }; + Transform nudge{ Pose(Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.1f, 0.0f, 0.0f }) }; + }; +} + +TEST_F(TestPoseInverseKinematics, round_trip_reachable_pose) +{ + const auto target{ puma.Forward(qTrue) }; + + const auto result{ Ik{ arm, config }.Solve(target, qNear) }; + + EXPECT_TRUE(result.converged); + EXPECT_LE(result.iterations, std::size_t{ 10 }); + EXPECT_LT(WeightedError(target, result.q), 1e-4f); +} + +TEST_F(TestPoseInverseKinematics, damping_slows_but_does_not_bias) +{ + const auto target{ puma.Forward(qTrue) }; + config.tolerance = 1e-5f; + config.maxIterations = 200; + const auto light{ Ik{ arm, config }.Solve(target, qNear) }; + config.damping = 0.2f; + + const auto heavy{ Ik{ arm, config }.Solve(target, qNear) }; + + EXPECT_TRUE(heavy.converged); + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(heavy.q[i], qTrue[i], 1e-3f); + EXPECT_GT(heavy.iterations, light.iterations); +} + +TEST_F(TestPoseInverseKinematics, zero_error_is_immediate) +{ + const auto result{ Ik{ arm, config }.Solve(puma.Forward(qNear), qNear) }; + + EXPECT_TRUE(result.converged); + EXPECT_EQ(result.iterations, std::size_t{ 0 }); +} + +TEST_F(TestPoseInverseKinematics, final_error_is_measured_after_the_last_update) +{ + config.damping = 0.0f; + config.characteristicLength = 1.0f; + + const auto result{ SolveWithMock(nudge, Transform::Identity(), Pose(Vector3{ 0.0f, 0.0f, 0.08f }, Vector3{ 0.08f, 0.0f, 0.0f }), Jacobian::Identity()) }; + + ExpectStep(result, Joints{ 0.1f, 0.0f, 0.0f, 0.0f, 0.0f, 0.1f }); + EXPECT_NEAR(result.finalError, 0.028284f, 1e-5f); + EXPECT_FALSE(result.converged); +} + +TEST_F(TestPoseInverseKinematics, rotation_rows_are_weighted_in_the_step) +{ + config.damping = 0.1f; + config.characteristicLength = 0.1f; + + const auto result{ SolveWithMock(nudge, Transform::Identity(), Transform::Identity(), Jacobian::Identity()) }; + + ExpectStep(result, Joints{ 0.0990099f, 0.0f, 0.0f, 0.0f, 0.0f, 0.05f }); + EXPECT_NEAR(result.finalError, 0.100499f, 1e-5f); +} + +TEST_F(TestPoseInverseKinematics, adaptive_damping_vanishes_away_from_singularity) +{ + config.manipulabilityThreshold = 0.5f; + config.damping = 0.1f; + config.characteristicLength = 1.0f; + const auto error{ Transform::PoseError(nudge, Transform::Identity()) }; + + const auto result{ SolveWithMock(nudge, Transform::Identity(), Transform::Identity(), Jacobian::Identity()) }; + + for (std::size_t i = 0; i < 6; ++i) + EXPECT_FLOAT_EQ(result.q[i], qTrue[i] + error[i]); +} + +TEST_F(TestPoseInverseKinematics, adaptive_damping_engages_near_singularity) +{ + config.manipulabilityThreshold = 0.5f; + config.damping = 0.1f; + config.characteristicLength = 1.0f; + auto nearlySingular{ Jacobian::Identity() }; + nearlySingular.at(5, 5) = 0.1f; + + const auto result{ SolveWithMock(Pose(Vector3{ 0.01f, 0.01f, 0.01f }, Vector3{ 0.01f, 0.01f, 0.01f }), Transform::Identity(), Transform::Identity(), nearlySingular) }; + + ExpectStep(result, Joints{ 0.0099049f, 0.0099049f, 0.0099049f, 0.0099049f, 0.0099049f, 0.0510204f }); +} + +TEST_F(TestPoseInverseKinematics, step_is_clamped) +{ + config.maxStep = 0.1f; + + const auto result{ SolveWithMock(Pose(Vector3{}, Vector3{ 0.3f, 0.4f, 0.0f }), Transform::Identity(), Transform::Identity(), Jacobian::Identity()) }; + + ExpectStep(result, Joints{ 0.06f, 0.08f, 0.0f, 0.0f, 0.0f, 0.0f }); +} + +TEST_F(TestPoseInverseKinematics, joint_limits_are_enforced) +{ + config.upperLimits[0] = qTrue[0] + 0.05f; + + const auto result{ SolveWithMock(Pose(Vector3{}, Vector3{ 0.3f, 0.0f, 0.0f }), Transform::Identity(), Transform::Identity(), Jacobian::Identity()) }; + + EXPECT_EQ(result.q[0], config.upperLimits[0]); +} + +TEST_F(TestPoseInverseKinematics, damping_survives_wrist_singularity) +{ + config.manipulabilityThreshold = 1e-3f; + config.maxStep = 0.2f; + const Joints wristSingular{ 0.3f, -0.4f, 0.5f, 0.6f, 0.0f, 0.8f }; + + const auto result{ Ik{ arm, config }.Solve(puma.Forward(Joints{ 0.3f, -0.4f, 0.5f, 0.9f, 0.3f, 0.5f }), wristSingular) }; + + EXPECT_TRUE(result.converged); + ExpectFinite(result.q); +} + +TEST_F(TestPoseInverseKinematics, unreachable_target_fails_gracefully) +{ + auto target{ puma.Forward(qTrue) }; + target.translation = Vector3{ 3.0f, 0.0f, 0.0f }; + + const auto result{ Ik{ arm, config }.Solve(target, qTrue) }; + + EXPECT_FALSE(result.converged); + EXPECT_GT(result.finalError, config.tolerance); + ExpectFinite(result.q); +} From 755b17948c95362efbced3b35a62716a93ca1149 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 13:02:47 +0000 Subject: [PATCH 7/9] feat(controllers): add ImpedanceControl (M17) Passive J^T stiffness/damping impedance with gravity compensation, and inertia-shaping impedance through the task-space inertia and a measured contact wrench. Adds the TaskError helper shared with M18/M19. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 4 - .../manipulator/ImpedanceControl.md | 110 ++++++++ doc/controllers/manipulator/README.md | 9 +- roadmap/README.md | 4 +- .../implementation.md | 4 +- .../ImpedanceControl/explanation.md | 41 --- .../ImpedanceControl/implementation.md | 122 --------- .../manipulator/ImpedanceControl/tests.md | 82 ------ .../OperationalSpaceControl/implementation.md | 4 +- .../controllers/manipulator/CMakeLists.txt | 5 + .../manipulator/ImpedanceControl.cpp | 7 + .../manipulator/ImpedanceControl.hpp | 81 ++++++ .../controllers/manipulator/TaskError.hpp | 26 ++ .../manipulator/test/CMakeLists.txt | 2 + .../manipulator/test/TestImpedanceControl.cpp | 238 ++++++++++++++++++ 16 files changed, 481 insertions(+), 260 deletions(-) create mode 100644 doc/controllers/manipulator/ImpedanceControl.md delete mode 100644 roadmap/controllers/manipulator/ImpedanceControl/explanation.md delete mode 100644 roadmap/controllers/manipulator/ImpedanceControl/implementation.md delete mode 100644 roadmap/controllers/manipulator/ImpedanceControl/tests.md create mode 100644 robotics/controllers/manipulator/ImpedanceControl.cpp create mode 100644 robotics/controllers/manipulator/ImpedanceControl.hpp create mode 100644 robotics/controllers/manipulator/TaskError.hpp create mode 100644 robotics/controllers/manipulator/test/TestImpedanceControl.cpp diff --git a/README.md b/README.md index 23d8840..ad8eb1a 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | | [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 17d2fd3..123953f 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -119,7 +119,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|-------------|------------| -| M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | | M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | @@ -138,9 +137,6 @@ retired here. ### Tier 3 — Moderate ★★★☆☆ -**M17. Impedance control.** Stiffness/damping impedance via `Jᵀ` (no force sensor), and inertia-shaping impedance with measured contact wrench via the task-space inertia. -- *Algorithm / paper:* N. Hogan, *ASME J. Dyn. Sys. Meas. Control*, 1985. - ### Tier 4 — Advanced ★★★★☆ **M18. Operational-space control.** Task-space inertia `Λ = (J M⁻¹ Jᵀ)⁻¹`, `J̇q̇` compensation, full joint-space gravity/Coriolis compensation and a dynamically-consistent null space. diff --git a/doc/controllers/manipulator/ImpedanceControl.md b/doc/controllers/manipulator/ImpedanceControl.md new file mode 100644 index 0000000..9bd6257 --- /dev/null +++ b/doc/controllers/manipulator/ImpedanceControl.md @@ -0,0 +1,110 @@ +# Impedance Control + +## Overview & Motivation + +Position control makes a robot as stiff as possible: any deviation from the commanded trajectory is fought with full actuator effort. That is the wrong behaviour as soon as the tool touches something. A small position error against a rigid surface produces huge forces, and a person bumping into the arm meets a wall. + +**Impedance control** (Hogan, 1985) regulates the **relationship** between motion and force instead of either alone. The tool behaves like a programmable mass–spring–damper attached to the desired pose: it follows the trajectory in free space and yields with a chosen stiffness on contact. +This is the basis of polishing, assembly, hand guiding and safe human–robot collaboration. + +Two variants are provided: + +- **Stiffness/damping impedance** needs only the Jacobian transpose and gravity compensation: no force sensor, no mass matrix, and robust at singularities. +- **Impedance with inertia shaping** also renders a chosen task-space inertia. It needs the mass matrix, the Jacobian derivative term and a measured contact wrench. + +## Mathematical Theory + +### Task Error + +For a desired pose $T_d$ and the current tool pose $T(q)$, the task error $e = T_d \ominus T$ is + +- the pose error $(p_d - p;\ \log(R_d R^T)^\vee)$ for six-dimensional tasks, never a difference of orientation parameters; +- the first $m$ components of $p_d - p$ for position tasks ($m \le 3$). + +With the task Jacobian $J$, the error rate is $\dot{e} = \dot{x}_d - J\dot{q}$. The contact wrench $f_{\text{ext}}$ is the wrench the environment exerts on the tool, so the arm obeys + +$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + J^T f_{\text{ext}}$$ + +### Stiffness/Damping Impedance + +$$\tau = J^T\left(K e + D \dot{e}\right) + g(q)$$ + +At rest with $J$ of full rank, $K e = -f_{\text{ext}}$, so the tool deflects by $x - x_d = K^{-1} f_{\text{ext}}$: it yields **along** the external force with the programmed compliance. The inertia felt at the tool is the arm's own task-space inertia $\Lambda(q)$. + +The Coriolis term is deliberately **not** cancelled. With the skew-symmetry of $\dot{M} - 2C$, the storage function $V = \tfrac12\dot{q}^T M\dot{q} + \tfrac12 e^T K e$ gives, for a constant position target, + +$$\dot{V} = -\dot{x}^T D\,\dot{x} + \dot{x}^T f_{\text{ext}}$$ + +The controlled arm is **passive** at the port $(\dot{x}, f_{\text{ext}})$, so it remains stable in contact with any passive environment. + +### Impedance with Inertia Shaping + +To render the target impedance + +$$M_d\,\ddot{\tilde{x}} + D\,\dot{\tilde{x}} + K\tilde{x} = f_{\text{ext}}, \qquad \tilde{x} = x - x_d = -e$$ + +the controller commands the task acceleration + +$$a_x = \ddot{x}_d + M_d^{-1}\left(D\dot{e} + K e + f_{\text{ext}}\right)$$ + +and realizes it through the task-space inertia $\Lambda = (J M^{-1} J^T)^{-1}$: + +$$\tau = J^T\left[\Lambda\left(a_x - \dot{J}\dot{q}\right) - f_{\text{ext}}\right] + C\dot{q} + g$$ + +$\Lambda$ is applied through linear solves: first $X = M^{-1}J^T$, then $(JX)\,F' = a_x - \dot{J}\dot{q}$. Neither $M^{-1}$ nor $\Lambda$ is formed explicitly. Substituting into the dynamics gives $\ddot{x} = a_x$ exactly, hence the target impedance for position tasks. For pose tasks the rotational part holds to first order about zero orientation error. + +With $M_d = \Lambda(q)$ the contact-wrench terms cancel, and the law reduces to the stiffness/damping law plus feed-forward $\Lambda(\ddot{x}_d - \dot{J}\dot{q}) + C\dot{q}$. + +## Complexity Analysis + +| Variant | Time | Space | Notes | +|-------------------|--------------------|---------------|------------------------------------------------------------------------| +| Stiffness/damping | $O(mn)$ + gravity | $O(mn)$ | $J\dot{q}$, $J^T F$ and one gravity pass | +| Inertia shaping | $O(m n^3)$ + model | $O(mn + m^2)$ | Mass matrix, $m$ column solves for $M^{-1}J^T$, one $m \times m$ solve | +| Construction | $O(m^3)$ | $O(m^2)$ | $M_d^{-1}$ computed once | + +## Step-by-Step Walkthrough + +A planar task ($m = 2$) with $K = \operatorname{diag}(500, 300)$ N/m, $D = \operatorname{diag}(40, 30)$ N·s/m and $M_d = \operatorname{diag}(2, 1.5)$ kg drives a Cartesian two-axis plant with mass $\operatorname{diag}(1, 2)$ kg and gravity load $(0, 19.62)$ N. + +1. **Free-space law.** With $J = I$, $e = (0.05, -0.05)$ m, $\dot{e} = (-0.1, 0.15)$ m/s and gravity torque $g$, the stiffness/damping torque is $K e + D\dot{e} + g = (25 - 4, -15 + 4.5) + g$. +2. **Jacobian transpose.** With $J = \begin{bmatrix} 1 & 0.5 \\ 0 & 2 \end{bmatrix}$, the task wrench $K e = (25, -15)$ maps to joint torques $J^T K e = (25, -17.5)$. +3. **Static contact.** A constant wrench $f_{\text{ext}} = (3, -4)$ N pushes the tool, which settles at $x - x_d = K^{-1} f_{\text{ext}} = (0.006, -0.0133)$ m with zero velocity. +4. **Rendered inertia.** With inertia shaping, the plant's deflection after a step contact follows the target system $M_d\ddot{\tilde{x}} + D\dot{\tilde{x}} + K\tilde{x} = f_{\text{ext}}$ to within $10^{-5}$ m over the whole transient, although the plant's own mass differs from $M_d$. + +## Pitfalls & Edge Cases + +- **Orientation error**: subtracting Euler angles or quaternions produces wrong and discontinuous restoring torques. Use the log-map pose error. +- **Cancelling Coriolis in the compliant law**: adding $C\dot{q}$ to the stiffness/damping law looks more exact but destroys passivity. +- **Force-sensor bias**: inertia shaping with $M_d \ne \Lambda$ feeds the measured wrench forward, so a sensor bias shifts the rendered equilibrium. Low-pass the measurement and match $D$ to the sensor bandwidth. +- **Singularities**: inertia shaping needs $\Lambda$, which blows up at singular configurations. Damp $J M^{-1} J^T + \sigma^2 I$ there; the stiffness/damping law uses only $J^T$ and stays bounded. +- **Very high stiffness**: approaches rigid position control and excites contact instability at a given sample rate. +- **Redundant arms**: both laws leave the null space uncontrolled. Add a null-space posture term. + +## Variants & Generalizations + +- **Admittance control**: the dual causality. Measure the wrench, integrate the target dynamics to a motion command and track it with a stiff position loop. Better on non-backdrivable robots. +- **Variable impedance**: time-varying $K$ and $D$, with passivity preserved through energy tanks. +- **Operational-space control**: full task-space dynamics decoupling with a dynamically consistent null space. +- **Hybrid position/force control**: force regulation along constrained directions and motion along free ones. + +## Applications + +- Peg-in-hole assembly with stiff normal and compliant tangential axes. +- Polishing, grinding and deburring with regulated contact. +- Hand guiding and kinesthetic teaching. +- Safe physical human–robot interaction. + +## Connections to Other Algorithms + +- [PD Control with Gravity Compensation](PdGravityCompensation.md): the joint-space special case of the stiffness/damping law. +- [Computed-Torque Control](ComputedTorqueControl.md): joint-space feedback linearization, the counterpart of inertia shaping. +- [Geometric Jacobian](../../kinematics/GeometricJacobian.md): supplies $J$, $\dot{J}\dot{q}$ and the tool pose. +- [Chain Dynamics Model](../../dynamics/ChainDynamicsModel.md): supplies $M$, $C\dot{q}$ and $g$. + +## References & Further Reading + +- Hogan, N. (1985). "Impedance control: an approach to manipulation, Parts I–III." *ASME Journal of Dynamic Systems, Measurement, and Control*, 107(1), 1–24. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 9. +- Ott, C. (2008). *Cartesian Impedance Control of Redundant and Flexible-Joint Robots*. Springer. +- Colgate, J.E. and Hogan, N. (1988). "Robust control of dynamically interacting systems." *International Journal of Control*, 48(1), 65–88. diff --git a/doc/controllers/manipulator/README.md b/doc/controllers/manipulator/README.md index df7a04e..74fe025 100644 --- a/doc/controllers/manipulator/README.md +++ b/doc/controllers/manipulator/README.md @@ -4,7 +4,8 @@ Joint- and task-space controllers for robot manipulators. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------| -| [PD Control with Gravity Compensation](PdGravityCompensation.md) | Globally stable set-point regulator $\tau = K_p e - K_d \dot{q} + g(q)$ — needs only the gravity torque | -| [Computed-Torque Control](ComputedTorqueControl.md) | Feedback linearization: one inverse-dynamics pass at the commanded acceleration gives decoupled error dynamics $\ddot{e} + K_d\dot{e} + K_p e = 0$ | +| Algorithm | Description | +|------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [PD Control with Gravity Compensation](PdGravityCompensation.md) | Globally stable set-point regulator $\tau = K_p e - K_d \dot{q} + g(q)$ — needs only the gravity torque | +| [Computed-Torque Control](ComputedTorqueControl.md) | Feedback linearization: one inverse-dynamics pass at the commanded acceleration gives decoupled error dynamics $\ddot{e} + K_d\dot{e} + K_p e = 0$ | +| [Impedance Control](ImpedanceControl.md) | Task-space mass–spring–damper behaviour: passive $J^T(Ke + D\dot{e}) + g$ without force sensing, or full inertia shaping through $\Lambda(q)$ and a measured wrench | diff --git a/roadmap/README.md b/roadmap/README.md index fd9d835..dcb6cd0 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -23,7 +23,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[controllers/manipulator/ImpedanceControl](controllers/manipulator/ImpedanceControl/implementation.md). +[controllers/manipulator/OperationalSpaceControl](controllers/manipulator/OperationalSpaceControl/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md) (NT53), and planar @@ -104,4 +104,4 @@ prerequisites. `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) ### `controllers/manipulator` -`ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) +`OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md index 75fb547..ee215a4 100644 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md @@ -97,6 +97,6 @@ function ComputeTorque(q, qDot, desiredPose, xdDot, fMeasured, fd): # OPTIMIZE - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError.hpp` - (specified in M17, deployed with whichever of M17/M18/M19 lands first). +- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError` from + `robotics/controllers/manipulator/TaskError.hpp` (shipped with M17). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/ImpedanceControl/explanation.md b/roadmap/controllers/manipulator/ImpedanceControl/explanation.md deleted file mode 100644 index 23ab8cd..0000000 --- a/roadmap/controllers/manipulator/ImpedanceControl/explanation.md +++ /dev/null @@ -1,41 +0,0 @@ -# Impedance Control — Overview - -## What it is -Instead of commanding a position, impedance control makes the end-effector *behave* like a chosen -spring–damper (and, optionally, mass). You program the stiffness, damping, and — with a force sensor — -the inertia the robot presents to the world: a tunable "softness" rather than a rigid trajectory. - -## Why it matters (embedded) -Rigid position control shatters on contact: the tiniest position error against a hard surface -generates enormous force. Impedance control lets a robot push, insert, wipe, and physically interact -with people *safely*, with predictable and adjustable compliance. It is the foundation of -collaborative robots, assembly, and teleoperation. - -## How it works (intuition) -Measure the pose error between where the tip is and where it should be (orientation as a rotation -vector, never a difference of angles). The simple law turns that error and the velocity error into -the force of a virtual spring–damper, maps it to joint torques with the Jacobian *transpose*, and -adds the gravity torque. Pushed by the environment, the tip settles where the spring balances the -push — it yields along the force by exactly the programmed compliance. It does **not** change the -inertia you feel: that is still the arm's own task-space inertia `Λ(q)`, which varies with posture. -Because it uses `Jᵀ` (never an inverse) and leaves the arm's natural energy exchange intact, it stays -passive and safe near singularities. To make the tip also *feel* like a chosen mass, the second law -measures the contact wrench, computes the tip acceleration the target mass–spring–damper would have, -and realises it through `Λ(q)` with full gravity/Coriolis compensation — the operational-space -machinery. If the chosen mass equals `Λ(q)`, the sensor drops out and it collapses back to the -simple law plus feed-forward. - -## Key parameters -- **K (stiffness), D (damping)** — the target spring–damper per task axis. -- **Md (target inertia)** — used only by the inertia-shaping law; needs a measured contact wrench. -- **model, jacobian** — injected dynamics (`g`; plus `M`, `Cq̇` for shaping) and task Jacobian - provider (`J`, `J̇q̇`, tool pose). -- **fExternal** — wrench exerted by the environment on the tool, from a wrist force/torque sensor. - -## Reference -N. Hogan, "Impedance Control: An Approach to Manipulation, Parts I–III," -*ASME J. Dynamic Systems, Measurement, and Control*, 1985. - -## See also -`OperationalSpaceControl` (the `Λ` machinery behind inertia shaping); `HybridPositionForceControl` -(partition force/motion axes); `ComputedTorqueControl` (rigid tracking counterpart). diff --git a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md deleted file mode 100644 index cf74d35..0000000 --- a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md +++ /dev/null @@ -1,122 +0,0 @@ -# Impedance Control — Implementation Pseudocode - -> Roadmap ref: #M17 (Tier 3) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Sign convention: `fExternal` is the wrench exerted **by the environment on the tool**, base frame, -`(f; n)` about the tool point (NT53 ordering), so the arm obeys `M q̈ + C q̇ + g = τ + Jᵀ·fExternal`. -Task error `e = xd ⊖ x` (`TaskError`, below); `x̃ = x − xd = −e`. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class ImpedanceControl: # TaskDim = 6 (pose) or ≤ 3 (position; 2 = planar xy) - const dynamics::EulerLagrangeDynamics& model # M, C q̇, g — typically ChainDynamicsModel (M29) - const kinematics::JacobianProvider& jacobian # J, J̇q̇, tool pose — ChainTaskJacobian (M8) - math::SquareMatrix K # stiffness (SPD) - math::SquareMatrix D # damping (SPD) — the target Dd in (b) - math::SquareMatrix MdInverse # target inertia Md⁻¹, precomputed once: SolveSystem(Md, I) - -# shared by M17/M18/M19 — robotics/controllers/manipulator/TaskError.hpp -template -math::Vector TaskError(const math::SE3Transform& desired, - const math::SE3Transform& current) -``` - -## Interface - -```text -ImpedanceControl(const dynamics::EulerLagrangeDynamics& model, - const kinematics::JacobianProvider& jacobian, - const SquareMatrix& K, const SquareMatrix& D, const SquareMatrix& Md) - -# (a) stiffness/damping impedance — no force sensor, no mass matrix: -JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, - const math::SE3Transform& desiredPose, - const TaskVector& xdDot) # hot path - -# (b) full impedance with inertia shaping — needs M, J̇q̇ and the measured fExternal: -JointVector ComputeTorqueWithInertiaShaping(const JointVector& q, const JointVector& qDot, - const math::SE3Transform& desiredPose, - const TaskVector& xdDot, const TaskVector& xdDdot, - const TaskVector& fExternal) # hot path -``` - -## Algorithm (pseudocode) - -```text -function TaskError(desired, current): # never subtracts orientations - if TaskDim == 6: return math::SE3Transform::PoseError(desired, current) # (Δp; rotation vector) - return first TaskDim entries of (desired.p − current.p) - -function ComputeTorque(q, qDot, desiredPose, xdDot): # (a) OPTIMIZE_FOR_SPEED - J = jacobian.Jacobian(q) - e = TaskError(desiredPose, jacobian.ToolPose(q)) - eDot = xdDot − J·qDot - return Jᵀ·(K·e + D·eDot) + model.ComputeGravityTerms(q) # C q̇ deliberately NOT added - -function ComputeTorqueWithInertiaShaping(q, qDot, desiredPose, xdDot, xdDdot, fExt): # (b) OPTIMIZE_FOR_SPEED - J = jacobian.Jacobian(q) - e = TaskError(desiredPose, jacobian.ToolPose(q)) - eDot = xdDot − J·qDot - # commanded task acceleration realising Md·x̃̈ + D·x̃̇ + K·x̃ = fExt - aX = xdDdot + MdInverse·(D·eDot + K·e + fExt) - # Λ = (J M⁻¹ Jᵀ)⁻¹ applied through solves — M⁻¹ and Λ never formed explicitly - X = solvers::SolveSystem(model.ComputeMassMatrix(q), Jᵀ) # M⁻¹Jᵀ (Dof×TaskDim) - F = solvers::SolveSystem(J·X, aX − jacobian.BiasAcceleration(q, qDot)) − fExt # Λ(aX − J̇q̇) − fExt - return Jᵀ·F + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) -``` - -## Complexity & memory - -- (a): `O(TaskDim·Dof)` for `J·q̇` and `Jᵀ·F`, plus one `O(Dof)` gravity pass. -- (b): adds `M` (`O(Dof²)`, CRBA), the `M⁻¹Jᵀ` solve (`O(TaskDim·Dof³)` with `SolveSystem`, which eliminates - per column; factor `M` once with `math::CholeskyDecomposition` for `O(Dof³ + TaskDim·Dof²)`) and a - `TaskDim×TaskDim` solve. -- Memory: `O(TaskDim²)` gains, `O(Dof·TaskDim)` workspace; no dynamic state, no heap. - -## Numerical / embedded notes - -- **(a) contact equilibrium.** Closed loop `M q̈ + C q̇ = Jᵀ(K·e + D·ė + fExt)`. At rest with `J` full rank: - `K·e = −fExt` ⇒ `x − xd = K⁻¹·fExt` — the tool yields *along* the external force with the programmed - compliance. The inertia felt at the tool is the arm's own `Λ(q)`; `Md` is unused by (a). -- **(a) passivity.** `C q̇` is not added: with `q̇ᵀ(Ṁ − 2C)q̇ = 0`, `V = ½q̇ᵀMq̇ + ½eᵀKe` gives - `V̇ = −ẋᵀD ẋ + ẋᵀfExt` for constant `xd` (position tasks) ⇒ passive port `(ẋ, fExt)`, stable against - any passive environment. Cancelling `C q̇` would leave the indefinite term `½q̇ᵀṀq̇`. -- **(b) rendered impedance.** `ẍ = J q̈ + J̇q̇ = Λ⁻¹(F + fExt) + J̇q̇ = aX` ⇒ `Md·x̃̈ + D·x̃̇ + K·x̃ = fExt` - exactly for position tasks. For `TaskDim = 6`, `ωd − ω` is the rate of the rotation-vector error only - to first order, so the rotational impedance is exact to first order about `eR = 0`. -- **(b) equivalent classic form.** Equal to `F = Λ(aX − J̇q̇) + J̄ᵀC q̇ + J̄ᵀg − fExt`, `τ = JᵀF` - (`J̄ = M⁻¹JᵀΛ`) plus `N·(C q̇ + g)`, `N = I − JᵀJ̄ᵀ`: that extra term is zero for a non-redundant arm and - compensates null-space gravity/Coriolis on a redundant one (as in `OperationalSpaceControl`). -- **(b) with `Md = Λ(q)`** the `fExt` terms cancel: `τ = Jᵀ(K·e + D·ė) + g + [JᵀΛ(ẍd − J̇q̇) + C q̇]` — - law (a) plus feed-forward, no force sensor needed. Shaping `Md ≠ Λ` requires a measured `fExt` - (low-pass it and match `D` to the sensor bandwidth); sensor bias shifts the rendered equilibrium. -- (b) needs `Λ`, so it inherits the singularity caveat of `OperationalSpaceControl` (damp - `J M⁻¹ Jᵀ + σ²I`); (a) uses only `Jᵀ` and passes through singularities safely. -- **Admittance** is the dual: measure `fExt`, integrate to a motion command, feed a position loop — - better on stiff/non-backdrivable robots; impedance is better on backdrivable ones. -- Diagonal `K`/`D` are chosen per task axis (e.g. stiff normal, compliant tangential for insertion). -- The injected interfaces are a virtual seam for StrictMock tests; hard real-time builds may template - the controller on the concrete `ChainDynamicsModel` / `ChainTaskJacobian` to avoid virtual calls - (AGENTS.md: no virtual calls in real-time paths). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/controllers/manipulator/ImpedanceControl.hpp` (+ `TaskError.hpp`, shared with M18/M19; - deploy it with whichever lands first) — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on both torque methods, and `extern template class ImpedanceControl;` / - `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/controllers/manipulator/ImpedanceControl.cpp` → - `template class ImpedanceControl;` / `` -- Test: `robotics/controllers/manipulator/test/TestImpedanceControl.cpp` -- Doc: `doc/controllers/manipulator/ImpedanceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestImpedanceControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a - `doc/controllers/manipulator/` folder. -- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/ImpedanceControl/tests.md b/roadmap/controllers/manipulator/ImpedanceControl/tests.md deleted file mode 100644 index 47fa5fc..0000000 --- a/roadmap/controllers/manipulator/ImpedanceControl/tests.md +++ /dev/null @@ -1,82 +0,0 @@ -# Impedance Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestImpedanceControl : public ::testing::Test: - # both injected dependencies mocked; Dof = 2, TaskDim = 2 (planar xy point): - StrictMock> model - StrictMock> jacobian - SquareMatrix K = diag(500, 300) - SquareMatrix D = diag( 40, 30) - SquareMatrix Md = diag( 2, 1.5) - ImpedanceControl controller{ model, jacobian, K, D, Md } - # ToolPose mocked as { I, (x, y, 0) }; desiredPose = { I, (xd, yd, 0) } - -class TestImpedanceControlPose : public ::testing::Test: # TaskDim = 6 branch of TaskError - StrictMock> model - StrictMock> jacobian - ImpedanceControl controller{ model, jacobian, K6 = diag(500,500,500,20,20,20), D6, Md6 = I } -# each case below is a TEST_F(, ); default fixture TestImpedanceControl -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -# ---- (a) ComputeTorque: stiffness/damping, no force sensing ---- -compliant_law_with_identity_jacobian: - Arrange: J = I; e = xd − x != 0; qDot, xdDot != 0; g != 0 - Act: τ = ComputeTorque(q, qDot, desiredPose, xdDot) - Assert: τ == K·e + D·(xdDot − qDot) + g - -jacobian_transpose_maps_wrench: - Arrange: J = [[1,0],[0,2]], e != 0, qDot = 0, xdDot = 0, g = 0 - Assert: τ == Jᵀ·(K·e) - -compliant_law_queries_only_gravity: - Arrange: any state - Assert: Jacobian, ToolPose, ComputeGravityTerms each once; ComputeMassMatrix, - ComputeCoriolisTerms, BiasAcceleration never called (StrictMock) - -static_deflection_equals_compliance: - Arrange: simulated 2-DOF Cartesian plant M q̈ + g = τ + fExt with M = diag(1, 2), g = (0, 19.62), - J = I and x = q via Invoke; constant fExt = (3, −4); xd constant; semi-implicit Euler, - dt = 1 ms, 5 s from x = xd - Assert: x − xd → K⁻¹·fExt = (0.006, −0.013333), q̇ → 0 (tool yields along fExt) - -pose_error_uses_rotation_vector (TestImpedanceControlPose): - Arrange: J = I; ToolPose = { Rz(0.2), 0 }, desiredPose = Identity; qDot = 0, xdDot = 0, g = 0 - Assert: τ == K6·(0, 0, 0, 0, 0, −0.2) (PoseError, never a difference of orientations) - -# ---- (b) ComputeTorqueWithInertiaShaping ---- -inertia_shaping_with_unit_model: - Arrange: M = I, J = I, J̇q̇ = 0, C q̇ = 0, g = 0; e, eDot, xdDdot, fExt != 0 - Assert: τ == aX − fExt, aX = xdDdot + Md⁻¹·(D·eDot + K·e + fExt) - -inertia_shaping_uses_task_inertia_bias_and_model: - Arrange: M = diag(2, 3), J = I (Λ = diag(2, 3)), J̇q̇ = b, C q̇ = c, g != 0 - Assert: τ == Λ·(aX − b) − fExt + c + g; each model / provider method called once - -matched_inertia_reduces_to_compliant_law: - Arrange: local controller with Md = Λ = diag(2, 3) (M = diag(2, 3), J = I); J̇q̇ = b, C q̇ = c; fExt != 0 - Assert: ComputeTorqueWithInertiaShaping == ComputeTorque + Λ·(xdDdot − b) + c, independent of fExt - -rendered_impedance_matches_target: - Arrange: plant of static_deflection (M = diag(1, 2) ≠ Md, C = 0, J̇q̇ = 0), step fExt, xd constant; - integrate the plant and the target Md·x̃̈ + D·x̃̇ + K·x̃ = fExt side by side (same integrator) - Assert: x − xd tracks x̃(t) within integration tolerance for the whole run -``` - -## Reference vectors - -- (a) equilibrium: `K = diag(500, 300)`, `fExt = (3, −4)` ⇒ `x − xd = (0.006, −0.013333)`. -- (b) `M = I`, `J = I`: `Λ = I`, `τ = aX − fExt + C q̇ + g`. -- `Rz(0.2)` current vs identity desired ⇒ `e = (0, 0, 0, 0, 0, −0.2)`. - -## Edge cases - -- Near-singular `J`: (a) torque stays finite (`Jᵀ` only); (b) needs the damped `Λ` (see implementation). -- Very high `K`: approaches rigid position control; watch contact instability vs sample rate. -- Sensor bias `β` in (b): static `x̃ = K⁻¹·(fExt + (I − Md·Λ⁻¹)·β)` — no effect only when `Md = Λ`. diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md index d4f5848..e68da43 100644 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md @@ -93,7 +93,7 @@ function ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, tauSecondary): # O - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError.hpp` - (specified in M17, deployed with whichever of M17/M18/M19 lands first); `solvers::SolveSystem` from +- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError` from + `robotics/controllers/manipulator/TaskError.hpp` (shipped with M17); `solvers::SolveSystem` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/robotics/controllers/manipulator/CMakeLists.txt b/robotics/controllers/manipulator/CMakeLists.txt index c7d3dfc..7c79af2 100644 --- a/robotics/controllers/manipulator/CMakeLists.txt +++ b/robotics/controllers/manipulator/CMakeLists.txt @@ -7,16 +7,21 @@ target_include_directories(robotics.controllers.manipulator ${ROBOTICS_VISIBILIT target_link_libraries(robotics.controllers.manipulator ${ROBOTICS_VISIBILITY} numerical.math + numerical.solver robotics.dynamics + robotics.kinematics ) target_sources(robotics.controllers.manipulator PRIVATE ComputedTorqueControl.hpp + ImpedanceControl.hpp PdGravityCompensation.hpp + TaskError.hpp ) robotics_add_coverage_sources(robotics.controllers.manipulator ComputedTorqueControl.cpp + ImpedanceControl.cpp PdGravityCompensation.cpp ) diff --git a/robotics/controllers/manipulator/ImpedanceControl.cpp b/robotics/controllers/manipulator/ImpedanceControl.cpp new file mode 100644 index 0000000..c1c1cf6 --- /dev/null +++ b/robotics/controllers/manipulator/ImpedanceControl.cpp @@ -0,0 +1,7 @@ +#include "robotics/controllers/manipulator/ImpedanceControl.hpp" + +namespace controllers +{ + template class ImpedanceControl; + template class ImpedanceControl; +} diff --git a/robotics/controllers/manipulator/ImpedanceControl.hpp b/robotics/controllers/manipulator/ImpedanceControl.hpp new file mode 100644 index 0000000..05fe204 --- /dev/null +++ b/robotics/controllers/manipulator/ImpedanceControl.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "numerical/solvers/GaussianElimination.hpp" +#include "robotics/controllers/manipulator/TaskError.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include + +namespace controllers +{ + template + class ImpedanceControl + { + static_assert(std::is_floating_point_v, "ImpedanceControl supports floating-point types"); + + public: + using JointVector = math::Vector; + using TaskVector = math::Vector; + using TaskMatrix = math::SquareMatrix; + using Model = dynamics::EulerLagrangeDynamics; + using Provider = kinematics::JacobianProvider; + + ImpedanceControl(const Model& model, const Provider& jacobian, const TaskMatrix& stiffness, const TaskMatrix& damping, const TaskMatrix& targetInertia); + + OPTIMIZE_FOR_SPEED JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, const TaskVector& xdDot) const; + + OPTIMIZE_FOR_SPEED JointVector ComputeTorqueWithInertiaShaping(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, + const TaskVector& xdDot, const TaskVector& xdDdot, const TaskVector& fExternal) const; + + private: + const Model& model; + const Provider& jacobian; + TaskMatrix stiffness; + TaskMatrix damping; + TaskMatrix targetInertiaInverse; + }; + + template + ImpedanceControl::ImpedanceControl(const Model& model, const Provider& jacobian, const TaskMatrix& stiffness, const TaskMatrix& damping, const TaskMatrix& targetInertia) + : model{ model } + , jacobian{ jacobian } + , stiffness{ stiffness } + , damping{ damping } + , targetInertiaInverse{ solvers::SolveSystem(targetInertia, TaskMatrix::Identity()) } + {} + + template + OPTIMIZE_FOR_SPEED auto ImpedanceControl::ComputeTorque(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, const TaskVector& xdDot) const -> JointVector + { + const auto taskJacobian{ jacobian.Jacobian(q) }; + const TaskVector error{ TaskError(desiredPose, jacobian.ToolPose(q)) }; + const TaskVector errorRate{ xdDot - taskJacobian * qDot }; + + return taskJacobian.Transpose() * TaskVector{ stiffness * error + damping * errorRate } + model.ComputeGravityTerms(q); + } + + template + OPTIMIZE_FOR_SPEED auto ImpedanceControl::ComputeTorqueWithInertiaShaping(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, + const TaskVector& xdDot, const TaskVector& xdDdot, const TaskVector& fExternal) const -> JointVector + { + const auto taskJacobian{ jacobian.Jacobian(q) }; + const auto transposed{ taskJacobian.Transpose() }; + const TaskVector error{ TaskError(desiredPose, jacobian.ToolPose(q)) }; + const TaskVector errorRate{ xdDot - taskJacobian * qDot }; + const TaskVector acceleration{ xdDdot + targetInertiaInverse * TaskVector{ damping * errorRate + stiffness * error + fExternal } }; + + const auto inverseMassTimesTransposed{ solvers::SolveSystem(model.ComputeMassMatrix(q), transposed) }; + const TaskVector force{ solvers::SolveSystem(TaskMatrix{ taskJacobian * inverseMassTimesTransposed }, TaskVector{ acceleration - jacobian.BiasAcceleration(q, qDot) }) - fExternal }; + + return transposed * force + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ImpedanceControl; + extern template class ImpedanceControl; +#endif +} diff --git a/robotics/controllers/manipulator/TaskError.hpp b/robotics/controllers/manipulator/TaskError.hpp new file mode 100644 index 0000000..aebc2d5 --- /dev/null +++ b/robotics/controllers/manipulator/TaskError.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include +#include + +namespace controllers +{ + template + math::Vector TaskError(const math::SE3Transform& desired, const math::SE3Transform& current) + { + static_assert(std::is_floating_point_v, "TaskError supports floating-point types"); + static_assert(TaskDim == 6 || (TaskDim > 0 && TaskDim <= 3), "TaskError supports pose (6) or position (1 to 3) tasks"); + + if constexpr (TaskDim == 6) + return math::SE3Transform::PoseError(desired, current); + else + { + math::Vector error{}; + for (std::size_t i = 0; i < TaskDim; ++i) + error[i] = desired.translation[i] - current.translation[i]; + return error; + } + } +} diff --git a/robotics/controllers/manipulator/test/CMakeLists.txt b/robotics/controllers/manipulator/test/CMakeLists.txt index 3fe6846..c572db3 100644 --- a/robotics/controllers/manipulator/test/CMakeLists.txt +++ b/robotics/controllers/manipulator/test/CMakeLists.txt @@ -6,9 +6,11 @@ target_link_libraries(robotics.controllers.manipulator_test PUBLIC gmock_main robotics.controllers.manipulator robotics.dynamics_test_doubles + robotics.kinematics_test_doubles ) target_sources(robotics.controllers.manipulator_test PRIVATE TestComputedTorqueControl.cpp + TestImpedanceControl.cpp TestPdGravityCompensation.cpp ) diff --git a/robotics/controllers/manipulator/test/TestImpedanceControl.cpp b/robotics/controllers/manipulator/test/TestImpedanceControl.cpp new file mode 100644 index 0000000..9132e87 --- /dev/null +++ b/robotics/controllers/manipulator/test/TestImpedanceControl.cpp @@ -0,0 +1,238 @@ +#include "numerical/math/SE3Transform.hpp" +#include "robotics/controllers/manipulator/ImpedanceControl.hpp" +#include "robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp" +#include "robotics/kinematics/test_doubles/MockJacobianProvider.hpp" +#include +#include + +namespace +{ + using ::testing::_; + using ::testing::Invoke; + using ::testing::Return; + using Vector2 = math::Vector; + using Vector3 = math::Vector3; + using Matrix2 = math::SquareMatrix; + using Vector6 = math::Vector; + using Matrix6 = math::SquareMatrix; + using Transform = math::SE3Transform; + using Controller = controllers::ImpedanceControl; + + constexpr float dt{ 1e-3f }; + + Transform At(float x, float y) + { + return Transform{ math::Matrix3::Identity(), Vector3{ x, y, 0.0f } }; + } + + Matrix2 Diagonal(float first, float second) + { + return Matrix2{ { first, 0.0f }, { 0.0f, second } }; + } + + Matrix6 Diagonal6(float linear, float angular) + { + Matrix6 diagonal{}; + for (std::size_t i = 0; i < 6; ++i) + diagonal.at(i, i) = i < 3 ? linear : angular; + return diagonal; + } + + class TestImpedanceControl + : public ::testing::Test + { + protected: + void ExpectNear(const Vector2& actual, const Vector2& expected, float tolerance = 1e-4f) const + { + for (std::size_t i = 0; i < 2; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + void ExpectArm(const Matrix2& jacobianMatrix) + { + EXPECT_CALL(jacobian, Jacobian(_)).WillOnce(Return(jacobianMatrix)); + EXPECT_CALL(jacobian, ToolPose(_)).WillOnce(Return(current)); + } + + void ExpectModel(const Matrix2& mass) + { + EXPECT_CALL(model, ComputeMassMatrix(_)).WillOnce(Return(mass)); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).WillOnce(Return(coriolis)); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(gravity)); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillOnce(Return(bias)); + } + + void ExpectSimulatedPlant() + { + EXPECT_CALL(jacobian, Jacobian(_)).WillRepeatedly(Return(Matrix2::Identity())); + EXPECT_CALL(jacobian, ToolPose(_)).WillRepeatedly(Invoke([](const Vector2& position) + { + return At(position[0], position[1]); + })); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillRepeatedly(Return(plantGravity)); + } + + void StepPlant(const Vector2& tau, const Vector2& fExternal) + { + const Vector2 net{ tau + fExternal - plantGravity }; + qDot = qDot + Vector2{ net[0] / plantMass.at(0, 0), net[1] / plantMass.at(1, 1) } * dt; + q = q + qDot * dt; + } + + ::testing::StrictMock> model; + ::testing::StrictMock> jacobian; + Matrix2 stiffness{ Diagonal(500.0f, 300.0f) }; + Matrix2 damping{ Diagonal(40.0f, 30.0f) }; + Matrix2 targetInertia{ Diagonal(2.0f, 1.5f) }; + Controller controller{ model, jacobian, stiffness, damping, targetInertia }; + Transform current{ At(0.3f, 0.2f) }; + Transform desired{ At(0.35f, 0.15f) }; + Vector2 error{ 0.05f, -0.05f }; + Vector2 q{ 0.3f, 0.2f }; + Vector2 qDot{ 0.2f, -0.1f }; + Vector2 xdDot{ 0.1f, 0.05f }; + Vector2 xdDdot{ 0.5f, -0.4f }; + Vector2 fExternal{ 3.0f, -4.0f }; + Vector2 gravity{ 1.5f, -2.0f }; + Vector2 coriolis{ 0.4f, -0.3f }; + Vector2 bias{ 0.2f, 0.1f }; + Matrix2 plantMass{ Diagonal(1.0f, 2.0f) }; + Vector2 plantGravity{ 0.0f, 19.62f }; + }; + + class TestImpedanceControlPose + : public ::testing::Test + { + protected: + ::testing::StrictMock> model; + ::testing::StrictMock> jacobian; + Matrix6 stiffness{ Diagonal6(500.0f, 20.0f) }; + controllers::ImpedanceControl controller{ model, jacobian, stiffness, Diagonal6(40.0f, 2.0f), Matrix6::Identity() }; + }; +} + +TEST_F(TestImpedanceControl, compliant_law_with_identity_jacobian) +{ + ExpectArm(Matrix2::Identity()); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(gravity)); + + const auto tau{ controller.ComputeTorque(q, qDot, desired, xdDot) }; + + ExpectNear(tau, Vector2{ stiffness * error + damping * Vector2{ xdDot - qDot } + gravity }); +} + +TEST_F(TestImpedanceControl, jacobian_transpose_maps_wrench) +{ + const Matrix2 skewed{ { 1.0f, 0.5f }, { 0.0f, 2.0f } }; + ExpectArm(skewed); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(Vector2{})); + + const auto tau{ controller.ComputeTorque(q, Vector2{}, desired, Vector2{}) }; + + ExpectNear(tau, Vector2{ 25.0f, -17.5f }); +} + +TEST_F(TestImpedanceControl, compliant_law_queries_only_gravity) +{ + ExpectArm(Matrix2::Identity()); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(gravity)); + + static_cast(controller.ComputeTorque(q, qDot, desired, xdDot)); +} + +TEST_F(TestImpedanceControl, static_deflection_equals_compliance) +{ + ExpectSimulatedPlant(); + const auto target{ At(q[0], q[1]) }; + qDot = Vector2{}; + + for (std::size_t k = 0; k < 5000; ++k) + StepPlant(controller.ComputeTorque(q, qDot, target, Vector2{}), fExternal); + + ExpectNear(Vector2{ q - target.translation.GetBlock<2, 1>(0, 0) }, Vector2{ 0.006f, -0.0133333f }, 1e-5f); + ExpectNear(qDot, Vector2{}, 1e-4f); +} + +TEST_F(TestImpedanceControlPose, pose_error_uses_rotation_vector) +{ + math::Vector6 rotation{}; + rotation[5] = 0.2f; + EXPECT_CALL(jacobian, Jacobian(_)).WillOnce(Return(Matrix6::Identity())); + EXPECT_CALL(jacobian, ToolPose(_)).WillOnce(Return(Transform{ Transform::Exp(rotation, 1.0f).rotation, Vector3{} })); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(Vector6{})); + + const auto tau{ controller.ComputeTorque(Vector6{}, Vector6{}, Transform::Identity(), Vector6{}) }; + + for (std::size_t i = 0; i < 5; ++i) + EXPECT_NEAR(tau[i], 0.0f, 1e-5f); + EXPECT_NEAR(tau[5], 20.0f * -0.2f, 1e-5f); +} + +TEST_F(TestImpedanceControl, inertia_shaping_with_unit_model) +{ + ExpectArm(Matrix2::Identity()); + EXPECT_CALL(model, ComputeMassMatrix(_)).WillOnce(Return(Matrix2::Identity())); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).WillOnce(Return(Vector2{})); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(Vector2{})); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillOnce(Return(Vector2{})); + + const auto tau{ controller.ComputeTorqueWithInertiaShaping(q, qDot, desired, xdDot, xdDdot, fExternal) }; + + const Vector2 shaped{ damping * Vector2{ xdDot - qDot } + stiffness * error + fExternal }; + const Vector2 acceleration{ xdDdot + Vector2{ shaped[0] / 2.0f, shaped[1] / 1.5f } }; + ExpectNear(tau, Vector2{ acceleration - fExternal }); +} + +TEST_F(TestImpedanceControl, inertia_shaping_uses_task_inertia_bias_and_model) +{ + const auto taskInertia{ Diagonal(2.0f, 3.0f) }; + ExpectArm(Matrix2::Identity()); + ExpectModel(taskInertia); + + const auto tau{ controller.ComputeTorqueWithInertiaShaping(q, qDot, desired, xdDot, xdDdot, fExternal) }; + + const Vector2 shaped{ damping * Vector2{ xdDot - qDot } + stiffness * error + fExternal }; + const Vector2 acceleration{ xdDdot + Vector2{ shaped[0] / 2.0f, shaped[1] / 1.5f } }; + ExpectNear(tau, Vector2{ taskInertia * Vector2{ acceleration - bias } - fExternal + coriolis + gravity }); +} + +TEST_F(TestImpedanceControl, matched_inertia_reduces_to_compliant_law) +{ + const auto taskInertia{ Diagonal(2.0f, 3.0f) }; + const Controller matched{ model, jacobian, stiffness, damping, taskInertia }; + EXPECT_CALL(jacobian, Jacobian(_)).WillRepeatedly(Return(Matrix2::Identity())); + EXPECT_CALL(jacobian, ToolPose(_)).WillRepeatedly(Return(current)); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillRepeatedly(Return(bias)); + EXPECT_CALL(model, ComputeMassMatrix(_)).WillRepeatedly(Return(taskInertia)); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).WillRepeatedly(Return(coriolis)); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillRepeatedly(Return(gravity)); + + const Vector2 expected{ matched.ComputeTorque(q, qDot, desired, xdDot) + taskInertia * Vector2{ xdDdot - bias } + coriolis }; + + ExpectNear(matched.ComputeTorqueWithInertiaShaping(q, qDot, desired, xdDot, xdDdot, fExternal), expected); + ExpectNear(matched.ComputeTorqueWithInertiaShaping(q, qDot, desired, xdDot, xdDdot, Vector2{ -7.0f, 2.5f }), expected); +} + +TEST_F(TestImpedanceControl, rendered_impedance_matches_target) +{ + ExpectSimulatedPlant(); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillRepeatedly(Return(Vector2{})); + EXPECT_CALL(model, ComputeMassMatrix(_)).WillRepeatedly(Return(plantMass)); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).WillRepeatedly(Return(Vector2{})); + const auto target{ At(q[0], q[1]) }; + const Vector2 origin{ q }; + qDot = Vector2{}; + Vector2 offset{}; + Vector2 offsetRate{}; + + for (std::size_t k = 0; k < 2000; ++k) + { + StepPlant(controller.ComputeTorqueWithInertiaShaping(q, qDot, target, Vector2{}, Vector2{}, fExternal), fExternal); + + const Vector2 targetForce{ fExternal - damping * offsetRate - stiffness * offset }; + offsetRate = offsetRate + Vector2{ targetForce[0] / 2.0f, targetForce[1] / 1.5f } * dt; + offset = offset + offsetRate * dt; + + ExpectNear(Vector2{ q - origin }, offset, 1e-5f); + } +} From 94c27ef1201af8f8c79e1642565e9103c8e50d06 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 13:08:02 +0000 Subject: [PATCH 8/9] feat(planning): add GridSearch (P3) A* and Dijkstra on bounded 2-D/3-D grids: per-cell costs from a callable, an indexed binary heap with decrease-key, octile/Manhattan admissible heuristics and corner-cutting control, all in fixed storage. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 4 - doc/planning/GridSearch.md | 94 ++++ doc/planning/README.md | 11 +- roadmap/README.md | 2 +- roadmap/planning/GridSearch/explanation.md | 33 -- roadmap/planning/GridSearch/implementation.md | 105 ----- roadmap/planning/GridSearch/tests.md | 51 --- robotics/planning/CMakeLists.txt | 2 + robotics/planning/GridSearch.cpp | 9 + robotics/planning/GridSearch.hpp | 405 ++++++++++++++++++ robotics/planning/test/CMakeLists.txt | 1 + robotics/planning/test/TestGridSearch.cpp | 176 ++++++++ 13 files changed, 695 insertions(+), 200 deletions(-) create mode 100644 doc/planning/GridSearch.md delete mode 100644 roadmap/planning/GridSearch/explanation.md delete mode 100644 roadmap/planning/GridSearch/implementation.md delete mode 100644 roadmap/planning/GridSearch/tests.md create mode 100644 robotics/planning/GridSearch.cpp create mode 100644 robotics/planning/GridSearch.hpp create mode 100644 robotics/planning/test/TestGridSearch.cpp diff --git a/README.md b/README.md index ad8eb1a..f916851 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. |--------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| | [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares), Pose Inverse Kinematics | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Grid Search (A* and Dijkstra) | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | | [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control | diff --git a/ROADMAP.md b/ROADMAP.md index 123953f..8b092f4 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -377,13 +377,9 @@ Cross-family building blocks that several of the families below depend on. | # | Component | Target module | Depends on | Difficulty | |----|---------------------------------------------------|---------------|------------|------------| -| P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | | P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | | P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | -**P3. Grid search.** A* and Dijkstra on 2-D/3-D grids with per-cell costs from a callable, an indexed binary heap with decrease-key, 16-bit indices on small grids and the octile heuristic (no heap allocation). -- *Algorithm / paper:* P. Hart, N. Nilsson, B. Raphael, *IEEE Trans. Systems Science and Cybernetics*, 4(2), 1968. - **P5. GJK + EPA.** Distance between convex shapes (sphere, capsule, box, bounded hull) from support functions, run on cores plus margins; penetration depth and normal by a bounded EPA, robust to tied supports. - *Algorithm / paper:* E. Gilbert, D. Johnson, S. Keerthi, *IEEE J. Robotics and Automation*, 4(2), 1988; G. van den Bergen, *Collision Detection in Interactive 3D Environments* (2003). diff --git a/doc/planning/GridSearch.md b/doc/planning/GridSearch.md new file mode 100644 index 0000000..c48efa3 --- /dev/null +++ b/doc/planning/GridSearch.md @@ -0,0 +1,94 @@ +# Grid Search (A* and Dijkstra) + +## Overview & Motivation + +Mobile robots, drones and manipulators often plan on a **grid**: an occupancy map from a laser scanner, a costmap with inflated obstacles, or a voxel map of the workspace. Shortest paths on such grids are the backbone of global planners. They feed local controllers with waypoints and give reference routes for sampling planners. + +**Dijkstra's algorithm** expands cells in order of their cost from the start and finds optimal paths. **A\*** adds an admissible heuristic, an optimistic estimate of the remaining cost, so it expands cells towards the goal first. It returns the same optimal cost while visiting far fewer cells. +This implementation keeps all its storage in fixed-size arrays sized by the grid. It uses an indexed binary heap with decrease-key, and reads cell costs through a callable, so any map representation plugs in without copying. + +## Mathematical Theory + +### Graph Model + +Each cell is a node. Neighbours share a face (4-connected in 2-D, 6-connected in 3-D) or, with full connectivity, also an edge or a corner (8- and 26-connected). Moving into cell $n$ along a step that changes $k$ coordinates costs + +$$c(m \to n) = \sqrt{k}\,\kappa(n), \qquad k \in \{1, 2, 3\}$$ + +where $\kappa(n) \ge \kappa_{\min} > 0$ is the cell's traversal cost and blocked cells have no cost. + +### Best-First Expansion + +Each open cell carries $g(n)$, the best known cost from the start, and a priority $f(n) = g(n) + h(n)$. Cells are expanded in increasing $f$: + +1. Pop the open cell with the smallest $f$ and close it. Stop if it is the goal. +2. For each neighbour that is free and not closed, compute $g(m) + c(m \to n)$. If it improves $g(n)$, record $m$ as the parent of $n$ and push $n$, or decrease its key if it is already open. + +Ties in $f$ are broken towards larger $g$, which dives towards the goal. Dijkstra is the special case $h \equiv 0$. + +### Admissible Heuristic + +With $|\Delta x|, |\Delta y|, |\Delta z|$ sorted into $d_1 \ge d_2 \ge d_3$: + +$$h_{\text{full}} = \kappa_{\min}\left(d_1 + (\sqrt{2} - 1)\,d_2 + (\sqrt{3} - \sqrt{2})\,d_3\right), \qquad h_{\text{faces}} = \kappa_{\min}\,(d_1 + d_2 + d_3)$$ + +$h_{\text{full}}$ is the exact octile distance on an empty grid of unit cost, and $h_{\text{faces}}$ the Manhattan distance. Neither ever overestimates the true remaining cost, so A\* stays optimal. Since $h$ is also consistent, every cell is closed once. + +### Corner Cutting + +By default a diagonal step is rejected when any cell it sweeps past is blocked: the two side cells of a planar diagonal, or the six intermediate cells of a 3-D diagonal. Otherwise the path would clip obstacle corners with any real robot footprint. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|-----------------------|---------------|------------------------------------|-----------------------------------------------------| +| $C$ reachable cells | $O(C \log C)$ | $O(C)$ | Each cell pushed once, sifted on decrease-key | +| Per cell | — | $4 + 3\cdot\text{index} + 1$ bytes | 16-bit indices up to 65 534 cells, otherwise 32-bit | +| $100 \times 100$ grid | — | 110 KB | 11 bytes per cell with `float` costs | +| $32^3$ voxel grid | — | 360 KB | Still 16-bit indices: 32 768 cells | + +## Step-by-Step Walkthrough + +An $8 \times 6$ map has a wall at $x = 4$ for $y = 0$ to $4$, leaving a gap at $y = 5$. All free cells cost $1$, and the query runs from $(1,1)$ to $(6,1)$. + +1. **8-connected A\*.** The path climbs diagonally to $(3,3)$, runs up to the gap, crosses at $(4,5)$, steps diagonally down to $(6,4)$ and descends to the goal: 11 cells, cost $7 + 3\sqrt{2} = 11.243$. Dijkstra returns the same cost. +2. **Effort.** A\* expands 29 cells, Dijkstra 39. With 4-connectivity, the cost is $13$ over 14 cells, with 27 versus 40 expansions. +3. **Weighted cells.** On a free map where cells $x \in \{3, 4\}$, $y \le 2$ cost $5$, the straight route would cost $13$. The optimal path detours through $y = 3$ instead: $(1,1), (2,2), (3,3), (4,3), (5,2), (6,1)$, for $4\sqrt{2} + 1 = 6.657$. A\* expands 10 cells and Dijkstra 37. +4. **No path.** Closing the gap, or placing the goal on the wall, returns no result, and path extraction reports zero cells. + +## Pitfalls & Edge Cases + +- **Inadmissible heuristic**: a minimum cell cost set above the true minimum makes A\* faster but no longer optimal. Use the smallest traversal cost the callable can return. +- **Footprint**: the search treats the robot as a point. Inflate obstacles by the robot radius before searching, or the path grazes walls. +- **Ties**: grids have many equal-cost paths, and which one is returned depends on tie-breaking. Compare costs and validity, not exact cell sequences. +- **Memory**: storage scales with the full grid, not the explored part. Size the grid to the platform's RAM. +- **Path buffer**: extraction returns zero when the output buffer is shorter than the path. Size it for the worst case or retry with a larger one. +- **Coordinate range**: each dimension is limited to 65 535 cells by the 16-bit coordinates. + +## Variants & Generalizations + +- **Weighted A\***: inflate the heuristic by $\varepsilon > 1$ for faster, $\varepsilon$-suboptimal paths. +- **Jump point search**: prunes symmetric paths on uniform-cost grids for large speed-ups. +- **D\* Lite and LPA\***: incremental replanning when map cells change. +- **Theta\* and any-angle planners**: allow line-of-sight shortcuts instead of grid-aligned steps. +- **Hybrid A\***: searches over continuous headings for car-like robots. + +## Applications + +- Global path planning for mobile robots on occupancy grids and costmaps. +- 3-D route planning for drones in voxel maps. +- Coarse workspace routing that seeds sampling-based planners. +- Coverage and exploration planners that need cost-to-go fields. + +## Connections to Other Algorithms + +- [Artificial Potential Fields](PotentialField.md): a reactive local planner that can follow the waypoints produced here. +- [Primitive Distance Queries](PrimitiveDistance.md): build costmaps by inflating geometric obstacles. +- [Self-Collision Checking](SelfCollision.md): the configuration-space validity test for manipulator planners. + +## References & Further Reading + +- Dijkstra, E.W. (1959). "A note on two problems in connexion with graphs." *Numerische Mathematik*, 1, 269–271. +- Hart, P.E., Nilsson, N.J. and Raphael, B. (1968). "A formal basis for the heuristic determination of minimum cost paths." *IEEE Transactions on Systems Science and Cybernetics*, 4(2), 100–107. +- LaValle, S.M. (2006). *Planning Algorithms*. Cambridge University Press. Chapter 2. +- Koenig, S. and Likhachev, M. (2002). "D\* Lite." *AAAI Conference on Artificial Intelligence*, 476–483. diff --git a/doc/planning/README.md b/doc/planning/README.md index 87dee0f..b4afe4c 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -4,8 +4,9 @@ Geometric queries and planners for collision-free robot motion. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------| -| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | -| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | -| [Self-Collision Checking](SelfCollision.md) | Joint-to-joint capsules per link, pairwise signed distances with an allowed-collision matrix and a safety margin — $O(n^2)$ | +| Algorithm | Description | +|----------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------| +| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | +| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | +| [Self-Collision Checking](SelfCollision.md) | Joint-to-joint capsules per link, pairwise signed distances with an allowed-collision matrix and a safety margin — $O(n^2)$ | +| [Grid Search (A* and Dijkstra)](GridSearch.md) | Optimal shortest paths on bounded 2-D/3-D grids with per-cell costs, an indexed binary heap and the admissible octile heuristic | diff --git a/roadmap/README.md b/roadmap/README.md index dcb6cd0..5b6bfe7 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -98,7 +98,7 @@ prerequisites. `PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) ### `planning` -`GridSearch` (P3) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) +`ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) ### `localization` `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) diff --git a/roadmap/planning/GridSearch/explanation.md b/roadmap/planning/GridSearch/explanation.md deleted file mode 100644 index 42669ba..0000000 --- a/roadmap/planning/GridSearch/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# A* / Dijkstra on Bounded Grids — Overview - -## What it is -The classic shortest-path search on a grid map: from a start cell to a goal cell, through free cells, -minimising the total travel cost, which can be higher in cells the robot should avoid. Dijkstra explores -evenly in all directions. A* uses an optimistic estimate of the remaining distance to explore towards the -goal first, and returns the same optimal path. - -## Why it matters (embedded) -Grid search is the global planner of most mobile robots, from vacuum cleaners to warehouse fleets, and in -3-D it plans for drones in voxel maps. Here all bookkeeping lives in fixed arrays sized at compile time: -no allocation, a known worst-case memory footprint, and small index types when the grid is small. - -## How it works (intuition) -The search keeps a priority queue of cells ordered by cost so far plus estimated cost to go. It -repeatedly takes the most promising cell, marks it final and offers its neighbours a cheaper route through -it. When a cheaper route to a queued cell appears, that cell moves forward in the queue instead of being -added twice. The estimate is the "octile" distance, the length of the best path on an empty grid. It never -overestimates, which is what guarantees the result is optimal. When the goal is taken from the queue, -following the stored parents back to the start gives the path. - -## Key parameters -- **connectivity** — 4/8 neighbours in 2-D, 6/26 in 3-D. -- **mode** — A-star or Dijkstra; **minimum cell cost** — keeps the A-star estimate admissible. -- **corner cutting** — whether diagonal moves may brush past obstacle corners. - -## Reference -P. E. Hart, N. J. Nilsson, B. Raphael, "A formal basis for the heuristic determination of minimum cost -paths," *IEEE Trans. Systems Science and Cybernetics*, 4(2), 1968; E. W. Dijkstra, "A note on two problems -in connexion with graphs," *Numerische Mathematik*, 1, 1959. - -## See also -`OccupancyGridMap` (E2, a cost source), `PotentialField` (P2), `PurePursuit` (W5, follows the resulting path). diff --git a/roadmap/planning/GridSearch/implementation.md b/roadmap/planning/GridSearch/implementation.md deleted file mode 100644 index 6e0df42..0000000 --- a/roadmap/planning/GridSearch/implementation.md +++ /dev/null @@ -1,105 +0,0 @@ -# A* / Dijkstra on Bounded Occupancy Grids — Implementation Pseudocode - -> Roadmap ref: #P3 (★★★☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) - -Optimal shortest paths on 2-D or 3-D grids with per-cell traversal costs, all in compile-time storage. It -uses an indexed binary heap with decrease-key, so each cell sits in the heap at most once. Index types -shrink to 16 bits when the grid has at most 65 535 cells, and the heuristic is the admissible octile -distance. Cell costs come from a callable, so the same search runs on E2's occupancy grid, a costmap, or a -voxel map without copying. - -## Data structures - -```cpp -enum class Connectivity : std::uint8_t { faces, full } # 4 / 8 neighbours in 2-D, 6 / 26 in 3-D -enum class SearchMode : std::uint8_t { aStar, dijkstra } - -struct GridIndex { std::uint16_t x, y, z; } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct GridSearchParameters: - Connectivity connectivity - SearchMode mode - bool allowCornerCutting{ false } # diagonal moves past a blocked side neighbour - T minimumCellCost{ 1 } # lower bound of the cost callable: keeps the heuristic admissible - -template -class GridSearch: - static constexpr std::size_t Cells = Width·Height·Depth - using Index = std::conditional_t # max value = "none" - std::array costFromStart # g - std::array parent - std::array heap # cell indices ordered by f = g + h - std::array heapSlot # position of each cell in the heap - std::array state # unseen / open / closed - std::size_t heapSize, expanded - GridSearchParameters parameters -``` - -## Interface - -```cpp -explicit GridSearch(const GridSearchParameters& parameters) -template -std::optional Search(GridIndex start, GridIndex goal, CellCost&& cost) # cost(GridIndex) ⇒ std::optional; nullopt = blocked -std::size_t ExtractPath(std::span out) const # start → goal; 0 if `out` is too small or no path -std::size_t Expanded() const -``` - -## Algorithm (pseudocode) - -```text -function Heuristic(c): # octile distance × cheapest cell - sort |Δx|, |Δy|, |Δz| descending into (d₁, d₂, d₃) - return mode == aStar ? minimumCellCost·(d₁ + (√2 − 1)·d₂ + (√3 − √2)·d₃) : 0 - # faces connectivity: Manhattan d₁ + d₂ + d₃ - -function Search(s, g, cost): # OPTIMIZE_FOR_SPEED - if cost(s) or cost(g) is nullopt: return nullopt - reset state/heap; g[s] = 0; push s with f = Heuristic(s) - while heap not empty: - c = pop min f (ties: larger g first, so the search dives towards the goal) - state[c] = closed; expanded += 1 - if c == g: return g[c] - for each neighbour n of c (connectivity, inside the grid): - if state[n] == closed: continue - k = cost(n); if k is nullopt: continue - if diagonal and not allowCornerCutting and a side neighbour sharing the move is blocked: continue - candidate = g[c] + stepLength(c, n)·k # stepLength ∈ {1, √2, √3} - if state[n] == unseen: g[n] = candidate, parent[n] = c, push n - elif candidate < g[n]: g[n] = candidate, parent[n] = c, decrease-key n - return nullopt # goal unreachable - -function ExtractPath(out): # walk parents from the goal, then reverse in place -``` - -## Complexity & memory - -- `O(C log C)` time for `C` reachable cells. Each cell is pushed once and sifted on decrease-key. -- Memory per cell: `sizeof(T) + 3·sizeof(Index) + 1`. A 100 × 100 `float` grid uses 16-bit indices: - 11 bytes per cell, 110 KB. A 32³ voxel grid is 360 KB. Size the grid to the platform. No heap. - -## Numerical / embedded notes - -- **Admissibility.** The octile heuristic is exact on an empty grid of unit cost. Scaled by the minimum - cell cost it never overestimates, so A* returns the same optimal cost as Dijkstra (tested on a weighted - map) while expanding fewer cells. -- **Corner cutting** is off by default: a diagonal step between two blocked side cells would clip the - obstacle corners with any real footprint. Inflate obstacles by the robot radius before searching. -- **Ties.** Equal-cost paths are common on grids; which one is returned depends on tie-breaking. Tests - check the cost, the path length and its validity, not the exact cell sequence. -- `GridIndex` uses 16-bit coordinates, so each grid dimension must be ≤ 65 535. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/planning/GridSearch.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Search`, and - `extern template class GridSearch;` / `GridSearch` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/planning/GridSearch.cpp` → the same instantiations. -- Test: `robotics/planning/test/TestGridSearch.cpp` -- Doc: `doc/planning/GridSearch.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.planning_test`. -- Depends on: nothing in the library (E2's map plugs in through the cost callable). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/GridSearch/tests.md b/roadmap/planning/GridSearch/tests.md deleted file mode 100644 index f4eab7a..0000000 --- a/roadmap/planning/GridSearch/tests.md +++ /dev/null @@ -1,51 +0,0 @@ -# Grid Search — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestGridSearch : public ::testing::Test: - # 8 × 6 map: wall at x = 4 for y = 0..4 (gap at y = 5); cost 1 elsewhere - GridSearch search{ { full, aStar } } - Cost(c) ⇒ wall ? nullopt : 1 - # ValidPath(path): starts at start, ends at goal, consecutive cells are neighbours, all cells free, - # no corner cutting; its summed step cost equals the returned cost -# each case below is a TEST_F(TestGridSearch, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -eight_connected_path_goes_through_the_gap: - Act: Search((1,1), (6,1), Cost) - Assert: cost ≈ 11.242641 (= 7 + 3√2; Dijkstra agrees); ExtractPath gives 11 cells; ValidPath -four_connected_path_is_longer: - Arrange: connectivity faces - Assert: cost == 13; 14 cells; ValidPath -astar_and_dijkstra_agree_and_astar_expands_no_more: - Assert: for both connectivities: aStar cost == dijkstra cost; aStar Expanded() ≤ dijkstra Expanded() -weighted_cells_are_avoided_when_cheaper: - Arrange: free 8 × 6 map with cost 5 on x ∈ {3, 4}, y ∈ {0, 1, 2} - Assert: cost ≈ 6.656854 (both modes); the path climbs to y = 3 to cross x = 3..4; ValidPath -blocked_goal_or_closed_gap_has_no_path: - Assert: gap closed (cell (4,5) blocked) ⇒ nullopt; goal on the wall (4,2) ⇒ nullopt; ExtractPath ⇒ 0 -start_equal_to_goal_costs_nothing: - Assert: Search((2,2), (2,2), Cost) == 0; ExtractPath gives the single cell (2,2) -three_dimensional_grid_uses_diagonal_steps: - Arrange: GridSearch, all free - Assert: full: Search((0,0,0), (2,2,2)) ≈ 2√3 = 3.464102; faces: 6 -extract_path_rejects_a_short_buffer: - Assert: after the first case, ExtractPath into a 5-cell span ⇒ 0 -``` - -## Reference vectors - -- Wall map, 8-connected: `(1,1)` to `(6,1)` costs `11.242641` through the gap at `y = 5`; 4-connected `13`. -- Weighted map: `6.656854` (`4√2 + 1` around the cost-5 block). - -## Edge cases - -- Corner cutting enabled ⇒ diagonal moves past a blocked side cell are allowed (never shorter than - through the gap here, which the test documents). -- A grid larger than 65 535 cells switches `Index` to 32 bits (compile-time check). diff --git a/robotics/planning/CMakeLists.txt b/robotics/planning/CMakeLists.txt index befdac1..ceedbe8 100644 --- a/robotics/planning/CMakeLists.txt +++ b/robotics/planning/CMakeLists.txt @@ -12,12 +12,14 @@ target_link_libraries(robotics.planning ${ROBOTICS_VISIBILITY} ) target_sources(robotics.planning PRIVATE + GridSearch.hpp PotentialField.hpp PrimitiveDistance.hpp SelfCollision.hpp ) robotics_add_coverage_sources(robotics.planning + GridSearch.cpp PotentialField.cpp PrimitiveDistance.cpp SelfCollision.cpp diff --git a/robotics/planning/GridSearch.cpp b/robotics/planning/GridSearch.cpp new file mode 100644 index 0000000..74ad982 --- /dev/null +++ b/robotics/planning/GridSearch.cpp @@ -0,0 +1,9 @@ +#include "robotics/planning/GridSearch.hpp" + +namespace planning +{ + template class GridSearch; + template class GridSearch; + template std::optional GridSearch::Search (&)(GridIndex)>(GridIndex, GridIndex, std::optional (&)(GridIndex)); + template std::optional GridSearch::Search (&)(GridIndex)>(GridIndex, GridIndex, std::optional (&)(GridIndex)); +} diff --git a/robotics/planning/GridSearch.hpp b/robotics/planning/GridSearch.hpp new file mode 100644 index 0000000..cd2797e --- /dev/null +++ b/robotics/planning/GridSearch.hpp @@ -0,0 +1,405 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning +{ + enum class Connectivity : std::uint8_t + { + Faces, + Full + }; + + enum class SearchMode : std::uint8_t + { + AStar, + Dijkstra + }; + + struct GridIndex + { + std::uint16_t x{}; + std::uint16_t y{}; + std::uint16_t z{}; + + friend bool operator==(const GridIndex&, const GridIndex&) = default; + }; + + template + struct GridSearchParameters + { + static_assert(std::is_floating_point_v, "GridSearchParameters supports floating-point types"); + + Connectivity connectivity{ Connectivity::Full }; + SearchMode mode{ SearchMode::AStar }; + bool allowCornerCutting{ false }; + T minimumCellCost{ 1 }; + }; + + template + class GridSearch + { + static_assert(std::is_floating_point_v, "GridSearch supports floating-point types"); + static_assert(Width > 0 && Height > 0 && Depth > 0, "Grid dimensions must be positive"); + static_assert(Width <= 65535 && Height <= 65535 && Depth <= 65535, "GridIndex coordinates are 16-bit"); + + public: + static constexpr std::size_t Cells{ Width * Height * Depth }; + + using Index = std::conditional_t<(Cells < 65535), std::uint16_t, std::uint32_t>; + + static_assert(Cells < std::numeric_limits::max(), "Grid too large for 32-bit cell indices"); + + explicit GridSearch(const GridSearchParameters& parameters); + + template + OPTIMIZE_FOR_SPEED std::optional Search(GridIndex start, GridIndex target, CellCost&& cost); + + std::size_t ExtractPath(std::span out) const; + std::size_t Expanded() const; + + private: + enum class CellState : std::uint8_t + { + Unseen, + Open, + Closed + }; + + struct Move + { + int dx; + int dy; + int dz; + + std::size_t Axes() const; + unsigned Mask() const; + }; + + static constexpr Index none{ std::numeric_limits::max() }; + static constexpr std::array stepLengths{ T{}, T{ 1 }, std::numbers::sqrt2_v, std::numbers::sqrt3_v }; + + static bool Inside(GridIndex cell); + static Index Linear(GridIndex cell); + static GridIndex Coordinates(Index cell); + static std::optional Offset(GridIndex cell, const Move& move); + + template + void Expand(Index current, CellCost& cost); + template + void TryMove(Index current, GridIndex cell, const Move& move, CellCost& cost); + template + bool CornerBlocked(GridIndex cell, const Move& move, CellCost& cost) const; + void Relax(Index current, Index next, T candidate); + + T Heuristic(Index cell) const; + bool Before(Index lhs, Index rhs) const; + void Push(Index cell); + Index Pop(); + void SiftUp(std::size_t slot); + void SiftDown(std::size_t slot); + void Place(std::size_t slot, Index cell); + + GridSearchParameters parameters; + std::array costFromStart{}; + std::array parent{}; + std::array heap{}; + std::array heapSlot{}; + std::array state{}; + std::size_t heapSize{ 0 }; + std::size_t expanded{ 0 }; + GridIndex goal{}; + Index goalCell{ none }; + bool found{ false }; + }; + + template + GridSearch::GridSearch(const GridSearchParameters& parameters) + : parameters{ parameters } + {} + + template + template + OPTIMIZE_FOR_SPEED std::optional GridSearch::Search(GridIndex start, GridIndex target, CellCost&& cost) + { + found = false; + expanded = 0; + heapSize = 0; + if (!Inside(start) || !Inside(target) || !cost(start) || !cost(target)) + return std::nullopt; + + state.fill(CellState::Unseen); + goal = target; + goalCell = Linear(target); + const Index startCell{ Linear(start) }; + costFromStart[startCell] = T{}; + parent[startCell] = none; + Push(startCell); + + while (heapSize > 0) + { + const Index current{ Pop() }; + state[current] = CellState::Closed; + ++expanded; + if (current == goalCell) + { + found = true; + return costFromStart[current]; + } + Expand(current, cost); + } + + return std::nullopt; + } + + template + std::size_t GridSearch::ExtractPath(std::span out) const + { + if (!found) + return 0; + + std::size_t length{ 0 }; + for (Index cell = goalCell; cell != none; cell = parent[cell]) + ++length; + if (length > out.size()) + return 0; + + std::size_t slot{ length }; + for (Index cell = goalCell; cell != none; cell = parent[cell]) + out[--slot] = Coordinates(cell); + return length; + } + + template + std::size_t GridSearch::Expanded() const + { + return expanded; + } + + template + std::size_t GridSearch::Move::Axes() const + { + return static_cast(dx != 0) + static_cast(dy != 0) + static_cast(dz != 0); + } + + template + unsigned GridSearch::Move::Mask() const + { + return (dx != 0 ? 1u : 0u) | (dy != 0 ? 2u : 0u) | (dz != 0 ? 4u : 0u); + } + + template + bool GridSearch::Inside(GridIndex cell) + { + return cell.x < Width && cell.y < Height && cell.z < Depth; + } + + template + auto GridSearch::Linear(GridIndex cell) -> Index + { + return static_cast(cell.x + Width * (cell.y + Height * cell.z)); + } + + template + GridIndex GridSearch::Coordinates(Index cell) + { + return GridIndex{ static_cast(cell % Width), static_cast((cell / Width) % Height), static_cast(cell / (Width * Height)) }; + } + + template + std::optional GridSearch::Offset(GridIndex cell, const Move& move) + { + const int x{ cell.x + move.dx }; + const int y{ cell.y + move.dy }; + const int z{ cell.z + move.dz }; + if (x < 0 || y < 0 || z < 0 || x >= static_cast(Width) || y >= static_cast(Height) || z >= static_cast(Depth)) + return std::nullopt; + + return GridIndex{ static_cast(x), static_cast(y), static_cast(z) }; + } + + template + template + void GridSearch::Expand(Index current, CellCost& cost) + { + constexpr int depthReach{ Depth > 1 ? 1 : 0 }; + const GridIndex cell{ Coordinates(current) }; + + for (int dz = -depthReach; dz <= depthReach; ++dz) + for (int dy = -1; dy <= 1; ++dy) + for (int dx = -1; dx <= 1; ++dx) + TryMove(current, cell, Move{ dx, dy, dz }, cost); + } + + template + template + void GridSearch::TryMove(Index current, GridIndex cell, const Move& move, CellCost& cost) + { + const std::size_t axes{ move.Axes() }; + if (axes == 0 || (parameters.connectivity == Connectivity::Faces && axes > 1)) + return; + + const auto neighbour{ Offset(cell, move) }; + if (!neighbour || state[Linear(*neighbour)] == CellState::Closed) + return; + + const auto cellCost{ cost(*neighbour) }; + if (!cellCost || (axes > 1 && !parameters.allowCornerCutting && CornerBlocked(cell, move, cost))) + return; + + Relax(current, Linear(*neighbour), costFromStart[current] + stepLengths[axes] * *cellCost); + } + + template + template + bool GridSearch::CornerBlocked(GridIndex cell, const Move& move, CellCost& cost) const + { + const unsigned mask{ move.Mask() }; + for (unsigned subset = 1; subset < mask; ++subset) + { + if ((subset & ~mask) != 0u) + continue; + + const auto side{ Offset(cell, Move{ (subset & 1u) != 0u ? move.dx : 0, (subset & 2u) != 0u ? move.dy : 0, (subset & 4u) != 0u ? move.dz : 0 }) }; + if (side && !cost(*side)) + return true; + } + + return false; + } + + template + void GridSearch::Relax(Index current, Index next, T candidate) + { + if (state[next] == CellState::Unseen) + { + costFromStart[next] = candidate; + parent[next] = current; + Push(next); + } + else if (candidate < costFromStart[next]) + { + costFromStart[next] = candidate; + parent[next] = current; + SiftUp(heapSlot[next]); + } + } + + template + T GridSearch::Heuristic(Index cell) const + { + if (parameters.mode == SearchMode::Dijkstra) + return T{}; + + const GridIndex position{ Coordinates(cell) }; + int largest{ std::abs(position.x - goal.x) }; + int middle{ std::abs(position.y - goal.y) }; + int smallest{ std::abs(position.z - goal.z) }; + if (largest < middle) + std::swap(largest, middle); + if (middle < smallest) + std::swap(middle, smallest); + if (largest < middle) + std::swap(largest, middle); + + if (parameters.connectivity == Connectivity::Faces) + return parameters.minimumCellCost * static_cast(largest + middle + smallest); + + return parameters.minimumCellCost * (static_cast(largest) + (stepLengths[2] - stepLengths[1]) * static_cast(middle) + (stepLengths[3] - stepLengths[2]) * static_cast(smallest)); + } + + template + bool GridSearch::Before(Index lhs, Index rhs) const + { + const T lhsEstimate{ costFromStart[lhs] + Heuristic(lhs) }; + const T rhsEstimate{ costFromStart[rhs] + Heuristic(rhs) }; + if (lhsEstimate < rhsEstimate) + return true; + if (rhsEstimate < lhsEstimate) + return false; + + return costFromStart[lhs] > costFromStart[rhs]; + } + + template + void GridSearch::Push(Index cell) + { + state[cell] = CellState::Open; + Place(heapSize, cell); + ++heapSize; + SiftUp(heapSize - 1); + } + + template + auto GridSearch::Pop() -> Index + { + const Index top{ heap[0] }; + --heapSize; + if (heapSize > 0) + { + Place(0, heap[heapSize]); + SiftDown(0); + } + + return top; + } + + template + void GridSearch::SiftUp(std::size_t slot) + { + const Index cell{ heap[slot] }; + while (slot > 0) + { + const std::size_t above{ (slot - 1) / 2 }; + if (!Before(cell, heap[above])) + break; + + Place(slot, heap[above]); + slot = above; + } + + Place(slot, cell); + } + + template + void GridSearch::SiftDown(std::size_t slot) + { + const Index cell{ heap[slot] }; + for (std::size_t child = 2 * slot + 1; child < heapSize; child = 2 * slot + 1) + { + if (child + 1 < heapSize && Before(heap[child + 1], heap[child])) + ++child; + if (!Before(heap[child], cell)) + break; + + Place(slot, heap[child]); + slot = child; + } + + Place(slot, cell); + } + + template + void GridSearch::Place(std::size_t slot, Index cell) + { + heap[slot] = cell; + heapSlot[cell] = static_cast(slot); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class GridSearch; + extern template class GridSearch; + extern template std::optional GridSearch::Search (&)(GridIndex)>(GridIndex, GridIndex, std::optional (&)(GridIndex)); + extern template std::optional GridSearch::Search (&)(GridIndex)>(GridIndex, GridIndex, std::optional (&)(GridIndex)); +#endif +} diff --git a/robotics/planning/test/CMakeLists.txt b/robotics/planning/test/CMakeLists.txt index 51efb2a..42bb705 100644 --- a/robotics/planning/test/CMakeLists.txt +++ b/robotics/planning/test/CMakeLists.txt @@ -8,6 +8,7 @@ target_link_libraries(robotics.planning_test PUBLIC ) target_sources(robotics.planning_test PRIVATE + TestGridSearch.cpp TestPotentialField.cpp TestPrimitiveDistance.cpp TestSelfCollision.cpp diff --git a/robotics/planning/test/TestGridSearch.cpp b/robotics/planning/test/TestGridSearch.cpp new file mode 100644 index 0000000..e6d86b9 --- /dev/null +++ b/robotics/planning/test/TestGridSearch.cpp @@ -0,0 +1,176 @@ +#include "robotics/planning/GridSearch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + using planning::Connectivity; + using planning::GridIndex; + using planning::SearchMode; + using Search = planning::GridSearch; + using Path = std::array; + + std::optional WallCost(GridIndex cell) + { + if (cell.x == 4 && cell.y <= 4) + return std::nullopt; + return 1.0f; + } + + std::optional ClosedGapCost(GridIndex cell) + { + if (cell.x == 4) + return std::nullopt; + return 1.0f; + } + + std::optional OpenSpace(GridIndex) + { + return 1.0f; + } + + std::optional WeightedCost(GridIndex cell) + { + if ((cell.x == 3 || cell.x == 4) && cell.y <= 2) + return 5.0f; + return 1.0f; + } + + class TestGridSearch + : public ::testing::Test + { + protected: + template + void ExpectValidPath(std::span path, GridIndex start, GridIndex goal, CellCost cost, float expectedCost, Connectivity connectivity) const + { + ASSERT_FALSE(path.empty()); + EXPECT_EQ(path.front(), start); + EXPECT_EQ(path.back(), goal); + + float total{ 0.0f }; + for (std::size_t i = 1; i < path.size(); ++i) + { + const int dx{ path[i].x - path[i - 1].x }; + const int dy{ path[i].y - path[i - 1].y }; + const int dz{ path[i].z - path[i - 1].z }; + const int axes{ std::abs(dx) + std::abs(dy) + std::abs(dz) }; + EXPECT_LE(std::max({ std::abs(dx), std::abs(dy), std::abs(dz) }), 1); + EXPECT_GE(axes, 1); + EXPECT_TRUE(connectivity == Connectivity::Full || axes == 1); + ASSERT_TRUE(cost(path[i]).has_value()); + if (axes == 2 && dz == 0) + { + EXPECT_TRUE(cost(GridIndex{ path[i].x, path[i - 1].y, path[i].z }).has_value()); + EXPECT_TRUE(cost(GridIndex{ path[i - 1].x, path[i].y, path[i].z }).has_value()); + } + total += std::sqrt(static_cast(axes)) * *cost(path[i]); + } + EXPECT_NEAR(total, expectedCost, 1e-4f); + } + + Search search{ { Connectivity::Full, SearchMode::AStar } }; + Search dijkstra{ { Connectivity::Full, SearchMode::Dijkstra } }; + Search faces{ { Connectivity::Faces, SearchMode::AStar } }; + Search facesDijkstra{ { Connectivity::Faces, SearchMode::Dijkstra } }; + GridIndex start{ 1, 1 }; + GridIndex goal{ 6, 1 }; + Path path{}; + }; +} + +TEST_F(TestGridSearch, eight_connected_path_goes_through_the_gap) +{ + const auto cost{ search.Search(start, goal, WallCost) }; + + ASSERT_TRUE(cost.has_value()); + EXPECT_NEAR(*cost, 11.242641f, 1e-4f); + EXPECT_NEAR(dijkstra.Search(start, goal, WallCost).value_or(-1.0f), *cost, 1e-4f); + const auto length{ search.ExtractPath(path) }; + EXPECT_EQ(length, std::size_t{ 11 }); + ExpectValidPath(std::span{ path.data(), length }, start, goal, WallCost, *cost, Connectivity::Full); +} + +TEST_F(TestGridSearch, four_connected_path_is_longer) +{ + const auto cost{ faces.Search(start, goal, WallCost) }; + + ASSERT_TRUE(cost.has_value()); + EXPECT_NEAR(*cost, 13.0f, 1e-5f); + const auto length{ faces.ExtractPath(path) }; + EXPECT_EQ(length, std::size_t{ 14 }); + ExpectValidPath(std::span{ path.data(), length }, start, goal, WallCost, *cost, Connectivity::Faces); +} + +TEST_F(TestGridSearch, astar_and_dijkstra_agree_and_astar_expands_no_more) +{ + for (auto [informed, uninformed] : { std::pair{ &search, &dijkstra }, std::pair{ &faces, &facesDijkstra } }) + { + const auto informedCost{ informed->Search(start, goal, WallCost) }; + const auto uninformedCost{ uninformed->Search(start, goal, WallCost) }; + + ASSERT_TRUE(informedCost.has_value()); + ASSERT_TRUE(uninformedCost.has_value()); + EXPECT_NEAR(*informedCost, *uninformedCost, 1e-4f); + EXPECT_LE(informed->Expanded(), uninformed->Expanded()); + } +} + +TEST_F(TestGridSearch, weighted_cells_are_avoided_when_cheaper) +{ + for (auto* planner : { &search, &dijkstra }) + { + const auto cost{ planner->Search(start, goal, WeightedCost) }; + + ASSERT_TRUE(cost.has_value()); + EXPECT_NEAR(*cost, 6.656854f, 1e-4f); + const auto length{ planner->ExtractPath(path) }; + const std::span route{ path.data(), length }; + for (const auto& cell : route) + if (cell.x == 3 || cell.x == 4) + EXPECT_GE(cell.y, 3); + ExpectValidPath(route, start, goal, WeightedCost, *cost, Connectivity::Full); + } +} + +TEST_F(TestGridSearch, blocked_goal_or_closed_gap_has_no_path) +{ + EXPECT_FALSE(search.Search(start, goal, ClosedGapCost).has_value()); + EXPECT_EQ(search.ExtractPath(path), std::size_t{ 0 }); + EXPECT_FALSE(search.Search(start, GridIndex{ 4, 2 }, WallCost).has_value()); + EXPECT_EQ(search.ExtractPath(path), std::size_t{ 0 }); +} + +TEST_F(TestGridSearch, start_equal_to_goal_costs_nothing) +{ + const GridIndex cell{ 2, 2 }; + + const auto cost{ search.Search(cell, cell, WallCost) }; + + ASSERT_TRUE(cost.has_value()); + EXPECT_EQ(*cost, 0.0f); + ASSERT_EQ(search.ExtractPath(path), std::size_t{ 1 }); + EXPECT_EQ(path[0], cell); +} + +TEST_F(TestGridSearch, three_dimensional_grid_uses_diagonal_steps) +{ + planning::GridSearch full{ { Connectivity::Full, SearchMode::AStar } }; + planning::GridSearch axisAligned{ { Connectivity::Faces, SearchMode::AStar } }; + + EXPECT_NEAR(full.Search(GridIndex{ 0, 0, 0 }, GridIndex{ 2, 2, 2 }, OpenSpace).value_or(-1.0f), 3.464102f, 1e-5f); + EXPECT_NEAR(axisAligned.Search(GridIndex{ 0, 0, 0 }, GridIndex{ 2, 2, 2 }, OpenSpace).value_or(-1.0f), 6.0f, 1e-5f); +} + +TEST_F(TestGridSearch, extract_path_rejects_a_short_buffer) +{ + ASSERT_TRUE(search.Search(start, goal, WallCost).has_value()); + std::array shortBuffer{}; + + EXPECT_EQ(search.ExtractPath(shortBuffer), std::size_t{ 0 }); +} From 98860d6668e1e5435f7f0cb8fc4ac4599fdc4bfe Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 13:10:28 +0000 Subject: [PATCH 9/9] docs(dynamics): align MomentumObserver complexity table Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- doc/dynamics/MomentumObserver.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/dynamics/MomentumObserver.md b/doc/dynamics/MomentumObserver.md index 1f18922..2aea8e3 100644 --- a/doc/dynamics/MomentumObserver.md +++ b/doc/dynamics/MomentumObserver.md @@ -48,12 +48,12 @@ A collision is declared when any joint exceeds its threshold, $|r_i| > \bar{r}_i ## Complexity Analysis -| Operation | Time | Space | Notes | -|------------------|----------|----------|--------------------------------------------------| -| `Update` | $O(n^2)$ | $O(n^2)$ | Mass matrix and $C^T\dot{q}$ dominate | -| `Reset` | $O(n^2)$ | $O(n)$ | One mass-matrix product | -| `Collision` | $O(n)$ | $O(1)$ | One comparison per joint | -| Observer state | — | $O(n)$ | Initial momentum, integral and residual | +| Operation | Time | Space | Notes | +|----------------|----------|----------|-----------------------------------------| +| `Update` | $O(n^2)$ | $O(n^2)$ | Mass matrix and $C^T\dot{q}$ dominate | +| `Reset` | $O(n^2)$ | $O(n)$ | One mass-matrix product | +| `Collision` | $O(n)$ | $O(1)$ | One comparison per joint | +| Observer state | — | $O(n)$ | Initial momentum, integral and residual | ## Step-by-Step Walkthrough