diff --git a/README.md b/README.md index f916851..c51099e 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), 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, 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 | +| 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), Pose Inverse Kinematics, Analytical Inverse Kinematics (OPW), Continuum Kinematics, Parallel Manipulator 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, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect | +| [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, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | 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 8b092f4..36b1794 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -117,17 +117,10 @@ existing `dynamics/` convention. ### Manipulator list (by priority) -| # | Component | Target module | Depends on | Difficulty | -|-----|-----------------------------------------------------------------|---------------------------|-------------|------------| -| 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 | +|-----|---------------------------------------------------|--------------------|-------------|------------| +| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14, W1 | ★★★★☆ | +| 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,34 +132,10 @@ retired here. ### 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. -- *Algorithm / paper:* O. Khatib, *IEEE J. Robotics and Automation*, 3(1), 1987. - -**M19. Hybrid position/force control.** Selection matrix in a constraint frame, PI force loop with anti-windup and damping, motion PD. -- *Algorithm / paper:* M. Raibert, J. Craig, 1981; An & Hollerbach, 1987. - -**M20. Passivity-based adaptive control (Slotine-Li).** Tracks trajectories while estimating inertial parameters on-line through the regressor `Y(q,q̇,q̇r,q̈r)`. -- *Algorithm / paper:* J.-J. Slotine, W. Li, *Int. J. Robotics Research*, 6(3), 1987. - -**M21. Analytical IK for ortho-parallel 6R arms with a spherical wrist (OPW).** Closed-form, all eight solutions, covering shoulder, lateral and elbow offsets of common industrial arms. -- *Algorithm / paper:* M. Brandstötter, A. Angerer, M. Hofbaur, Austrian Robotics Workshop, 2014; D. Pieper, PhD thesis, Stanford, 1968. -- *Reuses / builds on:* NT53; joint wrapping from NT54. - -**M23. Parallel-manipulator kinematics.** Stewart-Gough (leg-length IK, Newton FK) and Delta (per-arm closed-form IK, three-sphere FK). -- *Algorithm / paper:* J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel (1990). -- *Reuses / builds on:* NT53 retraction; the Stewart Newton FK runs on upstream `LevenbergMarquardt` (NT64). - **M24. Mobile-manipulator / nonholonomic-base kinematics.** Combined base + arm Jacobian in the world frame with differential-drive constraints. - *Algorithm / paper:* Y. Yamamoto, X. Yun, *IEEE Trans. Automatic Control*, 39(6), 1994. - *Reuses / builds on:* the W1 differential-drive model for the base. -**M25. Cable-driven tension distribution.** Structure matrix from cable geometry, Pott's closed-form (improved) tension distribution within `[tMin, tMax]`, and an exact QP path for the workspace boundary. -- *Algorithm / paper:* Pott, Bruckmann, Mikelsons (2009); A. Pott (2014). -- *Reuses / builds on:* upstream `solvers::ActiveSetQp` (NT65) for the exact path. - -**M26. Continuum / soft constant-curvature kinematics.** Piecewise-constant-curvature FK and single-section IK. -- *Algorithm / paper:* R. Webster, B. Jones, *Int. J. Robotics Research*, 29(13), 2010. - ### Tier 5 — Hard / research-grade ★★★★★ **M27. Time-optimal path parameterization (TOPP-RA).** Minimum-time traversal of a fixed path under joint velocity and torque limits via reachability analysis (two-variable LPs per grid stage, solved locally by Fourier–Motzkin elimination). @@ -375,17 +344,8 @@ Cross-family building blocks that several of the families below depend on. ## Motion planning and collision -| # | Component | Target module | Depends on | Difficulty | -|----|---------------------------------------------------|---------------|------------|------------| -| P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | -| P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | - -**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). - -**P6. RRT-Connect.** Bidirectional rapidly-exploring random trees in joint space with compile-time node pools, an injected sampler and validity test, and random shortcutting. -- *Algorithm / paper:* J. Kuffner, S. LaValle, *ICRA* 2000. -- *Reuses / builds on:* P4 (and P1/P5) as the validity test, supplied by the caller. +| # | Component | Target module | Depends on | Difficulty | +|---|-----------|---------------|------------|------------| ## Localization and mapping diff --git a/doc/controllers/manipulator/CableTensionDistribution.md b/doc/controllers/manipulator/CableTensionDistribution.md new file mode 100644 index 0000000..44bb870 --- /dev/null +++ b/doc/controllers/manipulator/CableTensionDistribution.md @@ -0,0 +1,96 @@ +# Cable Tension Distribution + +## Overview & Motivation + +**Cable-driven parallel robots** move a platform with several cables wound on winches: stadium cameras, large-scale 3D printers, rehabilitation devices and wind-tunnel model supports. They reach huge workspaces with tiny moving mass. The catch is that cables can only **pull**. A tension that would have to be negative means the cable goes slack and control is lost. + +With more cables than platform degrees of freedom, infinitely many tension sets produce the same platform wrench. The controller must pick one that stays within $[t_{\min}, t_{\max}]$ (taut, but below the winch limit) at every control cycle, deterministically and quickly. +This module computes the structure matrix from the cable geometry, distributes tensions with Pott's closed-form method and its improved variant, and provides an exact quadratic-programming path for the boundary of the workspace. + +## Mathematical Theory + +### Structure Matrix + +For base anchors $a_i$ (world frame), platform anchors $b_i$ (platform frame) and platform pose $(R, p)$, cable $i$ pulls the platform along the unit vector + +$$u_i = \frac{a_i - p - R\,b_i}{\|a_i - p - R\,b_i\|}$$ + +and the cables balance the required platform wrench $w$ when + +$$A\,t = w, \qquad A = \begin{bmatrix} u_1 & \cdots & u_m \\ R b_1 \times u_1 & \cdots & R b_m \times u_m \end{bmatrix}$$ + +For point-mass platforms only the force rows are kept, and the platform anchors are ignored. + +### Closed-Form Distribution + +With the mid tension $t_m = (t_{\min} + t_{\max})/2$, the tension vector closest to $t_m\mathbf{1}$ that balances $w$ is + +$$t = t_m\mathbf{1} + A^T(AA^T)^{-1}(w - A\,t_m\mathbf{1})$$ + +It keeps every cable as far from slack and from overload as the wrench allows (Pott, Bruckmann and Mikelsons, 2009). With exactly as many cables as wrench dimensions, it reduces to the unique $A^{-1}w$. + +### Improved Closed Form + +When some tension leaves its bounds, the worst-violating cable is clamped to its bound, its contribution moves to the wrench side, and the reduced system is solved again (Pott, 2014). This repeats at most $m - n + 1$ times. If fewer than $n$ free cables remain, the wrench is declared infeasible for this method. + +### Exact Distribution + +Close to the boundary of the wrench-feasible workspace, the improved closed form can fail although a solution exists. The exact path solves the strictly convex quadratic program + +$$\min_t \tfrac12\|t - t_m\mathbf{1}\|^2 \quad \text{s.t.} \quad A\,t = w, \quad t_{\min} \le t \le t_{\max}$$ + +with a dual active-set method. No result means the wrench is truly outside the feasible set. + +## Complexity Analysis + +| Method | Time | Space | Notes | +|----------------------|-------------------------------|---------------|---------------------------------------------------------| +| Structure matrix | $O(mn)$ | $O(mn)$ | One unit vector and cross product per cable | +| Improved closed form | $O((m - n + 1)(n^2 m + n^3))$ | $O(mn + n^2)$ | At most $m - n + 1$ passes, one $n \times n$ solve each | +| Exact QP | $O(k \cdot m^3)$ | $O(m^2)$ | Active-set iterations $k$, bounded by the solver's cap | + +## Step-by-Step Walkthrough + +A planar point mass hangs from three cables anchored $120°$ apart at radius 2, with $t_{\min} = 10$ and $t_{\max} = 200$, so $t_m = 105$. The cable directions are $(0, 1)$ and $(\mp 0.866, -0.5)$. + +1. **No wrench**: the three tensions are the pretension $(105, 105, 105)$, which already balances because the directions sum to zero. +2. **Upward wrench** $(0, 60)$: $t = (145, 85, 85)$, the balancing set closest to $t_m\mathbf{1}$. +3. **Upper bound** $(0, 150)$: the plain closed form asks for $205$ on the top cable. Clamping it at $200$ and re-solving the two lower cables gives $(200, 50, 50)$, which is also the exact optimum. +4. **Lower bound** $(0, -150)$: the top cable would go slack at $5$. It is clamped to $10$, and the others carry $160$ each. +5. **Infeasible** $(0, 300)$: even at $200$ on the top cable, the lower cables would need negative tension, so no distribution exists. +6. **Spatial boundary case**: on an eight-cable crossed platform, a wrench near the workspace boundary defeats the improved closed form. The exact QP finds tensions with cable 0 at $t_{\max}$ and cable 3 at $t_{\min}$. + +## Pitfalls & Edge Cases + +- **Slack cables**: $t_{\min}$ must be positive. A slack cable leaves the platform under-constrained and lets it swing. +- **Degenerate geometry**: collinear or coincident cables make $AA^T$ singular. The distribution then reports no solution rather than infinite tensions. +- **Zero-length cables**: a platform anchor on its base anchor has no direction and is rejected. +- **Wrench reference point**: the wrench is about the platform origin in the world frame. Include gravity as $-m g$ plus the dynamic wrench from the motion controller. +- **Boundary behaviour**: call the exact path only when the closed form fails, since it is slower and its cost varies. +- **Cable sag and elasticity**: the massless straight-line model ignores sagging long cables. Use catenary models for large installations. + +## Variants & Generalizations + +- **Puncture and barycentric methods**: alternative closed-form choices inside the feasible polytope. +- **Linear-programming distributions**: minimum-norm or minimum-maximum tensions for energy or safety criteria. +- **Stiffness-optimal tensions**: tension choices that maximize platform stiffness. +- **Dynamic workspace analysis**: wrench-feasible workspace computation from the same structure matrix. + +## Applications + +- Large-volume camera systems and robotic cranes. +- Cable-driven rehabilitation and haptic devices. +- Large 3D printers, façade cleaning and warehouse automation. + +## Connections to Other Algorithms + +- [Parallel Manipulator Kinematics](../../kinematics/ParallelManipulatorKinematics.md): the rigid-leg counterparts, whose leg Jacobian plays the role of the structure matrix. +- [Hybrid Position/Force Control](HybridPositionForceControl.md): another controller that splits a wrench between constraints. +- The linear solver and the dual active-set QP come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Pott, A., Bruckmann, T. and Mikelsons, L. (2009). "Closed-form force distribution for parallel wire robots." *Computational Kinematics*, Springer, 25–34. +- Pott, A. (2014). "An improved force distribution algorithm for over-constrained cable-driven parallel robots." *Computational Kinematics*, Springer, 139–146. +- Goldfarb, D. and Idnani, A. (1983). "A numerically stable dual method for solving strictly convex quadratic programs." *Mathematical Programming*, 27, 1–33. +- Pott, A. (2018). *Cable-Driven Parallel Robots: Theory and Application*. Springer. diff --git a/doc/controllers/manipulator/HybridPositionForceControl.md b/doc/controllers/manipulator/HybridPositionForceControl.md new file mode 100644 index 0000000..91dccad --- /dev/null +++ b/doc/controllers/manipulator/HybridPositionForceControl.md @@ -0,0 +1,97 @@ +# Hybrid Position/Force Control + +## Overview & Motivation + +Contact tasks constrain some directions and leave others free. When wiping a table, the tool must press down with a chosen force while sliding freely across the surface. When inserting a peg, it pushes along the hole axis while keeping the lateral force near zero. Pure position control fights the constraint, and pure force control cannot follow a path. + +**Hybrid position/force control** (Raibert and Craig, 1981) splits the task space into complementary subspaces in a **constraint frame** aligned with the contact. A selection matrix assigns each axis either to a position loop or to a force loop. The two loops act on disjoint axes, so they never fight. + +## Mathematical Theory + +### Constraint Frame and Selection + +The rotation $R_c$ maps constraint-frame axes to the base frame. A diagonal selection matrix $S$ with entries 0 or 1 marks motion-controlled axes (1) and force-controlled axes (0). Since $S^2 = S$ and $S(I - S) = 0$, the two subspaces are complementary. + +Base-frame quantities are rotated into the constraint frame: + +$$e_x = R_c^T(x_d \ominus x), \qquad \dot{e}_x = R_c^T(\dot{x}_d - J\dot{q}), \qquad v_c = R_c^T J\dot{q}, \qquad e_f = R_c^T(f_d - f)$$ + +Here $f$ and $f_d$ are the measured and desired wrench the tool applies on the environment. + +### Motion Loop + +A PD law acts on the motion axes: + +$$F_m = K_p e_x + K_d \dot{e}_x$$ + +### Force Loop + +A PI law with feed-forward and velocity damping acts on the force axes: + +$$F_f = R_c^T f_d + K_f e_f + K_i \int (I - S)\,e_f\,dt - K_{df} v_c$$ + +The integral removes steady force error against stiff surfaces. It accumulates only on force axes and is clamped element-wise to $\pm$`integralLimit` (anti-windup). The damping term $-K_{df} v_c$ suppresses oscillation on contact with stiff environments. + +### Joint Torque + +$$F = R_c\left(S F_m + (I - S) F_f\right), \qquad \tau = J^T F + C(q,\dot{q})\dot{q} + g(q)$$ + +Mapping with $J^T$ only (no inverse) keeps the torque bounded through singularities. + +### Stability + +Purely kinematic hybrid control, which maps the selected errors through $J^{-1}$, can be unstable (An and Hollerbach, 1987). This law compensates gravity and Coriolis torques and uses $J^T$. For full dynamic decoupling of the subspaces, combine the selection with the task-space inertia of operational-space control. + +## Complexity Analysis + +| Step | Time | Space | Notes | +|--------------------------|---------------|--------|----------------------------------------------| +| Task velocity and errors | $O(mn + m^2)$ | $O(m)$ | $J\dot{q}$ and constraint-frame rotations | +| Motion and force loops | $O(m^2)$ | $O(m)$ | Gain products and one clamped integral | +| Joint torque | $O(mn)$ | $O(n)$ | Plus one Coriolis and one gravity evaluation | + +## Step-by-Step Walkthrough + +A planar task ($m = 2$) uses $S = \operatorname{diag}(1, 0)$: axis 0 is motion-controlled and axis 1 force-controlled. The gains are $K_p = 100$, $K_d = 20$, $K_f = 2$, $K_i = 5$ and $K_{df} = 3$, with $\Delta t = 1$ ms. + +1. **Motion axis**: a position error of $0.05$ m and a velocity error of $0.2$ m/s on axis 0 give $100 \cdot 0.05 + 20 \cdot 0.2 = 9$ N there and nothing on axis 1. +2. **Force axis**: $f_d = 5$ N, a measured $3$ N and $0.4$ m/s of motion along axis 1 give $5 + 2 \cdot 2 + 5 \cdot 2 \cdot 0.001 - 3 \cdot 0.4 = 7.81$ N. +3. **Cross terms masked**: a position error on the force axis and a force error on the motion axis produce no torque at all. +4. **Rotated constraint**: with $R_c = \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix}$, the base-$y$ position error drives the motion loop, the base-$x$ error falls on the force axis and is ignored, and the force loop pushes along base $-x$. The resulting wrench is $(-9.01, 10)$ N. +5. **Anti-windup**: a persistent $10$ N force error saturates the integral at its limit of $1$, so the force term settles at $10 + 2 \cdot 10 + 5 \cdot 1 = 35$ N. + +## Pitfalls & Edge Cases + +- **Wrong constraint frame**: a misaligned $R_c$ lets the motion loop push into the surface. Align it with the measured contact normal. +- **Contact loss**: when the tool leaves the surface, the force error stays large and the integral winds up to its limit. Call `Reset()` on contact transitions. +- **Stiff environments**: high force gains on stiff contacts oscillate. Use velocity damping and keep $K_f$ low, with a low-pass-filtered force signal. +- **Non-binary selection**: the selection matrix must be diagonal with 0/1 entries; this is checked at construction. +- **Sensor frame**: express the measured wrench in the base frame, about the tool point, before passing it in. + +## Variants & Generalizations + +- **Dynamic hybrid control**: operational-space formulation with selection in task-inertia-weighted subspaces (Khatib, 1987). +- **Parallel force/position control**: both loops act on all axes, with the force loop dominating through an integral term. +- **Impedance control**: a single compliant relationship instead of separate subspaces. +- **Explicit contact models**: switching between free and constrained motion with contact detection. + +## Applications + +- Polishing, grinding and deburring with controlled normal force. +- Peg-in-hole and connector insertion. +- Surface following and contour tracing. +- Pushing, scraping and cleaning tasks. + +## Connections to Other Algorithms + +- [Impedance Control](ImpedanceControl.md): the compliant alternative; its contact wrench convention is the opposite sign. +- [Operational-Space Control](OperationalSpaceControl.md): adds task-space inertia for dynamically decoupled subspaces. +- [Geometric Jacobian](../../kinematics/GeometricJacobian.md): supplies $J$ and the tool pose. +- [Chain Dynamics Model](../../dynamics/ChainDynamicsModel.md): supplies the Coriolis and gravity torques. + +## References & Further Reading + +- Raibert, M.H. and Craig, J.J. (1981). "Hybrid position/force control of manipulators." *ASME Journal of Dynamic Systems, Measurement, and Control*, 103(2), 126–133. +- An, C.H. and Hollerbach, J.M. (1987). "Kinematic stability issues in force control of manipulators." *IEEE International Conference on Robotics and Automation*, 897–903. +- Khatib, O. (1987). "A unified approach for motion and force control of robot manipulators: the operational space formulation." *IEEE Journal on Robotics and Automation*, 3(1), 43–53. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 9. diff --git a/doc/controllers/manipulator/OperationalSpaceControl.md b/doc/controllers/manipulator/OperationalSpaceControl.md new file mode 100644 index 0000000..dcdd584 --- /dev/null +++ b/doc/controllers/manipulator/OperationalSpaceControl.md @@ -0,0 +1,94 @@ +# Operational-Space Control + +## Overview & Motivation + +Many tasks are naturally specified at the tool: follow a weld seam, hold a camera on a target, press with a given stiffness. Joint-space controllers must first convert such a task into joint trajectories through inverse kinematics, and they still do not shape how the tool **responds** to errors and disturbances. + +**Operational-space control** (Khatib, 1987) writes the manipulator dynamics directly in task coordinates. It commands a task acceleration through the **task-space inertia** $\Lambda(q)$, so the tool behaves like a decoupled, unit-mass second-order system along every task axis. +On redundant arms, a **dynamically consistent null-space projector** lets a secondary objective (posture, joint-limit avoidance) act through the remaining freedom without disturbing the task at all. + +## Mathematical Theory + +### Task-Space Dynamics + +With the task Jacobian $J$ ($m \times n$), $\dot{x} = J\dot{q}$ and $\ddot{x} = J\ddot{q} + \dot{J}\dot{q}$. Substituting the joint dynamics $M\ddot{q} + C\dot{q} + g = \tau$ with $\tau = J^T F$ gives + +$$\Lambda(\ddot{x} - \dot{J}\dot{q}) = F - J^{T\#}(C\dot{q} + g), \qquad \Lambda = (J M^{-1} J^T)^{-1}$$ + +### Control Law + +The task command is a PD law with feed-forward: + +$$a_x = \ddot{x}_d + K_d(\dot{x}_d - J\dot{q}) + K_p\,e, \qquad e = x_d \ominus x$$ + +and the joint torque is + +$$\tau = J^T\Lambda\left(a_x - \dot{J}\dot{q}\right) + C\dot{q} + g + N\tau_0, \qquad N = I - J^T\bar{J}^T, \quad \bar{J} = M^{-1}J^T\Lambda$$ + +Here $\tau_0$ is a secondary joint torque. Substituting gives $\ddot{x} = a_x$ exactly: the error dynamics are $\ddot{e} + K_d\dot{e} + K_p e = 0$ per axis, independent of the arm's configuration. + +### Why Compensate Coriolis and Gravity in Joint Space + +The classic form $F = \Lambda(a_x - \dot{J}\dot{q}) + \bar{J}^T(C\dot{q} + g)$ compensates only the task-space part $J^T\bar{J}^T(C\dot{q} + g)$. On a redundant arm the null-space part $N(C\dot{q} + g)$ is left uncompensated, so the self-motion sags under gravity. +Adding $C\dot{q} + g$ in joint space equals the classic form plus that missing term. The task behaviour is unchanged, and at rest with zero error the torque is exactly $g$. + +### Dynamic Consistency + +$J M^{-1} N = 0$, so the secondary torque produces no task acceleration: $\tau_0$ only moves the arm within the null space of the task. + +### Singularities + +$\Lambda$ diverges when $J$ loses rank. A damping term $\sigma$ regularizes it as $\Lambda = (J M^{-1} J^T + \sigma^2 I)^{-1}$. The torque stays bounded, at the price of approximate task tracking and approximate dynamic consistency. + +All inverses are applied through linear solves: first $X = M^{-1}J^T$, then the small $m \times m$ system for $\Lambda$. $M^{-1}$ is never formed. + +## Complexity Analysis + +| Step | Time | Space | Notes | +|------------------------|---------------|----------|---------------------------------------------------------| +| $M^{-1}J^T$ | $O(m n^3)$ | $O(n^2)$ | Column-wise elimination; $O(n^3 + m n^2)$ with Cholesky | +| Task inertia $\Lambda$ | $O(m^3)$ | $O(m^2)$ | One small solve | +| Null-space projector | $O(n^2 m)$ | $O(n^2)$ | $I - J^T\Lambda X^T$ | +| Torque | $O(nm + n^2)$ | $O(n)$ | Plus one mass-matrix, Coriolis and gravity evaluation | + +## Step-by-Step Walkthrough + +1. **Unit inertia**: with $M = I$ and $J = I$, $\Lambda = I$ and the controller reduces to task-space PD, $\tau = \ddot{x}_d + K_d\dot{e} + K_p e$. +2. **Task inertia**: with $M = \operatorname{diag}(2, 3)$ and $J = I$, $\Lambda = \operatorname{diag}(2, 3)$ scales the command axis by axis. +3. **Non-square mapping**: with $M = I$ and $J = \begin{bmatrix} 1 & 0 \\ 1 & 2 \end{bmatrix}$, a commanded task acceleration $(1, 2)$ becomes the joint torque $J^{-1}(1, 2) = (1, 0.5)$. +4. **Redundant arm at rest**: a three-joint arm with a two-dimensional task, $M = \begin{bmatrix} 3 & 0.5 & 0.2 \\ 0.5 & 2 & 0.3 \\ 0.2 & 0.3 & 1 \end{bmatrix}$ and gravity torque $(4, -2, 1.5)$, returns exactly $(4, -2, 1.5)$. The classic $\bar{J}^T g$ form would return $(3.34, 0.27, -0.31)$ and let the arm sag. +5. **Secondary torque**: $\tau_0 = (1, -2, 0.5)$ changes the torque by $N\tau_0 = (0.389, -1.328, 1.062)$, and $J M^{-1} N\tau_0 = 0$: the task does not see it. + +## Pitfalls & Edge Cases + +- **Singularities**: with $\sigma = 0$ a rank-deficient Jacobian makes the task-inertia solve fail. Use $\sigma > 0$ near singular configurations. +- **Model errors**: $\Lambda$, $C\dot{q}$ and $g$ come from the model, so mismatch couples the task axes. The PD gains keep the loop stable, but decoupling degrades. +- **Orientation tasks**: the rotation-vector error rate equals $\omega_d - \omega$ only to first order, so large orientation errors are not exactly decoupled. +- **Cost**: the $M^{-1}J^T$ solve dominates. Factor $M$ once (Cholesky) and cache $\Lambda$ when the task dimension is small. +- **Secondary objectives**: $N\tau_0$ is a torque, not a velocity. Design $\tau_0$ as a joint-space PD on the posture, for example. + +## Variants & Generalizations + +- **Hierarchical (prioritized) control**: several tasks projected into each other's null spaces in priority order. +- **Whole-body control**: floating-base and contact-constrained operational-space formulations for humanoids. +- **Unified force/motion control**: force regulation along constrained directions inside the same framework. +- **Impedance shaping**: using $\Lambda$ to render a chosen task-space inertia instead of unit mass. + +## Applications + +- Task-space trajectory tracking on redundant arms with posture control. +- Humanoid and mobile-manipulator whole-body control. +- Haptic and teleoperation interfaces that need decoupled task dynamics. + +## Connections to Other Algorithms + +- [Impedance Control](ImpedanceControl.md): shares the task error and the task-space inertia solve; renders compliance instead of decoupled tracking. +- [Computed-Torque Control](ComputedTorqueControl.md): the joint-space counterpart of feedback linearization. +- [Redundancy Resolution](../../kinematics/RedundancyResolution.md): kinematic null-space projection, the velocity-level analogue. +- [Chain Dynamics Model](../../dynamics/ChainDynamicsModel.md): supplies $M$, $C\dot{q}$ and $g$. + +## References & Further Reading + +- Khatib, O. (1987). "A unified approach for motion and force control of robot manipulators: the operational space formulation." *IEEE Journal on Robotics and Automation*, 3(1), 43–53. +- Nakanishi, J., Cory, R., Mistry, M., Peters, J. and Schaal, S. (2008). "Operational space control: a theoretical and empirical comparison." *International Journal of Robotics Research*, 27(6), 737–757. +- Siciliano, B. and Khatib, O. (eds.) (2016). *Springer Handbook of Robotics*. Springer. Chapter 8. diff --git a/doc/controllers/manipulator/README.md b/doc/controllers/manipulator/README.md index 74fe025..aab65a4 100644 --- a/doc/controllers/manipulator/README.md +++ b/doc/controllers/manipulator/README.md @@ -4,8 +4,12 @@ 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$ | -| [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 | +| 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 | +| [Operational-Space Control](OperationalSpaceControl.md) | Decoupled task-space tracking through $\Lambda(q)$, joint-space Coriolis/gravity compensation and a dynamically consistent null space for secondary torques | +| [Hybrid Position/Force Control](HybridPositionForceControl.md) | Constraint-frame selection between a motion PD and a damped force PI with anti-windup, mapped through $J^T$ with gravity and Coriolis compensation | +| [Slotine–Li Adaptive Control](SlotineLiAdaptiveControl.md) | Globally convergent tracking with unknown inertial parameters: regressor feedforward, sliding-variable damping and the passivity-based update $\dot{\hat{a}} = -\Gamma Y^T s$ | +| [Cable Tension Distribution](CableTensionDistribution.md) | Structure matrix from cable geometry, Pott\'s improved closed-form tension distribution within $[t_{\min}, t_{\max}]$, and an exact QP path at the workspace boundary | diff --git a/doc/controllers/manipulator/SlotineLiAdaptiveControl.md b/doc/controllers/manipulator/SlotineLiAdaptiveControl.md new file mode 100644 index 0000000..7c6a873 --- /dev/null +++ b/doc/controllers/manipulator/SlotineLiAdaptiveControl.md @@ -0,0 +1,94 @@ +# Slotine–Li Adaptive Control + +## Overview & Motivation + +Model-based controllers such as computed torque cancel the arm's dynamics with a model. When the masses and inertias are wrong (an unknown payload, a tool change, worn cables), the cancellation is imperfect and tracking errors appear. + +The **Slotine–Li adaptive controller** (1987) removes the need for accurate parameters. It uses the fact that the rigid-body dynamics are linear in the inertial parameters, and it adjusts a parameter estimate online, driven by the tracking error. The tracking error converges to zero **globally** for any initial estimate, while the parameters stay bounded. +It needs no joint-acceleration measurement and no inversion of the estimated mass matrix. It is the classic example of passivity-based adaptive control. + +## Mathematical Theory + +### Sliding Variable and Reference Motion + +For a desired trajectory $q_d(t)$ with errors $\tilde{q} = q - q_d$ and $\dot{\tilde{q}} = \dot{q} - \dot{q}_d$, define + +$$s = \dot{\tilde{q}} + \Lambda\tilde{q}, \qquad \dot{q}_r = \dot{q}_d - \Lambda\tilde{q}, \qquad \ddot{q}_r = \ddot{q}_d - \Lambda\dot{\tilde{q}}$$ + +so that $s = \dot{q} - \dot{q}_r$. With $\Lambda$ positive definite, $s \to 0$ implies $\tilde{q} \to 0$ exponentially. The reference acceleration $\ddot{q}_r$ is computed from velocities, so no acceleration is ever measured. + +### Linear Parametrization + +With $C$ in Christoffel form, the dynamics evaluated on the reference motion are linear in the parameter vector $a$: + +$$M(q)\ddot{q}_r + C(q,\dot{q})\dot{q}_r + g(q) = Y(q, \dot{q}, \dot{q}_r, \ddot{q}_r)\,a$$ + +### Control and Adaptation Laws + +$$\tau = Y\hat{a} - K_d\,s, \qquad \dot{\hat{a}} = -\Gamma\,Y^T s$$ + +The update is integrated with forward Euler at the control period $\Delta t$. + +### Stability + +With the estimation error $\tilde{a} = \hat{a} - a$, the closed loop is $M\dot{s} + Cs + K_d s = Y\tilde{a}$. The Lyapunov function + +$$V = \tfrac12 s^T M s + \tfrac12 \tilde{a}^T\Gamma^{-1}\tilde{a}$$ + +has, using the skew-symmetry of $\dot{M} - 2C$, + +$$\dot{V} = -s^T K_d s \le 0$$ + +So $s$ and $\tilde{a}$ stay bounded, and Barbalat's lemma gives $s \to 0$ and therefore $\tilde{q} \to 0$. The parameters converge to their true values only under persistent excitation. The proof depends on the Christoffel factorization: any other $C$ breaks the skew-symmetry. + +## Complexity Analysis + +| Step | Time | Space | Notes | +|----------------------|----------------------|----------------|-----------------------------------------------------------| +| Errors and reference | $O(n^2)$ | $O(n)$ | Products with $\Lambda$ | +| Regressor | $O(n \cdot p)$ | $O(n \cdot p)$ | Supplied by the injected regressor, $p = 10n$ for a chain | +| Torque and update | $O(n \cdot p + p^2)$ | $O(p)$ | $Y\hat{a}$, $Y^T s$ and the $\Gamma$ product | + +## Step-by-Step Walkthrough + +A horizontal planar two-link arm has three base parameters $a = (a_1, a_2, a_3)$, with $M = \begin{bmatrix} a_1 + 2a_2 c_2 & a_3 + a_2 c_2 \\ a_3 + a_2 c_2 & a_3 \end{bmatrix}$. The true values are $a = (3, 0.5, 0.6)$, and the controller starts from $\hat{a} = 0$ with $\Lambda = 5I$, $K_d = 20I$, $\Gamma = I$ and $\Delta t = 1$ ms. + +1. **Errors**: at $\tilde{q} = (0.05, -0.1)$ and $\dot{\tilde{q}} = (0.1, -0.2)$, the sliding variable is $s = (0.35, -0.7)$. The reference velocity is $\dot{q}_d - \Lambda\tilde{q}$. +2. **First step**: with $\hat{a} = 0$ the torque is pure damping, $-K_d s$. The estimate moves by $-\Gamma Y^T s\,\Delta t$. +3. **Tracking**: following $q_d = (0.5\sin t, 0.3\cos 2t)$ from an initial offset of $(0.2, -0.1)$ rad, the tracking error falls below $10^{-2}$ rad within 10 s. +4. **Parameters**: the estimate of $a_1$ is still more than $0.1$ away from its true value. Tracking converges without parameter convergence. + +## Pitfalls & Edge Cases + +- **Wrong Coriolis factorization**: a regressor built from an arbitrary $C$ satisfying $C\dot{q}$ invalidates the proof. Use the Christoffel-consistent regressor. +- **Parameter drift**: measurement noise and unmodelled dynamics make $\hat{a}$ wander. Add projection onto a physically valid set, a dead zone on $s$, or $\sigma$-modification. +- **Large adaptation gain**: a high $\Gamma$ makes the estimate oscillate and can excite unmodelled flexible modes. +- **No persistent excitation**: the estimate settles on a manifold of parameters consistent with the data. It does not identify the true values. +- **Real-time cost**: the regressor is $n \times 10n$. Reduce to base parameters on embedded targets. + +## Variants & Generalizations + +- **Composite adaptation**: combine the tracking-error update with a prediction-error update for faster parameter convergence. +- **Robust adaptive control**: projection, dead zones and leakage terms for bounded disturbances. +- **Task-space Slotine–Li**: the same construction with a task-space sliding variable. +- **Physically consistent adaptation**: geodesic or Bregman-divergence updates that keep the estimate physical. + +## Applications + +- Tracking with unknown or changing payloads. +- Manipulators whose CAD inertial parameters are unreliable. +- Online compensation of slow dynamic changes such as cable drag. + +## Connections to Other Algorithms + +- [Computed-Torque Control](ComputedTorqueControl.md): the non-adaptive counterpart with a fixed model. +- [Coriolis Matrix and Inertial Regressor](../../dynamics/CoriolisMatrixAndRegressor.md): supplies the Christoffel-consistent regressor $Y$. +- [Dynamic Parameter Identification](../../dynamics/DynamicParameterIdentification.md): offline estimation of the same parameters, a good initial $\hat{a}$. +- [PD Control with Gravity Compensation](PdGravityCompensation.md): another passivity-based law, for set-points. + +## References & Further Reading + +- Slotine, J.-J.E. and Li, W. (1987). "On the adaptive control of robot manipulators." *International Journal of Robotics Research*, 6(3), 49–59. +- Slotine, J.-J.E. and Li, W. (1991). *Applied Nonlinear Control*. Prentice Hall. Chapter 9. +- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2006). *Robot Modeling and Control*. Wiley. Chapter 8. +- Ortega, R. and Spong, M.W. (1989). "Adaptive motion control of rigid robots: a tutorial." *Automatica*, 25(6), 877–888. diff --git a/doc/kinematics/AnalyticalIkOpw.md b/doc/kinematics/AnalyticalIkOpw.md new file mode 100644 index 0000000..7392e0e --- /dev/null +++ b/doc/kinematics/AnalyticalIkOpw.md @@ -0,0 +1,98 @@ +# Analytical Inverse Kinematics (OPW) + +## Overview & Motivation + +Most industrial six-axis arms (KUKA, ABB, Fanuc, Stäubli, Yaskawa) share one layout: axes 2 and 3 are parallel to each other and orthogonal to axis 1, and the last three axes intersect in a **spherical wrist**. For this ortho-parallel layout the inverse kinematics has a closed form: every solution is found in constant time, with no iteration, no seed and no convergence failure. + +The **OPW** formulation (Brandstötter, Angerer and Hofbaur, 2014) describes the arm with seven lengths plus a sign and an offset per joint, read straight from the datasheet. No Denavit–Hartenberg table or frame convention is involved. +It returns up to **eight** configurations: two shoulder poses, two elbow poses and two wrist poses. The caller picks one, typically the closest to the current joint values. + +## Mathematical Theory + +### Geometric Model + +In the model zero pose, the upper arm and forearm point along $+z$. The parameters are: + +| Parameter | Meaning | +|-----------|---------------------------------------------| +| $a_1$ | Radial offset from axis 1 to axis 2 | +| $a_2$ | Forearm offset perpendicular to the forearm | +| $b$ | Lateral offset of the arm plane | +| $c_1$ | Height of axis 2 above the base | +| $c_2$ | Upper-arm length, axis 2 to axis 3 | +| $c_3$ | Forearm length, axis 3 to the wrist centre | +| $c_4$ | Wrist centre to flange, along axis 6 | + +Joint values map to model angles by $\theta_i = s_i q_i - o_i$, with sign $s_i = \pm 1$ and offset $o_i$. The forward kinematics is + +$$T(q) = R_z(\theta_1)\,\mathrm{Tr}(a_1, b, c_1)\,R_y(\theta_2)\,\mathrm{Tr}(0, 0, c_2)\,R_y(\theta_3)\,\mathrm{Tr}(a_2, 0, c_3)\,R_z(\theta_4)\,R_y(\theta_5)\,R_z(\theta_6)\,\mathrm{Tr}(0, 0, c_4)$$ + +### Decoupling at the Wrist Centre + +The spherical wrist decouples position from orientation. The wrist centre $C = p - c_4 R\hat{z}$ depends only on $\theta_1, \theta_2, \theta_3$. With the virtual forearm $k = \sqrt{a_2^2 + c_3^2}$ and its angle $\psi_3 = \operatorname{atan2}(a_2, c_3)$: + +1. **Shoulder.** With $n = \sqrt{C_x^2 + C_y^2 - b^2} - a_1$, the front and back solutions are $\theta_1 = \operatorname{atan2}(C_y, C_x) \mp \operatorname{atan2}(b, n + a_1)$, the back one shifted by $\pi$. +2. **Elbow.** In the arm plane, the distances $s_1^2 = n^2 + z^2$ (front) and $s_2^2 = (n + 2a_1)^2 + z^2$ (back), with $z = C_z - c_1$, close triangles with sides $c_2$ and $k$. The law of cosines gives the angles $\beta$ at the shoulder and $\gamma$ at the elbow, and elbow up and down follow from $\pm\beta$, $\pm\gamma$. +3. **Wrist.** With the arm fixed, $W = (R_z(\theta_1)R_y(\theta_2 + \theta_3))^T R = R_z(\theta_4)R_y(\theta_5)R_z(\theta_6)$, a ZYZ Euler decomposition: + +$$\theta_4 = \operatorname{atan2}(W_{12}, W_{02}), \qquad \theta_5 = \operatorname{atan2}\left(\sqrt{W_{02}^2 + W_{12}^2},\ W_{22}\right), \qquad \theta_6 = \operatorname{atan2}(W_{21}, -W_{20})$$ + +The flipped wrist $(\theta_4 + \pi, -\theta_5, \theta_6 - \pi)$ gives the same orientation. + +### Solution Slots + +Slots 0–3 hold the four arm branches with $\theta_5 \ge 0$: shoulder front with elbow a/b, then shoulder back with elbow a/b. Slots 4–7 hold the same arms with the wrist flipped. A branch whose cosine argument exceeds one is unreachable and left empty. + +## Complexity Analysis + +| Step | Time | Space | Notes | +|------------------------|--------|--------|----------------------------------------------------| +| Wrist centre, shoulder | $O(1)$ | $O(1)$ | Two `atan2`, one square root | +| Elbow branches | $O(1)$ | $O(1)$ | Four clamped `acos` | +| Wrist per arm branch | $O(1)$ | $O(1)$ | One 3×3 product and three `atan2` | +| All eight solutions | $O(1)$ | $O(1)$ | Fixed work; no iteration, Jacobian or linear solve | + +## Step-by-Step Walkthrough + +A KUKA KR6 R700 sixx has $a_1 = 0.025$, $a_2 = -0.035$, $b = 0$, $c_1 = 0.400$, $c_2 = 0.315$, $c_3 = 0.365$ and $c_4 = 0.080$ m. Its offsets are $(0, -\pi/2, 0, 0, 0, 0)$ and its signs $(-1, 1, 1, -1, 1, -1)$. + +1. **Home pose**: $q = 0$ puts the flange at $(0.785, 0, 0.435)$ m with the tool axis along $+x$. +2. **Target**: $q^\ast = (0.3, -1.0, 1.2, 0.5, 0.6, 0.7)$ puts the flange at $(0.583, -0.203, 0.575)$ m. +3. **Eight solutions**: slot 0 recovers $q^\ast$, and slot 1 is the other elbow, $(0.3, 0.198, -1.009, 0.274, 1.553, 1.118)$. Slots 2 and 3 are the back-shoulder pair with $q_1 = -2.842$. Slots 4–7 flip the wrist. All eight reproduce the target pose. +4. **Wrist singularity**: with $q_5 = 0$, axes 4 and 6 align, so only $\theta_4 + \theta_6$ is determined. The solver returns $q_4 = 0$ and $q_6 = 1.2$, flags the solution as wrist-singular, and leaves the flipped slot empty. +5. **Unreachable target**: a flange at $(1.5, 0, 0.4)$ m is beyond reach, so every slot is empty. + +## Pitfalls & Edge Cases + +- **Layout assumption**: the formulas hold only for ortho-parallel arms with a spherical wrist. Collaborative arms with offset wrists (UR-type) need different closed forms. +- **Precision near singularities**: at full stretch, $\operatorname{acos}$ near $\pm 1$ amplifies rounding to about $\sqrt{2\varepsilon}$, roughly $5 \times 10^{-4}$ rad in single precision. Close to the wrist singularity, $\theta_4$ and $\theta_6$ are individually ill-conditioned while their sum stays accurate. The pose is reproduced in both cases. +- **Wrist singularity**: computing $\sin\theta_5$ as $\sqrt{1 - W_{22}^2}$ loses precision near zero. The norm of $(W_{02}, W_{12})$ keeps it accurate, so the singular branch triggers only at rounding level. +- **Joint limits**: they are not applied. Filter the solutions and choose, for example, the one nearest the current configuration. +- **Flange versus tool**: the target is the flange pose. For a tool, solve for $T_{\text{target}}\,T_{\text{tool}}^{-1}$. +- **Shoulder singularity**: when the wrist centre lies on axis 1 (with $b = 0$), any $\theta_1$ works and $\theta_1 = 0$ is returned. + +## Variants & Generalizations + +- **Offset-wrist arms**: UR-type closed forms with up to eight solutions from different geometry. +- **Seven-axis arms**: an arm-angle parameterization reduces redundant arms (e.g. KUKA iiwa) to closed form per elbow angle. +- **Numerical fallback**: damped least squares for arms without a closed form, seeded with an analytical solution when one exists. +- **Solution selection**: weighting joint distance, limits and singularity margin when choosing among the branches. + +## Applications + +- Real-time Cartesian jogging and teleoperation on industrial controllers. +- Offline programming and path planning that must enumerate all configurations. +- Seeding numerical IK and checking reachability. + +## Connections to Other Algorithms + +- [Pose Inverse Kinematics](PoseInverseKinematics.md): the iterative alternative for arbitrary geometries. +- [Denavit-Hartenberg Parameters](DenavitHartenberg.md): the frame-based description that OPW avoids. +- [Manipulability Index](ManipulabilityIndex.md): measures how close each returned configuration is to a singularity. +- The angle wrapping and SE(3) types come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Brandstötter, M., Angerer, A. and Hofbaur, M. (2014). "An analytical solution of the inverse kinematics problem of industrial serial manipulators with an ortho-parallel basis and a spherical wrist." *Proceedings of the Austrian Robotics Workshop*, 7–11. +- Pieper, D.L. (1968). *The Kinematics of Manipulators under Computer Control*. PhD thesis, Stanford University. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 2. diff --git a/doc/kinematics/ContinuumKinematics.md b/doc/kinematics/ContinuumKinematics.md new file mode 100644 index 0000000..79e997f --- /dev/null +++ b/doc/kinematics/ContinuumKinematics.md @@ -0,0 +1,93 @@ +# Continuum Kinematics (Constant Curvature) + +## Overview & Motivation + +Continuum robots have no discrete joints: tendon-driven endoscopes, concentric-tube needles, pneumatic soft arms and elephant-trunk manipulators bend continuously along their length. Rigid-link kinematics does not apply, because there is no finite set of joint axes. + +The **piecewise constant-curvature** model describes each section as a circular arc, set by three numbers: curvature $\kappa$, bending-plane angle $\phi$ and arc length $s$. It captures the shape of tendon- and pressure-driven sections well, and gives closed-form forward and single-section inverse kinematics. +It also separates the **robot-independent** map $(\kappa, \phi, s) \to SE(3)$ from the **robot-specific** map from actuator values (tendon lengths, chamber pressures) to arc parameters. Only the latter changes between designs. + +## Mathematical Theory + +### One Section + +The section starts at its base frame with tangent $\hat{z}$ and bends by a total angle $\theta = \kappa s$ in the plane at angle $\phi$ about $\hat{z}$. The tip pose is + +$$R = R_z(\phi)\,R_y(\theta)\,R_z(-\phi), \qquad p = s\left(f_1(\theta)\begin{bmatrix}\cos\phi \\ \sin\phi \\ 0\end{bmatrix} + f_2(\theta)\,\hat{z}\right)$$ + +with + +$$f_1(\theta) = \frac{1 - \cos\theta}{\theta} = \frac{2\sin^2(\theta/2)}{\theta}, \qquad f_2(\theta) = \frac{\sin\theta}{\theta}$$ + +$R$ rotates by $\theta$ about the bending axis $(-\sin\phi, \cos\phi, 0)$. Its third column is the tip tangent, equal to $dp/ds$ at the tip. + +### Straight Limit + +The textbook form $p = \frac{1}{\kappa}(1 - \cos\theta)\ldots$ divides by zero for a straight section. Written as $s\,f(\theta)$, the formula stays finite. Below $|\theta| = 10^{-2}$ the series + +$$f_1 \approx \frac{\theta}{2} - \frac{\theta^3}{24}, \qquad f_2 \approx 1 - \frac{\theta^2}{6}$$ + +is accurate to better than $10^{-9}$, and the straight pose $(I, (0, 0, s))$ follows exactly at $\theta = 0$, independent of $\phi$. + +### Multiple Sections + +Sections are stacked by composing their transforms, each expressed in the previous section's tip frame: + +$$T_{\text{tip}} = T_1\,T_2 \cdots T_n$$ + +### Single-Section Inverse + +From a tip position $p$ in the section base frame, with $\rho = \sqrt{p_x^2 + p_y^2}$ and $L = \|p\|$, the chord identity $L = 2\sin(\theta/2)/\kappa$ gives + +$$\phi = \operatorname{atan2}(p_y, p_x), \qquad \theta = 2\operatorname{atan2}(\rho, p_z), \qquad \kappa = \frac{2\rho}{L^2}, \qquad s = L\,\frac{\theta/2}{\sin(\theta/2)}$$ + +The result has $\kappa \ge 0$ and $\phi \in (-\pi, \pi]$. A negative curvature folds into the plane angle: $(-\kappa, \phi) \equiv (\kappa, \phi + \pi)$. The inverse holds for $\kappa s < 2\pi$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|--------------------|--------|--------|-----------------------------------------| +| Section transform | $O(1)$ | $O(1)$ | Two sine/cosine pairs and one 3×3 build | +| Forward kinematics | $O(n)$ | $O(1)$ | One SE(3) product per section | +| Single-section IK | $O(1)$ | $O(1)$ | Two `atan2`, two square roots, one sine | + +## Step-by-Step Walkthrough + +1. **Straight section** $(\kappa, \phi, s) = (0, 0, 1)$: pure translation to $(0, 0, 1)$. +2. **Quarter circle** $(\pi/2, 0, 1)$: radius $2/\pi$, so the tip is at $(0.6366, 0, 0.6366)$ with its tangent along $\hat{x}$. The chord is $2\sqrt{2}/\pi = 0.9003$. +3. **Bending plane** $\phi = \pi/2$: the same arc swings into the $yz$ plane, with the tip at $(0, 0.6366, 0.6366)$. +4. **General section** $(2, 0.7, 1.2)$, $\theta = 2.4$: tip $(0.6644, 0.5596, 0.3377)$, chord $\sin(1.2) = 0.9320$, tangent $(0.5166, 0.4351, -0.7374)$. Two such sections stacked reach $(0.3490, 0.2939, -0.4981)$. +5. **Inverse**: the tip of $(-1.5, 0.3, 0.8)$ returns $(1.5,\ 0.3 - \pi,\ 0.8)$, the same arc with the sign folded into $\phi$. + +## Pitfalls & Edge Cases + +- **Division by curvature**: the $1/\kappa$ form fails for straight sections. Use the $s\,f(\theta)$ form with its series. +- **Undefined plane angle**: for a straight tip, $\phi$ is arbitrary. The inverse returns $\phi = 0$ and $s = p_z$. +- **Full loops**: at $\theta = 2\pi$ the tip returns to the base, and the inverse is ambiguous. Keep $\kappa s < 2\pi$. +- **Model error**: gravity, tip loads and friction bend real sections off the arc. Use Cosserat-rod models when accuracy matters under load. +- **Multi-section inverse**: no closed form exists. Iterate on $(\kappa_i, \phi_i, s_i)$ with damped least squares. + +## Variants & Generalizations + +- **Variable curvature**: Cosserat-rod and Kirchhoff-rod models with distributed loads. +- **Concentric-tube robots**: precurved tubes rotated and translated relative to each other, with torsional coupling. +- **Actuator maps**: tendon lengths $\ell_i = s - \kappa\,r\cos(\phi - \alpha_i)$ for tendons at radius $r$ and angle $\alpha_i$. +- **Velocity kinematics**: the Jacobian of the arc map, for resolved-rate control of continuum sections. + +## Applications + +- Minimally invasive surgical instruments and steerable catheters. +- Soft robotic arms and grippers driven by pressure. +- Inspection snakes for turbines, pipes and confined spaces. + +## Connections to Other Algorithms + +- [Pose Inverse Kinematics](PoseInverseKinematics.md): the iterative solver pattern for multi-section inverse kinematics. +- [Product of Exponentials](ProductOfExponentials.md): the rigid-link counterpart built on the same SE(3) composition. +- The rotation and SE(3) composition come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Webster, R.J. III and Jones, B.A. (2010). "Design and kinematic modeling of constant curvature continuum robots: a review." *International Journal of Robotics Research*, 29(13), 1661–1683. +- Burgner-Kahrs, J., Rucker, D.C. and Choset, H. (2015). "Continuum robots for medical applications: a survey." *IEEE Transactions on Robotics*, 31(6), 1261–1280. +- Rucker, D.C. and Webster, R.J. III (2011). "Statics and dynamics of continuum robots with general tendon routing and external loading." *IEEE Transactions on Robotics*, 27(6), 1033–1044. diff --git a/doc/kinematics/ParallelManipulatorKinematics.md b/doc/kinematics/ParallelManipulatorKinematics.md new file mode 100644 index 0000000..0d57348 --- /dev/null +++ b/doc/kinematics/ParallelManipulatorKinematics.md @@ -0,0 +1,106 @@ +# Parallel Manipulator Kinematics (Stewart–Gough and Delta) + +## Overview & Motivation + +A **parallel manipulator** connects its moving platform to the base through several kinematic chains at once. Load is shared between the legs, so parallel robots are stiff, light and fast: flight simulators and machining hexapods use the **Stewart–Gough platform**, and pick-and-place lines use the **Delta robot** at several hundred picks per minute. + +Their kinematics mirror serial arms. **Inverse** kinematics, from platform pose to actuator values, is easy: each leg is independent. **Forward** kinematics is hard in general, with many assembly modes and no closed form for the hexapod. +This module provides closed-form inverse kinematics for both mechanisms, the hexapod's leg Jacobian with Newton forward kinematics on SE(3), and closed-form forward kinematics for the Delta. + +## Mathematical Theory + +### Stewart–Gough Platform + +Six prismatic legs connect base anchors $b_i$ (base frame) to platform anchors $p_i$ (platform frame). For a platform pose $(R, p)$: + +$$\ell_i = \left\| R\,p_i + p - b_i \right\|$$ + +**Leg Jacobian.** With the unit leg vector $n_i$ and the rotated anchor $a_i = R\,p_i$, the leg rates for a platform twist $(v; \omega)$, expressed in the base frame at the platform origin, are + +$$\dot{\ell}_i = n_i^T v + (a_i \times n_i)^T \omega, \qquad \dot{\ell} = J\begin{bmatrix} v \\ \omega \end{bmatrix}$$ + +$J$ is the platform's inverse Jacobian. By statics, $J^T$ maps leg forces to the platform wrench. + +**Forward kinematics.** Given measured lengths $L$, solve $\ell(T) - L = 0$ for the pose with Newton's method on SE(3). Each step is $\delta = -J^{-1}(\ell(T) - L)$, retracted as + +$$R \leftarrow \exp([\delta_\omega]_\times)\,R, \qquad p \leftarrow p + \delta_v$$ + +A damping $\lambda$ turns the step into Levenberg–Marquardt, $\delta = -J^T(JJ^T + \lambda^2 I)^{-1}(\ell - L)$, which stays finite where $J$ is singular. A hexapod has up to 40 assembly modes, and the initial guess selects which one is found. + +### Delta Robot + +Three revolute shoulders at azimuths $\phi_i = 0°, 120°, 240°$ on a base circle of radius $R$ drive upper arms of length $r_f$. Parallelogram forearms of length $r_e$ keep the effector parallel to the base, attached at radius $r$ on the effector. The effector therefore translates only. + +**Inverse kinematics.** In arm $i$'s frame, with $X = x' + r - R$, the elbow must lie both on the shoulder circle and on the forearm sphere around the attachment point: + +$$a\cos\theta + b\sin\theta = c, \qquad a = 2r_f X, \quad b = -2r_f z, \quad c = X^2 + y'^2 + z^2 + r_f^2 - r_e^2$$ + +so $\theta = \operatorname{atan2}(b, a) - \arccos\left(c / \sqrt{a^2 + b^2}\right)$, the elbow-out root. The point is unreachable when $|c| > \sqrt{a^2 + b^2}$. + +**Forward kinematics.** Shifting each elbow inward by the effector radius turns the problem into intersecting three spheres of radius $r_e$ centred at + +$$C_i = R_z(\phi_i)\begin{bmatrix} R - r + r_f\cos\theta_i \\ 0 \\ -r_f\sin\theta_i \end{bmatrix}$$ + +Trilateration in the frame spanned by the three centres gives two mirror solutions. The lower one is the physical effector. No solution exists when the spheres do not meet. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------------|------------------|---------|--------------------------------------------| +| Hexapod inverse | $O(1)$ | $O(1)$ | Six vector norms | +| Hexapod leg Jacobian | $O(1)$ | $O(1)$ | Six unit vectors and cross products | +| Hexapod forward | $O(k \cdot 6^3)$ | $O(36)$ | $k$ Newton iterations, one 6×6 solve each | +| Delta inverse | $O(1)$ | $O(1)$ | Three `atan2`/`acos` pairs | +| Delta forward | $O(1)$ | $O(1)$ | Closed-form trilateration, one square root | + +## Step-by-Step Walkthrough + +**Hexapod.** Base anchors sit on a unit circle and platform anchors on a radius-$0.6$ circle, paired at $\pm 15°$ and $\pm 45°$ around three sectors. + +1. At the home pose, platform $0.8$ m above the base, all six legs measure $0.980$ m. Raising it to $0.9$ m lengthens every leg to $1.063$ m. +2. A tilted pose, yaw $0.1$ rad and roll $0.05$ rad at $(0.05, -0.03, 0.85)$, gives legs from $0.956$ to $1.083$ m. +3. Newton forward kinematics recovers that pose from a guess at yaw $0.15$ rad within six iterations. +4. The same home lengths solved from a guess below the base converge to the mirror assembly at $z = -0.8$ m. +5. At yaw $90°$ the leg Jacobian is singular. With damping $\lambda = 10^{-3}$ the leg residual still falls below $10^{-4}$ m, while the yaw itself is poorly determined. + +**Delta.** The geometry is $R = 0.1$, $r = 0.03$, $r_f = 0.2$ and $r_e = 0.45$ m. + +1. With all arms horizontal, the effector hangs on the axis at $z = -\sqrt{0.45^2 - 0.27^2} = -0.36$ m. +2. The point $(0.05, -0.03, -0.40)$ requires arm angles $(0.0425, 0.3712, 0.2057)$ rad. +3. Forward and inverse kinematics round-trip over a grid of $4^3$ arm configurations to $10^{-4}$ rad. +4. The point $(0, 0, -0.9)$ is beyond reach, so the inverse returns no solution. + +## Pitfalls & Edge Cases + +- **Assembly modes**: hexapod forward kinematics converges to the mode nearest the guess. Seed it with the previous cycle's pose. +- **Singularities**: where the leg Jacobian loses rank, the platform gains an uncontrollable motion and Newton steps blow up. Use damping and avoid such poses in the workspace design. +- **Delta branch**: the elbow-out root is selected. Mechanisms built elbow-in need the other root. +- **Reach boundary**: on the boundary, $|c| = \sqrt{a^2 + b^2}$ and the two roots coincide. The arccos argument is clamped against rounding. +- **Effector convention**: the Delta effector lies below the base ($z < 0$). Mounting it upright flips the sign convention. + +## Variants & Generalizations + +- **Other parallel mechanisms**: 3-RPR planar platforms, hexaglides, and the 4-axis Delta with a rotary wrist. +- **Closed-form hexapod forward kinematics**: special geometries with coincident anchors admit polynomial solutions. +- **Dynamics**: virtual-work and Lagrangian formulations built on the leg Jacobian. +- **Calibration**: identification of anchor positions from measured leg lengths and poses. + +## Applications + +- Motion simulators, vibration isolation and precision positioning (hexapods). +- High-speed pick-and-place and packaging (Delta robots). +- Machine-tool spindle heads and telescope secondary-mirror alignment. + +## Connections to Other Algorithms + +- [Pose Inverse Kinematics](PoseInverseKinematics.md): the same Levenberg–Marquardt pattern applied to a serial arm. +- [Geometric Jacobian](GeometricJacobian.md): the serial counterpart of the leg Jacobian. +- [Analytical Inverse Kinematics (OPW)](AnalyticalIkOpw.md): another closed-form solver with several solution branches. +- The SE(3) types and the Levenberg–Marquardt solver come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Stewart, D. (1965). "A platform with six degrees of freedom." *Proceedings of the Institution of Mechanical Engineers*, 180(1), 371–386. +- Clavel, R. (1988). "DELTA, a fast robot with parallel geometry." *Proceedings of the 18th International Symposium on Industrial Robots*, 91–100. +- Merlet, J.-P. (2006). *Parallel Robots*, 2nd ed. Springer. +- Husty, M.L. (1996). "An algorithm for solving the direct kinematics of general Stewart–Gough platforms." *Mechanism and Machine Theory*, 31(4), 365–379. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index fde5367..8cc5f3a 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -4,14 +4,17 @@ 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 | -| [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 | +| 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)$ | +| [Continuum Kinematics](ContinuumKinematics.md) | Piecewise constant-curvature arcs $(\kappa, \phi, s)$: singularity-free section transform, multi-section forward kinematics and closed-form single-section inverse | +| [Parallel Manipulator Kinematics](ParallelManipulatorKinematics.md) | Stewart–Gough leg lengths, leg Jacobian and Newton forward kinematics on SE(3); closed-form Delta-robot inverse and forward kinematics | +| [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 | +| [Analytical Inverse Kinematics (OPW)](AnalyticalIkOpw.md) | Closed-form IK for ortho-parallel 6R arms with a spherical wrist: all eight configurations in constant time, with wrist-singularity handling | diff --git a/doc/planning/ConvexCollision.md b/doc/planning/ConvexCollision.md new file mode 100644 index 0000000..74a33c6 --- /dev/null +++ b/doc/planning/ConvexCollision.md @@ -0,0 +1,98 @@ +# Convex Collision (GJK and EPA) + +## Overview & Motivation + +Closed-form distances exist for spheres, capsules and a point against a box, but not for most pairs a planner meets: capsule against box, box against box, or an arbitrary convex hull against anything. Writing a dedicated routine for every pair does not scale. + +The **Gilbert–Johnson–Keerthi (GJK)** algorithm computes the distance between any two convex shapes using only their **support functions**, the farthest point of the shape in a given direction. When the shapes overlap, the **expanding polytope algorithm (EPA)** continues from GJK's final simplex to find the penetration depth and contact normal. +Together they give one routine for distance, closest points, penetration depth and contact normal across spheres, capsules, oriented boxes and bounded convex hulls, with bounded memory and iteration counts. + +## Mathematical Theory + +### Minkowski Difference + +Two shapes $A$ and $B$ intersect if and only if their Minkowski difference $A \ominus B = \{a - b\}$ contains the origin. Their distance equals the distance from the origin to $A \ominus B$. The support function of the difference is + +$$s_{A \ominus B}(d) = s_A(d) - s_B(-d)$$ + +so the difference never has to be built explicitly. + +### Core and Margin + +Spheres and capsules are a **core** (a point or a segment) swept by a radius. GJK runs on the cores, and the radii are subtracted afterwards: + +$$d(A, B) = d(\text{core}_A, \text{core}_B) - r_A - r_B$$ + +This is exact, and a negative value is an exact penetration depth as long as the cores themselves are separated. GJK on a full sphere would only approach the answer asymptotically. + +### GJK + +Starting from any point of $A \ominus B$, GJK keeps a simplex of up to four support points and the point $v$ of that simplex closest to the origin. Each iteration: + +1. Takes a new support point $w = s_{A \ominus B}(-v)$. +2. Stops if $\|v\|^2 - v \cdot w \le \varepsilon\|v\|^2$ (no further progress) or if $w$ repeats. +3. Otherwise adds $w$, computes the closest point of the new simplex with Johnson's sub-algorithm, and keeps only the supporting face. + +Johnson's sub-algorithm enumerates the up to 15 faces of the simplex, solves each face's small affine system, and keeps the closest point with non-negative barycentric weights. A full tetrahedron, or a closest point at the origin, means the shapes intersect. Otherwise the witness points follow from the barycentric weights. + +### EPA + +When the cores intersect, EPA expands a polytope inside $A \ominus B$ that contains the origin: + +1. Build a tetrahedron from the GJK simplex, completed with supports along fixed directions when it is degenerate. +2. Repeatedly take the face closest to the origin with normal $n$, and the support point $w = s_{A \ominus B}(n)$. +3. If $w \cdot n$ barely exceeds the face distance, that face lies on the boundary: the depth is its distance and its normal is the contact normal. +4. Otherwise remove every face that sees $w$, including coplanar ones, and connect $w$ to the horizon edges. + +Faces are oriented away from the initial tetrahedron's centroid rather than from the origin, because the origin can lie exactly on a face for touching or axis-aligned shapes. + +## Complexity Analysis + +| Phase | Time | Space | Notes | +|-----------------|------------------------------|--------|---------------------------------------------------| +| Support mapping | $O(1)$, hulls $O(V)$ | $O(1)$ | $V$ hull vertices | +| GJK | $O(k \cdot 15)$ small solves | $O(1)$ | Typically 2–5 iterations for polytopes | +| EPA | $O(k \cdot F)$ | $O(F)$ | $F \le$ `MaxFaces` faces, about 6 KB for 64 faces | + +## Step-by-Step Walkthrough + +1. **Spheres**: radius 1 at the origin and radius $0.5$ at $(3, 1, 0)$ are $\sqrt{10} - 1.5 = 1.662$ apart, with the closest point on the first at $(0.949, 0.316, 0)$. +2. **Boxes**: a $1 \times 0.8 \times 0.6$ box at the origin and a smaller box rotated by $0.7$ rad about $(1, 1, 0)$ at $(1.4, 0.6, 0.5)$ are $0.511$ apart. This matches exhaustive vertex–face and edge–edge enumeration. +3. **Rounded overlap**: a unit sphere at $(1.8, 0, 0)$ overlaps a unit-half-extent cube by $0.2$. The cores are separated, so the depth is exact without EPA. +4. **Axis-aligned overlap**: two such cubes $1.5$ apart overlap by $0.5$ along $x$. The origin lies on the blown-up simplex boundary, which the centroid orientation handles. +5. **Random boxes**: across 300 random pairs, separated distances match exact enumeration to $10^{-5}$ and penetration depths match the separating-axis test to $10^{-4}$. + +## Pitfalls & Edge Cases + +- **Smooth shapes in EPA**: when the cores of a sphere or capsule intersect, EPA approximates a curved surface with facets and converges slowly. Prefer core-separated configurations, or accept a looser tolerance. +- **Origin on the boundary**: orienting faces from the origin fails for touching shapes. Orient them from an interior point. +- **Tied supports**: boxes return supports with tied coordinates. Treating coplanar faces as visible prevents degenerate, zero-area faces. +- **Budgets**: when the face budget is exhausted, the result is reported as not converged, with the best estimate so far. +- **Flat shapes**: a planar hull against a point has a flat Minkowski difference with no interior. EPA cannot build a polytope and reports non-convergence. +- **Non-convex shapes**: GJK requires convexity. Decompose concave geometry into convex parts. + +## Variants & Generalizations + +- **Signed-volume sub-algorithm**: a faster, more robust replacement for Johnson's enumeration (Montanari et al., 2017). +- **Incremental GJK**: warm-starting from the previous simplex for coherent motion. +- **Continuous collision detection**: conservative advancement and ray casting against $A \ominus B$. +- **Multi-point contact**: clipping-based manifold generation on top of the EPA normal. + +## Applications + +- Distance queries for collision-free motion planning and safety margins. +- Contact detection and penetration resolution in physics simulation. +- Clearance monitoring between robot links and obstacles. + +## Connections to Other Algorithms + +- [Primitive Distance Queries](PrimitiveDistance.md): closed forms for the simple pairs, and the references for these tests. +- [Self-Collision Checking](SelfCollision.md): capsule-based link checks that this routine generalizes to boxes and hulls. +- [RRT-Connect](RrtConnect.md): consumes these queries inside its validity test. + +## References & Further Reading + +- Gilbert, E.G., Johnson, D.W. and Keerthi, S.S. (1988). "A fast procedure for computing the distance between complex objects in three-dimensional space." *IEEE Journal on Robotics and Automation*, 4(2), 193–203. +- van den Bergen, G. (2001). "Proximity queries and penetration depth computation on 3D game objects." *Game Developers Conference*. +- Ericson, C. (2004). *Real-Time Collision Detection*. Morgan Kaufmann. Chapter 9. +- Montanari, M., Petrinic, N. and Barbieri, E. (2017). "Improving the GJK algorithm for faster and more reliable distance queries between convex objects." *ACM Transactions on Graphics*, 36(3), 30. diff --git a/doc/planning/README.md b/doc/planning/README.md index b4afe4c..f55b416 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -4,9 +4,11 @@ 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)$ | -| [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 | +| 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)$ | +| [Convex Collision (GJK and EPA)](ConvexCollision.md) | Distance, witness points, penetration depth and contact normal between spheres, capsules, oriented boxes and convex hulls from their support functions | +| [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 | +| [RRT-Connect](RrtConnect.md) | Bidirectional rapidly-exploring random trees in joint space with fixed node pools, injected sampler and validity test, and random shortcutting | diff --git a/doc/planning/RrtConnect.md b/doc/planning/RrtConnect.md new file mode 100644 index 0000000..2615b55 --- /dev/null +++ b/doc/planning/RrtConnect.md @@ -0,0 +1,96 @@ +# RRT-Connect + +## Overview & Motivation + +A manipulator moving among obstacles needs a collision-free path in **joint space**, where every point is a full arm configuration. Grids do not scale beyond three or four dimensions: a six-joint arm at 100 cells per joint has $10^{12}$ cells. Potential fields get stuck in local minima. + +**Sampling-based planners** avoid both problems. They explore the space by random sampling and only need a yes/no validity test per configuration. **RRT-Connect** (Kuffner and LaValle, 2000) grows two rapidly-exploring random trees, one from the start and one from the goal, and tries to join them greedily after every step. +It is one of the fastest single-query planners for arms, and it is **probabilistically complete**: if a path exists, the probability of finding it tends to one as the iterations grow. + +This implementation stores both trees in fixed-size pools, injects the sampler and the validity test, and adds random shortcutting to smooth the result. + +## Mathematical Theory + +### Extend + +To extend a tree towards a sample $x$: + +1. Find the nearest node $q_{\text{near}} = \arg\min_j \|q_j - x\|$. +2. Step towards $x$ by at most $\delta$: $q_{\text{new}} = x$ if $\|x - q_{\text{near}}\| \le \delta$, otherwise $q_{\text{near}} + \delta\,\frac{x - q_{\text{near}}}{\|x - q_{\text{near}}\|}$. +3. Accept $q_{\text{new}}$ if the edge from $q_{\text{near}}$ is collision-free. + +The result is **trapped** (edge blocked), **advanced** (one step taken) or **reached** ($x$ itself added). + +### Connect + +Connect repeats Extend towards the same target until the result is no longer "advanced". It either reaches the target or gets trapped. + +### Bidirectional Loop + +Each iteration extends tree $A$ towards a random sample. If that succeeds, it connects tree $B$ to $A$'s new node. When $B$ reaches it, the two branches joined at that node form the path. Otherwise the trees swap roles. + +The Voronoi bias of nearest-neighbour extension pulls the trees into unexplored regions, and the greedy connect closes the gap quickly once the trees see each other. + +### Edge Validity + +An edge from $a$ to $b$ is checked at $n = \lceil \|b - a\| / r \rceil$ points $a + (b - a)\,k/n$ for $k = 1, \ldots, n$, where $r$ is the collision resolution. + +### Shortcutting + +Raw paths are jagged. Random shortcutting repeatedly picks two waypoints $i < j - 1$. If the straight edge between them is free, it removes the waypoints in between. The path never gets longer and stays valid. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------|------------------------|-----------------------------|------------------------------------------------------| +| Extend | $O(N + L/r)$ | $O(1)$ | Linear nearest-neighbour scan plus edge checks | +| Planning | $O(I \cdot (N + L/r))$ | $O(N \cdot n)$ | $I$ iterations, $N$ nodes in a fixed pool | +| Shortcut attempt | $O(L/r + W)$ | $O(1)$ | One edge check and an in-place removal of $W$ points | +| Storage | — | $2N(n \cdot s + 2) + 2Nn s$ | Two trees and the path buffer; $s$ = scalar size | + +## Step-by-Step Walkthrough + +A two-joint configuration space $[-\pi, \pi]^2$ contains a wall, $|q_0| \le 0.3$ for $q_1 \le 1$, open above $q_1 = 1$. The planner uses step $0.3$ rad, collision resolution $0.05$ rad, a 256-node pool per tree and a seeded xorshift32 sampler. + +1. **Query** from $(-2, -2)$ to $(2, -2)$: the straight edge crosses the wall, so the trees are grown. +2. **Seed 1**: found after 78 iterations with 70 nodes. The path has 40 waypoints and length 11.53, against the shortest possible 7.50 over the wall top. +3. **Shortcutting**: 100 attempts reduce it to 4 waypoints and length 8.12. +4. **Other seeds**: seed 7 gives 9.83, then 7.84 after shortcutting; seed 12345 gives 11.83, then 8.05. +5. **Trivial query**: from $(-2, 2)$ to $(2, 2)$ the straight edge is free, so the two-point path returns with zero iterations. +6. **Small pool**: with 10 nodes per tree, the pool fills before the trees meet, and the planner reports exhaustion. + +## Pitfalls & Edge Cases + +- **Correlated samplers**: the trees alternate every iteration, so consecutive samples must be independent. A base-2 Halton sequence consumed in order sends every other sample to the same half of the space, and in the validation the trees stalled at 19 nodes. Use a seeded PRNG, or a separate low-discrepancy sequence per tree. +- **Collision resolution**: discrete edge checks can step over thin obstacles. Choose the resolution below the thinnest obstacle as seen in joint space, scaled by the longest lever arm. +- **Path quality**: raw paths are far from optimal. Shortcut them, then time-parameterize them before execution. +- **Metric**: plain Euclidean distance in joint space treats every joint alike. Weight joints by their lever arm when one dominates Cartesian motion. +- **Pool size**: when the pool is exhausted, the query fails. Size the pools for the hardest expected query, or raise the step size. + +## Variants & Generalizations + +- **RRT\***: asymptotically optimal rewiring at higher cost per iteration. +- **BiTRRT and T-RRT**: cost-aware trees for low-cost paths in cost maps. +- **PRM**: multi-query roadmaps built once and reused. +- **KPIECE and EST**: exploration guided by coverage estimates for kinodynamic problems. +- **Informed sampling**: restricting samples to an ellipsoid once a solution is known. + +## Applications + +- Collision-free motion planning for industrial and service manipulators. +- Mobile-manipulator and humanoid planning in high-dimensional spaces. +- Grasp approach and retreat planning. + +## Connections to Other Algorithms + +- [Self-Collision Checking](SelfCollision.md): a typical component of the validity test. +- [Primitive Distance Queries](PrimitiveDistance.md): environment checks inside the validity test. +- [Grid Search (A* and Dijkstra)](GridSearch.md): the resolution-complete alternative for low-dimensional spaces. +- [Cubic Spline Trajectory](../trajectory/CubicSplineTrajectory.md): turns the waypoints into a smooth joint trajectory. + +## References & Further Reading + +- Kuffner, J.J. and LaValle, S.M. (2000). "RRT-Connect: an efficient approach to single-query path planning." *IEEE International Conference on Robotics and Automation*, 995–1001. +- LaValle, S.M. (2006). *Planning Algorithms*. Cambridge University Press. Chapter 5. +- Geraerts, R. and Overmars, M.H. (2007). "Creating high-quality paths for motion planning." *International Journal of Robotics Research*, 26(8), 845–863. +- Karaman, S. and Frazzoli, E. (2011). "Sampling-based algorithms for optimal motion planning." *International Journal of Robotics Research*, 30(7), 846–894. diff --git a/roadmap/README.md b/roadmap/README.md index 5b6bfe7..1b8ebc5 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/OperationalSpaceControl](controllers/manipulator/OperationalSpaceControl/implementation.md). +[mobile/PurePursuit](mobile/PurePursuit/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 @@ -89,7 +89,7 @@ prerequisites. ## Index ### `kinematics` -`AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) +`MobileManipulatorKinematics` (M24) ### `trajectory` `TimeOptimalPathParameterization` (M27) @@ -98,10 +98,7 @@ prerequisites. `PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) ### `planning` -`ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) +`DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) ### `localization` `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) - -### `controllers/manipulator` -`OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md deleted file mode 100644 index 48a8587..0000000 --- a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md +++ /dev/null @@ -1,43 +0,0 @@ -# Cable Tension Distribution — Overview - -## What it is -The force-allocation step for a cable-driven parallel robot: given a desired wrench (force + torque) -on the moving platform, compute a set of cable tensions — each between a positive minimum and a -maximum — that produce exactly that wrench. Because cables can only *pull*, and there are usually more -cables than platform degrees of freedom, this is a constrained problem, not a plain linear solve. - -## Why it matters (embedded) -Cable robots — warehouse cranes, camera rigs (SkyCam), tendon-driven hands, large 3D printers — -actuate through tension only. Every control cycle must hand the winches a feasible, bounded tension -vector; a negative or over-limit request is physically impossible and can slacken a cable or snap -it. A closed-form allocator with a known worst-case cost fits the same real-time loop that computes -the wrench. - -## How it works (intuition) -From the platform pose, each cable's direction is the unit vector from its platform attachment point -to its winch; together with the attachment offsets (moment arms) these form the **structure matrix** -`A`, and the cables apply `A·t` to the platform. With more cables than DOF, many tension sets give the -same wrench — the extra freedom is internal *pretension*. The closed form takes the tension set closest -to the mid-range value `(tMin + tMax)/2` that still reproduces the wrench, which keeps every cable -comfortably away from slack and overload. If some cable still falls outside its range, the improved -method pins the worst one at its limit, subtracts its pull from the wrench, and re-solves with the -remaining cables — repeating at most once per cable, and reporting "not feasible" if too few cables are -left. Pinning the worst cable is a greedy choice that can be wrong near the edge of the feasible -workspace; there, the exact variant solves the same "closest to mid-range" problem as a small quadratic -program, which finds a solution whenever one exists. - -## Key parameters -- **baseAnchors, platformAnchors** — winch exit points (world) and attachment points (platform frame). -- **tMin** — minimum tension (> 0) so cables never go slack. -- **tMax** — maximum tension set by winch/cable strength. - -## Reference -A. Pott, T. Bruckmann, L. Mikelsons, "Closed-form Force Distribution for Parallel Wire Robots," -*Computational Kinematics*, 2009. A. Pott, "An Improved Force Distribution Algorithm for -Over-Constrained Cable-Driven Parallel Robots," *Computational Kinematics*, 2014. - -## See also -`math::SE3Transform` (NT53, platform pose and wrench convention); `ParallelManipulatorKinematics` (M23, -platform pose from actuator lengths); `solvers::TrySolveSystem` (the small wrench-space solve) and -`solvers::ActiveSetQp` (NT65, the exact fallback), from -[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md deleted file mode 100644 index 50ac039..0000000 --- a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md +++ /dev/null @@ -1,130 +0,0 @@ -# Cable Tension Distribution — Implementation Pseudocode - -> Roadmap ref: #M25 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Closed-form force distribution for cable-driven parallel robots (Pott, Bruckmann, Mikelsons 2009) with -Pott's improved iteration (2014). The structure matrix comes from the **cable geometry** — never from a -serial-arm Jacobian. The closed form is the deterministic fast path; near the boundary of the -wrench-feasible workspace, where it can fail, `DistributeExact` solves the same least-distance problem as a -QP with upstream `solvers::ActiveSetQp` (NT65). - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class CableTensionDistribution: # static_assert(WrenchDim == 6 || WrenchDim <= 3); NumCables ≥ WrenchDim - # WrenchDim = 6: spatial platform, wrench (f; n) about the platform origin, world frame (NT53 order) - # WrenchDim ≤ 3: point-mass platform, force rows only (2 = planar xy); platform anchors ignored - std::array, NumCables> baseAnchors # aᵢ, world frame - std::array, NumCables> platformAnchors # bᵢ, platform frame - T tMin # minimum tension (> 0: cables never go slack) - T tMax # maximum tension (winch / cable limit) - -using StructureMatrix = math::Matrix -using TensionVector = math::Vector -using WrenchVector = math::Vector # wrench the cables must apply to the platform -``` - -## Interface - -```cpp -CableTensionDistribution(const AnchorArray& baseAnchors, const AnchorArray& platformAnchors, T tMin, T tMax) - -std::optional ComputeStructureMatrix(const math::SE3Transform& platformPose) const - -# nullopt when the wrench is not reachable with tMin ≤ t ≤ tMax (or the geometry is degenerate): -std::optional Distribute(const WrenchVector& wrench, - const math::SE3Transform& platformPose) const # hot path - -# exact minimum of ‖t − tm·1‖ subject to A·t = w, tMin ≤ t ≤ tMax; nullopt iff infeasible: -std::optional DistributeExact(const WrenchVector& wrench, - const math::SE3Transform& platformPose) const -``` - -## Algorithm (pseudocode) - -```cpp -function ComputeStructureMatrix(pose): # wrench balance A·t = w - for i in 0..NumCables-1: - r = pose.R·bᵢ # platform anchor offset, world frame - l = aᵢ − (pose.p + r) # platform anchor → base anchor - if ‖l‖ ≤ ε: return nullopt # zero-length cable: direction undefined - u = l / ‖l‖ # unit direction in which cable i pulls the platform - column i = (u ; CrossProduct(r, u)) # WrenchDim = 6 - = first WrenchDim entries of u # WrenchDim ≤ 3 (point mass) - return A - -function Distribute(w, pose): # OPTIMIZE_FOR_SPEED - A = ComputeStructureMatrix(pose); if not A: return nullopt - tm = (tMin + tMax) / 2 - fixed = {false…}; free = NumCables; wFree = w; t = 0 - while free ≥ WrenchDim: # ≤ NumCables − WrenchDim + 1 passes - Af = A with the columns of fixed cables zeroed - tmF = tm on free cables, 0 on fixed ones - # closed form t = tm + A⁺(w − A·tm), A⁺ = Aᵀ(AAᵀ)⁻¹ — min ‖t − tm‖ subject to A·t = w - y = solvers::TrySolveSystem(Af·Afᵀ, wFree − Af·tmF) # WrenchDim×WrenchDim - if not y: return nullopt # free cables do not span the wrench space - t[i] = tm + (Afᵀ·y)[i] for every free i - k = argmax over free i of violation(t[i]) # violation = max(tMin − t, t − tMax, 0); ties → lowest i - if violation(t[k]) == 0: return t # all tensions within bounds - t[k] = clamp(t[k], tMin, tMax); fixed[k] = true; free −= 1 - wFree = wFree − A[:, k]·t[k] # fixed cable's contribution moves to the wrench side - return nullopt # fewer than WrenchDim free cables remain - -function DistributeExact(w, pose): # QP, NT65: ActiveSetQp - A = ComputeStructureMatrix(pose); if not A: return nullopt - qp = { hessian = I, gradient = −tm·1, - equalityMatrix = A, equalityVector = w, - inequalityMatrix = [I; −I], inequalityVector = (tMin·1; −tMax·1) } # t ≥ tMin, −t ≥ −tMax - r = ActiveSetQp.Solve(qp) - return r.status == Optimal ? r.x : nullopt # Infeasible: w outside the wrench-feasible set -``` - -## Complexity & memory - -- Time: per pass `O(WrenchDim²·NumCables)` for `Af·Afᵀ` plus an `O(WrenchDim³)` solve; at most - `NumCables − WrenchDim + 1` passes — deterministic, bounded, no iterative optimiser. -- `DistributeExact`: one Cholesky of the `NumCables×NumCables` identity (trivial) plus at most the QP's - iteration cap of `O(NumCables²)` steps; bounded but slower than the closed form — call it when - `Distribute` returns `nullopt`, not every cycle. -- Memory: one `WrenchDim×NumCables` matrix, one `WrenchDim×WrenchDim` system, a `NumCables` flag array; - stack only, no heap. - -## Numerical / embedded notes - -- **Cables pull only** (`t ≥ tMin > 0`) — the defining constraint; `tMin > 0` keeps cables taut - (no backlash / loss of control), `tMax` is sized to the winch. -- **Centring.** With redundancy (`NumCables > WrenchDim`) the closed form is the tension closest to - `tm·1` in the 2-norm, keeping cables away from slack and overload (Pott 2009). With - `NumCables = WrenchDim` it reduces to the unique `A⁻¹w` (`tm` cancels). -- **Improved closed form (Pott 2014).** Fixing the worst-violating cable at its bound and re-solving the - reduced system enlarges the usable workspace over the plain closed form. It can still return `nullopt` - close to the wrench-feasible workspace boundary where the exact QP succeeds (spatial 8-cable fixture: - 90 of 4000 boundary-biased feasible wrenches); fall back to `DistributeExact` there, and treat its - `nullopt` as a true workspace-boundary event (no exceptions). -- `TrySolveSystem` (upstream, pivot-threshold Gaussian elimination) reports rank deficiency — near-parallel - cables or a degenerate pose — as `nullopt`; `Af·Afᵀ` is SPD otherwise, so `math::CholeskyDecomposition` - (`numerical/math/CholeskyDecomposition.hpp`) is an equivalent choice. -- The wrench is `(f; n)` about the platform origin `pose.p` in the world frame; `w` is what the cables - must supply (e.g. `−(m·g_vec)` plus the dynamic wrench from the platform controller). -- 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/CableTensionDistribution.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Distribute`, and - `extern template class CableTensionDistribution;` / `` / `` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/controllers/manipulator/CableTensionDistribution.cpp` → - `template class CableTensionDistribution;` / `` / `` -- Test: `robotics/controllers/manipulator/test/TestCableTensionDistribution.cpp` -- Doc: `doc/controllers/manipulator/CableTensionDistribution.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestCableTensionDistribution.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`), NT65 (`solvers::ActiveSetQp`); `solvers::TrySolveSystem` from - [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md b/roadmap/controllers/manipulator/CableTensionDistribution/tests.md deleted file mode 100644 index 288a2cc..0000000 --- a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md +++ /dev/null @@ -1,105 +0,0 @@ -# Cable Tension Distribution — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. No mocks needed: pure geometry, closed form and QP. - -## Fixture - -```cpp -class TestCableTensionDistribution : public ::testing::Test: # planar point mass, 3 cables, WrenchDim = 2 - std::array,3> base = { (0, 2, 0), (−√3, −1, 0), (√3, −1, 0) } # 120° apart, radius 2 - std::array,3> platform = { 0, 0, 0 } - float tMin = 10, tMax = 200 # tm = 105 - CableTensionDistribution distributor{ base, platform, tMin, tMax } - SE3Transform pose = Identity # point mass at the origin - # u = (0, 1), (−√3/2, −1/2), (√3/2, −1/2); A·Aᵀ = 1.5·I; A·(1,1,1) = 0 - -class TestCableTensionDistributionSquare : public ::testing::Test: # 2 cables, WrenchDim = 2 - base = { (−1, 1, 0), (1, 1, 0) }, platform = { 0, 0 }, tMin = 10, tMax = 200 - CableTensionDistribution distributor{ base, platform, tMin, tMax } - -class TestCableTensionDistributionSpatial : public ::testing::Test: # 8 cables, WrenchDim = 6 - base[0] = (2, 2, 2), platform[0] = (0.2, 0, 0); remaining anchors: cube / box corners - CableTensionDistribution distributor{ base, platform, tMin, tMax } - -class TestCableTensionDistributionCrossed : public ::testing::Test: # 8 cables, WrenchDim = 6, crossed - base = (±2, ±2, {0, 3}) in the order (−,−,0) (−,−,3) (−,+,0) (−,+,3) (+,−,0) (+,−,3) (+,+,0) (+,+,3) - platform = same order with x negated: (0.2,−0.2,−0.1) (0.2,−0.2,0.1) (0.2,0.2,−0.1) (0.2,0.2,0.1) - (−0.2,−0.2,−0.1) (−0.2,−0.2,0.1) (−0.2,0.2,−0.1) (−0.2,0.2,0.1) - pose = { I, p = (0.3, −0.2, 1.4) }, tMin = 10, tMax = 200 # rank(A) = 6 -# each case below is a TEST_F(, ); default fixture TestCableTensionDistribution -``` - -## Test cases (Arrange / Act / Assert) - -```text -structure_matrix_from_geometry: - Act: A = ComputeStructureMatrix(pose) - Assert: columns == (0, 1), (−0.866025, −0.5), (0.866025, −0.5) - -spatial_structure_matrix_includes_moment_arm (TestCableTensionDistributionSpatial): - Arrange: pose = { Rz(π/2), p = 0 } ⇒ R·b₀ = (0, 0.2, 0) - Assert: column 0 == (u₀; (R·b₀) × u₀) = (0.59655, 0.53689, 0.59655, 0.11931, 0, −0.11931) - -zero_wrench_gives_centred_pretension: - Arrange: w = 0 - Assert: t == (I − A⁺A)·tm·1 = (105, 105, 105) (tm·1 already lies in null(A) here) - -redundant_case_centres_tensions: - Arrange: w = (0, 60) - Assert: t == tm·1 + A⁺·w = (145, 85, 85) (min ‖t − tm‖, all within bounds) - -wrench_is_reproduced: - Arrange: w = (30, −40) - Assert: A·t ≈ w and tMin ≤ t ≤ tMax; t ≈ (78.333, 101.013, 135.654) - -upper_bound_fixes_worst_cable: - Arrange: w = (0, 150) (plain closed form: t₀ = 205 > tMax) - Assert: t == (200, 50, 50), A·t ≈ w - -lower_bound_fixes_slack_cable: - Arrange: w = (0, −150) (plain closed form: t₀ = 5 < tMin) - Assert: t == (10, 160, 160), A·t ≈ w - -infeasible_wrench_returns_nullopt: - Arrange: w = (0, 300) (t₀ fixed at 200 ⇒ remaining pair needs −100 ⇒ < WrenchDim free cables) - Assert: Distribute(w, pose) == nullopt - -square_case_unique_solution (TestCableTensionDistributionSquare): - Arrange: w = (0, 100) - Assert: t == A⁻¹·w = (70.7107, 70.7107) (tm cancels) - -collinear_cables_return_nullopt (TestCableTensionDistributionSquare): - Arrange: pose.p = (0, 1, 0) (both cables horizontal and opposite ⇒ rank(A) = 1) - Assert: Distribute(w, pose) == nullopt - -exact_distribution_matches_closed_form_with_active_bound: - Arrange: w = (0, 150) - Assert: DistributeExact(w, pose) ≈ (200, 50, 50) == Distribute(w, pose) - -exact_distribution_succeeds_where_closed_form_fails (TestCableTensionDistributionCrossed): - Arrange: w = (56.8, −183.5, −35.3, −2.0, 41.7, −2.7) - Assert: Distribute(w, pose) == nullopt; - DistributeExact(w, pose) ≈ (200, 21.0187, 23.9864, 10, 88.1208, 197.9432, 76.4249, 23.7712), A·t ≈ w - -exact_distribution_reports_infeasible_wrench: - Arrange: w = (0, 300) - Assert: DistributeExact(w, pose) == nullopt -``` - -## Reference vectors - -- Planar fixture: `A⁺ = (2/3)·Aᵀ`, so `t = 105 + (2/3)·Aᵀw` while unclamped; `w = (0, W)` ⇒ - `t = 105 + (2W/3)·(1, −½, −½)` — symmetric tensions on cables 1 and 2. -- Upper-bound pass: fix `t₀ = 200` ⇒ `w' = (0, −50)` ⇒ square pair gives `t₁ = t₂ = 50`. -- Square fixture: `u = (∓1, 1)/√2`, vertical `w = (0, W)` ⇒ `t₀ = t₁ = W/√2`. -- Planar `w = (0, 150)`: null line `t = (100, −50, −50) + β·(1, 1, 1)`, bounds ⇒ `β ∈ [60, 100]`, unconstrained - `β* = 105` ⇒ `β = 100` ⇒ `(200, 50, 50)` — the closed form is exact here. -- Crossed fixture: the exact optimum has cables 0 (`tMax`) and 3 (`tMin`) at bounds; found by enumerating - the `3⁸` bound patterns (python3/numpy). The improved closed form fixes cable 3 at `tMin`, then cable 5 at - `tMax` (the optimum keeps it at 197.9432) and cable 0, and runs out of free cables (6 < WrenchDim + 1). - -## Edge cases - -- Platform anchor coincident with a base anchor (zero-length cable): `nullopt`. -- Wrench exactly on the workspace boundary: one tension equals `tMin` or `tMax`, still feasible. -- Near-parallel cables (`Af·Afᵀ` ill-conditioned): `TrySolveSystem` pivot threshold ⇒ `nullopt`, never `inf`. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md deleted file mode 100644 index 6ce3664..0000000 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md +++ /dev/null @@ -1,39 +0,0 @@ -# Hybrid Position/Force Control — Overview - -## What it is -A task-space controller that splits the end-effector's directions into two disjoint groups: some -axes are **position-controlled**, the rest are **force-controlled**. The axes are those of a -*constraint frame* attached to the contact surface; a diagonal selection matrix `S` decides which is -which, and its complement `I−S` handles the others. - -## Why it matters (embedded) -Many contact tasks are naturally hybrid: sliding a tool on a surface, you want to *track a path* -tangentially while *regulating the normal force* — you cannot command both position and force on the -same axis, because the environment already fixes one of them. Real-time force regulation with clean -axis partitioning is essential for deburring, polishing, assembly, and grinding on resource-limited -controllers. - -## How it works (intuition) -Rotate the pose error, velocity and forces into the constraint frame. Along motion axes a PD law pulls -the tool toward the reference path. Along force axes a PI law drives the measured contact force to the -desired force, with a little velocity damping to calm impacts and a clamped integral so it cannot wind -up when contact is lost. The two commands live in orthogonal subspaces, are summed, rotated back to the -base frame, and mapped to joint torques by `Jᵀ`, with gravity and Coriolis compensated. Because `S` and -`I−S` never overlap, the loops do not fight. Purely kinematic hybrid schemes can go unstable (An & -Hollerbach); the dynamic compensation here — or the fully decoupled operational-space form — avoids it. - -## Key parameters -- **Rc (constraint rotation)** — orientation of the contact frame relative to the base. -- **S (selection matrix)** — which constraint axes are motion (1) vs force (0). -- **Kp, Kd** — motion-subspace position/velocity gains. -- **Kf, Ki, Kdf** — force-subspace proportional, integral and velocity-damping gains. -- **integralLimit, dt** — anti-windup clamp and sample period of the force integral. - -## Reference -M. Raibert, J. Craig, "Hybrid Position/Force Control of Manipulators," *ASME J. Dyn. Sys. Meas. -Control*, 1981. C. An, J. Hollerbach, "Kinematic Stability Issues in Force Control of Manipulators," -*Proc. IEEE ICRA*, 1987. - -## See also -`ImpedanceControl` (M17, compliant unified motion/force), `OperationalSpaceControl` (M18, -dynamically decoupled task space), `GeometricJacobian` (M8, the `Jᵀ` wrench map). diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md deleted file mode 100644 index ee215a4..0000000 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md +++ /dev/null @@ -1,102 +0,0 @@ -# Hybrid Position/Force Control — Implementation Pseudocode - -> Roadmap ref: #M19 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Force convention: `fMeasured` and `fd` are the wrench the **tool applies on the environment** (base -frame, `(f; n)` about the tool point) — i.e. `−fExternal` of `ImpedanceControl`. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class HybridPositionForceControl: # TaskDim = 6 (pose) or ≤ 3 (position; 2 = planar xy) - const dynamics::EulerLagrangeDynamics& model # C q̇, g — typically ChainDynamicsModel (M29) - const kinematics::JacobianProvider& jacobian # J, tool pose — ChainTaskJacobian (M8) - math::SquareMatrix Rc # base ← constraint rotation: blkdiag(R, R) for 6, R for 3, 2-D rotation for 2 - math::SquareMatrix S # diagonal selection in the constraint frame: 1 = motion, 0 = force - Gains gains # { Kp, Kd (motion); Kf, Ki, Kdf (force); integralLimit } - math::Vector forceIntegral # ∫eF dt in the constraint frame (STATE) - T dt -``` - -## Interface - -```cpp -struct Gains { SquareMatrix Kp, Kd, Kf, Ki, Kdf; TaskVector integralLimit; } # integralLimit ≥ 0 - -HybridPositionForceControl(const dynamics::EulerLagrangeDynamics& model, - const kinematics::JacobianProvider& jacobian, - const SquareMatrix& Rc, const SquareMatrix& S, const Gains& gains, T dt) - -JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, - const math::SE3Transform& desiredPose, const TaskVector& xdDot, - const TaskVector& fMeasured, const TaskVector& fd) # hot path -void Reset() # clears force integral -``` - -## Algorithm (pseudocode) - -```text -function ComputeTorque(q, qDot, desiredPose, xdDot, fMeasured, fd): # OPTIMIZE_FOR_SPEED; gains.* unqualified - J = jacobian.Jacobian(q) - xDot = J·qDot - # --- base-frame errors (TaskError.hpp, see ImpedanceControl) rotated into the constraint frame --- - eX = Rcᵀ·TaskError(desiredPose, jacobian.ToolPose(q)) - eXDot = Rcᵀ·(xdDot − xDot) - vC = Rcᵀ·xDot - fdC = Rcᵀ·fd - eF = fdC − Rcᵀ·fMeasured - # --- motion subspace: PD --- - Fmotion = Kp·eX + Kd·eXDot - # --- force subspace: PI + velocity damping, integral clamped (anti-windup) --- - forceIntegral = clamp(forceIntegral + (I − S)·eF·dt, −integralLimit, +integralLimit) # element-wise - Fforce = fdC + Kf·eF + Ki·forceIntegral − Kdf·vC - # --- complementary partition in the constraint frame, rotate back, map to joints --- - F = Rc·(S·Fmotion + (I − S)·Fforce) - return Jᵀ·F + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) -``` - -## Complexity & memory - -- Time: `O(TaskDim·Dof)` for `J·q̇` and `Jᵀ·F`; `O(TaskDim²)` for the rotations; model terms `O(Dof)` each. -- Memory: `O(TaskDim²)` gains + `O(TaskDim)` integral state; no heap. - -## Numerical / embedded notes - -- `S` is diagonal with 0/1 entries (asserted in the constructor), so `S² = S` and `S·(I−S) = 0`: the - motion and force loops act on disjoint **constraint-frame** axes and never fight (Raibert–Craig). - `Rc` must be orthonormal; it aligns those axes with the contact surface (peg-in-hole: normal = - force-controlled, insertion/tangential = motion-controlled). Rotating with `Rc` only (no translation) - keeps the wrench reference point at the tool. -- Force loop = **PI + damping**: the integral removes steady force error on a stiff environment; the - `−Kdf·vC` term damps motion along force axes (contact impacts, stiff-surface oscillation). The integral - accumulates only on force axes and is clamped to `±integralLimit` (anti-windup); call `Reset()` on - contact loss. -- **Stability caution (An & Hollerbach, 1987):** kinematic hybrid control without dynamic compensation - can be unstable. This law compensates `g(q)` and `C(q,q̇)q̇` and maps - with `Jᵀ` only (no inverse, so it passes through singularities); for full dynamic decoupling of the - subspaces use the `Λ`-weighted form of `OperationalSpaceControl` (Khatib 1987). -- 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/HybridPositionForceControl.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class HybridPositionForceControl;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/controllers/manipulator/HybridPositionForceControl.cpp` → - `template class HybridPositionForceControl;` / `` -- Test: `robotics/controllers/manipulator/test/TestHybridPositionForceControl.cpp` -- Doc: `doc/controllers/manipulator/HybridPositionForceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestHybridPositionForceControl.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`); `TaskError` from - `robotics/controllers/manipulator/TaskError.hpp` (shipped with M17). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md b/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md deleted file mode 100644 index 898bcb5..0000000 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md +++ /dev/null @@ -1,82 +0,0 @@ -# Hybrid Position/Force Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestHybridPositionForceControl : public ::testing::Test: - # both dependencies mocked; planar TaskDim = 2, Dof = 2: - StrictMock> model - StrictMock> jacobian - SquareMatrix Rc = I # constraint frame = base frame - SquareMatrix S = diag(1, 0) # constraint axis 0 = motion, axis 1 = force - Gains gains{ Kp = diag(100,100), Kd = diag(20,20), Kf = diag(2,2), Ki = diag(5,5), - Kdf = diag(3,3), integralLimit = (1, 1) } - float dt = 0.001 - HybridPositionForceControl controller{ model, jacobian, Rc, S, gains, dt } - # ToolPose mocked as { I, (x, y, 0) } -# each case below is a TEST_F(TestHybridPositionForceControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -motion_axis_does_position_pd: - Arrange: J = I, model 0; only axis-0 position/velocity error, fd = fMeasured = 0 - Assert: τ[0] == Kp[0]·eX[0] + Kd[0]·eXDot[0]; τ[1] == 0 - -force_axis_does_damped_force_pi: - Arrange: J = I, model 0; axis-1 force error eF, qDot[1] = v, position error 0, one step - Assert: τ[1] == fd[1] + Kf[1]·eF[1] + Ki[1]·eF[1]·dt − Kdf[1]·v - -selection_masks_cross_terms: - Arrange: position error only on axis 1, force error only on axis 0, fd = 0, qDot = 0 - Assert: τ == 0 (each axis responds to exactly one loop) - -constraint_rotation_maps_axes: - Arrange: local controller with Rc = [[0,−1],[1,0]] (constraint x = base y, constraint y = base −x); - J = I, model 0, qDot = 0; eX = (0.02, 0.1), fd = (−5, 0), fMeasured = (−3, 0) (base frame) - Assert: τ == (−9.01, 10) (base-y error → motion loop; base-x error lies on the force axis - and is ignored; force loop acts along base −x) - -force_integral_accumulates: - Arrange: constant eF[1], N calls with N·dt·|eF[1]| < integralLimit[1] - Assert: integral contribution == Ki[1]·eF[1]·(N·dt) - -force_integral_clamped: - Arrange: constant eF[1] = 10, 1000 calls (unclamped ∫ = 10 > integralLimit = 1) - Assert: τ[1] == fd[1] + Kf[1]·10 + Ki[1]·1 (anti-windup) - -reset_clears_force_integral: - Arrange: accumulate integral, Reset() - Assert: next force term == fd[1] + Kf[1]·eF[1] + Ki[1]·eF[1]·dt (no history) - -jacobian_transpose_maps_wrench: - Arrange: J = [[1,0],[0,2]], known F - Assert: τ == Jᵀ·F - -gravity_and_coriolis_added: - Arrange: model g, C q̇ nonzero; errors 0, fd = fMeasured = 0, qDot = 0 - Assert: τ == C q̇ + g - -dependencies_queried_once: - Arrange: any state - Assert: Jacobian, ToolPose, ComputeCoriolisTerms, ComputeGravityTerms each once; - ComputeMassMatrix, BiasAcceleration never called (StrictMock) -``` - -## Reference vectors - -- `Rc = I`, `S = diag(1,0)`, `J = I`, model 0: - `F = [Kp·eX[0] + Kd·eXDot[0], fd[1] + Kf·eF[1] + Ki·∫eF[1] − Kdf·ẋ[1]]`, `τ = JᵀF` — hand-checkable. -- Rotated case: `Rcᵀ·eX = (0.1, −0.02)`, `Rcᵀ·fd = (0, 5)`, `eF = (0, 2)`, `∫eF = (0, 0.002)` ⇒ - constraint wrench `(10, 9.01)` ⇒ base `F = Rc·(10, 9.01) = (−9.01, 10)`. -- Contact equilibrium on a stiff surface: at rest the applied force equals the force-axis wrench, - and the PI drives `fMeasured → fd` ⇒ steady `eF = 0`. - -## Edge cases - -- Contact loss (`fMeasured → 0`): the integral saturates at `±integralLimit` (bounded) — `Reset()` on re-contact. -- `S = I` (all motion) reduces to Cartesian PD + `g` + `Cq̇`; `S = 0` (all force) to damped force PI. -- Near-singular `J`: `Jᵀ` stays finite, torque bounded (no inversion). diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md deleted file mode 100644 index f52733b..0000000 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md +++ /dev/null @@ -1,37 +0,0 @@ -# Operational-Space Control — Overview - -## What it is -A control framework that acts directly in Cartesian (task) space while correctly accounting for how -the arm's joint-space inertia *appears* at the end-effector — the task-space inertia `Λ`. Any leftover -joint freedom in a redundant arm is used by a secondary objective in the dynamically-consistent null -space. - -## Why it matters (embedded) -It is the natural formulation for interaction and force control and for redundant (7-DOF) arms. By -reflecting the inertia through the Jacobian, it **dynamically decouples** task directions — a push in -`x` no longer disturbs `y` — and it lets a redundant arm avoid joint limits or obstacles while -rigidly holding a Cartesian pose. - -## How it works (intuition) -Reflect the joint-space mass matrix through the Jacobian to obtain the effective end-effector inertia -`Λ = (J M⁻¹ Jᵀ)⁻¹`. Command a Cartesian acceleration with a task-space PD law on the pose error -(orientation as a rotation vector), remove the velocity-product term `J̇q̇`, convert it to a wrench by -multiplying with `Λ`, and map that wrench to joint torque with `Jᵀ`. Gravity and Coriolis are cancelled -in *joint* space, so a redundant arm's self-motion does not sag. Finally, inject any secondary joint -torque through a null-space projector chosen so it is *invisible* to the task. The one delicate step is -the `M⁻¹Jᵀ` solve, handled by Gaussian elimination rather than an explicit inverse. - -## Key parameters -- **Kp, Kd** — task-space position and damping gains. -- **model, jacobian** — injected dynamics (`M`, `Cq̇`, `g`) and task Jacobian provider (`J`, `J̇q̇`, tool pose). -- **tauSecondary** — secondary-objective joint torque (joint-limit / obstacle avoidance). -- **σ (damping factor)** — adds `σ²I` inside the `Λ` inverse near singularities. - -## Reference -O. Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational -Space Formulation," *IEEE J. Robotics and Automation*, 3(1), 1987. - -## See also -`ImpedanceControl` (`Λ` enables true inertia shaping); `HybridPositionForceControl` (adds force axes); -`RedundancyResolution` (kinematic null-space); `solvers::SolveSystem` (the `M⁻¹Jᵀ` solve, from -[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)). diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md deleted file mode 100644 index e68da43..0000000 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md +++ /dev/null @@ -1,99 +0,0 @@ -# Operational-Space Control — Implementation Pseudocode - -> Roadmap ref: #M18 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class OperationalSpaceControl: # 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 Kp # task-space position gain - math::SquareMatrix Kd # task-space damping gain - T sigma # singularity damping σ (0 ⇒ exact Λ) -``` - -## Interface - -```text -OperationalSpaceControl(const dynamics::EulerLagrangeDynamics& model, - const kinematics::JacobianProvider& jacobian, - const SquareMatrix& Kp, const SquareMatrix& Kd, T sigma) - -JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, - const math::SE3Transform& desiredPose, - const TaskVector& xdDot, const TaskVector& xdDdot, - const JointVector& tauSecondary) # hot path -``` - -## Algorithm (pseudocode) - -```text -function ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, tauSecondary): # OPTIMIZE_FOR_SPEED - J = jacobian.Jacobian(q) # TaskDim×Dof - M = model.ComputeMassMatrix(q) # Dof×Dof (SPD) - # --- task command (TaskError.hpp, see ImpedanceControl: PoseError for 6, position difference for ≤ 3) --- - eX = TaskError(desiredPose, jacobian.ToolPose(q)) - eXDot = xdDot − J·qDot - aX = xdDdot + Kd·eXDot + Kp·eX - # --- task-space inertia Λ = (J M⁻¹ Jᵀ + σ²I)⁻¹ --- - X = solvers::SolveSystem(M, Jᵀ) # M⁻¹Jᵀ, no explicit inverse - Lambda = solvers::SolveSystem(J·X + σ²·I, I) # TaskDim×TaskDim (small) - # --- dynamically-consistent null-space projector --- - JbarT = Lambda·Xᵀ # J̄ᵀ = Λ J M⁻¹ (M symmetric) - N = I − Jᵀ·JbarT # Dof×Dof - # --- torque: task term + FULL joint-space C q̇ + g + projected secondary torque --- - return Jᵀ·Lambda·(aX − jacobian.BiasAcceleration(q, qDot)) # J̇q̇ - + model.ComputeCoriolisTerms(q, qDot) - + model.ComputeGravityTerms(q) - + N·tauSecondary -``` - -## Complexity & memory - -- Time: `O(TaskDim·Dof³)` for the `M⁻¹Jᵀ` solve with `SolveSystem` (it eliminates per column; factoring - `M` once with `math::CholeskyDecomposition` gives `O(Dof³ + TaskDim·Dof²)`); `O(TaskDim³)` for `Λ`; - `O(Dof²·TaskDim)` for the projector. -- Memory: `O(Dof²)` working matrices; all stack/static, no heap. - -## Numerical / embedded notes - -- **Task dynamics.** With `σ = 0`: `ẍ = J q̈ + J̇q̇ = aX` exactly, and `J·M⁻¹·N = 0`, so `tauSecondary` - produces **no** task-space acceleration. -- **Why `C q̇ + g` in joint space.** The classic form `F = Λ(aX − J̇q̇) + J̄ᵀC q̇ + J̄ᵀg`, `τ = JᵀF + Nτ₀` - compensates only `JᵀJ̄ᵀ(C q̇ + g)`; on a redundant arm (`Dof > TaskDim`) the null-space part - `N·(C q̇ + g)` is left uncompensated and the self-motion sags. The law above equals the classic form - plus `N·(C q̇ + g)`; task behaviour is unchanged, and at rest with zero error `τ = g` exactly. -- Solve `M·X = Jᵀ` with `solvers::SolveSystem` (multi-column Gaussian elimination) rather than forming - `M⁻¹` — cheaper and better-conditioned. -- `Λ = (J M⁻¹ Jᵀ)⁻¹` blows up at kinematic singularities (`J` loses rank); near - `det(J M⁻¹ Jᵀ) → 0` use σ > 0 (adds `σ²I` inside the inverse). With σ > 0 the task is tracked only - approximately and `J·M⁻¹·N ≈ 0`; with σ = 0 a rank-deficient `J` violates `SolveSystem`'s pivot assert. -- For `TaskDim = 6` the rotation-vector error rate equals `ωd − ω` only to first order about `eR = 0`. -- `Λ` and `J̄` are small (`TaskDim`-wide) — cache them when the task dimension ≪ `Dof`. -- 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/OperationalSpaceControl.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class OperationalSpaceControl;` / `` / `` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/controllers/manipulator/OperationalSpaceControl.cpp` → - `template class OperationalSpaceControl;` / `` / `` -- Test: `robotics/controllers/manipulator/test/TestOperationalSpaceControl.cpp` -- Doc: `doc/controllers/manipulator/OperationalSpaceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestOperationalSpaceControl.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`); `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/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md b/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md deleted file mode 100644 index 24b8222..0000000 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md +++ /dev/null @@ -1,76 +0,0 @@ -# Operational-Space Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestOperationalSpaceControl : public ::testing::Test: # Dof = 2, TaskDim = 2 - StrictMock> model - StrictMock> jacobian - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - OperationalSpaceControl controller{ model, jacobian, Kp, Kd, 0 } # σ = 0 - # ToolPose mocked as { I, (x, y, 0) }; desiredPose = { I, (xd, yd, 0) } - -class TestOperationalSpaceControlRedundant : public ::testing::Test: # Dof = 3, TaskDim = 2 - StrictMock> model # M = [[3,.5,.2],[.5,2,.3],[.2,.3,1]], g = (4,−2,1.5) - StrictMock> jacobian # J = [[1,.5,.2],[0,1,.7]] - OperationalSpaceControl controller{ model, jacobian, Kp, Kd, 0 } -# each case below is a TEST_F(, ); default fixture TestOperationalSpaceControl -``` - -## Test cases (Arrange / Act / Assert) - -```text -unit_inertia_gives_task_pd: - Arrange: M = I, J = I, J̇q̇ = 0, C q̇ = 0, g = 0; eX, eXDot, xdDdot given - Act: τ = ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, 0) - Assert: τ == xdDdot + Kd·eXDot + Kp·eX - -task_inertia_scales_command: - Arrange: M = diag(2, 3), J = I (Λ = diag(2, 3)), J̇q̇ = 0, model terms 0 - Assert: τ == diag(2, 3)·aX - -bias_acceleration_subtracted: - Arrange: M = I, J = I, all errors 0, xdDdot = 0, J̇q̇ = b - Assert: τ == −b - -coriolis_and_gravity_added_in_joint_space: - Arrange: M = I, J = I, aX = 0, J̇q̇ = 0, C q̇ = c, g != 0 - Assert: τ == c + g - -jacobian_transpose_realises_wrench: - Arrange: M = I, J = [[1,0],[1,2]], errors 0, xdDdot = aX, model terms 0 - Assert: τ == Jᵀ·(J Jᵀ)⁻¹·aX (= J⁻¹·aX) - -redundant_at_rest_holds_gravity_exactly (TestOperationalSpaceControlRedundant): - Arrange: q̇ = 0 (C q̇ = 0, J̇q̇ = 0), ToolPose = desiredPose, xdDot = xdDdot = 0, tauSecondary = 0 - Assert: τ == g = (4, −2, 1.5) (null-space gravity compensated) - -secondary_torque_invisible_to_task (TestOperationalSpaceControlRedundant): - Arrange: same state; τ₀ = (1, −2, 0.5); Δτ = τ(τ₀) − τ(0) - Assert: Δτ == N·τ₀ = (0.38863, −1.32780, 1.06224) != 0 and J·M⁻¹·Δτ ≈ 0 - -singular_jacobian_uses_damping: - Arrange: local controller with σ = 0.1; M = I, J = [[1,1],[1,1]] (rank 1), aX != 0 - Assert: τ finite (no NaN / inf) - -dependencies_queried_once: - Arrange: any state - Assert: ComputeMassMatrix, ComputeCoriolisTerms, ComputeGravityTerms, Jacobian, BiasAcceleration, - ToolPose each called exactly once (StrictMock) -``` - -## Reference vectors - -- `M = I`, `J = I`: `Λ = I`, controller collapses to task-space PD `τ = xdDdot + Kd·eXDot + Kp·eX + C q̇ + g`. -- Redundant fixture: the classic `J̄ᵀg` form would return `JᵀJ̄ᵀg = (3.3365, 0.2670, −0.3136) ≠ g`; - the law returns `g` exactly. `J·M⁻¹·N·τ₀ ≈ 0` (verified ≈ 1e-16 in double). - -## Edge cases - -- Redundant arm (`Dof > TaskDim`): non-trivial null space; secondary objective realised without - disturbing the task, and self-motion does not sag under gravity. -- Singularity: σ > 0 keeps torque bounded; task tracking becomes approximate. -- Model mismatch: task decoupling degrades gracefully, stays stable. diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md deleted file mode 100644 index 508e5f0..0000000 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Slotine-Li Adaptive Control — Overview - -## What it is -A trajectory-tracking manipulator controller that **learns the robot's inertial parameters online** -while it moves. It exploits the fact that rigid-body dynamics are *linear in the inertial -parameters*: `M(q)q̈r + C(q,q̇)q̇r + g(q) = Y(q,q̇,q̇r,q̈r)·a` (with the Christoffel-consistent Coriolis -matrix `C`), so the unknown masses/inertias `a` can be estimated by a simple adaptation law. - -## Why it matters (embedded) -Real robots carry unknown or changing payloads. Rather than re-identifying the model offline, this -controller adapts its feedforward in real time, giving accurate tracking without precise a-priori -mass data — valuable when the same low-cost arm must handle varied loads. - -## How it works (intuition) -It combines a *sliding variable* `s = ė + Λe` (a filtered tracking error) with a regressor `Y` -evaluated at a **reference** acceleration, so no noisy joint-acceleration measurement is needed. The -torque is a model feedforward `Y·â` plus damping `−Kd·s`. Whenever `s` is nonzero the parameter -estimate `â` slides *downhill* on a Lyapunov function (`â̇ = −Γ Yᵀ s`), simultaneously shrinking the -tracking error and improving the model. Passivity guarantees stability. - -## Key parameters -- **Λ (surface slope)** — bandwidth of the `s = ė + Λe` error manifold. -- **Kd (sliding gain)** — damping on `s`; sets transient stiffness. -- **Γ (adaptation gain)** — how fast parameters are learned; too large ⇒ oscillation. -- **â₀ (initial estimate)** — parameter seed; a rough guess speeds convergence. - -## Reference -J.-J. Slotine, W. Li, "On the Adaptive Control of Robot Manipulators," *Int. J. Robotics Research*, -6(3), 1987. - -## See also -`ComputedTorqueControl` (M12, exact-model sibling), `CoriolisMatrixAndRegressor` (M31, supplies `Y`), -`DynamicParameterIdentification` (M22, offline regressor identification); generic-plant model-reference -adaptive control lives in [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md deleted file mode 100644 index b586623..0000000 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md +++ /dev/null @@ -1,95 +0,0 @@ -# Slotine-Li Adaptive Control — Implementation Pseudocode - -> Roadmap ref: #M20 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class SlotineLiAdaptiveControl: - const dynamics::InertialRegressor& regressor # Y(q,q̇,q̇r,q̈r) — M31 - math::SquareMatrix Lambda # sliding-surface slope (SPD) - math::SquareMatrix Kd # sliding-variable gain (SPD) - math::SquareMatrix Gamma # adaptation gain (SPD) - math::Vector aHat # inertial-parameter estimate (STATE) - T dt -``` - -## Interface - -```text -# Regressor injected (DIP): supplies Y such that M(q)q̈r + C(q,q̇)q̇r + g(q) = Y·a, -# C the Christoffel-consistent Coriolis matrix (M31, dynamics::InertialRegressor) -SlotineLiAdaptiveControl(const dynamics::InertialRegressor& regressor, const SquareMatrix& Lambda, - const SquareMatrix& Kd, const SquareMatrix& Gamma, - const Vector& aHat0, T dt) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const StateVector& qd, const StateVector& qdDot, - const StateVector& qdDdot) # hot path, updates aHat -const Vector& ParameterEstimate() const -void Reset(const Vector& aHat0) -``` - -## Algorithm (pseudocode) - -```text -function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - qTilde = q - qd - qTildeDot = qDot - qdDot - # sliding variable and reference (not measured) motion — no q̈ measurement needed: - s = qTildeDot + Lambda * qTilde - qrDot = qdDot - Lambda * qTilde - qrDdot = qdDdot - Lambda * qTildeDot - # linearity-in-parameters: M q̈r + C q̇r + g = Y·a - Y = regressor.Compute(q, qDot, qrDot, qrDdot) # Dof×NumParams - # control: model feedforward on the current estimate + sliding-variable damping - tau = Y * aHat - Kd * s - # passivity-based parameter update (integrate â̇ = −Γ Yᵀ s): - aHat = aHat - (Gamma * transpose(Y) * s) * dt - return tau -``` - -## Complexity & memory - -- Time: `O(Dof·NumParams)` for `Y·aHat` and `Yᵀ·s`; regressor build is `O(Dof·NumParams)` - (`ChainInertialRegressor`, M31, `NumParams = 10·N`). -- Memory: `O(NumParams)` parameter state + `O(Dof²)` gains; no heap. - -## Numerical / embedded notes - -- **No acceleration measurement:** `Y` is evaluated at the *reference* `q̈r`, not measured `q̈` — the - key robustness advantage over direct inverse-dynamics identification. -- Global tracking convergence via Lyapunov `V = ½sᵀM(q)s + ½ãᵀΓ⁻¹ã` (SPD `M`, `ã = âHat − a`): - `s → 0` hence `q̃ → 0`; parameters stay **bounded** but converge only under persistent excitation. -- **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 - 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 `Γ` - causes estimate oscillation and can excite unmodelled modes. -- Sibling of `ComputedTorqueControl` (M12, non-adaptive). The interface is a virtual seam for StrictMock - tests; hard real-time builds may template the controller on the concrete `ChainInertialRegressor` - to avoid the virtual call (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/SlotineLiAdaptiveControl.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class SlotineLiAdaptiveControl;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD` (`20 = 10·N` matches `ChainInertialRegressor`). -- Coverage: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp` → - `template class SlotineLiAdaptiveControl;` / `` -- Test: `robotics/controllers/manipulator/test/TestSlotineLiAdaptiveControl.cpp` -- Doc: `doc/controllers/manipulator/SlotineLiAdaptiveControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestSlotineLiAdaptiveControl.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: M31 (`InertialRegressor`, `ChainInertialRegressor`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md deleted file mode 100644 index 9e9e95e..0000000 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md +++ /dev/null @@ -1,79 +0,0 @@ -# Slotine-Li Adaptive Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSlotineLiAdaptiveControl : public ::testing::Test: - # regressor injected & mocked; Dof = 2, NumParams = 3: - StrictMock> regressor - SquareMatrix Lambda = diag(5, 5) - SquareMatrix Kd = diag(20, 20) - SquareMatrix Gamma = diag(1, 1, 1) - Vector aHat0 = {0, 0, 0} - float dt = 0.001 - SlotineLiAdaptiveControl controller{ regressor, Lambda, Kd, Gamma, aHat0, dt } -# each case below is a TEST_F(TestSlotineLiAdaptiveControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -zero_error_uses_feedforward_only: - Arrange: q=qd, qDot=qdDot ⇒ s=0; regressor Y given - Act: τ = ComputeTorque(...) - Assert: τ == Y · aHat (no −Kd·s term) - -sliding_variable_damping: - Arrange: s != 0 via qTildeDot, regressor Y = 0 - Assert: τ == -Kd · s - -sliding_variable_definition: - Arrange: known qTilde, qTildeDot - Assert: s == qTildeDot + Lambda · qTilde - -reference_motion_uses_lambda: - Arrange: known errors - Assert: regressor called with qrDot == qdDot − Λ·qTilde and qrDdot == qdDdot − Λ·qTildeDot - -parameter_estimate_adapts: - Arrange: s != 0, Y != 0, one step - Assert: aHat == aHat0 − Γ·Yᵀ·s·dt - -no_adaptation_on_zero_sliding: - Arrange: s = 0 - Assert: aHat unchanged after the step - -estimate_persists_across_calls: - Arrange: two steps with the same s, Y - Assert: aHat integrates twice; ParameterEstimate() reflects both - -convergence_tracks_trajectory: - Arrange: horizontal 2-link plant M q̈ + C q̇ = τ with true a (3 params); mock Invoke returns its - regressor Y(q,q̇,q̇r,q̈r) built with the Christoffel C; aHat0 = 0 ≠ a; run K steps - Assert: ||q − qd|| -> 0 (tracking) even though aHat != a - -reset_restores_initial_estimate: - Arrange: adapt, then Reset(aHat0) - Assert: ParameterEstimate() == aHat0 - -regressor_queried_once_per_call: - Arrange: any state - Assert: regressor.Compute called exactly once (StrictMock) -``` - -## Reference vectors - -- By construction `s = q̃̇ + Λq̃`; single joint, `Λ=λ`, constant `q̃=e`, `q̃̇=0` ⇒ `s = λe` (hand-checkable). -- With `aHat = a` (true params) the law reduces to computed-torque-like `τ = Y·a − Kd·s`. -- One adaptation step: `Δa = −Γ·Yᵀ·s·dt` — exact, hand-verifiable. -- Horizontal 2-link arm, `a = (a1, a2, a3)` (Spong, Hutchinson, Vidyasagar): - `M = [[a1 + 2a2c₂, a3 + a2c₂], [a3 + a2c₂, a3]]`, Christoffel `C = a2s₂·[[−q̇₂, −(q̇₁+q̇₂)], [q̇₁, 0]]` ⇒ - `Y = [[q̈r₁, c₂(2q̈r₁ + q̈r₂) − s₂(q̇₂q̇r₁ + (q̇₁+q̇₂)q̇r₂), q̈r₂], [0, c₂q̈r₁ + s₂q̇₁q̇r₁, q̈r₁ + q̈r₂]]`. - -## Edge cases - -- Non-persistent excitation: `aHat` settles on a manifold; tracking still converges. -- Large `Γ`: estimate oscillation — assert bounded, no divergence. -- Parameter drift under noisy `s`: projection / dead-zone keeps `aHat` physical. diff --git a/roadmap/kinematics/AnalyticalIkOpw/explanation.md b/roadmap/kinematics/AnalyticalIkOpw/explanation.md deleted file mode 100644 index aacc27b..0000000 --- a/roadmap/kinematics/AnalyticalIkOpw/explanation.md +++ /dev/null @@ -1,41 +0,0 @@ -# Analytical IK for ortho-parallel 6R arms with a spherical wrist (OPW) — Overview - -## What it is -A **closed-form** inverse-kinematics solver for the six-revolute layout used by most industrial arms: -the second and third axes are parallel to each other and perpendicular to the first, and the last -three axes meet at one point (a spherical wrist). Seven lengths — shoulder offset, lateral offset, -elbow offset, and the base, upper-arm, forearm and wrist lengths — plus a zero offset and a direction -sign per joint describe the arm. It returns every posture — up to eight — that reaches a flange pose. - -## Why it matters (embedded) -Closed-form IK is exact, has no convergence loop and runs in constant time — ideal for a hard real-time -controller that cannot afford an iterative solver's worst case. Getting *all* solutions lets a planner -pick the posture that respects joint limits or stays close to the current one. The seven-parameter -description is read straight off a datasheet drawing, avoiding DH frame-placement mistakes. - -## How it works (intuition) -Pieper's result is the theoretical basis: when the last three axes intersect, position and -orientation decouple. The wrist centre sits a fixed distance behind the flange along the approach axis, -and it depends only on the first three joints. The base joint turns the arm plane toward the wrist -centre — facing it or facing away, correcting for the lateral offset — giving two shoulder branches. -Inside that plane the shoulder offset shifts the triangle's corner, and the forearm offset turns the -forearm into a slightly longer virtual link at a fixed angle; the law of cosines on that triangle gives -elbow-up and elbow-down. With the arm placed, the remaining rotation is exactly a Z-Y-Z rotation of the -wrist, read off with `atan2` in two mirror-image flavours. When the wrist's middle joint is straight, -the first and last wrist axes line up and only their sum is determined, so one is fixed by convention. - -## Key parameters -- **a1, a2, b, c1, c2, c3, c4** — shoulder, elbow and lateral offsets; base, upper-arm, forearm and wrist lengths. -- **joint offsets and signs** — map the manufacturer's joint zero and direction onto the model. -- **branch selection** — shoulder front/back, elbow up/down, wrist flip. - -## Reference -M. Brandstötter, A. Angerer, M. Hofbaur, "An Analytical Solution of the Inverse Kinematics Problem of -Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist," *Proc. Austrian -Robotics Workshop*, 2014; D. L. Pieper, "The Kinematics of Manipulators Under Computer Control," PhD -thesis, Stanford University, 1968. - -## See also -`math::SE3Transform` (NT53, the flange pose), `PoseInverseKinematics` (M13, the iterative fallback for other -layouts), `DenavitHartenberg` (M7), `Cordic` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), -fixed-cost trig). diff --git a/roadmap/kinematics/AnalyticalIkOpw/implementation.md b/roadmap/kinematics/AnalyticalIkOpw/implementation.md deleted file mode 100644 index 3290eeb..0000000 --- a/roadmap/kinematics/AnalyticalIkOpw/implementation.md +++ /dev/null @@ -1,125 +0,0 @@ -# Analytical IK — OPW (Ortho-Parallel 6R with Spherical Wrist) — Implementation Pseudocode - -> Roadmap ref: #M21 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Closed-form IK of Brandstötter, Angerer & Hofbaur (2014) for the industrial 6R layout (KUKA, ABB, -Fanuc, Stäubli, …): axes 2 and 3 parallel and orthogonal to axis 1, spherical wrist. Seven lengths plus -per-joint offsets and signs describe the arm; no DH table and no frame convention to pin. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct OpwParameters: # model zero pose: upper arm and forearm point along +z - T a1 # axis 1 → axis 2, radial (x) - T a2 # forearm offset ⊥ forearm, axis 3 → wrist line (x at zero pose) - T b # lateral offset of the arm plane (y) - T c1 # base → axis 2, along axis 1 (z) - T c2 # upper arm, axis 2 → axis 3 - T c3 # forearm, axis 3 → wrist centre - T c4 # wrist centre → flange, along axis 6 - std::array offsets # θ = sign·q − offset (model angle from joint value) - std::array signs # ±1 - -template -struct OpwSolution: - math::Vector q # joint values, wrapped to (−π, π] - bool wristSingular # sin θ5 ≈ 0: only θ4 ± θ6 is determined - -template -using OpwSolutions = std::array>, 8> - # slot j (0..3): arm branch j with wrist θ5 ≥ 0; slot j + 4: same arm, flipped wrist - # arm j: 0/1 shoulder front (elbow a/b), 2/3 shoulder back (elbow a/b) - -template -class AnalyticalIkOpw: - OpwParameters parameters -``` - -Forward model (defines the parameters; `Forward` implements it): -`T(q) = Rz(θ1)·Trans(a1, b, c1)·Ry(θ2)·Trans(0, 0, c2)·Ry(θ3)·Trans(a2, 0, c3)·Rz(θ4)·Ry(θ5)·Rz(θ6)·Trans(0, 0, c4)`. - -## Interface - -```text -explicit AnalyticalIkOpw(const OpwParameters& parameters) -OpwSolutions Solve(const SE3Transform& flange) const # all real branches; hot path -SE3Transform Forward(const JointVector& q) const # closed-form FK (round trips, tests) -``` - -## Algorithm (pseudocode) - -```cpp -function Forward(q): - θ = signs ⊙ q − offsets; k = √(a2² + c3²); ψ3 = atan2(a2, c3) - x = c2·sin θ2 + k·sin(θ2 + θ3 + ψ3) + a1; z = c2·cos θ2 + k·cos(θ2 + θ3 + ψ3) - C = (x·cos θ1 − b·sin θ1, x·sin θ1 + b·cos θ1, z + c1) # wrist centre - R = Rz(θ1)·Ry(θ2 + θ3)·Rz(θ4)·Ry(θ5)·Rz(θ6) - return { R, C + c4·R·ẑ } - -function Solve(flange): # OPTIMIZE_FOR_SPEED — closed form, no iteration - (R, p) = flange; C = p − c4·R·ẑ - ρ² = Cx² + Cy² − b²; if ρ² < 0: return all empty - n = √ρ² − a1; z = Cz − c1; k² = a2² + c3²; ψ3 = atan2(a2, c3) - θ1ᶠ = atan2(Cy, Cx) − atan2(b, n + a1) # shoulder front - θ1ᵇ = atan2(Cy, Cx) + atan2(b, n + a1) − π # shoulder back - s1² = n² + z²; s2² = (n + 2·a1)² + z² - β1 = acosV((s1² + c2² − k²) / (2·s1·c2)); γ1 = acosV((s1² − c2² − k²) / (2·c2·k)) - β2 = acosV((s2² + c2² − k²) / (2·s2·c2)); γ2 = acosV((s2² − c2² − k²) / (2·c2·k)) - arm[0] = (θ1ᶠ, atan2(n, z) − β1, γ1 − ψ3) - arm[1] = (θ1ᶠ, atan2(n, z) + β1, −γ1 − ψ3) - arm[2] = (θ1ᵇ, −atan2(n + 2·a1, z) − β2, γ2 − ψ3) - arm[3] = (θ1ᵇ, −atan2(n + 2·a1, z) + β2, −γ2 − ψ3) - for j in 0..3 with arm[j] valid (its β, γ defined): - (θ1, θ2, θ3) = arm[j] - W = Transpose(Rz(θ1)·Ry(θ2 + θ3)) · R # = Rz(θ4)·Ry(θ5)·Rz(θ6) - s5 = √max(0, 1 − W₂₂²) - if s5 > ε: # regular wrist: two branches - w = (atan2(W₁₂, W₀₂), atan2(s5, W₂₂), atan2(W₂₁, −W₂₀)) - slot[j] = Emit(arm[j], w, false) - slot[j + 4] = Emit(arm[j], (w0 + π, −w1, w2 − π), false) - else: # axes 4 and 6 aligned - θ4 = 0 # tie-break; caller may redistribute - w = W₂₂ > 0 ? (θ4, 0, atan2(W₁₀, W₁₁) − θ4) # θ4 + θ6 fixed - : (θ4, π, θ4 − atan2(−W₁₀, W₁₁)) # θ4 − θ6 fixed - slot[j] = Emit(arm[j], w, true); slot[j + 4] = empty - return slot - -function acosV(c): zero denominator or |c| > 1 + tol ⇒ invalid (branch unreachable); else acos(clamp(c, −1, 1)) -function Emit(arm, w, singular): q = signs ⊙ ((arm, w) + offsets), each through math::WrapToPi (NT54, (−π, π]) -``` - -## Complexity & memory - -- `O(1)`: a fixed set of `sqrt`/`acos`/`atan2` for the arm (`k`, `ψ3` precomputed at construction), - then 3 `atan2` and one 3×3 product per arm branch. -- Memory: 8 optional solutions on the stack; no iteration, no Jacobian, no linear solve. - -## Numerical / embedded notes - -- **Why not a planar two-link elbow:** `a1`, `b` and the forearm offset `a2` break the planar - law-of-cosines triangle used for offset-free arms; OPW folds them into `n`, `n + 2a1` and the virtual - forearm length `k = √(a2² + c3²)` with angle `ψ3`, which is why it covers KUKA/ABB/Fanuc arms. -- **Reachability:** `|acos argument| > 1` prunes that arm branch; `ρ² < 0` (wrist centre inside the - `b`-cylinder) prunes all. Generic poses yield 8 or 4 solutions (shoulder-back branch often out of reach). -- **Boundary precision:** at full stretch `acos` of ≈ 1 amplifies rounding (`δθ ≈ √(2ε)`, ≈ 5e-4 rad in - `float`); tolerate it in tests, never feed `acos` an unclamped argument. -- **Wrist singularity** (`sin θ5 ≈ 0`): the two wrist branches coincide; one solution is emitted with - `θ4 = 0` and `wristSingular = true`. **Shoulder singularity** (`Cx = Cy = 0`, `b = 0`): `atan2(0, 0) = 0`, - any `θ1` is valid — returned as `θ1 = 0`. -- Joint limits are not applied; the caller filters/chooses (e.g. nearest to the current `q`). -- The target is the **flange** pose; for a tool use `target · tool⁻¹`. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/AnalyticalIkOpw.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class AnalyticalIkOpw;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/AnalyticalIkOpw.cpp` → `template class AnalyticalIkOpw;` -- Test: `robotics/kinematics/test/TestAnalyticalIkOpw.cpp` -- Doc: `doc/kinematics/AnalyticalIkOpw.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestAnalyticalIkOpw.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`), NT54 (`math::WrapToPi`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/AnalyticalIkOpw/tests.md b/roadmap/kinematics/AnalyticalIkOpw/tests.md deleted file mode 100644 index f35002e..0000000 --- a/roadmap/kinematics/AnalyticalIkOpw/tests.md +++ /dev/null @@ -1,81 +0,0 @@ -# Analytical IK (OPW) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestAnalyticalIkOpw : public ::testing::Test: - # KUKA KR6 R700 sixx - OpwParameters kr6{ a1 = 0.025, a2 = −0.035, b = 0, c1 = 0.400, c2 = 0.315, c3 = 0.365, c4 = 0.080, - offsets = (0, −π/2, 0, 0, 0, 0), signs = (−1, 1, 1, −1, 1, −1) } - AnalyticalIkOpw ik{ kr6 } - JointVector qTrue{ 0.3, −1.0, 1.2, 0.5, 0.6, 0.7 } -# each case below is a TEST_F(TestAnalyticalIkOpw, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -forward_matches_elementary_chain: - Assert: ik.Forward(q) ≈ Rz(θ1)·Trans(a1,b,c1)·Ry(θ2)·Trans(0,0,c2)·Ry(θ3)·Trans(a2,0,c3) - ·Rz(θ4)·Ry(θ5)·Rz(θ6)·Trans(0,0,c4) (θ = signs⊙q − offsets), for qTrue and q = 0 -home_flange_pose: - Assert: ik.Forward(0) ≈ { [[0,0,1],[0,1,0],[−1,0,0]], (0.785, 0, 0.435) } -every_solution_reproduces_target: - Arrange: target = ik.Forward(qTrue) - Act: sols = ik.Solve(target) - Assert: all 8 slots valid; Forward(sols[k].q) ≈ target (position and R, tol 1e-4) -finds_the_known_configuration: - Assert: slot 0 ≈ qTrue (mod 2π) -wrist_flip_pairs: - Assert: slot j+4 = slot j with q4 + π, −θ5, q6 − π (in model angles), for j = 0..3 -random_poses_round_trip: - Arrange: 200 deterministic pseudo-random q ∈ [−π, π]⁶ (fixed LCG seed, std::array) - Assert: every valid slot reproduces Forward(q) (tol 1e-3); some slot ≈ q (mod 2π, tol 1e-3) -general_offsets_signs_and_lateral_offset: - Arrange: a1=0.10 a2=−0.05 b=0.07 c1=0.50 c2=0.60 c3=0.55 c4=0.10, - offsets=(0.1,−0.2,0.3,0,0.4,−0.5), signs=(1,−1,1,1,−1,1) - Assert: as random_poses_round_trip (exercises b ≠ 0 and every offset/sign path) -wrist_singularity_keeps_the_sum: - Arrange: q = (0.3, −1.0, 1.2, 0.5, 0, 0.7); target = Forward(q) - Assert: slot 0 = (0.3, −1.0, 1.2, 0, 0, 1.2), wristSingular; slot 4 empty; Forward(slot 0) ≈ target -stretched_arm_elbows_coincide: - Arrange: q = (0.3, −0.3, −ψ3 = 0.095598, 0.5, 0.6, 0.7) - Assert: slots 0 and 1 agree to 1e-3; both reproduce the target (tol 1e-3) -unreachable_target_returns_no_solution: - Arrange: flange at (1.5, 0, 0.4) - Assert: all 8 slots empty -``` - -## Reference vectors - -- `Forward(qTrue)`: `p = (0.582764, −0.202939, 0.574882)`, `R = [[−0.707382, −0.375709, 0.598710], - [−0.689753, 0.551987, −0.468563], [−0.154437, −0.744415, −0.649612]]`. -- The 8 solutions for that target (joint space, wrapped): - - | slot | q1 | q2 | q3 | q4 | q5 | q6 | - |------|---------|---------|---------|---------|---------|---------| - | 0 | 0.3 | −1.0 | 1.2 | 0.5 | 0.6 | 0.7 | - | 1 | 0.3 | 0.1977 | −1.0088 | 0.2742 | 1.5525 | 1.1184 | - | 2 | −2.8416 | 3.0760 | 0.9021 | −2.8675 | 1.5770 | 1.1253 | - | 3 | −2.8416 | −2.3360 | −0.7109 | −2.7791 | 0.8687 | 0.8834 | - | 4 | 0.3 | −1.0 | 1.2 | −2.6416 | −0.6 | −2.4416 | - | 5 | 0.3 | 0.1977 | −1.0088 | −2.8674 | −1.5525 | −2.0232 | - | 6 | −2.8416 | 3.0760 | 0.9021 | 0.2741 | −1.5770 | −2.0163 | - | 7 | −2.8416 | −2.3360 | −0.7109 | 0.3625 | −0.8687 | −2.2582 | - -- Wrist-singular target `p = (0.609771, −0.188624, 0.610958)`: 7 slots valid (slot 4 empty). -- Verified in double over 2000 random poses each for KR6 R700, ABB IRB2400 (a1=0.100, a2=−0.135, - c1=0.615, c2=0.705, c3=0.755, c4=0.085, offset₃ = −π/2), Fanuc R2000iB (0.720, −0.225, 0.600, 1.075, - 1.280, 0.235, offset₃ = −π/2) and the `b ≠ 0` set: max pose error `2.3e-12`, the true `q` always found, - 8 or 4 solutions per pose. A `float32`-emulated solver (every operation rounded) over 200 random - KR6 poses: worst pose error `4.3e-5`, worst recovery of the true `q` `2.7e-4` rad; at `qTrue` `3.7e-7`. - -## Edge cases - -- `|acos argument| > 1` ⇒ that arm branch (and its wrist flip) empty. -- Wrist centre inside the `b`-cylinder ⇒ every slot empty. -- Shoulder singularity (`Cx = Cy = 0`) ⇒ `θ1 = 0` returned, solutions still valid. -- `θ5 = ±π` ⇒ singular branch with `θ4 − θ6` fixed. -- Angle wrap ⇒ compare modulo `2π`. diff --git a/roadmap/kinematics/ContinuumKinematics/explanation.md b/roadmap/kinematics/ContinuumKinematics/explanation.md deleted file mode 100644 index ecf5456..0000000 --- a/roadmap/kinematics/ContinuumKinematics/explanation.md +++ /dev/null @@ -1,39 +0,0 @@ -# Continuum Kinematics — Overview - -## What it is -Kinematics for arms that bend continuously instead of pivoting at discrete joints — tendon-driven, -pneumatic or "soft" robots shaped like an elephant's trunk. The standard model treats each section as -a **circular arc of constant curvature**, described by three numbers: how sharply it bends, in which -plane, and over what length. - -## Why it matters (embedded) -Continuum robots reach into cluttered, delicate spaces — inside the body for surgery, around obstacles -for inspection — where a rigid arm cannot go. The constant-curvature model reduces a flexible body to -three parameters per section, small and cheap enough to evaluate on the embedded controller that -drives the tendons or air chambers. - -## How it works (intuition) -Each section bows into a circular arc. The curvature `κ` sets how tight the arc is, the plane angle -`φ` which way it bends, and the arc length `s` how far along it runs; the total bend is `θ = κ·s`. The -tip lies on that circle and the tip frame is the base frame rotated by `θ` within the bending plane. -Writing the tip position as `s` times a function of `θ` avoids dividing by the curvature, so a nearly -straight section needs only a short series instead of a special case that blows up. Chaining section -transforms — like multiplying joint transforms on a rigid arm — gives the whole backbone. For one -section the inverse is closed form: the tip's direction gives the plane, the ratio of its sideways to -forward distance gives half the bend angle, and the straight-line distance to the tip — the chord -`2·sin(θ/2)/κ` — gives the curvature. The "robot-independent" arc map stays separate from the -"robot-specific" step that turns tendon pulls or chamber pressures into arc parameters. - -## Key parameters -- **curvature `κ`** — inverse bend radius; its sign can be folded into the plane angle. -- **bending-plane angle `φ`** — bend direction (undefined when straight). -- **arc length `s`** — section length. - -## Reference -R. J. Webster III, B. A. Jones, "Design and Kinematic Modeling of Constant Curvature Continuum Robots: -A Review," *Int. J. Robotics Research*, 29(13), 2010. - -## See also -`math::SE3Transform` (NT53, per-section transform and composition), `PoseInverseKinematics` (M13, the -iterative multi-section inverse), `Quaternion` -([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), orientation blending). diff --git a/roadmap/kinematics/ContinuumKinematics/implementation.md b/roadmap/kinematics/ContinuumKinematics/implementation.md deleted file mode 100644 index 547cf61..0000000 --- a/roadmap/kinematics/ContinuumKinematics/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# Continuum Kinematics (Constant Curvature) — Implementation Pseudocode - -> Roadmap ref: #M26 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct ArcParameters: # configuration of one section - T kappa # curvature κ (1/m); sign folds into φ: (−κ, φ) ≡ (κ, φ + π) - T phi # bending-plane angle about the section's base z - T length # arc length s - -template -class ContinuumKinematics: - std::array, NumSections> sections -``` - -## Interface - -```cpp -explicit ContinuumKinematics(const std::array, NumSections>& sections) -static SE3Transform SectionTransform(const ArcParameters& arc) # one arc; hot path -SE3Transform Forward() const # tip pose; hot path -static ArcParameters InverseSection(const Vector3& tip) # single section, closed form -``` - -## Algorithm (pseudocode) - -```text -function SectionTransform(arc): # OPTIMIZE_FOR_SPEED, robot-independent map - θ = arc.kappa · arc.length # total bend angle - if |θ| < θ_series: # straight-ish: no 1/κ, no 0/0 - f1 = θ/2 − θ³/24; f2 = 1 − θ²/6 - else: - f1 = 2·sin²(θ/2) / θ; f2 = sin θ / θ # (1 − cos θ)/θ without cancellation - p = arc.length · ( f1·(cos φ, sin φ, 0) + f2·ẑ ) - R = Rz(φ) · Ry(θ) · Rz(−φ) # tangent R·ẑ = (cos φ sin θ, sin φ sin θ, cos θ) = dp/ds - return { R, p } - -function Forward(): # OPTIMIZE_FOR_SPEED - A = Identity - for i in 0..NumSections-1: A = A * SectionTransform(sections[i]) # NT53 compose - return A - -function InverseSection(tip): # tip position in the section base frame, κs < 2π - ρ = ‖(tip.x, tip.y)‖; L = ‖tip‖ - if ρ < ε: return { 0, 0, tip.z } # straight: φ undefined, returned as 0 - φ = atan2(tip.y, tip.x) - θ = 2·atan2(ρ, tip.z) # tan(θ/2) = ρ / z - κ = 2ρ / L² # from L² = 2ρ/κ - s = L · (θ/2) / sin(θ/2) # = θ/κ, finite as θ → 0 - return { κ, φ, s } -``` - -## Complexity & memory - -- `SectionTransform`: `O(1)` — two `sin`/`cos` pairs and a 3×3 build. -- `Forward`: `O(NumSections)` SE(3) products; `InverseSection`: `O(1)`. -- Memory: the section array plus one accumulator; stack only. - -## Numerical / embedded notes - -- **Straight limit:** writing the arc as `s·f(θ)` removes the `1/κ` blow-up; below `θ_series ≈ 1e-2` - (`float`) the series is accurate to `θ⁴/120 < 1e-9`. The result is independent of `φ` when straight. -- **Chord identity:** `‖p‖ = 2|sin(θ/2)|/|κ|` — the basis of `InverseSection` and of the tests. -- Keep the robot-independent map `(κ, φ, s) → SE(3)` separate from the robot-specific actuator map - (tendon lengths / chamber pressures → `(κ, φ, s)`); only the latter changes between hardware. -- Constant curvature is a modelling assumption; gravity and tip loads bend real sections off-arc. -- The single-section inverse returns `κ ≥ 0`, `φ ∈ (−π, π]`; multi-section inverse needs iteration - (M13-style damped least squares on `(κᵢ, φᵢ, sᵢ)`). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/ContinuumKinematics.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `SectionTransform`/`Forward`, and - `extern template class ContinuumKinematics;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ContinuumKinematics.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestContinuumKinematics.cpp` -- Doc: `doc/kinematics/ContinuumKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestContinuumKinematics.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ContinuumKinematics/tests.md b/roadmap/kinematics/ContinuumKinematics/tests.md deleted file mode 100644 index e3eafe5..0000000 --- a/roadmap/kinematics/ContinuumKinematics/tests.md +++ /dev/null @@ -1,50 +0,0 @@ -# Continuum Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestContinuumKinematics : public ::testing::Test: - ArcParameters quarter{ π/2, 0, 1 } # θ = π/2, radius 2/π - ArcParameters general{ 2.0, 0.7, 1.2 } # θ = 2.4 - ContinuumKinematics twoSections{ { general, general } } -# each case below is a TEST_F(TestContinuumKinematics, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -straight_section_is_pure_translation: - Assert: SectionTransform({0, 0, 1}) ≈ { I, (0, 0, 1) } -quarter_circle_bend: - Assert: SectionTransform(quarter).p ≈ (2/π)·(1, 0, 1) = (0.636620, 0, 0.636620); R ≈ Ry(π/2) -bending_plane_angle_rotates_tip: - Assert: SectionTransform({π/2, π/2, 1}).p ≈ (0, 0.636620, 0.636620) -tip_distance_equals_chord: - Assert: ‖SectionTransform(arc).p‖ ≈ 2·|sin(κs/2)|/|κ| for quarter (0.900316), general (0.932039), - and {−1.5, 0.3, 0.8} (0.752857) -tip_tangent_matches_bend: - Assert: SectionTransform(general).R·ẑ ≈ (cos 0.7·sin 2.4, sin 0.7·sin 2.4, cos 2.4) = (0.516623, 0.435145, −0.737394) -forward_composes_sections: - Assert: twoSections.Forward() ≈ SectionTransform(general) * SectionTransform(general); p ≈ (0.348960, 0.293925, −0.498082) -inverse_recovers_arc_parameters: - Assert: InverseSection(SectionTransform(a).p) ≈ a for a ∈ {quarter, general}; - {−1.5, 0.3, 0.8} ⇒ (1.5, 0.3 − π, 0.8) (sign folded into φ) -near_zero_curvature_uses_series: - Assert: SectionTransform({1e-9, 0.4, 1}) ≈ { I, (0, 0, 1) }; SectionTransform({1e-5, 0.4, 1}).p ≈ (4.605e-6, 1.947e-6, 1), finite -``` - -## Reference vectors - -- Straight unit section ⇒ `(0, 0, 1)`, `R = I`. -- `κ = π/2, s = 1, φ = 0` ⇒ tip `(0.636620, 0, 0.636620)`, chord `2√2/π = 0.900316`. -- `κ = 2, φ = 0.7, s = 1.2` ⇒ tip `(0.664416, 0.559630, 0.337732)`, chord `sin 1.2 = 0.932039`. -- All closed-form tips agree with a 4000-step numerical integration of the tangent `R(s)·ẑ` to `1e-8`. - -## Edge cases - -- Full loop `θ = 2π` ⇒ tip at the base `(0, 0, 0)`; `InverseSection` is limited to `κs < 2π`. -- Negative `κ` ⇒ same arc as `(|κ|, φ + π)`. -- Straight tip in `InverseSection` ⇒ `κ = 0`, `φ = 0`, `s = z`. -- Multi-section inverse ⇒ iterative (M13), not here. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md deleted file mode 100644 index 6d4ccf9..0000000 --- a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md +++ /dev/null @@ -1,39 +0,0 @@ -# Parallel Manipulator Kinematics — Overview - -## What it is -Kinematics for robots whose moving platform is held by several chains in parallel: the six-legged -**Stewart–Gough** hexapod, driven by leg lengths, and the pick-and-place **Delta**, driven by three -rotary arms whose parallelogram forearms keep the effector level. Inverse kinematics maps a platform -pose to actuator values; forward kinematics maps actuator values back to the pose. - -## Why it matters (embedded) -Parallel robots are stiff, fast and accurate — the workhorses of high-speed packaging and motion -platforms. Their servo layer needs actuator commands from a desired pose on every tick, and both -mechanisms make that a short, closed-form, per-chain computation. Forward kinematics, needed for state -estimation, is cheap for the Delta and a few Newton steps for the hexapod. - -## How it works (intuition) -For the **hexapod**, each leg is independent in the inverse problem: move the platform anchor into the -base frame and measure its distance to the base anchor. The forward problem couples all legs, so it is -solved by Newton's method on the pose: predict the leg lengths, compare, and correct position and -orientation using the leg Jacobian (each row is the leg direction and its moment about the platform -origin). Several poses share the same leg lengths, so the initial guess picks the assembly mode; -near singular poses a little damping keeps the correction finite. -For the **Delta**, each arm's elbow moves on a circle, and the forearm end must lie on a sphere around -the effector attachment. Rotating the target into the arm's plane reduces that to one equation -`a·cos θ + b·sin θ = c`, solved with one `atan2` and one `acos`. Going forward, each elbow (shifted -inward by the effector radius) is the centre of a sphere of forearm length, and the effector centre is -the lower of the two points where the three spheres meet. - -## Key parameters -- **Hexapod:** base and platform anchors; Newton tolerance, damping and iteration limit. -- **Delta:** base radius, effector radius, upper-arm length, forearm length. - -## Reference -J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, "Delta, a Fast Robot with Parallel -Geometry," *Int. Symp. on Industrial Robots*, 1988. - -## See also -`math::SE3Transform` (NT53, platform pose and retraction), upstream `LevenbergMarquardt` (NT64, the -Newton step), -`ManipulabilityIndex` (M11, singularity monitoring). diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md deleted file mode 100644 index e87033c..0000000 --- a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md +++ /dev/null @@ -1,135 +0,0 @@ -# Parallel Manipulator Kinematics (Stewart–Gough / Delta) — Implementation Pseudocode - -> Roadmap ref: #M23 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Two mechanisms, two classes: the Stewart–Gough hexapod (six prismatic legs, 6-DOF platform) and the -Delta robot (three revolute actuated arms with parallelogram forearms, 3-DOF translation). Their -actuated variables differ (leg lengths vs arm angles), so they share no data model. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct StewartGoughGeometry: - std::array, 6> baseAnchors # bᵢ, base frame - std::array, 6> platformAnchors # pᵢ, platform frame - -template -struct StewartForwardConfig: - T tolerance; T damping; std::size_t maxIterations # damping λ: 0 ⇒ plain Newton - -template -struct StewartForwardResult: - SE3Transform pose; std::size_t iterations; bool converged - -template # implements optimization::LeastSquaresProblem, 6, 6> -class StewartLegResidual: # used by Forward with upstream LevenbergMarquardt (NT64) - const StewartGoughKinematics& kinematics; std::array lengths - -template -class StewartGoughKinematics: - StewartGoughGeometry geometry - StewartForwardConfig config - -template -struct DeltaGeometry: # base plane z = 0, effector below (z < 0) - T baseRadius # R: centre → shoulder axis (triangle side = 2√3·R) - T effectorRadius # r: effector centre → forearm attachment - T upperArm # rf - T forearm # re (parallelogram length) - # arm i shoulder azimuth φᵢ = 0°, 120°, 240°; θᵢ = 0 ⇒ upper arm horizontal, outward; θ > 0 ⇒ down - -template -class DeltaKinematics: - DeltaGeometry geometry - std::array cosPhi, sinPhi # precomputed -``` - -## Interface - -```cpp -StewartGoughKinematics(const StewartGoughGeometry& geometry, const StewartForwardConfig& config) -std::array Inverse(const SE3Transform& pose) const # leg lengths; hot path -Matrix6 LegJacobian(const SE3Transform& pose) const # l̇ = J·(v; ω) -StewartForwardResult Forward(const std::array& lengths, const SE3Transform& guess) const - -explicit DeltaKinematics(const DeltaGeometry& geometry) -std::optional> Inverse(const Vector3& effector) const # returns (θ₁, θ₂, θ₃); hot path -std::optional> Forward(const Vector3& angles) const # effector position -``` - -## Algorithm (pseudocode) - -```cpp -# ---- Stewart–Gough ---- -function Inverse(pose): # OPTIMIZE_FOR_SPEED, closed form - for i: lengths[i] = ‖pose.R·pᵢ + pose.p − bᵢ‖ - return lengths - -function LegJacobian(pose): # row i = (nᵢᵀ, (R·pᵢ × nᵢ)ᵀ), nᵢ = unit leg vector - for i: a = pose.R·pᵢ; n = Normalize(a + pose.p − bᵢ); row i = (n ; CrossProduct(a, n)) - # (v; ω): platform-origin velocity and angular velocity, base frame (NT53 order); statics: wrench (f; n) = Jᵀ·legForces - -# Newton on SE(3) through upstream optimization::LevenbergMarquardt (NT64): -function StewartLegResidual::Residual(x): return kinematics.Inverse(x) − lengths -function StewartLegResidual::Jacobian(x): return kinematics.LegJacobian(x) # (v; ω) increments -function StewartLegResidual::Retract(x, δ): # left (base-frame) retraction - return { math::SE3Transform::Exp((0; δ.angular), 1).R · x.R, x.p + δ.linear } - -function Forward(L, guess): - r = optimization::LevenbergMarquardt{ StewartLegResidual{ *this, L }, ConstantDamping{ λ² }, - { tolerance, +∞, maxIterations } }.Solve(guess) - return { r.x, r.iterations, r.converged } # λ = 0: δ = −J⁻¹f (plain Newton) - -# ---- Delta ---- -function ArmAngle(i, P): # circle (elbow) ∩ sphere (forearm) in arm i's plane - x' = cosφᵢ·P.x + sinφᵢ·P.y; y' = −sinφᵢ·P.x + cosφᵢ·P.y; z = P.z - X = x' + r − R - a = 2·rf·X; b = −2·rf·z; c = X² + y'² + z² + rf² − re² # a·cos θ + b·sin θ = c - ρ = √(a² + b²); if |c| > ρ: return nullopt # out of reach - return atan2(b, a) − acos(c / ρ) # elbow-out root (larger cos θ) for z < 0 - -function Inverse(P): # OPTIMIZE_FOR_SPEED - return (ArmAngle(0, P), ArmAngle(1, P), ArmAngle(2, P)) # nullopt if any arm fails - -function Forward(θ): # three-sphere intersection, closed form - Cᵢ = Rz(φᵢ)·(R − r + rf·cos θᵢ, 0, −rf·sin θᵢ) # elbow shifted by −effector radius - ex = Normalize(C₂ − C₁); d = ‖C₂ − C₁‖; i = ex·(C₃ − C₁) - ey = Normalize(C₃ − C₁ − i·ex); j = ey·(C₃ − C₁); ez = ex × ey - x = d/2; y = (i² + j² − 2·i·x) / (2j); h² = re² − x² − y² # equal radii re - if h² < 0: return nullopt - return the one of C₁ + x·ex + y·ey ± √h²·ez with the smaller z # effector below the base -``` - -## Complexity & memory - -- Stewart `Inverse` / `LegJacobian`: `O(6)`; `Forward`: `O(iters·6³)` (one 6×6 LU per step). -- Delta `Inverse`: 3 × (`sqrt`, `acos`, `atan2`); `Forward`: `O(1)`, one `sqrt`. -- Memory: geometry + one `6×6` matrix; stack only. - -## Numerical / embedded notes - -- Parallel robots invert the serial difficulty: Stewart IK is closed form, FK needs Newton and has up to - 40 assembly modes — `guess` selects one (seed from the previous cycle). The Delta's special - geometry (parallelograms keep the effector parallel to the base) makes both directions closed form. -- `LegJacobian` is the inverse Jacobian of the platform; it is singular where the platform gains - uncontrollable motion (the symmetric fixture at yaw 90°). `λ > 0` keeps the step finite there; - the leg residual still converges but the pose along the singular direction is ill-determined. -- Delta branch choice: `atan2(b, a) ± acos(·)` are elbow-out / elbow-in; for `z < 0`, `b > 0` so the - minus root has the larger `cos θ` (elbow out). The lower sphere intersection is the physical effector. -- The 6×6 step solve (`LuDecomposition`) happens inside the upstream solver (NT64). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Headers: `robotics/kinematics/StewartGoughKinematics.hpp`, `robotics/kinematics/DeltaKinematics.hpp` — - `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Inverse`, and - `extern template class StewartGoughKinematics;` / `extern template class DeltaKinematics;` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/StewartGoughKinematics.cpp`, `robotics/kinematics/DeltaKinematics.cpp` - → `template class StewartGoughKinematics;` / `template class DeltaKinematics;` -- Tests: `robotics/kinematics/test/TestStewartGoughKinematics.cpp`, `robotics/kinematics/test/TestDeltaKinematics.cpp` -- Doc: `doc/kinematics/ParallelManipulatorKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; both tests → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`), NT64 (`optimization::LevenbergMarquardt`, `ConstantDamping`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/tests.md b/roadmap/kinematics/ParallelManipulatorKinematics/tests.md deleted file mode 100644 index 29427f1..0000000 --- a/roadmap/kinematics/ParallelManipulatorKinematics/tests.md +++ /dev/null @@ -1,80 +0,0 @@ -# Parallel Manipulator Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture — Stewart–Gough - -```cpp -class TestStewartGoughKinematics : public ::testing::Test: - # leg k = 0..5, j = k/2, s = (k even ? −1 : +1) - # bₖ = 1.0·(cos β, sin β, 0), β = 120°·j + s·15°; pₖ = 0.6·(cos γ, sin γ, 0), γ = 120°·j + s·45° - StewartGoughKinematics hexapod{ geometry, { tol = 1e-5, λ = 0, 50 } } - SE3Transform home{ I, (0, 0, 0.8) } - SE3Transform tilted{ Rz(0.1)·Rx(0.05), (0.05, −0.03, 0.85) } -# each case below is a TEST_F(TestStewartGoughKinematics, ) -``` - -## Test cases — Stewart–Gough - -```text -home_legs_are_equal: - Assert: Inverse(home) ≈ 0.980189 for all six legs -raising_the_platform_lengthens_every_leg: - Assert: Inverse({ I, (0, 0, 0.9) }) ≈ 1.063376 for all six legs -inverse_of_a_general_pose: - Assert: Inverse(tilted) ≈ (0.955703, 1.037816, 1.049122, 1.083232, 0.981789, 1.042429) -leg_jacobian_matches_finite_difference: - Arrange: twist (v; ω) = (0.1, −0.2, 0.05; 0.3, −0.1, 0.2), pose perturbed as p + εv, Exp(εω)·R - Assert: LegJacobian(tilted)·twist ≈ (Inverse(perturbed) − Inverse(tilted)) / ε (ε = 1e-3, tol 1e-3) -forward_recovers_the_pose: - Act: r = Forward(Inverse(tilted), { Rz(0.15), (0, 0, 0.8) }) - Assert: r.converged; r.iterations ≤ 6; r.pose ≈ tilted (tol 1e-4) -assembly_mode_is_selected_by_the_guess: - Arrange: L = Inverse(home) - Assert: Forward(L, { I, (0,0,0.7) }).pose.p ≈ (0,0,0.8); Forward(L, { I, (0,0,−0.7) }).pose.p ≈ (0,0,−0.8) -singular_pose_step_stays_finite: - Arrange: target { Rz(90°), (0,0,0.8) } (det LegJacobian = 0); λ = 1e-3; guess yaw 80° - Assert: pose finite; ‖Inverse(r.pose) − L‖ < 1e-4 -``` - -## Fixture — Delta - -```cpp -class TestDeltaKinematics : public ::testing::Test: - DeltaKinematics delta{ { R = 0.1, r = 0.03, rf = 0.2, re = 0.45 } } -# each case below is a TEST_F(TestDeltaKinematics, ) -``` - -## Test cases — Delta - -```text -zero_angles_put_the_effector_on_the_axis: - Assert: Forward((0,0,0)) ≈ (0, 0, −0.36) (√(re² − (R − r + rf)²) = √(0.2025 − 0.0729)) -inverse_of_known_points: - Assert: Inverse((0.05, −0.03, −0.40)) ≈ (0.042468, 0.371178, 0.205749) - Inverse((−0.08, 0.06, −0.32)) ≈ (0.195443, −0.538250, −0.084938) -inverse_forward_round_trip: - Assert: Forward(Inverse(P)) ≈ P for P ∈ {(0.05,−0.03,−0.40), (−0.08,0.06,−0.32), (0.12,0.10,−0.45)} (tol 1e-5) -forward_inverse_round_trip: - Assert: Inverse(Forward(θ)) ≈ θ over the grid θᵢ ∈ {−0.6, 0, 0.6, 1.2} (all 64 assemblable; tol 1e-4) -forearm_constraint_holds: - Assert: ‖elbowᵢ − (P + Rz(φᵢ)·(r,0,0))‖ ≈ re for each arm at Inverse(P) -unreachable_point_has_no_solution: - Assert: Inverse((0, 0, −0.9)) == nullopt -``` - -## Reference vectors - -- Hexapod: `L₀ = √(1 + 0.36 − 1.2·cos 30° + 0.64) = 0.980189`; singular at yaw `90°` - (`det J` from `−0.8417` at 0° through `0`), legs there `(1.183216, 1.612452) × 3`. -- Newton from the stated guess converges in 4 iterations (double); with `λ = 1e-3` near the - singularity the leg residual reaches `7e-7` while the yaw is only fixed to `≈ 0.1°`. -- Delta: home `(0, 0, −0.36)`; `θ → Forward → Inverse` reproduces `θ` to `2e-15` (double) over 1000 - random `θ ∈ [−0.6, 1.2]³`; the minus root was the elbow-out root in all 46 265 sampled arm solves. - -## Edge cases - -- Stewart: wrong assembly-mode guess converges to a different valid pose (documented). -- Stewart: non-converging lengths ⇒ `converged == false`, best-effort pose, finite. -- Delta: `h² < 0` in `Forward` ⇒ nullopt (angles not assemblable). -- Delta: target on the reach boundary ⇒ `|c| = ρ`, single (tangent) solution; clamp `acos` argument. diff --git a/roadmap/planning/ConvexCollision/explanation.md b/roadmap/planning/ConvexCollision/explanation.md deleted file mode 100644 index 7f58df2..0000000 --- a/roadmap/planning/ConvexCollision/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# GJK + EPA Convex Collision — Overview - -## What it is -A general method for measuring the distance between two convex shapes, or how deeply they overlap, -that works for any convex shape that can answer one question: "which of your points lies farthest in this -direction?" That covers spheres, capsules, boxes and convex hulls of a few points. - -## Why it matters (embedded) -Real robots and their surroundings need more than spheres and capsules: boxes for tables and shelves, -hulls for grippers and tools. GJK gives exact distances for every combination with one compact algorithm. -EPA adds the penetration depth and direction that contact handling and collision recovery need. Both run -in bounded time and fixed memory. - -## How it works (intuition) -Two shapes overlap exactly when their "difference" set, every point of one minus every point of the -other, contains the origin; their distance is how far that set is from the origin. GJK never builds the set. -It walks towards the origin with a small simplex of up to four points, each found by asking the shapes -for their farthest points in a direction. Rounded shapes are handled as a core plus a radius, so their -distance is exact. When the cores overlap, EPA starts from GJK's final simplex and grows a polytope inside -the difference set until its face nearest the origin stops moving. That face gives the depth and the -contact normal. - -## Key parameters -- **shape descriptions** — kind, pose, radius, box half extents, hull vertices (bounded count). -- **face budget** — compile-time cap on the EPA polytope. - -## Reference -E. G. Gilbert, D. W. Johnson, S. S. Keerthi, "A fast procedure for computing the distance between complex -objects in three-dimensional space," *IEEE J. Robotics and Automation*, 4(2), 1988; G. van den Bergen, -*Collision Detection in Interactive 3D Environments* (2003), Ch. 4. - -## See also -`PrimitiveDistance` (P1, closed forms and test references), `SelfCollision` (P4), `math::SE3Transform` (NT53). diff --git a/roadmap/planning/ConvexCollision/implementation.md b/roadmap/planning/ConvexCollision/implementation.md deleted file mode 100644 index 8a032db..0000000 --- a/roadmap/planning/ConvexCollision/implementation.md +++ /dev/null @@ -1,137 +0,0 @@ -# GJK + EPA Convex Distance and Penetration — Implementation Pseudocode - -> Roadmap ref: #P5 (★★★★☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) - -Distance, closest points, penetration depth and contact normal between any two convex shapes described by -a support function: spheres, capsules, oriented boxes and bounded convex hulls. It covers the pairs P1 has -no closed form for (capsule–box, box–box, hull–anything). -- **Separated:** Gilbert–Johnson–Keerthi on the shape cores, with the radii subtracted afterwards (the - "core + margin" representation). Rounded shapes stay exact, and the iteration count stays small. -- **Overlapping:** the expanding polytope algorithm on the Minkowski difference, with bounded face and - edge storage. - -## Data structures - -```cpp -enum class ShapeKind : std::uint8_t { sphere, capsule, box, hull } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct ConvexShape: - ShapeKind kind - math::SE3Transform pose # shape frame in the world (NT53) - T radius # margin: sphere / capsule radius (0 for box and hull) - math::Vector3 halfExtents # box - T halfLength # capsule core along the local z axis - std::array, MaxHullVertices> vertices; std::size_t vertexCount # hull, shape frame - static ConvexShape Sphere(const math::Vector3& centre, T radius) - static ConvexShape Capsule(const math::Vector3& a, const math::Vector3& b, T radius) - static ConvexShape Box(const math::SE3Transform& pose, const math::Vector3& halfExtents) - static ConvexShape Hull(const math::SE3Transform& pose, std::span> vertices) # really_assert ≤ MaxHullVertices - math::Vector3 CoreSupport(const math::Vector3& direction) const # farthest core point along direction - math::Vector3 Support(const math::Vector3& direction) const # core + radius·direction/‖direction‖ - -template -struct ConvexProximity: - T distance # ≥ 0 separated; < 0 penetration depth - math::Vector3 onFirst, onSecond # witness points (contact points when overlapping) - math::Vector3 normal # unit, from the first shape to the second - bool converged # false if an iteration or face budget ran out - std::size_t iterations - -template -class ConvexCollision # stateless; static members only -``` - -## Interface - -```cpp -static ConvexProximity Query(const ConvexShape& a, const ConvexShape& b) # hot path -``` - -## Algorithm (pseudocode) - -```text -CoreSupport(d), world frame (l = Rᵀd in the shape frame): - sphere: centre; capsule: centre ± halfLength·ẑ_world (sign of d·ẑ_world) - box: c + R·(sign(l) ⊙ h) with sign(0) = +1; hull: c + R·argmax_v (v·l) - -function Gjk(a, b, support): # support = CoreSupport (distance) or Support (EPA seed) - v = centre(a) − centre(b) (any non-zero point of A ⊖ B); simplex W = ∅ with witnesses (aᵢ, bᵢ) - repeat at most 32 times: - aᵢ = support_a(−v); bᵢ = support_b(v); w = aᵢ − bᵢ - if ‖v‖² − v·w ≤ 1e-6·‖v‖² or w ∈ W: stop # no further progress - W ∪= { w }; (v, λ, W) = ClosestOnSimplex(W) # keep only the support subset - if |W| == 4 or ‖v‖² ≤ 1e-12: return intersecting(W) - return separated: distance ‖v‖, onA = Σ λᵢ aᵢ, onB = Σ λᵢ bᵢ - -function ClosestOnSimplex(W): # Johnson's sub-algorithm by enumeration (≤ 15 subsets) - for every non-empty subset S of W (k = |S| ≤ 4): - if k == 1: λ = (1) else solve G·μ = −Dᵀ·s₀ with D = [s₁ − s₀ … s_{k−1} − s₀], G = DᵀD (Cholesky, skip if singular) - λ = (1 − Σμ, μ) - if all λ ≥ 0: candidate x = Σ λᵢ sᵢ - return the candidate with the smallest ‖x‖ (the closest point of the hull lies in one face's relative interior) - -function Query(a, b): - core = Gjk(a, b, CoreSupport) - if core separated: # exact whether or not the radii overlap - n = (onB − onA)/‖·‖ - return { core.distance − r_a − r_b, onA + r_a·n, onB − r_b·n, n, true } # negative ⇒ rounded overlap - return Epa(a, b, Gjk(a, b, Support).simplex) # cores intersect - -function Epa(a, b, W): # cores overlap: expand a polytope of A ⊖ B - drop duplicate points of W; then add supports along the 6 axes and 8 diagonals, keeping a point only if - it raises the rank, until four points span a volume - c = centroid of the tetrahedron # orient faces away from c, NOT from the origin: - faces = 4 triangles with outward normals # the origin may lie on the boundary (touching, axis-aligned boxes) - repeat at most MaxFaces times: - f = face with the smallest distance n_f·v_f to the origin; w = Support_a(n_f) − Support_b(−n_f) - if w·n_f − dist_f < 1e-6: break # converged - remove faces that see w, coplanar ones included (n·(w − v) > −1e-6·scale); collect the horizon edges - (edges seen once, kept in order) - add one face (edge, w) per horizon edge; if faces > MaxFaces: converged = false; break - depth = dist_f; normal = n_f; λ = barycentric of depth·n_f in f - return { −depth, Σλ·a_witness, Σλ·b_witness, normal, converged } -``` - -## Complexity & memory - -- GJK: typically 2–5 iterations for polytopes, each up to 15 subset solves (≤ 3×3). EPA: up to 21 iterations - and 44 faces for deep random box overlaps in the validation, so `MaxFaces = 64` leaves headroom. -- Memory: `MaxFaces` triangles (3 indices plus a normal and a distance each), `MaxFaces/2 + 2` polytope - vertices with witness points (a closed triangle mesh has `F = 2V − 4`), a horizon list of at most - `MaxFaces` edges. All on the stack; with - `MaxFaces = 64` that is about 6 KB in `float`. No heap. - -## Numerical / embedded notes - -- **Core + margin.** Running GJK on the cores (point, segment) of rounded shapes and subtracting the radii - is exact and converges in a couple of iterations. GJK on the full sphere would only approach the answer - asymptotically. EPA on a smooth shape converges slowly (5e-6 depth error for spheres in the validation), - so it runs only when the cores themselves intersect. -- **Interior-point orientation.** A blown-up simplex can have the origin exactly on a face (axis-aligned - boxes, touching shapes). Orienting the faces from the origin then gives inconsistent normals and wrong - depths. Orienting them away from the initial tetrahedron's centroid fixed exactly that case in the - validation, where the axis-aligned box pair came out as 0.150756 instead of 0.5. -- **Coplanar faces count as visible.** Box supports with tied coordinates return points inside the faces of - A ⊖ B. With a strict `> 0` visibility test, a new support point can be collinear with a horizon edge, and - the zero-area face it creates poisons EPA with NaNs (identical boxes in the validation). Removing coplanar - faces as well prevents it: 634 axis-aligned overlaps full of ties then match the separating-axis depth - exactly. -- Johnson's sub-algorithm by explicit enumeration trades a few extra small solves for robustness. Signed - volumes (Montanari et al., 2017) are a faster drop-in if profiling demands it. -- Float tolerances (`1e-6` relative) bound the iterations; `converged = false` reports budget exhaustion - instead of looping. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/planning/ConvexCollision.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Query`, and - `extern template class ConvexCollision;` / `extern template struct ConvexShape;` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/planning/ConvexCollision.cpp` → the same instantiations. -- Test: `robotics/planning/test/TestConvexCollision.cpp` -- Doc: `doc/planning/ConvexCollision.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.planning_test`. -- Depends on: NT53 (`SE3Transform`); upstream `Geometry3D`, `CholeskyDecomposition`; P1 in the tests (closed-form references). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/ConvexCollision/tests.md b/roadmap/planning/ConvexCollision/tests.md deleted file mode 100644 index 679d484..0000000 --- a/roadmap/planning/ConvexCollision/tests.md +++ /dev/null @@ -1,55 +0,0 @@ -# GJK + EPA — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestConvexCollision : public ::testing::Test: - using Collision = ConvexCollision; using Shape = ConvexShape - Shape boxA{ Box({ I, (0,0,0) }, (0.5, 0.4, 0.3)) } - Shape boxB{ Box({ Rot((1,1,0)/√2, 0.7), (1.4, 0.6, 0.5) }, (0.3, 0.3, 0.2)) } - Shape cube{ Box({ I, 0 }, (1, 1, 1)) } - # ExactBoxBox(a, b): min over vertex–box (P1 PointBox) and edge–edge (P1 SegmentSegment) distances - # SatDepth(a, b): minimum overlap over the 15 separating axes of two boxes -# each case below is a TEST_F(TestConvexCollision, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -rounded_shapes_match_the_closed_forms: - Assert: Query(Sphere((0,0,0), 1), Sphere((3,1,0), 0.5)) ⇒ distance 1.662278, onFirst (0.948683, 0.316228, 0) - Query(Capsule((0,0,0), (1,2,3), 0.1), Capsule((2,0,1), (0,1,−1), 0.2)) ⇒ 0.648683 # P1: 0.948683 − 0.3 - Query(Sphere((2,1,0), 0.3), Box({ Rz(π/6), (1,0,0) }, (0.5,0.25,0.1))) ⇒ 0.573763 -box_pair_distance_matches_the_exact_value: - Assert: Query(boxA, boxB) ⇒ distance ≈ 0.511063 == ExactBoxBox; onFirst (0.5, 0.391106, 0.3), onSecond (1.008894, 0.391106, 0.347032) -capsule_box_and_hull_pairs: - Assert: Query(Capsule((1,−0.5,0.8), (1,0.5,0.8), 0.1), boxA) ⇒ 0.607107 # core to the edge x = 0.5, z = 0.3: √0.5 − 0.1 - Query(Hull({ I, (2,0,0) }, unit tetrahedron (0,0,0),(1,0,0),(0,1,0),(0,0,1)), Sphere((0,0,0), 1)) ⇒ 1.0 -rounded_overlap_with_separated_cores_is_exact: - Assert: Query(cube, Sphere((1.8, 0, 0), 1)) ⇒ distance −0.2, normal (1, 0, 0), converged -epa_resolves_axis_aligned_boxes: - Assert: Query(cube, Box({ I, (1.5, 0, 0) }, (1, 1, 1))) ⇒ distance −0.5, normal (1, 0, 0) # origin on the blown-up simplex boundary -epa_matches_separating_axes_for_rotated_boxes: - Assert: Query(boxA, Box({ Rot((1,1,0)/√2, 0.7), (0.6, 0.3, 0.2) }, (0.3, 0.3, 0.2))) ⇒ distance ≈ −0.291106 == −SatDepth, normal (1, 0, 0) -symmetric_and_tied_configurations_stay_finite: - Assert: Query(boxA, boxA) ⇒ distance −0.6, normal ±ẑ, converged # identical boxes, same pose - for axis-aligned boxes with half extents and centres on a 0.25 grid (fixed list of 50): −distance == SatDepth (1e-5), no NaN -random_box_pairs_agree_with_exact_references: - Arrange: 300 box pairs from a fixed list (seeded xorshift32 in the test): half extents in [0.2, 1], random rotations, centres within ±0.6 - Assert: separated: |distance − ExactBoxBox| < 1e-5; overlapping: |−distance − SatDepth| < 1e-4; all converged -``` - -## Reference vectors - -- `boxA`/`boxB` separation `0.511063` (exact vertex/edge enumeration agrees to `3e-17` in double). -- Validation (double): 300 random box pairs. Separated pairs match the exact distance to `3e-17`; overlapping - pairs match the separating-axis depth to `7e-16`, with at most 15 EPA iterations and 32 faces. -- Sphere–box overlap through EPA (cores intersecting) is accurate to `5e-6` (smooth-surface convergence). -- 500 deep random overlaps (centres within ±0.1): exact to `1e-15`, at most 21 iterations and 44 faces; 634 axis-aligned - overlaps with tied coordinates: exact, no NaN. - -## Edge cases - -- Touching rounded shapes (core distance = sum of radii) ⇒ distance 0 through the core path. diff --git a/roadmap/planning/RrtConnect/explanation.md b/roadmap/planning/RrtConnect/explanation.md deleted file mode 100644 index 2ca0b63..0000000 --- a/roadmap/planning/RrtConnect/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# RRT-Connect — Overview - -## What it is -A motion planner that finds a collision-free path for a robot arm in joint space by growing two random -trees, one from the start and one from the goal, until they meet. It does not need a map of free space, -only a test that says whether a given configuration collides. - -## Why it matters (embedded) -Arms with six or seven joints have configuration spaces far too big for grids. Sampling planners like -RRT-Connect find a path in milliseconds to seconds for typical pick-and-place scenes. With fixed node pools -and an injected random generator they run without allocation and give the same answer every time for a -given seed, which is what an embedded controller and its tests need. - -## How it works (intuition) -Each step picks a random configuration and grows one tree a short step towards it, if that step is -collision-free. The other tree then grows towards the new node as far as it can in a straight line. If it -reaches it, the trees are connected and the path is read off by walking both back to their roots. The trees -take turns, which makes the search reach out from both ends at once. The raw path zig-zags, so a -shortcutting pass repeatedly tries to replace a stretch of waypoints with one straight, collision-free -segment. - -## Key parameters -- **step size** and **collision resolution** — growth per step and how finely edges are checked. -- **node pool size** (compile-time) and **iteration limit**. -- **sampler** and **validity test** — supplied by the caller; the sampler must not correlate consecutive draws. - -## Reference -J. J. Kuffner, S. M. LaValle, "RRT-Connect: An efficient approach to single-query path planning," *ICRA* -2000; R. Geraerts, M. H. Overmars, "Creating high-quality paths for motion planning," *IJRR*, 26(8), 2007 -(shortcutting). - -## See also -`SelfCollision` (P4) and `ConvexCollision` (P5), the usual validity test; `TimeOptimalPathParameterization` (M27), -time-scales the result. diff --git a/roadmap/planning/RrtConnect/implementation.md b/roadmap/planning/RrtConnect/implementation.md deleted file mode 100644 index 7205ba8..0000000 --- a/roadmap/planning/RrtConnect/implementation.md +++ /dev/null @@ -1,113 +0,0 @@ -# RRT-Connect in Joint Space with a Fixed Node Pool — Implementation Pseudocode - -> Roadmap ref: #P6 (★★★★☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) - -Bidirectional rapidly-exploring random trees (Kuffner & LaValle, 2000) for collision-free joint-space -paths. One tree grows from the start and one from the goal. Each iteration extends one tree one step -towards a random sample, then greedily connects the other tree to the new node, and the trees swap roles. -Nodes live in compile-time pools. The sampler, the configuration validity test (typically P4 plus -environment checks with P1/P5) and the shortcutting randomness are all injected, so a seeded run is exactly -reproducible. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct RrtConnectParameters: - T stepSize # maximum joint-space extension per step (rad) - T collisionResolution # edge validity: checked every ≤ this distance - std::size_t maxIterations - -enum class PlanStatus : std::uint8_t { found, startInCollision, goalInCollision, poolExhausted, iterationLimit } - -template # static_assert(MaxNodes ≤ 65535) -class RrtConnect: - using JointVector = math::Vector - struct Tree: std::array configuration; std::array parent; std::size_t size - Tree trees[2] # [0] rooted at start, [1] at goal - std::array path; std::size_t pathLength - std::size_t iterations - RrtConnectParameters parameters -``` - -## Interface - -```cpp -explicit RrtConnect(const RrtConnectParameters& parameters) -template -PlanStatus Plan(const JointVector& start, const JointVector& goal, Sample&& sample, IsFree&& isFree) # sample() ⇒ JointVector -template -void Shortcut(std::size_t attempts, Uniform&& uniform, IsFree&& isFree) # uniform() ⇒ [0, 1) -std::span Path() const; std::size_t Iterations() const; std::size_t NodeCount() const -``` - -## Algorithm (pseudocode) - -```text -function EdgeFree(a, b): # n = ⌈‖b − a‖ / resolution⌉ interior checks plus b - return isFree(a + (b − a)·k/n) for k = 1 … n - -function Extend(tree, x): # one step towards x - i = argmin_j ‖tree.q_j − x‖ # linear scan (bounded pool) - d = x − tree.q_i; L = ‖d‖; q_new = (L ≤ stepSize) ? x : tree.q_i + d·stepSize/L - if not EdgeFree(tree.q_i, q_new): return trapped - if tree.size == MaxNodes: return full - append q_new with parent i; return (L ≤ stepSize) ? reached : advanced - -function Connect(tree, x): repeat Extend(tree, x) while it returns advanced - -function Plan(start, goal, sample, isFree): - if not isFree(start): return startInCollision; if not isFree(goal): return goalInCollision - if EdgeFree(start, goal): path = [start, goal]; return found # trivial case, no sampling - reset trees; A = tree 0 (start), B = tree 1 (goal) - for iterations = 1 … maxIterations: - status = Extend(A, sample()) - if status == full: return poolExhausted - if status != trapped: - status = Connect(B, A.newest) - if status == full: return poolExhausted - if status == reached: path = branch(A to its root) joined with branch(B to its root), - oriented start → goal; return found - swap(A, B) - return iterationLimit - -function Shortcut(attempts, uniform, isFree): # random shortcutting: drop interior waypoints a straight edge can bypass - repeat attempts times: - i = ⌊uniform·len⌋; j = ⌊uniform·len⌋; order so i < j; if j − i < 2: continue - if EdgeFree(path[i], path[j]): remove path[i+1 … j−1] -``` - -## Complexity & memory - -- Each `Extend` is `O(n)` for the nearest-neighbour scan over `n` nodes plus `O(L/resolution)` validity checks. - The validity checks (forward kinematics plus P4 and environment queries) usually dominate. -- Memory: two trees of `MaxNodes·(Dof·sizeof(T) + 2)` bytes plus a `2·MaxNodes·Dof` path buffer. Dof 6 with - 1000 nodes is about 100 KB in `float`. No heap. - -## Numerical / embedded notes - -- **Sampler independence (normative).** The trees alternate every iteration, so the sampler must not - correlate consecutive draws. A base-2 Halton sequence consumed in order hands every odd sample to one half - of the space. In the validation it fed each tree samples from the far side of the wall, and the trees - stalled at 19 nodes after 2000 iterations. A seeded PRNG (xorshift32 in the tests), or a low-discrepancy - sequence drawn separately per tree, avoids this. -- **Resolution.** Edge checks are discrete: choose `collisionResolution` below the thinnest obstacle as - seen in joint space, scaled by the farthest link's lever arm. -- Paths from RRT-Connect are feasible but long and jagged (11.5 against a 7.5 lower bound in the test). - `Shortcut` brings them close to optimal (8.1) at the cost of extra validity checks; time-parameterize the - result with M27 or F2. -- The metric is unweighted Euclidean in joint space. Scale joints by their lever arm if one joint dominates - Cartesian motion. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/planning/RrtConnect.hpp` — `#pragma once`, and `extern template class RrtConnect;` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD` (the template members are instantiated by the tests). -- Coverage: `robotics/planning/RrtConnect.cpp` → `template class RrtConnect;` -- Test: `robotics/planning/test/TestRrtConnect.cpp` -- Doc: `doc/planning/RrtConnect.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.planning_test`. -- Depends on: nothing at compile time; P4 (and P1/P5) supply `isFree` in applications. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/RrtConnect/tests.md b/roadmap/planning/RrtConnect/tests.md deleted file mode 100644 index f147a9b..0000000 --- a/roadmap/planning/RrtConnect/tests.md +++ /dev/null @@ -1,49 +0,0 @@ -# RRT-Connect — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestRrtConnect : public ::testing::Test: - # 2-DOF configuration space [−π, π]², blocked region (wall): |q₀| ≤ 0.3 and q₁ ≤ 1.0 (open above q₁ = 1) - IsFree(q) ⇒ not (|q₀| ≤ 0.3 and q₁ ≤ 1.0) - XorShift32 generator(seed) (x ^= x << 13; x ^= x >> 17; x ^= x << 5); Uniform() ⇒ next()/2³² - Sample() ⇒ (−π + 2π·Uniform(), −π + 2π·Uniform()) - RrtConnect planner{ { 0.3, 0.05, 500 } } - # PathIsValid(path): first == start, last == goal, every edge free when checked every 0.05 rad -# each case below is a TEST_F(TestRrtConnect, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -finds_a_valid_path_around_the_wall: - Act: Plan((−2, −2), (2, −2), Sample with seed 1, IsFree) - Assert: found; PathIsValid; path length ≥ 7.496376 (geodesic over the wall top); Iterations() ≤ 500 - # double reference: seed 1 ⇒ 78 iterations, 70 nodes, 40 waypoints, length 11.53 -results_are_reproducible_for_a_seed: - Assert: two runs with seed 7 return identical paths -shortcut_shortens_and_stays_valid: - Act: Plan as above (seed 1); Shortcut(100, Uniform with seed 99, IsFree) - Assert: PathIsValid; length decreases (double reference: 11.53 ⇒ 8.12, 4 waypoints); length ≥ 7.496376 -trivial_case_returns_the_straight_edge: - Assert: Plan((−2, 2), (2, 2), …) == found, Path() == [(−2, 2), (2, 2)], Iterations() == 0 -invalid_endpoints_are_reported: - Assert: Plan((0, 0), (2, 2), …) == startInCollision; Plan((2, 2), (0, 0), …) == goalInCollision -small_pool_is_exhausted: - Arrange: RrtConnect - Assert: Plan((−2, −2), (2, −2), Sample with seed 1, IsFree) == poolExhausted -``` - -## Reference vectors - -- Seeds 1, 7 and 12345 (double): found after 78, 104 and 95 iterations; raw lengths 11.53, 9.83, 11.83; - after 100 shortcut attempts 8.12, 7.84, 8.05 against the 7.496 lower bound. -- A base-2 Halton sampler consumed in alternation finds nothing in 2000 iterations (19 nodes): documents the - sampler-independence rule (not a test). - -## Edge cases - -- `maxIterations = 0` with a blocked straight edge ⇒ `iterationLimit`. -- `Shortcut` on a two-waypoint path changes nothing. diff --git a/robotics/controllers/manipulator/CMakeLists.txt b/robotics/controllers/manipulator/CMakeLists.txt index 7c79af2..fa538aa 100644 --- a/robotics/controllers/manipulator/CMakeLists.txt +++ b/robotics/controllers/manipulator/CMakeLists.txt @@ -6,6 +6,7 @@ target_include_directories(robotics.controllers.manipulator ${ROBOTICS_VISIBILIT ) target_link_libraries(robotics.controllers.manipulator ${ROBOTICS_VISIBILITY} + infra.util numerical.math numerical.solver robotics.dynamics @@ -13,16 +14,24 @@ target_link_libraries(robotics.controllers.manipulator ${ROBOTICS_VISIBILITY} ) target_sources(robotics.controllers.manipulator PRIVATE + CableTensionDistribution.hpp ComputedTorqueControl.hpp + HybridPositionForceControl.hpp ImpedanceControl.hpp + OperationalSpaceControl.hpp PdGravityCompensation.hpp + SlotineLiAdaptiveControl.hpp TaskError.hpp ) robotics_add_coverage_sources(robotics.controllers.manipulator + CableTensionDistribution.cpp ComputedTorqueControl.cpp + HybridPositionForceControl.cpp ImpedanceControl.cpp + OperationalSpaceControl.cpp PdGravityCompensation.cpp + SlotineLiAdaptiveControl.cpp ) add_subdirectory(test) diff --git a/robotics/controllers/manipulator/CableTensionDistribution.cpp b/robotics/controllers/manipulator/CableTensionDistribution.cpp new file mode 100644 index 0000000..2617622 --- /dev/null +++ b/robotics/controllers/manipulator/CableTensionDistribution.cpp @@ -0,0 +1,8 @@ +#include "robotics/controllers/manipulator/CableTensionDistribution.hpp" + +namespace controllers +{ + template class CableTensionDistribution; + template class CableTensionDistribution; + template class CableTensionDistribution; +} diff --git a/robotics/controllers/manipulator/CableTensionDistribution.hpp b/robotics/controllers/manipulator/CableTensionDistribution.hpp new file mode 100644 index 0000000..98de5b4 --- /dev/null +++ b/robotics/controllers/manipulator/CableTensionDistribution.hpp @@ -0,0 +1,188 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "numerical/solvers/ActiveSetQp.hpp" +#include "numerical/solvers/GaussianElimination.hpp" +#include +#include +#include +#include +#include +#include + +namespace controllers +{ + template + class CableTensionDistribution + { + static_assert(std::is_floating_point_v, "CableTensionDistribution supports floating-point types"); + static_assert(WrenchDim == 6 || (WrenchDim > 0 && WrenchDim <= 3), "CableTensionDistribution supports spatial (6) or point-mass (1 to 3) platforms"); + static_assert(NumCables >= WrenchDim, "At least WrenchDim cables are required"); + + public: + using AnchorArray = std::array, NumCables>; + using StructureMatrix = math::Matrix; + using TensionVector = math::Vector; + using WrenchVector = math::Vector; + + CableTensionDistribution(const AnchorArray& baseAnchors, const AnchorArray& platformAnchors, T tMin, T tMax); + + std::optional ComputeStructureMatrix(const math::SE3Transform& platformPose) const; + OPTIMIZE_FOR_SPEED std::optional Distribute(const WrenchVector& wrench, const math::SE3Transform& platformPose) const; + std::optional DistributeExact(const WrenchVector& wrench, const math::SE3Transform& platformPose) const; + + private: + using Program = solvers::QuadraticProgram; + using FixedMask = std::array; + + bool SolveFree(const StructureMatrix& structure, const FixedMask& fixed, const WrenchVector& remaining, TensionVector& tensions) const; + std::optional WorstViolation(const FixedMask& fixed, const TensionVector& tensions) const; + T Violation(T tension) const; + + AnchorArray baseAnchors; + AnchorArray platformAnchors; + T tMin; + T tMax; + T tMid; + }; + + template + CableTensionDistribution::CableTensionDistribution(const AnchorArray& baseAnchors, const AnchorArray& platformAnchors, T tMin, T tMax) + : baseAnchors{ baseAnchors } + , platformAnchors{ platformAnchors } + , tMin{ tMin } + , tMax{ tMax } + , tMid{ (tMin + tMax) / T{ 2 } } + {} + + template + auto CableTensionDistribution::ComputeStructureMatrix(const math::SE3Transform& platformPose) const -> std::optional + { + StructureMatrix structure{}; + for (std::size_t i = 0; i < NumCables; ++i) + { + const math::Vector3 arm{ WrenchDim == 6 ? math::Vector3{ platformPose.rotation * platformAnchors[i] } : math::Vector3{} }; + const math::Vector3 cable{ baseAnchors[i] - platformPose.translation - arm }; + const T length{ math::VectorNorm(cable) }; + if (length <= std::numeric_limits::epsilon()) + return std::nullopt; + + const math::Vector3 direction{ cable * (T{ 1 } / length) }; + const math::Vector3 moment{ math::CrossProduct(arm, direction) }; + for (std::size_t r = 0; r < WrenchDim; ++r) + structure.at(r, i) = r < 3 ? direction[r] : moment[r - 3]; + } + return structure; + } + + template + OPTIMIZE_FOR_SPEED auto CableTensionDistribution::Distribute(const WrenchVector& wrench, const math::SE3Transform& platformPose) const + -> std::optional + { + const auto structure{ ComputeStructureMatrix(platformPose) }; + if (!structure) + return std::nullopt; + + FixedMask fixed{}; + WrenchVector remaining{ wrench }; + TensionVector tensions{}; + for (std::size_t freeCables = NumCables; freeCables >= WrenchDim; --freeCables) + { + if (!SolveFree(*structure, fixed, remaining, tensions)) + return std::nullopt; + + const auto worst{ WorstViolation(fixed, tensions) }; + if (!worst) + return tensions; + + tensions[*worst] = std::clamp(tensions[*worst], tMin, tMax); + fixed[*worst] = true; + for (std::size_t r = 0; r < WrenchDim; ++r) + remaining[r] -= structure->at(r, *worst) * tensions[*worst]; + } + + return std::nullopt; + } + + template + bool CableTensionDistribution::SolveFree(const StructureMatrix& structure, const FixedMask& fixed, const WrenchVector& remaining, TensionVector& tensions) const + { + StructureMatrix active{ structure }; + TensionVector centre{}; + for (std::size_t i = 0; i < NumCables; ++i) + { + centre[i] = fixed[i] ? T{} : tMid; + for (std::size_t r = 0; r < WrenchDim; ++r) + active.at(r, i) = fixed[i] ? T{} : active.at(r, i); + } + + const auto multipliers{ solvers::TrySolveSystem(math::SquareMatrix{ active * active.Transpose() }, WrenchVector{ remaining - active * centre }) }; + if (!multipliers) + return false; + + const TensionVector correction{ active.Transpose() * *multipliers }; + for (std::size_t i = 0; i < NumCables; ++i) + tensions[i] = fixed[i] ? tensions[i] : tMid + correction[i]; + return true; + } + + template + std::optional CableTensionDistribution::WorstViolation(const FixedMask& fixed, const TensionVector& tensions) const + { + std::optional worst{}; + T worstViolation{}; + for (std::size_t i = 0; i < NumCables; ++i) + { + const T violation{ fixed[i] ? T{} : Violation(tensions[i]) }; + if (violation > worstViolation) + { + worst = i; + worstViolation = violation; + } + } + return worst; + } + + template + auto CableTensionDistribution::DistributeExact(const WrenchVector& wrench, const math::SE3Transform& platformPose) const + -> std::optional + { + const auto structure{ ComputeStructureMatrix(platformPose) }; + if (!structure) + return std::nullopt; + + Program program{}; + program.hessian = math::SquareMatrix::Identity(); + program.equalityMatrix = *structure; + program.equalityVector = wrench; + for (std::size_t i = 0; i < NumCables; ++i) + { + program.gradient[i] = -tMid; + program.inequalityMatrix.at(i, i) = T{ 1 }; + program.inequalityMatrix.at(i + NumCables, i) = T{ -1 }; + program.inequalityVector[i] = tMin; + program.inequalityVector[i + NumCables] = -tMax; + } + + const auto result{ solvers::ActiveSetQp{}.Solve(program) }; + if (result.status != solvers::QpStatus::Optimal) + return std::nullopt; + + return result.x; + } + + template + T CableTensionDistribution::Violation(T tension) const + { + return std::max({ tMin - tension, tension - tMax, T{} }); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class CableTensionDistribution; + extern template class CableTensionDistribution; + extern template class CableTensionDistribution; +#endif +} diff --git a/robotics/controllers/manipulator/HybridPositionForceControl.cpp b/robotics/controllers/manipulator/HybridPositionForceControl.cpp new file mode 100644 index 0000000..672740c --- /dev/null +++ b/robotics/controllers/manipulator/HybridPositionForceControl.cpp @@ -0,0 +1,7 @@ +#include "robotics/controllers/manipulator/HybridPositionForceControl.hpp" + +namespace controllers +{ + template class HybridPositionForceControl; + template class HybridPositionForceControl; +} diff --git a/robotics/controllers/manipulator/HybridPositionForceControl.hpp b/robotics/controllers/manipulator/HybridPositionForceControl.hpp new file mode 100644 index 0000000..8eccb03 --- /dev/null +++ b/robotics/controllers/manipulator/HybridPositionForceControl.hpp @@ -0,0 +1,106 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/controllers/manipulator/TaskError.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include +#include + +namespace controllers +{ + template + class HybridPositionForceControl + { + static_assert(std::is_floating_point_v, "HybridPositionForceControl 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; + + struct Gains + { + TaskMatrix kp; + TaskMatrix kd; + TaskMatrix kf; + TaskMatrix ki; + TaskMatrix kdf; + TaskVector integralLimit; + }; + + HybridPositionForceControl(const Model& model, const Provider& jacobian, const TaskMatrix& constraintRotation, const TaskMatrix& selection, const Gains& gains, T dt); + + OPTIMIZE_FOR_SPEED JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, const TaskVector& xdDot, + const TaskVector& fMeasured, const TaskVector& fd); + void Reset(); + + private: + const Model& model; + const Provider& jacobian; + TaskMatrix constraintRotation; + TaskMatrix selection; + TaskMatrix complement; + Gains gains; + TaskVector forceIntegral{}; + T dt; + }; + + template + HybridPositionForceControl::HybridPositionForceControl(const Model& model, const Provider& jacobian, const TaskMatrix& constraintRotation, const TaskMatrix& selection, + const Gains& gains, T dt) + : model{ model } + , jacobian{ jacobian } + , constraintRotation{ constraintRotation } + , selection{ selection } + , complement{ TaskMatrix::Identity() - selection } + , gains{ gains } + , dt{ dt } + { + for (std::size_t r = 0; r < TaskDim; ++r) + for (std::size_t c = 0; c < TaskDim; ++c) + really_assert(r == c ? (selection.at(r, c) == T{} || selection.at(r, c) == T{ 1 }) : selection.at(r, c) == T{}); + } + + template + OPTIMIZE_FOR_SPEED auto HybridPositionForceControl::ComputeTorque(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, + const TaskVector& xdDot, const TaskVector& fMeasured, const TaskVector& fd) -> JointVector + { + const auto taskJacobian{ jacobian.Jacobian(q) }; + const TaskVector taskVelocity{ taskJacobian * qDot }; + const auto toConstraint{ constraintRotation.Transpose() }; + + const TaskVector error{ toConstraint * TaskError(desiredPose, jacobian.ToolPose(q)) }; + const TaskVector errorRate{ toConstraint * TaskVector{ xdDot - taskVelocity } }; + const TaskVector constraintVelocity{ toConstraint * taskVelocity }; + const TaskVector desiredForce{ toConstraint * fd }; + const TaskVector forceError{ desiredForce - toConstraint * fMeasured }; + + const TaskVector motionWrench{ gains.kp * error + gains.kd * errorRate }; + + forceIntegral = forceIntegral + complement * forceError * dt; + for (std::size_t i = 0; i < TaskDim; ++i) + forceIntegral[i] = std::min(std::max(forceIntegral[i], -gains.integralLimit[i]), gains.integralLimit[i]); + const TaskVector forceWrench{ desiredForce + gains.kf * forceError + gains.ki * forceIntegral - gains.kdf * constraintVelocity }; + + const TaskVector wrench{ constraintRotation * TaskVector{ selection * motionWrench + complement * forceWrench } }; + return taskJacobian.Transpose() * wrench + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q); + } + + template + void HybridPositionForceControl::Reset() + { + forceIntegral = TaskVector{}; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class HybridPositionForceControl; + extern template class HybridPositionForceControl; +#endif +} diff --git a/robotics/controllers/manipulator/OperationalSpaceControl.cpp b/robotics/controllers/manipulator/OperationalSpaceControl.cpp new file mode 100644 index 0000000..b3c5165 --- /dev/null +++ b/robotics/controllers/manipulator/OperationalSpaceControl.cpp @@ -0,0 +1,8 @@ +#include "robotics/controllers/manipulator/OperationalSpaceControl.hpp" + +namespace controllers +{ + template class OperationalSpaceControl; + template class OperationalSpaceControl; + template class OperationalSpaceControl; +} diff --git a/robotics/controllers/manipulator/OperationalSpaceControl.hpp b/robotics/controllers/manipulator/OperationalSpaceControl.hpp new file mode 100644 index 0000000..c503154 --- /dev/null +++ b/robotics/controllers/manipulator/OperationalSpaceControl.hpp @@ -0,0 +1,74 @@ +#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 OperationalSpaceControl + { + static_assert(std::is_floating_point_v, "OperationalSpaceControl supports floating-point types"); + static_assert(TaskDim <= Dof, "Task dimension cannot exceed the degrees of freedom"); + + public: + using JointVector = math::Vector; + using JointMatrix = math::SquareMatrix; + using TaskVector = math::Vector; + using TaskMatrix = math::SquareMatrix; + using Model = dynamics::EulerLagrangeDynamics; + using Provider = kinematics::JacobianProvider; + + OperationalSpaceControl(const Model& model, const Provider& jacobian, const TaskMatrix& kp, const TaskMatrix& kd, T sigma); + + OPTIMIZE_FOR_SPEED JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, const TaskVector& xdDot, + const TaskVector& xdDdot, const JointVector& tauSecondary) const; + + private: + const Model& model; + const Provider& jacobian; + TaskMatrix kp; + TaskMatrix kd; + T sigma; + }; + + template + OperationalSpaceControl::OperationalSpaceControl(const Model& model, const Provider& jacobian, const TaskMatrix& kp, const TaskMatrix& kd, T sigma) + : model{ model } + , jacobian{ jacobian } + , kp{ kp } + , kd{ kd } + , sigma{ sigma } + {} + + template + OPTIMIZE_FOR_SPEED auto OperationalSpaceControl::ComputeTorque(const JointVector& q, const JointVector& qDot, const math::SE3Transform& desiredPose, + const TaskVector& xdDot, const TaskVector& xdDdot, const JointVector& tauSecondary) 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 + kd * errorRate + kp * error }; + + const auto inverseMassTimesTransposed{ solvers::SolveSystem(model.ComputeMassMatrix(q), transposed) }; + const TaskMatrix taskInertia{ solvers::SolveSystem(TaskMatrix{ taskJacobian * inverseMassTimesTransposed + TaskMatrix::Identity() * (sigma * sigma) }, TaskMatrix::Identity()) }; + const JointMatrix nullSpace{ JointMatrix::Identity() - transposed * TaskMatrix{ taskInertia } * inverseMassTimesTransposed.Transpose() }; + + return transposed * TaskVector{ taskInertia * TaskVector{ acceleration - jacobian.BiasAcceleration(q, qDot) } } + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) + + nullSpace * tauSecondary; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class OperationalSpaceControl; + extern template class OperationalSpaceControl; + extern template class OperationalSpaceControl; +#endif +} diff --git a/robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp b/robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp new file mode 100644 index 0000000..d469ede --- /dev/null +++ b/robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp @@ -0,0 +1,7 @@ +#include "robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp" + +namespace controllers +{ + template class SlotineLiAdaptiveControl; + template class SlotineLiAdaptiveControl; +} diff --git a/robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp b/robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp new file mode 100644 index 0000000..2e94b6c --- /dev/null +++ b/robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/InertialRegressor.hpp" +#include +#include + +namespace controllers +{ + template + class SlotineLiAdaptiveControl + { + static_assert(std::is_floating_point_v, "SlotineLiAdaptiveControl supports floating-point types"); + static_assert(Dof > 0 && NumParams > 0, "Dimensions must be positive"); + + public: + using StateVector = math::Vector; + using GainMatrix = math::SquareMatrix; + using ParameterVector = math::Vector; + using AdaptationMatrix = math::SquareMatrix; + using Regressor = dynamics::InertialRegressor; + + SlotineLiAdaptiveControl(const Regressor& regressor, const GainMatrix& lambda, const GainMatrix& kd, const AdaptationMatrix& gamma, const ParameterVector& initialEstimate, T dt); + + OPTIMIZE_FOR_SPEED StateVector ComputeTorque(const StateVector& q, const StateVector& qDot, const StateVector& qd, const StateVector& qdDot, const StateVector& qdDdot); + const ParameterVector& ParameterEstimate() const; + void Reset(const ParameterVector& initialEstimate); + + private: + const Regressor& regressor; + GainMatrix lambda; + GainMatrix kd; + AdaptationMatrix gamma; + ParameterVector estimate; + T dt; + }; + + template + SlotineLiAdaptiveControl::SlotineLiAdaptiveControl(const Regressor& regressor, const GainMatrix& lambda, const GainMatrix& kd, const AdaptationMatrix& gamma, + const ParameterVector& initialEstimate, T dt) + : regressor{ regressor } + , lambda{ lambda } + , kd{ kd } + , gamma{ gamma } + , estimate{ initialEstimate } + , dt{ dt } + {} + + template + OPTIMIZE_FOR_SPEED auto SlotineLiAdaptiveControl::ComputeTorque(const StateVector& q, const StateVector& qDot, const StateVector& qd, const StateVector& qdDot, + const StateVector& qdDdot) -> StateVector + { + const StateVector positionError{ q - qd }; + const StateVector velocityError{ qDot - qdDot }; + const StateVector sliding{ velocityError + lambda * positionError }; + const StateVector referenceVelocity{ qdDot - lambda * positionError }; + const StateVector referenceAcceleration{ qdDdot - lambda * velocityError }; + + const auto y{ regressor.Compute(q, qDot, referenceVelocity, referenceAcceleration) }; + const StateVector torque{ y * estimate - kd * sliding }; + estimate = estimate - ParameterVector{ gamma * ParameterVector{ y.Transpose() * sliding } } * dt; + + return torque; + } + + template + auto SlotineLiAdaptiveControl::ParameterEstimate() const -> const ParameterVector& + { + return estimate; + } + + template + void SlotineLiAdaptiveControl::Reset(const ParameterVector& initialEstimate) + { + estimate = initialEstimate; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class SlotineLiAdaptiveControl; + extern template class SlotineLiAdaptiveControl; +#endif +} diff --git a/robotics/controllers/manipulator/test/CMakeLists.txt b/robotics/controllers/manipulator/test/CMakeLists.txt index c572db3..65ed35c 100644 --- a/robotics/controllers/manipulator/test/CMakeLists.txt +++ b/robotics/controllers/manipulator/test/CMakeLists.txt @@ -10,7 +10,11 @@ target_link_libraries(robotics.controllers.manipulator_test PUBLIC ) target_sources(robotics.controllers.manipulator_test PRIVATE + TestCableTensionDistribution.cpp TestComputedTorqueControl.cpp + TestHybridPositionForceControl.cpp TestImpedanceControl.cpp + TestOperationalSpaceControl.cpp TestPdGravityCompensation.cpp + TestSlotineLiAdaptiveControl.cpp ) diff --git a/robotics/controllers/manipulator/test/TestCableTensionDistribution.cpp b/robotics/controllers/manipulator/test/TestCableTensionDistribution.cpp new file mode 100644 index 0000000..65a0210 --- /dev/null +++ b/robotics/controllers/manipulator/test/TestCableTensionDistribution.cpp @@ -0,0 +1,205 @@ +#include "numerical/math/SE3Transform.hpp" +#include "robotics/controllers/manipulator/CableTensionDistribution.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Transform = math::SE3Transform; + using Planar = controllers::CableTensionDistribution; + using Square = controllers::CableTensionDistribution; + using Spatial = controllers::CableTensionDistribution; + + const float root3{ std::numbers::sqrt3_v }; + + template + void ExpectNear(const math::Vector& actual, const math::Vector& expected, float tolerance) + { + for (std::size_t i = 0; i < N; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + template + void ExpectWrench(const math::Matrix& structure, const math::Vector& tensions, const math::Vector& wrench, float tolerance) + { + ExpectNear(math::Vector{ structure * tensions }, wrench, tolerance); + } + + template + void ExpectWithinBounds(const math::Vector& tensions) + { + for (std::size_t i = 0; i < Cables; ++i) + { + EXPECT_GE(tensions[i], 10.0f - 1e-3f); + EXPECT_LE(tensions[i], 200.0f + 1e-3f); + } + } + + class TestCableTensionDistribution + : public ::testing::Test + { + protected: + Planar distributor{ { Vector3{ 0.0f, 2.0f, 0.0f }, Vector3{ -root3, -1.0f, 0.0f }, Vector3{ root3, -1.0f, 0.0f } }, {}, 10.0f, 200.0f }; + Transform pose{ Transform::Identity() }; + }; + + class TestCableTensionDistributionSquare + : public ::testing::Test + { + protected: + Square distributor{ { Vector3{ -1.0f, 1.0f, 0.0f }, Vector3{ 1.0f, 1.0f, 0.0f } }, {}, 10.0f, 200.0f }; + Transform pose{ Transform::Identity() }; + }; + + class TestCableTensionDistributionSpatial + : public ::testing::Test + { + protected: + Spatial distributor{ { Vector3{ 2.0f, 2.0f, 2.0f }, Vector3{ -2.0f, 2.0f, 2.0f }, Vector3{ -2.0f, -2.0f, 2.0f }, Vector3{ 2.0f, -2.0f, 2.0f }, Vector3{ 2.0f, 2.0f, 0.0f }, + Vector3{ -2.0f, 2.0f, 0.0f }, Vector3{ -2.0f, -2.0f, 0.0f }, Vector3{ 2.0f, -2.0f, 0.0f } }, + { Vector3{ 0.2f, 0.0f, 0.0f }, Vector3{ -0.2f, 0.2f, 0.1f }, Vector3{ -0.2f, -0.2f, 0.1f }, Vector3{ 0.2f, -0.2f, 0.1f }, Vector3{ 0.2f, 0.2f, -0.1f }, Vector3{ -0.2f, 0.2f, -0.1f }, + Vector3{ -0.2f, -0.2f, -0.1f }, Vector3{ 0.2f, -0.2f, -0.1f } }, + 10.0f, 200.0f }; + }; + + class TestCableTensionDistributionCrossed + : public ::testing::Test + { + protected: + Spatial distributor{ { Vector3{ -2.0f, -2.0f, 0.0f }, Vector3{ -2.0f, -2.0f, 3.0f }, Vector3{ -2.0f, 2.0f, 0.0f }, Vector3{ -2.0f, 2.0f, 3.0f }, Vector3{ 2.0f, -2.0f, 0.0f }, + Vector3{ 2.0f, -2.0f, 3.0f }, Vector3{ 2.0f, 2.0f, 0.0f }, Vector3{ 2.0f, 2.0f, 3.0f } }, + { Vector3{ 0.2f, -0.2f, -0.1f }, Vector3{ 0.2f, -0.2f, 0.1f }, Vector3{ 0.2f, 0.2f, -0.1f }, Vector3{ 0.2f, 0.2f, 0.1f }, Vector3{ -0.2f, -0.2f, -0.1f }, Vector3{ -0.2f, -0.2f, 0.1f }, + Vector3{ -0.2f, 0.2f, -0.1f }, Vector3{ -0.2f, 0.2f, 0.1f } }, + 10.0f, 200.0f }; + Transform pose{ Matrix3::Identity(), Vector3{ 0.3f, -0.2f, 1.4f } }; + }; +} + +TEST_F(TestCableTensionDistribution, structure_matrix_from_geometry) +{ + const auto structure{ distributor.ComputeStructureMatrix(pose) }; + + ASSERT_TRUE(structure.has_value()); + const math::Matrix expected{ { 0.0f, -0.866025f, 0.866025f }, { 1.0f, -0.5f, -0.5f } }; + for (std::size_t r = 0; r < 2; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(structure->at(r, c), expected.at(r, c), 1e-5f); +} + +TEST_F(TestCableTensionDistributionSpatial, spatial_structure_matrix_includes_moment_arm) +{ + const Transform rotated{ Matrix3{ { 0.0f, -1.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 1.0f } }, Vector3{} }; + + const auto structure{ distributor.ComputeStructureMatrix(rotated) }; + + ASSERT_TRUE(structure.has_value()); + const math::Vector expected{ 0.59655f, 0.53689f, 0.59655f, 0.11931f, 0.0f, -0.11931f }; + for (std::size_t r = 0; r < 6; ++r) + EXPECT_NEAR(structure->at(r, 0), expected[r], 1e-5f); +} + +TEST_F(TestCableTensionDistribution, zero_wrench_gives_centred_pretension) +{ + const auto tensions{ distributor.Distribute(math::Vector{}, pose) }; + + ASSERT_TRUE(tensions.has_value()); + ExpectNear(*tensions, math::Vector{ 105.0f, 105.0f, 105.0f }, 1e-3f); +} + +TEST_F(TestCableTensionDistribution, redundant_case_centres_tensions) +{ + const auto tensions{ distributor.Distribute(math::Vector{ 0.0f, 60.0f }, pose) }; + + ASSERT_TRUE(tensions.has_value()); + ExpectNear(*tensions, math::Vector{ 145.0f, 85.0f, 85.0f }, 1e-3f); +} + +TEST_F(TestCableTensionDistribution, wrench_is_reproduced) +{ + const math::Vector wrench{ 30.0f, -40.0f }; + + const auto tensions{ distributor.Distribute(wrench, pose) }; + + ASSERT_TRUE(tensions.has_value()); + ExpectWrench(*distributor.ComputeStructureMatrix(pose), *tensions, wrench, 1e-3f); + ExpectWithinBounds(*tensions); + ExpectNear(*tensions, math::Vector{ 78.333f, 101.013f, 135.654f }, 1e-3f); +} + +TEST_F(TestCableTensionDistribution, upper_bound_fixes_worst_cable) +{ + const math::Vector wrench{ 0.0f, 150.0f }; + + const auto tensions{ distributor.Distribute(wrench, pose) }; + + ASSERT_TRUE(tensions.has_value()); + ExpectNear(*tensions, math::Vector{ 200.0f, 50.0f, 50.0f }, 1e-3f); + ExpectWrench(*distributor.ComputeStructureMatrix(pose), *tensions, wrench, 1e-3f); +} + +TEST_F(TestCableTensionDistribution, lower_bound_fixes_slack_cable) +{ + const math::Vector wrench{ 0.0f, -150.0f }; + + const auto tensions{ distributor.Distribute(wrench, pose) }; + + ASSERT_TRUE(tensions.has_value()); + ExpectNear(*tensions, math::Vector{ 10.0f, 160.0f, 160.0f }, 1e-3f); + ExpectWrench(*distributor.ComputeStructureMatrix(pose), *tensions, wrench, 1e-3f); +} + +TEST_F(TestCableTensionDistribution, infeasible_wrench_returns_nullopt) +{ + EXPECT_FALSE(distributor.Distribute(math::Vector{ 0.0f, 300.0f }, pose).has_value()); +} + +TEST_F(TestCableTensionDistributionSquare, square_case_unique_solution) +{ + const auto tensions{ distributor.Distribute(math::Vector{ 0.0f, 100.0f }, pose) }; + + ASSERT_TRUE(tensions.has_value()); + ExpectNear(*tensions, math::Vector{ 70.7107f, 70.7107f }, 1e-3f); +} + +TEST_F(TestCableTensionDistributionSquare, collinear_cables_return_nullopt) +{ + const math::Vector wrench{ 0.0f, 100.0f }; + + EXPECT_FALSE(distributor.Distribute(wrench, Transform{ Matrix3::Identity(), Vector3{ 0.0f, 1.0f, 0.0f } }).has_value()); + EXPECT_FALSE(distributor.Distribute(wrench, Transform{ Matrix3::Identity(), Vector3{ -1.0f, 1.0f, 0.0f } }).has_value()); + EXPECT_FALSE(distributor.DistributeExact(wrench, Transform{ Matrix3::Identity(), Vector3{ -1.0f, 1.0f, 0.0f } }).has_value()); +} + +TEST_F(TestCableTensionDistribution, exact_distribution_matches_closed_form_with_active_bound) +{ + const math::Vector wrench{ 0.0f, 150.0f }; + + const auto exact{ distributor.DistributeExact(wrench, pose) }; + const auto closed{ distributor.Distribute(wrench, pose) }; + + ASSERT_TRUE(exact.has_value()); + ASSERT_TRUE(closed.has_value()); + ExpectNear(*exact, math::Vector{ 200.0f, 50.0f, 50.0f }, 1e-3f); + ExpectNear(*exact, *closed, 1e-3f); +} + +TEST_F(TestCableTensionDistributionCrossed, exact_distribution_succeeds_where_closed_form_fails) +{ + const math::Vector wrench{ 56.8f, -183.5f, -35.3f, -2.0f, 41.7f, -2.7f }; + + const auto closed{ distributor.Distribute(wrench, pose) }; + const auto exact{ distributor.DistributeExact(wrench, pose) }; + + EXPECT_FALSE(closed.has_value()); + ASSERT_TRUE(exact.has_value()); + ExpectNear(*exact, math::Vector{ 200.0f, 21.0187f, 23.9864f, 10.0f, 88.1208f, 197.9432f, 76.4249f, 23.7712f }, 1e-2f); + ExpectWrench(*distributor.ComputeStructureMatrix(pose), *exact, wrench, 1e-2f); +} + +TEST_F(TestCableTensionDistribution, exact_distribution_reports_infeasible_wrench) +{ + EXPECT_FALSE(distributor.DistributeExact(math::Vector{ 0.0f, 300.0f }, pose).has_value()); +} diff --git a/robotics/controllers/manipulator/test/TestHybridPositionForceControl.cpp b/robotics/controllers/manipulator/test/TestHybridPositionForceControl.cpp new file mode 100644 index 0000000..f2bd261 --- /dev/null +++ b/robotics/controllers/manipulator/test/TestHybridPositionForceControl.cpp @@ -0,0 +1,157 @@ +#include "numerical/math/SE3Transform.hpp" +#include "robotics/controllers/manipulator/HybridPositionForceControl.hpp" +#include "robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp" +#include "robotics/kinematics/test_doubles/MockJacobianProvider.hpp" +#include +#include + +namespace +{ + using ::testing::_; + using ::testing::Return; + using Vector2 = math::Vector; + using Matrix2 = math::SquareMatrix; + using Transform = math::SE3Transform; + using Controller = controllers::HybridPositionForceControl; + + Transform At(float x, float y) + { + return Transform{ math::Matrix3::Identity(), math::Vector3{ x, y, 0.0f } }; + } + + Matrix2 Diagonal(float first, float second) + { + return Matrix2{ { first, 0.0f }, { 0.0f, second } }; + } + + class TestHybridPositionForceControl + : public ::testing::Test + { + protected: + void ExpectNear(const Vector2& actual, const Vector2& expected) const + { + for (std::size_t i = 0; i < 2; ++i) + EXPECT_NEAR(actual[i], expected[i], 1e-4f); + } + + void ExpectDependencies(const Matrix2& jacobianMatrix, const Transform& current, std::size_t calls = 1, const Vector2& coriolis = Vector2{}, const Vector2& gravity = Vector2{}) + { + EXPECT_CALL(jacobian, Jacobian(_)).Times(static_cast(calls)).WillRepeatedly(Return(jacobianMatrix)); + EXPECT_CALL(jacobian, ToolPose(_)).Times(static_cast(calls)).WillRepeatedly(Return(current)); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).Times(static_cast(calls)).WillRepeatedly(Return(coriolis)); + EXPECT_CALL(model, ComputeGravityTerms(_)).Times(static_cast(calls)).WillRepeatedly(Return(gravity)); + } + + Vector2 Step(Controller& target, const Vector2& qDot, const Vector2& xdDot, const Vector2& fMeasured, const Vector2& fd) + { + return target.ComputeTorque(q, qDot, desired, xdDot, fMeasured, fd); + } + + ::testing::StrictMock> model; + ::testing::StrictMock> jacobian; + Controller::Gains gains{ Diagonal(100.0f, 100.0f), Diagonal(20.0f, 20.0f), Diagonal(2.0f, 2.0f), Diagonal(5.0f, 5.0f), Diagonal(3.0f, 3.0f), Vector2{ 1.0f, 1.0f } }; + float dt{ 0.001f }; + Controller controller{ model, jacobian, Matrix2::Identity(), Diagonal(1.0f, 0.0f), gains, dt }; + Vector2 q{ 0.3f, 0.2f }; + Transform desired{ At(0.3f, 0.2f) }; + }; +} + +TEST_F(TestHybridPositionForceControl, motion_axis_does_position_pd) +{ + ExpectDependencies(Matrix2::Identity(), At(0.25f, 0.2f)); + + const auto tau{ Step(controller, Vector2{ 0.1f, 0.0f }, Vector2{ 0.3f, 0.0f }, Vector2{}, Vector2{}) }; + + ExpectNear(tau, Vector2{ 100.0f * 0.05f + 20.0f * 0.2f, 0.0f }); +} + +TEST_F(TestHybridPositionForceControl, force_axis_does_damped_force_pi) +{ + ExpectDependencies(Matrix2::Identity(), desired); + + const auto tau{ Step(controller, Vector2{ 0.0f, 0.4f }, Vector2{}, Vector2{ 0.0f, 3.0f }, Vector2{ 0.0f, 5.0f }) }; + + EXPECT_NEAR(tau[1], 5.0f + 2.0f * 2.0f + 5.0f * 2.0f * dt - 3.0f * 0.4f, 1e-4f); +} + +TEST_F(TestHybridPositionForceControl, selection_masks_cross_terms) +{ + ExpectDependencies(Matrix2::Identity(), At(0.3f, 0.15f)); + + const auto tau{ Step(controller, Vector2{}, Vector2{}, Vector2{ 2.0f, 0.0f }, Vector2{}) }; + + ExpectNear(tau, Vector2{}); +} + +TEST_F(TestHybridPositionForceControl, constraint_rotation_maps_axes) +{ + Controller rotated{ model, jacobian, Matrix2{ { 0.0f, -1.0f }, { 1.0f, 0.0f } }, Diagonal(1.0f, 0.0f), gains, dt }; + desired = At(0.02f, 0.1f); + ExpectDependencies(Matrix2::Identity(), At(0.0f, 0.0f)); + + const auto tau{ Step(rotated, Vector2{}, Vector2{}, Vector2{ -3.0f, 0.0f }, Vector2{ -5.0f, 0.0f }) }; + + ExpectNear(tau, Vector2{ -9.01f, 10.0f }); +} + +TEST_F(TestHybridPositionForceControl, force_integral_accumulates) +{ + ExpectDependencies(Matrix2::Identity(), desired, 100); + + Vector2 tau{}; + for (std::size_t k = 0; k < 100; ++k) + tau = Step(controller, Vector2{}, Vector2{}, Vector2{ 0.0f, 3.0f }, Vector2{ 0.0f, 5.0f }); + + EXPECT_NEAR(tau[1] - (5.0f + 2.0f * 2.0f), 5.0f * 2.0f * 100.0f * dt, 1e-4f); +} + +TEST_F(TestHybridPositionForceControl, force_integral_clamped) +{ + ExpectDependencies(Matrix2::Identity(), desired, 1000); + + Vector2 tau{}; + for (std::size_t k = 0; k < 1000; ++k) + tau = Step(controller, Vector2{}, Vector2{}, Vector2{}, Vector2{ 0.0f, 10.0f }); + + EXPECT_NEAR(tau[1], 10.0f + 2.0f * 10.0f + 5.0f * 1.0f, 1e-4f); +} + +TEST_F(TestHybridPositionForceControl, reset_clears_force_integral) +{ + ExpectDependencies(Matrix2::Identity(), desired, 101); + for (std::size_t k = 0; k < 100; ++k) + static_cast(Step(controller, Vector2{}, Vector2{}, Vector2{ 0.0f, 3.0f }, Vector2{ 0.0f, 5.0f })); + + controller.Reset(); + const auto tau{ Step(controller, Vector2{}, Vector2{}, Vector2{ 0.0f, 3.0f }, Vector2{ 0.0f, 5.0f }) }; + + EXPECT_NEAR(tau[1], 5.0f + 2.0f * 2.0f + 5.0f * 2.0f * dt, 1e-4f); +} + +TEST_F(TestHybridPositionForceControl, jacobian_transpose_maps_wrench) +{ + ExpectDependencies(Matrix2{ { 1.0f, 0.5f }, { 0.0f, 2.0f } }, At(0.25f, 0.2f)); + + const auto tau{ Step(controller, Vector2{}, Vector2{}, Vector2{ 0.0f, 5.0f }, Vector2{ 0.0f, 5.0f }) }; + + ExpectNear(tau, Vector2{ 5.0f, 12.5f }); +} + +TEST_F(TestHybridPositionForceControl, gravity_and_coriolis_added) +{ + const Vector2 coriolis{ 0.3f, -0.2f }; + const Vector2 gravity{ 4.0f, 1.5f }; + ExpectDependencies(Matrix2::Identity(), desired, 1, coriolis, gravity); + + const auto tau{ Step(controller, Vector2{}, Vector2{}, Vector2{}, Vector2{}) }; + + ExpectNear(tau, Vector2{ coriolis + gravity }); +} + +TEST_F(TestHybridPositionForceControl, dependencies_queried_once) +{ + ExpectDependencies(Matrix2::Identity(), desired); + + static_cast(Step(controller, Vector2{ 0.1f, 0.2f }, Vector2{}, Vector2{ 1.0f, 2.0f }, Vector2{ 0.0f, 3.0f })); +} diff --git a/robotics/controllers/manipulator/test/TestOperationalSpaceControl.cpp b/robotics/controllers/manipulator/test/TestOperationalSpaceControl.cpp new file mode 100644 index 0000000..588a8ef --- /dev/null +++ b/robotics/controllers/manipulator/test/TestOperationalSpaceControl.cpp @@ -0,0 +1,184 @@ +#include "numerical/math/SE3Transform.hpp" +#include "numerical/solvers/GaussianElimination.hpp" +#include "robotics/controllers/manipulator/OperationalSpaceControl.hpp" +#include "robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp" +#include "robotics/kinematics/test_doubles/MockJacobianProvider.hpp" +#include +#include +#include + +namespace +{ + using ::testing::_; + using ::testing::Return; + using Vector2 = math::Vector; + using Vector3 = math::Vector; + using Matrix2 = math::SquareMatrix; + using Matrix3 = math::SquareMatrix; + using Transform = math::SE3Transform; + + Transform At(float x, float y) + { + return Transform{ math::Matrix3::Identity(), math::Vector3{ x, y, 0.0f } }; + } + + template + void ExpectNear(const math::Vector& actual, const math::Vector& expected, float tolerance = 1e-4f) + { + for (std::size_t i = 0; i < N; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + class TestOperationalSpaceControl + : public ::testing::Test + { + protected: + void ExpectDependencies(const Matrix2& mass, const Matrix2& jacobianMatrix, const Vector2& bias, const Vector2& coriolis, const Vector2& gravity) + { + EXPECT_CALL(model, ComputeMassMatrix(_)).WillOnce(Return(mass)); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).WillOnce(Return(coriolis)); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillOnce(Return(gravity)); + EXPECT_CALL(jacobian, Jacobian(_)).WillOnce(Return(jacobianMatrix)); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillOnce(Return(bias)); + EXPECT_CALL(jacobian, ToolPose(_)).WillOnce(Return(current)); + } + + Vector2 TaskCommand() const + { + return Vector2{ xdDdot + kd * Vector2{ xdDot - qDot } + kp * error }; + } + + ::testing::StrictMock> model; + ::testing::StrictMock> jacobian; + Matrix2 kp{ { 100.0f, 0.0f }, { 0.0f, 100.0f } }; + Matrix2 kd{ { 20.0f, 0.0f }, { 0.0f, 20.0f } }; + controllers::OperationalSpaceControl controller{ model, jacobian, kp, kd, 0.0f }; + Transform current{ At(0.3f, 0.2f) }; + Transform desired{ At(0.32f, 0.17f) }; + Vector2 error{ 0.02f, -0.03f }; + Vector2 q{ 0.3f, 0.2f }; + Vector2 qDot{ 0.1f, -0.2f }; + Vector2 xdDot{ 0.05f, 0.1f }; + Vector2 xdDdot{ 0.4f, -0.3f }; + }; + + class TestOperationalSpaceControlRedundant + : public ::testing::Test + { + protected: + void ExpectRestState() + { + EXPECT_CALL(model, ComputeMassMatrix(_)).WillRepeatedly(Return(mass)); + EXPECT_CALL(model, ComputeCoriolisTerms(_, _)).WillRepeatedly(Return(Vector3{})); + EXPECT_CALL(model, ComputeGravityTerms(_)).WillRepeatedly(Return(gravity)); + EXPECT_CALL(jacobian, Jacobian(_)).WillRepeatedly(Return(jacobianMatrix)); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillRepeatedly(Return(Vector2{})); + EXPECT_CALL(jacobian, ToolPose(_)).WillRepeatedly(Return(At(0.4f, 0.1f))); + } + + Vector3 Torque(const Vector3& tauSecondary) const + { + return controller.ComputeTorque(Vector3{ 0.1f, 0.2f, 0.3f }, Vector3{}, At(0.4f, 0.1f), Vector2{}, Vector2{}, tauSecondary); + } + + ::testing::StrictMock> model; + ::testing::StrictMock> jacobian; + Matrix3 mass{ { 3.0f, 0.5f, 0.2f }, { 0.5f, 2.0f, 0.3f }, { 0.2f, 0.3f, 1.0f } }; + Vector3 gravity{ 4.0f, -2.0f, 1.5f }; + math::Matrix jacobianMatrix{ { 1.0f, 0.5f, 0.2f }, { 0.0f, 1.0f, 0.7f } }; + controllers::OperationalSpaceControl controller{ model, jacobian, Matrix2{ { 100.0f, 0.0f }, { 0.0f, 100.0f } }, Matrix2{ { 20.0f, 0.0f }, { 0.0f, 20.0f } }, 0.0f }; + }; +} + +TEST_F(TestOperationalSpaceControl, unit_inertia_gives_task_pd) +{ + ExpectDependencies(Matrix2::Identity(), Matrix2::Identity(), Vector2{}, Vector2{}, Vector2{}); + + const auto tau{ controller.ComputeTorque(q, qDot, desired, xdDot, xdDdot, Vector2{}) }; + + ExpectNear(tau, TaskCommand()); +} + +TEST_F(TestOperationalSpaceControl, task_inertia_scales_command) +{ + const Matrix2 mass{ { 2.0f, 0.0f }, { 0.0f, 3.0f } }; + ExpectDependencies(mass, Matrix2::Identity(), Vector2{}, Vector2{}, Vector2{}); + + const auto tau{ controller.ComputeTorque(q, qDot, desired, xdDot, xdDdot, Vector2{}) }; + + ExpectNear(tau, Vector2{ mass * TaskCommand() }); +} + +TEST_F(TestOperationalSpaceControl, bias_acceleration_subtracted) +{ + const Vector2 bias{ 0.7f, -0.4f }; + current = desired; + ExpectDependencies(Matrix2::Identity(), Matrix2::Identity(), bias, Vector2{}, Vector2{}); + + const auto tau{ controller.ComputeTorque(q, Vector2{}, desired, Vector2{}, Vector2{}, Vector2{}) }; + + ExpectNear(tau, Vector2{ bias * -1.0f }); +} + +TEST_F(TestOperationalSpaceControl, coriolis_and_gravity_added_in_joint_space) +{ + const Vector2 coriolis{ 0.3f, -0.1f }; + const Vector2 gravity{ 5.0f, 2.0f }; + current = desired; + ExpectDependencies(Matrix2::Identity(), Matrix2::Identity(), Vector2{}, coriolis, gravity); + + const auto tau{ controller.ComputeTorque(q, Vector2{}, desired, Vector2{}, Vector2{}, Vector2{}) }; + + ExpectNear(tau, Vector2{ coriolis + gravity }); +} + +TEST_F(TestOperationalSpaceControl, jacobian_transpose_realises_wrench) +{ + current = desired; + ExpectDependencies(Matrix2::Identity(), Matrix2{ { 1.0f, 0.0f }, { 1.0f, 2.0f } }, Vector2{}, Vector2{}, Vector2{}); + + const auto tau{ controller.ComputeTorque(q, Vector2{}, desired, Vector2{}, Vector2{ 1.0f, 2.0f }, Vector2{}) }; + + ExpectNear(tau, Vector2{ 1.0f, 0.5f }); +} + +TEST_F(TestOperationalSpaceControlRedundant, redundant_at_rest_holds_gravity_exactly) +{ + ExpectRestState(); + + ExpectNear(Torque(Vector3{}), gravity); +} + +TEST_F(TestOperationalSpaceControlRedundant, secondary_torque_invisible_to_task) +{ + ExpectRestState(); + const Vector3 secondary{ 1.0f, -2.0f, 0.5f }; + + const Vector3 difference{ Torque(secondary) - Torque(Vector3{}) }; + + ExpectNear(difference, Vector3{ 0.38863f, -1.32780f, 1.06224f }); + ExpectNear(math::Vector{ jacobianMatrix * solvers::SolveSystem(mass, difference) }, Vector2{}, 1e-5f); +} + +TEST_F(TestOperationalSpaceControl, singular_jacobian_uses_damping) +{ + const controllers::OperationalSpaceControl damped{ model, jacobian, kp, kd, 0.1f }; + 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, Jacobian(_)).WillOnce(Return(Matrix2{ { 1.0f, 1.0f }, { 1.0f, 1.0f } })); + EXPECT_CALL(jacobian, BiasAcceleration(_, _)).WillOnce(Return(Vector2{})); + EXPECT_CALL(jacobian, ToolPose(_)).WillOnce(Return(current)); + + const auto tau{ damped.ComputeTorque(q, qDot, desired, xdDot, xdDdot, Vector2{}) }; + + EXPECT_TRUE(std::isfinite(tau[0])); + EXPECT_TRUE(std::isfinite(tau[1])); +} + +TEST_F(TestOperationalSpaceControl, dependencies_queried_once) +{ + ExpectDependencies(Matrix2::Identity(), Matrix2::Identity(), Vector2{}, Vector2{}, Vector2{}); + + static_cast(controller.ComputeTorque(q, qDot, desired, xdDot, xdDdot, Vector2{})); +} diff --git a/robotics/controllers/manipulator/test/TestSlotineLiAdaptiveControl.cpp b/robotics/controllers/manipulator/test/TestSlotineLiAdaptiveControl.cpp new file mode 100644 index 0000000..9b8e52a --- /dev/null +++ b/robotics/controllers/manipulator/test/TestSlotineLiAdaptiveControl.cpp @@ -0,0 +1,189 @@ +#include "robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp" +#include "robotics/dynamics/test_doubles/MockInertialRegressor.hpp" +#include +#include +#include + +namespace +{ + using ::testing::_; + using ::testing::Invoke; + using ::testing::Return; + using Vector2 = math::Vector; + using Vector3 = math::Vector; + using Matrix2 = math::SquareMatrix; + using Matrix3 = math::SquareMatrix; + using Regressor = math::Matrix; + using Controller = controllers::SlotineLiAdaptiveControl; + + MATCHER_P(VectorNear, expected, "") + { + for (std::size_t i = 0; i < 2; ++i) + if (std::abs(arg[i] - expected[i]) > 1e-5f) + return false; + return true; + } + + Regressor PlanarArmRegressor(const Vector2& q, const Vector2& qDot, const Vector2& qrDot, const Vector2& qrDdot) + { + const float c2{ std::cos(q[1]) }; + const float s2{ std::sin(q[1]) }; + return Regressor{ { qrDdot[0], c2 * (2.0f * qrDdot[0] + qrDdot[1]) - s2 * (qDot[1] * qrDot[0] + (qDot[0] + qDot[1]) * qrDot[1]), qrDdot[1] }, + { 0.0f, c2 * qrDdot[0] + s2 * qDot[0] * qrDot[0], qrDdot[0] + qrDdot[1] } }; + } + + class TestSlotineLiAdaptiveControl + : public ::testing::Test + { + protected: + void ExpectNear(const Vector2& actual, const Vector2& expected, float tolerance = 1e-5f) const + { + for (std::size_t i = 0; i < 2; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], 1e-6f); + } + + Vector3 OneStep(const Vector2& sliding, const Vector3& start) const + { + return Vector3{ start - Vector3{ gamma * Vector3{ y.Transpose() * sliding } } * dt }; + } + + ::testing::StrictMock> regressor; + Matrix2 lambda{ { 5.0f, 0.0f }, { 0.0f, 5.0f } }; + Matrix2 kd{ { 20.0f, 0.0f }, { 0.0f, 20.0f } }; + Matrix3 gamma{ Matrix3::Identity() }; + Vector3 initialEstimate{}; + float dt{ 0.001f }; + Controller controller{ regressor, lambda, kd, gamma, initialEstimate, dt }; + Regressor y{ { 1.0f, 0.5f, -0.2f }, { 0.3f, -0.4f, 0.8f } }; + Vector2 q{ 0.4f, -0.3f }; + Vector2 qDot{ 0.2f, 0.1f }; + Vector2 qd{ 0.35f, -0.2f }; + Vector2 qdDot{ 0.1f, 0.3f }; + Vector2 qdDdot{ 0.5f, -0.6f }; + Vector2 sliding{ Vector2{ qDot - qdDot } + Vector2{ lambda * Vector2{ q - qd } } }; + }; +} + +TEST_F(TestSlotineLiAdaptiveControl, zero_error_uses_feedforward_only) +{ + const Vector3 estimate{ 2.0f, 0.5f, 0.7f }; + controller.Reset(estimate); + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(y)); + + const auto tau{ controller.ComputeTorque(qd, qdDot, qd, qdDot, qdDdot) }; + + ExpectNear(tau, Vector2{ y * estimate }); +} + +TEST_F(TestSlotineLiAdaptiveControl, sliding_variable_damping) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(Regressor{})); + + const auto tau{ controller.ComputeTorque(qd, qDot, qd, qdDot, qdDdot) }; + + ExpectNear(tau, Vector2{ kd * Vector2{ qDot - qdDot } } * -1.0f); +} + +TEST_F(TestSlotineLiAdaptiveControl, sliding_variable_definition) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(Regressor{})); + + const auto tau{ controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot) }; + + ExpectNear(Vector2{ tau * (-1.0f / 20.0f) }, Vector2{ 0.1f + 5.0f * 0.05f, -0.2f + 5.0f * -0.1f }); +} + +TEST_F(TestSlotineLiAdaptiveControl, reference_motion_uses_lambda) +{ + const Vector2 qrDot{ Vector2{ qdDot - Vector2{ lambda * Vector2{ q - qd } } } }; + const Vector2 qrDdot{ Vector2{ qdDdot - Vector2{ lambda * Vector2{ qDot - qdDot } } } }; + EXPECT_CALL(regressor, Compute(VectorNear(q), VectorNear(qDot), VectorNear(qrDot), VectorNear(qrDdot))).WillOnce(Return(y)); + + static_cast(controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot)); + + ExpectNear(qrDot, Vector2{ -0.15f, 0.8f }); + ExpectNear(qrDdot, Vector2{ 0.0f, 0.4f }); +} + +TEST_F(TestSlotineLiAdaptiveControl, parameter_estimate_adapts) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(y)); + + static_cast(controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot)); + + ExpectNear(controller.ParameterEstimate(), OneStep(sliding, initialEstimate)); +} + +TEST_F(TestSlotineLiAdaptiveControl, no_adaptation_on_zero_sliding) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(y)); + + static_cast(controller.ComputeTorque(qd, qdDot, qd, qdDot, qdDdot)); + + ExpectNear(controller.ParameterEstimate(), initialEstimate); +} + +TEST_F(TestSlotineLiAdaptiveControl, estimate_persists_across_calls) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).Times(2).WillRepeatedly(Return(y)); + + static_cast(controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot)); + static_cast(controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot)); + + ExpectNear(controller.ParameterEstimate(), OneStep(sliding, OneStep(sliding, initialEstimate))); +} + +TEST_F(TestSlotineLiAdaptiveControl, convergence_tracks_trajectory) +{ + const Vector3 truth{ 3.0f, 0.5f, 0.6f }; + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillRepeatedly(Invoke(PlanarArmRegressor)); + Vector2 position{ 0.2f, -0.1f }; + Vector2 velocity{}; + float error{ 0.0f }; + + for (std::size_t k = 0; k < 10000; ++k) + { + const float time{ static_cast(k) * dt }; + const Vector2 desired{ 0.5f * std::sin(time), 0.3f * std::cos(2.0f * time) }; + const Vector2 desiredRate{ 0.5f * std::cos(time), -0.6f * std::sin(2.0f * time) }; + const Vector2 desiredAcceleration{ -0.5f * std::sin(time), -1.2f * std::cos(2.0f * time) }; + const auto tau{ controller.ComputeTorque(position, velocity, desired, desiredRate, desiredAcceleration) }; + + const float c2{ std::cos(position[1]) }; + const float s2{ std::sin(position[1]) }; + const Matrix2 mass{ { truth[0] + 2.0f * truth[1] * c2, truth[2] + truth[1] * c2 }, { truth[2] + truth[1] * c2, truth[2] } }; + const Matrix2 coriolis{ { -truth[1] * s2 * velocity[1], -truth[1] * s2 * (velocity[0] + velocity[1]) }, { truth[1] * s2 * velocity[0], 0.0f } }; + const Vector2 net{ tau - coriolis * velocity }; + const float determinant{ mass.at(0, 0) * mass.at(1, 1) - mass.at(0, 1) * mass.at(1, 0) }; + const Vector2 acceleration{ (mass.at(1, 1) * net[0] - mass.at(0, 1) * net[1]) / determinant, (mass.at(0, 0) * net[1] - mass.at(1, 0) * net[0]) / determinant }; + velocity = velocity + acceleration * dt; + position = position + velocity * dt; + error = std::hypot(position[0] - 0.5f * std::sin(time + dt), position[1] - 0.3f * std::cos(2.0f * (time + dt))); + } + + EXPECT_LT(error, 1e-2f); + EXPECT_GT(std::abs(controller.ParameterEstimate()[0] - truth[0]), 0.1f); +} + +TEST_F(TestSlotineLiAdaptiveControl, reset_restores_initial_estimate) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(y)); + static_cast(controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot)); + + controller.Reset(initialEstimate); + + ExpectNear(controller.ParameterEstimate(), initialEstimate); +} + +TEST_F(TestSlotineLiAdaptiveControl, regressor_queried_once_per_call) +{ + EXPECT_CALL(regressor, Compute(_, _, _, _)).WillOnce(Return(y)); + + static_cast(controller.ComputeTorque(q, qDot, qd, qdDot, qdDdot)); +} diff --git a/robotics/dynamics/test_doubles/CMakeLists.txt b/robotics/dynamics/test_doubles/CMakeLists.txt index fbcdd24..d1e2644 100644 --- a/robotics/dynamics/test_doubles/CMakeLists.txt +++ b/robotics/dynamics/test_doubles/CMakeLists.txt @@ -8,5 +8,6 @@ target_link_libraries(robotics.dynamics_test_doubles INTERFACE target_sources(robotics.dynamics_test_doubles PRIVATE MockEulerLagrangeDynamics.hpp + MockInertialRegressor.hpp MockInverseDynamicsModel.hpp ) diff --git a/robotics/dynamics/test_doubles/MockInertialRegressor.hpp b/robotics/dynamics/test_doubles/MockInertialRegressor.hpp new file mode 100644 index 0000000..1b7a48a --- /dev/null +++ b/robotics/dynamics/test_doubles/MockInertialRegressor.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "robotics/dynamics/InertialRegressor.hpp" +#include + +namespace dynamics +{ + template + class MockInertialRegressor + : public InertialRegressor + { + public: + using typename InertialRegressor::JointVector; + using typename InertialRegressor::Regressor; + + MOCK_METHOD(Regressor, Compute, (const JointVector& q, const JointVector& qDot, const JointVector& qDotReference, const JointVector& qDDotReference), (const, override)); + }; +} diff --git a/robotics/kinematics/AnalyticalIkOpw.cpp b/robotics/kinematics/AnalyticalIkOpw.cpp new file mode 100644 index 0000000..7cf01b1 --- /dev/null +++ b/robotics/kinematics/AnalyticalIkOpw.cpp @@ -0,0 +1,6 @@ +#include "robotics/kinematics/AnalyticalIkOpw.hpp" + +namespace kinematics +{ + template class AnalyticalIkOpw; +} diff --git a/robotics/kinematics/AnalyticalIkOpw.hpp b/robotics/kinematics/AnalyticalIkOpw.hpp new file mode 100644 index 0000000..594f9f5 --- /dev/null +++ b/robotics/kinematics/AnalyticalIkOpw.hpp @@ -0,0 +1,233 @@ +#pragma once + +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace kinematics +{ + template + struct OpwParameters + { + static_assert(std::is_floating_point_v, "OpwParameters supports floating-point types"); + + T a1{}; + T a2{}; + T b{}; + T c1{}; + T c2{}; + T c3{}; + T c4{}; + std::array offsets{}; + std::array signs{ 1, 1, 1, 1, 1, 1 }; + }; + + template + struct OpwSolution + { + static_assert(std::is_floating_point_v, "OpwSolution supports floating-point types"); + + math::Vector q{}; + bool wristSingular{ false }; + }; + + template + using OpwSolutions = std::array>, 8>; + + template + class AnalyticalIkOpw + { + static_assert(std::is_floating_point_v, "AnalyticalIkOpw supports floating-point types"); + + public: + using JointVector = math::Vector; + + explicit AnalyticalIkOpw(const OpwParameters& parameters); + + OPTIMIZE_FOR_SPEED OpwSolutions Solve(const math::SE3Transform& flange) const; + math::SE3Transform Forward(const JointVector& q) const; + + private: + using Matrix3 = math::Matrix3; + + struct ArmBranch + { + T theta1; + T theta2; + T theta3; + }; + + struct WristAngles + { + T theta4; + T theta5; + T theta6; + }; + + static constexpr T pi{ std::numbers::pi_v }; + static constexpr T acosTolerance{ T{ 100 } * std::numeric_limits::epsilon() }; + static constexpr T wristTolerance{ T{ 100 } * std::numeric_limits::epsilon() }; + + static Matrix3 RotationZ(T angle); + static Matrix3 RotationY(T angle); + static std::optional SafeAcos(T numerator, T denominator); + + void SolveWrist(OpwSolutions& solutions, std::size_t slot, const ArmBranch& arm, const Matrix3& rotation) const; + OpwSolution Emit(const ArmBranch& arm, const WristAngles& wrist, bool singular) const; + + OpwParameters parameters; + T forearm; + T forearmAngle; + }; + + template + AnalyticalIkOpw::AnalyticalIkOpw(const OpwParameters& parameters) + : parameters{ parameters } + , forearm{ math::Sqrt(parameters.a2 * parameters.a2 + parameters.c3 * parameters.c3) } + , forearmAngle{ math::Atan2(parameters.a2, parameters.c3) } + {} + + template + OPTIMIZE_FOR_SPEED OpwSolutions AnalyticalIkOpw::Solve(const math::SE3Transform& flange) const + { + OpwSolutions solutions{}; + const auto& rotation{ flange.rotation }; + const T cx{ flange.translation[0] - parameters.c4 * rotation.at(0, 2) }; + const T cy{ flange.translation[1] - parameters.c4 * rotation.at(1, 2) }; + const T cz{ flange.translation[2] - parameters.c4 * rotation.at(2, 2) }; + + const T radialSquared{ cx * cx + cy * cy - parameters.b * parameters.b }; + if (radialSquared < T{}) + return solutions; + + const T radial{ math::Sqrt(radialSquared) }; + const T n{ radial - parameters.a1 }; + const T z{ cz - parameters.c1 }; + const T azimuth{ math::Atan2(cy, cx) }; + const T lateral{ math::Atan2(parameters.b, radial) }; + const T frontShoulder{ azimuth - lateral }; + const T backShoulder{ azimuth + lateral - pi }; + + const T backReach{ n + T{ 2 } * parameters.a1 }; + const T frontDistanceSquared{ n * n + z * z }; + const T backDistanceSquared{ backReach * backReach + z * z }; + const T c2Squared{ parameters.c2 * parameters.c2 }; + const T forearmSquared{ forearm * forearm }; + const T elbowDenominator{ T{ 2 } * parameters.c2 * forearm }; + + const auto beta1{ SafeAcos(frontDistanceSquared + c2Squared - forearmSquared, T{ 2 } * math::Sqrt(frontDistanceSquared) * parameters.c2) }; + const auto gamma1{ SafeAcos(frontDistanceSquared - c2Squared - forearmSquared, elbowDenominator) }; + const auto beta2{ SafeAcos(backDistanceSquared + c2Squared - forearmSquared, T{ 2 } * math::Sqrt(backDistanceSquared) * parameters.c2) }; + const auto gamma2{ SafeAcos(backDistanceSquared - c2Squared - forearmSquared, elbowDenominator) }; + + if (beta1 && gamma1) + { + const T shoulder{ math::Atan2(n, z) }; + SolveWrist(solutions, 0, ArmBranch{ frontShoulder, shoulder - *beta1, *gamma1 - forearmAngle }, rotation); + SolveWrist(solutions, 1, ArmBranch{ frontShoulder, shoulder + *beta1, -*gamma1 - forearmAngle }, rotation); + } + + if (beta2 && gamma2) + { + const T shoulder{ -math::Atan2(backReach, z) }; + SolveWrist(solutions, 2, ArmBranch{ backShoulder, shoulder - *beta2, *gamma2 - forearmAngle }, rotation); + SolveWrist(solutions, 3, ArmBranch{ backShoulder, shoulder + *beta2, -*gamma2 - forearmAngle }, rotation); + } + + return solutions; + } + + template + math::SE3Transform AnalyticalIkOpw::Forward(const JointVector& q) const + { + std::array theta{}; + for (std::size_t i = 0; i < 6; ++i) + theta[i] = static_cast(parameters.signs[i]) * q[i] - parameters.offsets[i]; + + const T elbow{ theta[1] + theta[2] + forearmAngle }; + const T x{ parameters.c2 * math::Sin(theta[1]) + forearm * math::Sin(elbow) + parameters.a1 }; + const T z{ parameters.c2 * math::Cos(theta[1]) + forearm * math::Cos(elbow) }; + const T cos1{ math::Cos(theta[0]) }; + const T sin1{ math::Sin(theta[0]) }; + const math::Vector3 wristCentre{ x * cos1 - parameters.b * sin1, x * sin1 + parameters.b * cos1, z + parameters.c1 }; + + const Matrix3 rotation{ RotationZ(theta[0]) * RotationY(theta[1] + theta[2]) * RotationZ(theta[3]) * RotationY(theta[4]) * RotationZ(theta[5]) }; + const math::Vector3 approach{ rotation.at(0, 2), rotation.at(1, 2), rotation.at(2, 2) }; + return math::SE3Transform{ rotation, wristCentre + approach * parameters.c4 }; + } + + template + auto AnalyticalIkOpw::RotationZ(T angle) -> Matrix3 + { + const T c{ math::Cos(angle) }; + const T s{ math::Sin(angle) }; + return Matrix3{ { c, -s, T{} }, { s, c, T{} }, { T{}, T{}, T{ 1 } } }; + } + + template + auto AnalyticalIkOpw::RotationY(T angle) -> Matrix3 + { + const T c{ math::Cos(angle) }; + const T s{ math::Sin(angle) }; + return Matrix3{ { c, T{}, s }, { T{}, T{ 1 }, T{} }, { -s, T{}, c } }; + } + + template + std::optional AnalyticalIkOpw::SafeAcos(T numerator, T denominator) + { + if (denominator == T{}) + return std::nullopt; + + const T cosine{ numerator / denominator }; + if (math::Abs(cosine) > T{ 1 } + acosTolerance) + return std::nullopt; + + return math::Acos(std::min(std::max(cosine, T{ -1 }), T{ 1 })); + } + + template + void AnalyticalIkOpw::SolveWrist(OpwSolutions& solutions, std::size_t slot, const ArmBranch& arm, const Matrix3& rotation) const + { + const Matrix3 wrist{ Matrix3{ RotationZ(arm.theta1) * RotationY(arm.theta2 + arm.theta3) }.Transpose() * rotation }; + const T sin5{ math::Sqrt(wrist.at(0, 2) * wrist.at(0, 2) + wrist.at(1, 2) * wrist.at(1, 2)) }; + + if (sin5 > wristTolerance) + { + const WristAngles regular{ math::Atan2(wrist.at(1, 2), wrist.at(0, 2)), math::Atan2(sin5, wrist.at(2, 2)), math::Atan2(wrist.at(2, 1), -wrist.at(2, 0)) }; + solutions[slot] = Emit(arm, regular, false); + solutions[slot + 4] = Emit(arm, WristAngles{ regular.theta4 + pi, -regular.theta5, regular.theta6 - pi }, false); + return; + } + + if (wrist.at(2, 2) > T{}) + solutions[slot] = Emit(arm, WristAngles{ T{}, T{}, math::Atan2(wrist.at(1, 0), wrist.at(1, 1)) }, true); + else + solutions[slot] = Emit(arm, WristAngles{ T{}, pi, -math::Atan2(-wrist.at(1, 0), wrist.at(1, 1)) }, true); + } + + template + OpwSolution AnalyticalIkOpw::Emit(const ArmBranch& arm, const WristAngles& wrist, bool singular) const + { + const std::array theta{ arm.theta1, arm.theta2, arm.theta3, wrist.theta4, wrist.theta5, wrist.theta6 }; + + OpwSolution solution{}; + for (std::size_t i = 0; i < 6; ++i) + solution.q[i] = math::WrapToPi(static_cast(parameters.signs[i]) * (theta[i] + parameters.offsets[i])); + solution.wristSingular = singular; + return solution; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class AnalyticalIkOpw; +#endif +} diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index 5c8e963..6ba9f30 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -13,8 +13,11 @@ target_link_libraries(robotics.kinematics ${ROBOTICS_VISIBILITY} ) target_sources(robotics.kinematics PRIVATE + AnalyticalIkOpw.hpp ChainPoseKinematics.hpp ChainTaskJacobian.hpp + ContinuumKinematics.hpp + DeltaKinematics.hpp DenavitHartenberg.hpp DhTaskJacobian.hpp ForwardKinematics.hpp @@ -25,10 +28,14 @@ target_sources(robotics.kinematics PRIVATE PoseInverseKinematics.hpp ProductOfExponentials.hpp RedundancyResolution.hpp + StewartGoughKinematics.hpp ) robotics_add_coverage_sources(robotics.kinematics + AnalyticalIkOpw.cpp ChainPoseKinematics.cpp + ContinuumKinematics.cpp + DeltaKinematics.cpp DenavitHartenberg.cpp ForwardKinematics.cpp GeometricJacobian.cpp @@ -37,6 +44,7 @@ robotics_add_coverage_sources(robotics.kinematics PoseInverseKinematics.cpp ProductOfExponentials.cpp RedundancyResolution.cpp + StewartGoughKinematics.cpp ) add_subdirectory(test) diff --git a/robotics/kinematics/ContinuumKinematics.cpp b/robotics/kinematics/ContinuumKinematics.cpp new file mode 100644 index 0000000..176d494 --- /dev/null +++ b/robotics/kinematics/ContinuumKinematics.cpp @@ -0,0 +1,7 @@ +#include "robotics/kinematics/ContinuumKinematics.hpp" + +namespace kinematics +{ + template class ContinuumKinematics; + template class ContinuumKinematics; +} diff --git a/robotics/kinematics/ContinuumKinematics.hpp b/robotics/kinematics/ContinuumKinematics.hpp new file mode 100644 index 0000000..0ac5bf4 --- /dev/null +++ b/robotics/kinematics/ContinuumKinematics.hpp @@ -0,0 +1,100 @@ +#pragma once + +#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 +#include +#include +#include + +namespace kinematics +{ + template + struct ArcParameters + { + static_assert(std::is_floating_point_v, "ArcParameters supports floating-point types"); + + T kappa{}; + T phi{}; + T length{}; + }; + + template + class ContinuumKinematics + { + static_assert(std::is_floating_point_v, "ContinuumKinematics supports floating-point types"); + static_assert(NumSections > 0, "At least one section is required"); + + public: + using Sections = std::array, NumSections>; + + explicit ContinuumKinematics(const Sections& sections); + + OPTIMIZE_FOR_SPEED static math::SE3Transform SectionTransform(const ArcParameters& arc); + OPTIMIZE_FOR_SPEED math::SE3Transform Forward() const; + static ArcParameters InverseSection(const math::Vector3& tip); + + private: + static constexpr T seriesThreshold{ T{ 0.01f } }; + + Sections sections; + }; + + template + ContinuumKinematics::ContinuumKinematics(const Sections& sections) + : sections{ sections } + {} + + template + OPTIMIZE_FOR_SPEED math::SE3Transform ContinuumKinematics::SectionTransform(const ArcParameters& arc) + { + const T theta{ arc.kappa * arc.length }; + T chordial{ theta / T{ 2 } - theta * theta * theta / T{ 24 } }; + T axial{ T{ 1 } - theta * theta / T{ 6 } }; + if (math::Abs(theta) >= seriesThreshold) + { + const T halfSin{ math::Sin(theta / T{ 2 }) }; + chordial = T{ 2 } * halfSin * halfSin / theta; + axial = math::Sin(theta) / theta; + } + + const T cosPhi{ math::Cos(arc.phi) }; + const T sinPhi{ math::Sin(arc.phi) }; + const math::Vector3 position{ arc.length * chordial * cosPhi, arc.length * chordial * sinPhi, arc.length * axial }; + return math::SE3Transform{ math::RotationAboutAxis(math::Vector3{ -sinPhi, cosPhi, T{} }, theta), position }; + } + + template + OPTIMIZE_FOR_SPEED math::SE3Transform ContinuumKinematics::Forward() const + { + auto tip{ math::SE3Transform::Identity() }; + for (const auto& arc : sections) + tip = tip * SectionTransform(arc); + return tip; + } + + template + ArcParameters ContinuumKinematics::InverseSection(const math::Vector3& tip) + { + const T radial{ math::Sqrt(tip[0] * tip[0] + tip[1] * tip[1]) }; + const T distanceSquared{ radial * radial + tip[2] * tip[2] }; + const T distance{ math::Sqrt(distanceSquared) }; + if (radial <= std::numeric_limits::epsilon() * distance) + return ArcParameters{ T{}, T{}, tip[2] }; + + const T halfTheta{ math::Atan2(radial, tip[2]) }; + T arcOverChord{ T{ 1 } + halfTheta * halfTheta / T{ 6 } }; + if (math::Abs(halfTheta) >= seriesThreshold) + arcOverChord = halfTheta / math::Sin(halfTheta); + + return ArcParameters{ T{ 2 } * radial / distanceSquared, math::Atan2(tip[1], tip[0]), distance * arcOverChord }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ContinuumKinematics; + extern template class ContinuumKinematics; +#endif +} diff --git a/robotics/kinematics/DeltaKinematics.cpp b/robotics/kinematics/DeltaKinematics.cpp new file mode 100644 index 0000000..4d22902 --- /dev/null +++ b/robotics/kinematics/DeltaKinematics.cpp @@ -0,0 +1,6 @@ +#include "robotics/kinematics/DeltaKinematics.hpp" + +namespace kinematics +{ + template class DeltaKinematics; +} diff --git a/robotics/kinematics/DeltaKinematics.hpp b/robotics/kinematics/DeltaKinematics.hpp new file mode 100644 index 0000000..c167008 --- /dev/null +++ b/robotics/kinematics/DeltaKinematics.hpp @@ -0,0 +1,123 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include +#include +#include +#include +#include + +namespace kinematics +{ + template + struct DeltaGeometry + { + static_assert(std::is_floating_point_v, "DeltaGeometry supports floating-point types"); + + T baseRadius{}; + T effectorRadius{}; + T upperArm{}; + T forearm{}; + }; + + template + class DeltaKinematics + { + static_assert(std::is_floating_point_v, "DeltaKinematics supports floating-point types"); + + public: + explicit DeltaKinematics(const DeltaGeometry& geometry); + + OPTIMIZE_FOR_SPEED std::optional> Inverse(const math::Vector3& effector) const; + std::optional> Forward(const math::Vector3& angles) const; + + private: + std::optional ArmAngle(std::size_t arm, const math::Vector3& effector) const; + math::Vector3 ShiftedElbow(std::size_t arm, T angle) const; + + static constexpr std::array cosPhi{ T{ 1 }, T{ -0.5f }, T{ -0.5f } }; + static constexpr std::array sinPhi{ T{}, std::numbers::sqrt3_v / T{ 2 }, -std::numbers::sqrt3_v / T{ 2 } }; + + DeltaGeometry geometry; + }; + + template + DeltaKinematics::DeltaKinematics(const DeltaGeometry& geometry) + : geometry{ geometry } + {} + + template + OPTIMIZE_FOR_SPEED std::optional> DeltaKinematics::Inverse(const math::Vector3& effector) const + { + const auto first{ ArmAngle(0, effector) }; + const auto second{ ArmAngle(1, effector) }; + const auto third{ ArmAngle(2, effector) }; + if (!first || !second || !third) + return std::nullopt; + + return math::Vector3{ *first, *second, *third }; + } + + template + std::optional> DeltaKinematics::Forward(const math::Vector3& angles) const + { + const auto c1{ ShiftedElbow(0, angles[0]) }; + const auto c2{ ShiftedElbow(1, angles[1]) }; + const auto c3{ ShiftedElbow(2, angles[2]) }; + + const math::Vector3 toSecond{ c2 - c1 }; + const math::Vector3 toThird{ c3 - c1 }; + const T d{ math::VectorNorm(toSecond) }; + const math::Vector3 ex{ toSecond * (T{ 1 } / d) }; + const T i{ ex[0] * toThird[0] + ex[1] * toThird[1] + ex[2] * toThird[2] }; + const math::Vector3 lateral{ toThird - ex * i }; + const math::Vector3 ey{ lateral * (T{ 1 } / math::VectorNorm(lateral)) }; + const T j{ ey[0] * toThird[0] + ey[1] * toThird[1] + ey[2] * toThird[2] }; + const math::Vector3 ez{ math::CrossProduct(ex, ey) }; + + const T x{ d / T{ 2 } }; + const T y{ (i * i + j * j - T{ 2 } * i * x) / (T{ 2 } * j) }; + const T heightSquared{ geometry.forearm * geometry.forearm - x * x - y * y }; + if (heightSquared < T{}) + return std::nullopt; + + const math::Vector3 centre{ c1 + ex * x + ey * y }; + const math::Vector3 offset{ ez * math::Sqrt(heightSquared) }; + const math::Vector3 above{ centre + offset }; + const math::Vector3 below{ centre - offset }; + return above[2] < below[2] ? above : below; + } + + template + std::optional DeltaKinematics::ArmAngle(std::size_t arm, const math::Vector3& effector) const + { + const T radial{ cosPhi[arm] * effector[0] + sinPhi[arm] * effector[1] }; + const T tangential{ -sinPhi[arm] * effector[0] + cosPhi[arm] * effector[1] }; + const T z{ effector[2] }; + const T reach{ radial + geometry.effectorRadius - geometry.baseRadius }; + + const T a{ T{ 2 } * geometry.upperArm * reach }; + const T b{ T{ -2 } * geometry.upperArm * z }; + const T c{ reach * reach + tangential * tangential + z * z + geometry.upperArm * geometry.upperArm - geometry.forearm * geometry.forearm }; + const T magnitude{ math::Sqrt(a * a + b * b) }; + if (magnitude == T{} || math::Abs(c) > magnitude) + return std::nullopt; + + return math::Atan2(b, a) - math::Acos(std::clamp(c / magnitude, T{ -1 }, T{ 1 })); + } + + template + math::Vector3 DeltaKinematics::ShiftedElbow(std::size_t arm, T angle) const + { + const T radial{ geometry.baseRadius - geometry.effectorRadius + geometry.upperArm * math::Cos(angle) }; + return math::Vector3{ cosPhi[arm] * radial, sinPhi[arm] * radial, -geometry.upperArm * math::Sin(angle) }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DeltaKinematics; +#endif +} diff --git a/robotics/kinematics/StewartGoughKinematics.cpp b/robotics/kinematics/StewartGoughKinematics.cpp new file mode 100644 index 0000000..9eb9f65 --- /dev/null +++ b/robotics/kinematics/StewartGoughKinematics.cpp @@ -0,0 +1,7 @@ +#include "robotics/kinematics/StewartGoughKinematics.hpp" + +namespace kinematics +{ + template class StewartGoughKinematics; + template class StewartLegResidual; +} diff --git a/robotics/kinematics/StewartGoughKinematics.hpp b/robotics/kinematics/StewartGoughKinematics.hpp new file mode 100644 index 0000000..2a62f4c --- /dev/null +++ b/robotics/kinematics/StewartGoughKinematics.hpp @@ -0,0 +1,167 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.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 +#include +#include +#include + +namespace kinematics +{ + template + struct StewartGoughGeometry + { + static_assert(std::is_floating_point_v, "StewartGoughGeometry supports floating-point types"); + + std::array, 6> baseAnchors{}; + std::array, 6> platformAnchors{}; + }; + + template + struct StewartForwardConfig + { + static_assert(std::is_floating_point_v, "StewartForwardConfig supports floating-point types"); + + T tolerance{}; + T damping{}; + std::size_t maxIterations{}; + }; + + template + struct StewartForwardResult + { + static_assert(std::is_floating_point_v, "StewartForwardResult supports floating-point types"); + + math::SE3Transform pose{}; + std::size_t iterations{}; + bool converged{ false }; + }; + + template + class StewartGoughKinematics + { + static_assert(std::is_floating_point_v, "StewartGoughKinematics supports floating-point types"); + + public: + using LegLengths = std::array; + + StewartGoughKinematics(const StewartGoughGeometry& geometry, const StewartForwardConfig& config); + + OPTIMIZE_FOR_SPEED LegLengths Inverse(const math::SE3Transform& pose) const; + math::Matrix6 LegJacobian(const math::SE3Transform& pose) const; + StewartForwardResult Forward(const LegLengths& lengths, const math::SE3Transform& guess) const; + + private: + StewartGoughGeometry geometry; + StewartForwardConfig config; + }; + + template + class StewartLegResidual + : public optimization::LeastSquaresProblem, 6, 6> + { + static_assert(std::is_floating_point_v, "StewartLegResidual supports floating-point types"); + + public: + using LegLengths = typename StewartGoughKinematics::LegLengths; + + StewartLegResidual(const StewartGoughKinematics& kinematics, const LegLengths& lengths); + + math::Vector6 Residual(const math::SE3Transform& pose) const override; + math::Matrix6 Jacobian(const math::SE3Transform& pose) const override; + math::SE3Transform Retract(const math::SE3Transform& pose, const math::Vector6& delta) const override; + + private: + const StewartGoughKinematics& kinematics; + LegLengths lengths; + }; + + template + StewartGoughKinematics::StewartGoughKinematics(const StewartGoughGeometry& geometry, const StewartForwardConfig& config) + : geometry{ geometry } + , config{ config } + {} + + template + OPTIMIZE_FOR_SPEED auto StewartGoughKinematics::Inverse(const math::SE3Transform& pose) const -> LegLengths + { + LegLengths lengths{}; + for (std::size_t i = 0; i < 6; ++i) + lengths[i] = math::VectorNorm(math::Vector3{ pose.rotation * geometry.platformAnchors[i] + pose.translation - geometry.baseAnchors[i] }); + return lengths; + } + + template + math::Matrix6 StewartGoughKinematics::LegJacobian(const math::SE3Transform& pose) const + { + math::Matrix6 jacobian{}; + for (std::size_t i = 0; i < 6; ++i) + { + const math::Vector3 arm{ pose.rotation * geometry.platformAnchors[i] }; + const math::Vector3 leg{ arm + pose.translation - geometry.baseAnchors[i] }; + const math::Vector3 direction{ leg * (T{ 1 } / math::VectorNorm(leg)) }; + const math::Vector3 moment{ math::CrossProduct(arm, direction) }; + for (std::size_t c = 0; c < 3; ++c) + { + jacobian.at(i, c) = direction[c]; + jacobian.at(i, c + 3) = moment[c]; + } + } + return jacobian; + } + + template + StewartForwardResult StewartGoughKinematics::Forward(const LegLengths& lengths, const math::SE3Transform& guess) const + { + const StewartLegResidual problem{ *this, lengths }; + const optimization::ConstantDamping damping{ config.damping * config.damping }; + const optimization::LevenbergMarquardt, 6, 6> solver{ problem, damping, { config.tolerance, std::numeric_limits::infinity(), config.maxIterations } }; + + const auto solution{ solver.Solve(guess) }; + return StewartForwardResult{ solution.x, solution.iterations, solution.converged }; + } + + template + StewartLegResidual::StewartLegResidual(const StewartGoughKinematics& kinematics, const LegLengths& lengths) + : kinematics{ kinematics } + , lengths{ lengths } + {} + + template + math::Vector6 StewartLegResidual::Residual(const math::SE3Transform& pose) const + { + const auto current{ kinematics.Inverse(pose) }; + math::Vector6 residual{}; + for (std::size_t i = 0; i < 6; ++i) + residual[i] = current[i] - lengths[i]; + return residual; + } + + template + math::Matrix6 StewartLegResidual::Jacobian(const math::SE3Transform& pose) const + { + return kinematics.LegJacobian(pose); + } + + template + math::SE3Transform StewartLegResidual::Retract(const math::SE3Transform& pose, const math::Vector6& delta) const + { + math::Vector6 rotation{}; + for (std::size_t i = 3; i < 6; ++i) + rotation[i] = delta[i]; + + return math::SE3Transform{ math::Matrix3{ math::SE3Transform::Exp(rotation, T{ 1 }).rotation * pose.rotation }, + math::Vector3{ pose.translation + math::Vector3{ delta[0], delta[1], delta[2] } } }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class StewartGoughKinematics; + extern template class StewartLegResidual; +#endif +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index 157c043..de57d26 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -9,7 +9,10 @@ target_link_libraries(robotics.kinematics_test PUBLIC ) target_sources(robotics.kinematics_test PRIVATE + TestAnalyticalIkOpw.cpp TestChainPoseKinematics.cpp + TestContinuumKinematics.cpp + TestDeltaKinematics.cpp TestDenavitHartenberg.cpp TestForwardKinematics.cpp TestGeometricJacobian.cpp @@ -18,4 +21,5 @@ target_sources(robotics.kinematics_test PRIVATE TestPoseInverseKinematics.cpp TestProductOfExponentials.cpp TestRedundancyResolution.cpp + TestStewartGoughKinematics.cpp ) diff --git a/robotics/kinematics/test/TestAnalyticalIkOpw.cpp b/robotics/kinematics/test/TestAnalyticalIkOpw.cpp new file mode 100644 index 0000000..df4d2c3 --- /dev/null +++ b/robotics/kinematics/test/TestAnalyticalIkOpw.cpp @@ -0,0 +1,219 @@ +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/AnalyticalIkOpw.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Joints = math::Vector; + using Transform = math::SE3Transform; + using Parameters = kinematics::OpwParameters; + using Ik = kinematics::AnalyticalIkOpw; + + constexpr float pi{ std::numbers::pi_v }; + + Matrix3 RotationZ(float angle) + { + return Matrix3{ { std::cos(angle), -std::sin(angle), 0.0f }, { std::sin(angle), std::cos(angle), 0.0f }, { 0.0f, 0.0f, 1.0f } }; + } + + Matrix3 RotationY(float angle) + { + return Matrix3{ { std::cos(angle), 0.0f, std::sin(angle) }, { 0.0f, 1.0f, 0.0f }, { -std::sin(angle), 0.0f, std::cos(angle) } }; + } + + Transform Rotate(const Matrix3& rotation) + { + return Transform{ rotation, Vector3{} }; + } + + Transform Translate(float x, float y, float z) + { + return Transform{ Matrix3::Identity(), Vector3{ x, y, z } }; + } + + class TestAnalyticalIkOpw + : public ::testing::Test + { + protected: + static std::array ModelAngles(const Parameters& parameters, const Joints& q) + { + std::array theta{}; + for (std::size_t i = 0; i < 6; ++i) + theta[i] = static_cast(parameters.signs[i]) * q[i] - parameters.offsets[i]; + return theta; + } + + static Transform ElementaryChain(const Parameters& parameters, const Joints& q) + { + const auto theta{ ModelAngles(parameters, q) }; + return Rotate(RotationZ(theta[0])) * Translate(parameters.a1, parameters.b, parameters.c1) * Rotate(RotationY(theta[1])) * Translate(0.0f, 0.0f, parameters.c2) * + Rotate(RotationY(theta[2])) * Translate(parameters.a2, 0.0f, parameters.c3) * Rotate(RotationZ(theta[3])) * Rotate(RotationY(theta[4])) * Rotate(RotationZ(theta[5])) * + Translate(0.0f, 0.0f, parameters.c4); + } + + static void ExpectPose(const Transform& actual, const Transform& expected, float tolerance) + { + for (std::size_t r = 0; r < 3; ++r) + { + EXPECT_NEAR(actual.translation[r], expected.translation[r], tolerance); + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(actual.rotation.at(r, c), expected.rotation.at(r, c), tolerance); + } + } + + static bool SameAngles(const Joints& lhs, const Joints& rhs, float tolerance) + { + for (std::size_t i = 0; i < 6; ++i) + if (std::abs(math::WrapToPi(lhs[i] - rhs[i])) > tolerance) + return false; + return true; + } + + static void ExpectRoundTrips(const Parameters& parameters) + { + const Ik solver{ parameters }; + std::uint32_t state{ 12345u }; + const auto next{ [&state] + { + state = state * 1664525u + 1013904223u; + return static_cast(state) / 4294967296.0f * 2.0f * pi - pi; + } }; + + for (std::size_t sample = 0; sample < 200; ++sample) + { + const Joints q{ next(), next(), next(), next(), next(), next() }; + const auto target{ solver.Forward(q) }; + bool found{ false }; + for (const auto& solution : solver.Solve(target)) + if (solution) + { + ExpectPose(solver.Forward(solution->q), target, 1e-3f); + found = found || SameAngles(solution->q, q, 1e-3f); + } + const bool wristWellConditioned{ std::abs(std::sin(ModelAngles(parameters, q)[4])) > 1e-2f }; + EXPECT_TRUE(found || !wristWellConditioned) << "sample " << sample; + } + } + + Parameters kr6{ 0.025f, -0.035f, 0.0f, 0.400f, 0.315f, 0.365f, 0.080f, { 0.0f, -pi / 2.0f, 0.0f, 0.0f, 0.0f, 0.0f }, { -1, 1, 1, -1, 1, -1 } }; + Ik ik{ kr6 }; + Joints qTrue{ 0.3f, -1.0f, 1.2f, 0.5f, 0.6f, 0.7f }; + }; +} + +TEST_F(TestAnalyticalIkOpw, forward_matches_elementary_chain) +{ + ExpectPose(ik.Forward(qTrue), ElementaryChain(kr6, qTrue), 1e-5f); + ExpectPose(ik.Forward(Joints{}), ElementaryChain(kr6, Joints{}), 1e-5f); +} + +TEST_F(TestAnalyticalIkOpw, home_flange_pose) +{ + ExpectPose(ik.Forward(Joints{}), Transform{ Matrix3{ { 0.0f, 0.0f, 1.0f }, { 0.0f, 1.0f, 0.0f }, { -1.0f, 0.0f, 0.0f } }, Vector3{ 0.785f, 0.0f, 0.435f } }, 1e-5f); +} + +TEST_F(TestAnalyticalIkOpw, every_solution_reproduces_target) +{ + const auto target{ ik.Forward(qTrue) }; + + const auto solutions{ ik.Solve(target) }; + + for (const auto& solution : solutions) + { + ASSERT_TRUE(solution.has_value()); + EXPECT_FALSE(solution->wristSingular); + ExpectPose(ik.Forward(solution->q), target, 1e-4f); + } +} + +TEST_F(TestAnalyticalIkOpw, finds_the_known_configuration) +{ + const auto solutions{ ik.Solve(ik.Forward(qTrue)) }; + + ASSERT_TRUE(solutions[0].has_value()); + EXPECT_TRUE(SameAngles(solutions[0]->q, qTrue, 1e-4f)); +} + +TEST_F(TestAnalyticalIkOpw, wrist_flip_pairs) +{ + const auto solutions{ ik.Solve(ik.Forward(qTrue)) }; + + for (std::size_t j = 0; j < 4; ++j) + { + ASSERT_TRUE(solutions[j].has_value()); + ASSERT_TRUE(solutions[j + 4].has_value()); + const auto regular{ ModelAngles(kr6, solutions[j]->q) }; + const auto flipped{ ModelAngles(kr6, solutions[j + 4]->q) }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(math::WrapToPi(flipped[i] - regular[i]), 0.0f, 1e-5f); + EXPECT_NEAR(math::WrapToPi(flipped[3] - regular[3] - pi), 0.0f, 1e-5f); + EXPECT_NEAR(math::WrapToPi(flipped[4] + regular[4]), 0.0f, 1e-5f); + EXPECT_NEAR(math::WrapToPi(flipped[5] - regular[5] + pi), 0.0f, 1e-5f); + } +} + +TEST_F(TestAnalyticalIkOpw, random_poses_round_trip) +{ + ExpectRoundTrips(kr6); +} + +TEST_F(TestAnalyticalIkOpw, general_offsets_signs_and_lateral_offset) +{ + ExpectRoundTrips(Parameters{ 0.10f, -0.05f, 0.07f, 0.50f, 0.60f, 0.55f, 0.10f, { 0.1f, -0.2f, 0.3f, 0.0f, 0.4f, -0.5f }, { 1, -1, 1, 1, -1, 1 } }); +} + +TEST_F(TestAnalyticalIkOpw, wrist_singularity_keeps_the_sum) +{ + const auto target{ ik.Forward(Joints{ 0.3f, -1.0f, 1.2f, 0.5f, 0.0f, 0.7f }) }; + + const auto solutions{ ik.Solve(target) }; + + ASSERT_TRUE(solutions[0].has_value()); + EXPECT_TRUE(solutions[0]->wristSingular); + EXPECT_TRUE(SameAngles(solutions[0]->q, Joints{ 0.3f, -1.0f, 1.2f, 0.0f, 0.0f, 1.2f }, 1e-3f)); + EXPECT_FALSE(solutions[4].has_value()); + ExpectPose(ik.Forward(solutions[0]->q), target, 1e-4f); + + const auto flippedTarget{ ik.Forward(Joints{ 0.3f, -1.0f, 1.2f, 0.5f, pi, 0.7f }) }; + const auto flipped{ ik.Solve(flippedTarget) }; + ASSERT_TRUE(flipped[0].has_value()); + EXPECT_TRUE(flipped[0]->wristSingular); + EXPECT_FALSE(flipped[4].has_value()); + ExpectPose(ik.Forward(flipped[0]->q), flippedTarget, 1e-4f); +} + +TEST_F(TestAnalyticalIkOpw, stretched_arm_elbows_coincide) +{ + const auto target{ ik.Forward(Joints{ 0.3f, -0.3f, 0.095598f, 0.5f, 0.6f, 0.7f }) }; + + const auto solutions{ ik.Solve(target) }; + + ASSERT_TRUE(solutions[0].has_value()); + ASSERT_TRUE(solutions[1].has_value()); + EXPECT_TRUE(SameAngles(solutions[0]->q, solutions[1]->q, 1e-3f)); + ExpectPose(ik.Forward(solutions[0]->q), target, 1e-3f); + ExpectPose(ik.Forward(solutions[1]->q), target, 1e-3f); +} + +TEST_F(TestAnalyticalIkOpw, unreachable_target_returns_no_solution) +{ + const auto solutions{ ik.Solve(Translate(1.5f, 0.0f, 0.4f)) }; + + for (const auto& solution : solutions) + EXPECT_FALSE(solution.has_value()); + + const Ik lateral{ Parameters{ 0.10f, -0.05f, 0.07f, 0.50f, 0.60f, 0.55f, 0.10f, {}, { 1, 1, 1, 1, 1, 1 } } }; + for (const auto& solution : lateral.Solve(Translate(0.01f, 0.0f, 0.6f))) + EXPECT_FALSE(solution.has_value()); + + const Ik foldedOnShoulder{ Parameters{ 0.0f, -0.05f, 0.0f, 0.5f, 0.6f, 0.55f, 0.25f, {}, { 1, 1, 1, 1, 1, 1 } } }; + for (const auto& solution : foldedOnShoulder.Solve(Translate(0.0f, 0.0f, 0.75f))) + EXPECT_FALSE(solution.has_value()); +} diff --git a/robotics/kinematics/test/TestContinuumKinematics.cpp b/robotics/kinematics/test/TestContinuumKinematics.cpp new file mode 100644 index 0000000..2e28710 --- /dev/null +++ b/robotics/kinematics/test/TestContinuumKinematics.cpp @@ -0,0 +1,117 @@ +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/ContinuumKinematics.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Arc = kinematics::ArcParameters; + using Continuum = kinematics::ContinuumKinematics; + + constexpr float pi{ std::numbers::pi_v }; + + class TestContinuumKinematics + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected, float tolerance = 1e-5f) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + static void ExpectIdentityRotation(const math::Matrix3& rotation) + { + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(rotation.at(r, c), r == c ? 1.0f : 0.0f, 1e-6f); + } + + static float Norm(const Vector3& v) + { + return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + } + + static void ExpectArc(const Arc& actual, const Arc& expected) + { + EXPECT_NEAR(actual.kappa, expected.kappa, 1e-4f); + EXPECT_NEAR(actual.phi, expected.phi, 1e-5f); + EXPECT_NEAR(actual.length, expected.length, 1e-5f); + } + + Arc quarter{ pi / 2.0f, 0.0f, 1.0f }; + Arc general{ 2.0f, 0.7f, 1.2f }; + Arc negative{ -1.5f, 0.3f, 0.8f }; + Continuum twoSections{ { general, general } }; + }; +} + +TEST_F(TestContinuumKinematics, straight_section_is_pure_translation) +{ + const auto pose{ Continuum::SectionTransform(Arc{ 0.0f, 0.0f, 1.0f }) }; + + ExpectIdentityRotation(pose.rotation); + ExpectNear(pose.translation, Vector3{ 0.0f, 0.0f, 1.0f }); +} + +TEST_F(TestContinuumKinematics, quarter_circle_bend) +{ + const auto pose{ Continuum::SectionTransform(quarter) }; + + ExpectNear(pose.translation, Vector3{ 0.636620f, 0.0f, 0.636620f }); + ExpectNear(Vector3{ pose.rotation * Vector3{ 0.0f, 0.0f, 1.0f } }, Vector3{ 1.0f, 0.0f, 0.0f }); + ExpectNear(Vector3{ pose.rotation * Vector3{ 1.0f, 0.0f, 0.0f } }, Vector3{ 0.0f, 0.0f, -1.0f }); +} + +TEST_F(TestContinuumKinematics, bending_plane_angle_rotates_tip) +{ + ExpectNear(Continuum::SectionTransform(Arc{ pi / 2.0f, pi / 2.0f, 1.0f }).translation, Vector3{ 0.0f, 0.636620f, 0.636620f }); +} + +TEST_F(TestContinuumKinematics, tip_distance_equals_chord) +{ + EXPECT_NEAR(Norm(Continuum::SectionTransform(quarter).translation), 0.900316f, 1e-5f); + EXPECT_NEAR(Norm(Continuum::SectionTransform(general).translation), 0.932039f, 1e-5f); + EXPECT_NEAR(Norm(Continuum::SectionTransform(negative).translation), 0.752857f, 1e-5f); +} + +TEST_F(TestContinuumKinematics, tip_tangent_matches_bend) +{ + const auto pose{ Continuum::SectionTransform(general) }; + + ExpectNear(Vector3{ pose.rotation * Vector3{ 0.0f, 0.0f, 1.0f } }, Vector3{ 0.516623f, 0.435145f, -0.737394f }); + ExpectNear(pose.translation, Vector3{ 0.664416f, 0.559630f, 0.337732f }); +} + +TEST_F(TestContinuumKinematics, forward_composes_sections) +{ + const auto single{ Continuum::SectionTransform(general) }; + const auto composed{ single * single }; + + const auto tip{ twoSections.Forward() }; + + ExpectNear(tip.translation, composed.translation); + ExpectNear(tip.translation, Vector3{ 0.348960f, 0.293925f, -0.498082f }); + ExpectNear(Vector3{ tip.rotation * Vector3{ 0.0f, 0.0f, 1.0f } }, Vector3{ composed.rotation * Vector3{ 0.0f, 0.0f, 1.0f } }); +} + +TEST_F(TestContinuumKinematics, inverse_recovers_arc_parameters) +{ + ExpectArc(Continuum::InverseSection(Continuum::SectionTransform(quarter).translation), quarter); + ExpectArc(Continuum::InverseSection(Continuum::SectionTransform(general).translation), general); + ExpectArc(Continuum::InverseSection(Continuum::SectionTransform(negative).translation), Arc{ 1.5f, 0.3f - pi, 0.8f }); + ExpectArc(Continuum::InverseSection(Vector3{ 0.0f, 0.0f, 0.7f }), Arc{ 0.0f, 0.0f, 0.7f }); +} + +TEST_F(TestContinuumKinematics, near_zero_curvature_uses_series) +{ + const auto straight{ Continuum::SectionTransform(Arc{ 1e-9f, 0.4f, 1.0f }) }; + const auto slight{ Continuum::SectionTransform(Arc{ 1e-5f, 0.4f, 1.0f }) }; + + ExpectIdentityRotation(straight.rotation); + ExpectNear(straight.translation, Vector3{ 0.0f, 0.0f, 1.0f }); + ExpectNear(slight.translation, Vector3{ 4.605e-6f, 1.947e-6f, 1.0f }, 1e-8f); + ExpectArc(Continuum::InverseSection(slight.translation), Arc{ 1e-5f, 0.4f, 1.0f }); +} diff --git a/robotics/kinematics/test/TestDeltaKinematics.cpp b/robotics/kinematics/test/TestDeltaKinematics.cpp new file mode 100644 index 0000000..bd37e70 --- /dev/null +++ b/robotics/kinematics/test/TestDeltaKinematics.cpp @@ -0,0 +1,98 @@ +#include "robotics/kinematics/DeltaKinematics.hpp" +#include +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Delta = kinematics::DeltaKinematics; + + constexpr float pi{ std::numbers::pi_v }; + + class TestDeltaKinematics + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected, float tolerance) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + kinematics::DeltaGeometry geometry{ 0.1f, 0.03f, 0.2f, 0.45f }; + Delta delta{ geometry }; + std::array points{ Vector3{ 0.05f, -0.03f, -0.40f }, Vector3{ -0.08f, 0.06f, -0.32f }, Vector3{ 0.12f, 0.10f, -0.45f } }; + }; +} + +TEST_F(TestDeltaKinematics, zero_angles_put_the_effector_on_the_axis) +{ + const auto effector{ delta.Forward(Vector3{}) }; + + ASSERT_TRUE(effector.has_value()); + ExpectNear(*effector, Vector3{ 0.0f, 0.0f, -0.36f }, 1e-5f); +} + +TEST_F(TestDeltaKinematics, inverse_of_known_points) +{ + const auto first{ delta.Inverse(points[0]) }; + const auto second{ delta.Inverse(points[1]) }; + + ASSERT_TRUE(first.has_value()); + ASSERT_TRUE(second.has_value()); + ExpectNear(*first, Vector3{ 0.042468f, 0.371178f, 0.205749f }, 1e-5f); + ExpectNear(*second, Vector3{ 0.195443f, -0.538250f, -0.084938f }, 1e-5f); +} + +TEST_F(TestDeltaKinematics, inverse_forward_round_trip) +{ + for (const auto& point : points) + { + const auto angles{ delta.Inverse(point) }; + ASSERT_TRUE(angles.has_value()); + const auto effector{ delta.Forward(*angles) }; + ASSERT_TRUE(effector.has_value()); + ExpectNear(*effector, point, 1e-5f); + } +} + +TEST_F(TestDeltaKinematics, forward_inverse_round_trip) +{ + constexpr std::array grid{ -0.6f, 0.0f, 0.6f, 1.2f }; + for (float first : grid) + for (float second : grid) + for (float third : grid) + { + const Vector3 angles{ first, second, third }; + const auto effector{ delta.Forward(angles) }; + ASSERT_TRUE(effector.has_value()); + const auto recovered{ delta.Inverse(*effector) }; + ASSERT_TRUE(recovered.has_value()); + ExpectNear(*recovered, angles, 1e-4f); + } +} + +TEST_F(TestDeltaKinematics, forearm_constraint_holds) +{ + const auto angles{ delta.Inverse(points[0]) }; + ASSERT_TRUE(angles.has_value()); + + for (std::size_t arm = 0; arm < 3; ++arm) + { + const float phi{ 2.0f * pi / 3.0f * static_cast(arm) }; + const float elbowRadial{ geometry.baseRadius + geometry.upperArm * std::cos((*angles)[arm]) }; + const Vector3 elbow{ std::cos(phi) * elbowRadial, std::sin(phi) * elbowRadial, -geometry.upperArm * std::sin((*angles)[arm]) }; + const Vector3 attachment{ points[0][0] + std::cos(phi) * geometry.effectorRadius, points[0][1] + std::sin(phi) * geometry.effectorRadius, points[0][2] }; + EXPECT_NEAR(std::hypot(elbow[0] - attachment[0], elbow[1] - attachment[1], elbow[2] - attachment[2]), geometry.forearm, 1e-5f); + } +} + +TEST_F(TestDeltaKinematics, unreachable_point_has_no_solution) +{ + EXPECT_FALSE(delta.Inverse(Vector3{ 0.0f, 0.0f, -0.9f }).has_value()); + + const Delta shortForearms{ kinematics::DeltaGeometry{ 0.1f, 0.03f, 0.2f, 0.1f } }; + EXPECT_FALSE(shortForearms.Forward(Vector3{}).has_value()); +} diff --git a/robotics/kinematics/test/TestStewartGoughKinematics.cpp b/robotics/kinematics/test/TestStewartGoughKinematics.cpp new file mode 100644 index 0000000..7812a58 --- /dev/null +++ b/robotics/kinematics/test/TestStewartGoughKinematics.cpp @@ -0,0 +1,139 @@ +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/StewartGoughKinematics.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Transform = math::SE3Transform; + using Hexapod = kinematics::StewartGoughKinematics; + + constexpr float pi{ std::numbers::pi_v }; + + Matrix3 RotationZ(float angle) + { + return Matrix3{ { std::cos(angle), -std::sin(angle), 0.0f }, { std::sin(angle), std::cos(angle), 0.0f }, { 0.0f, 0.0f, 1.0f } }; + } + + Matrix3 RotationX(float angle) + { + return Matrix3{ { 1.0f, 0.0f, 0.0f }, { 0.0f, std::cos(angle), -std::sin(angle) }, { 0.0f, std::sin(angle), std::cos(angle) } }; + } + + kinematics::StewartGoughGeometry MakeGeometry() + { + kinematics::StewartGoughGeometry geometry{}; + for (std::size_t k = 0; k < 6; ++k) + { + const float side{ k % 2 == 0 ? -1.0f : 1.0f }; + const float sector{ 2.0f * pi / 3.0f * static_cast(k / 2) }; + const float base{ sector + side * pi / 12.0f }; + const float platform{ sector + side * pi / 4.0f }; + geometry.baseAnchors[k] = Vector3{ std::cos(base), std::sin(base), 0.0f }; + geometry.platformAnchors[k] = Vector3{ 0.6f * std::cos(platform), 0.6f * std::sin(platform), 0.0f }; + } + return geometry; + } + + class TestStewartGoughKinematics + : public ::testing::Test + { + protected: + static void ExpectLegs(const Hexapod::LegLengths& actual, const Hexapod::LegLengths& expected, float tolerance = 1e-5f) + { + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + static void ExpectPose(const Transform& actual, const Transform& expected, float tolerance) + { + for (std::size_t r = 0; r < 3; ++r) + { + EXPECT_NEAR(actual.translation[r], expected.translation[r], tolerance); + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(actual.rotation.at(r, c), expected.rotation.at(r, c), tolerance); + } + } + + kinematics::StewartGoughGeometry geometry{ MakeGeometry() }; + Hexapod hexapod{ geometry, { 1e-5f, 0.0f, 50 } }; + Transform home{ Matrix3::Identity(), Vector3{ 0.0f, 0.0f, 0.8f } }; + Transform tilted{ Matrix3{ RotationZ(0.1f) * RotationX(0.05f) }, Vector3{ 0.05f, -0.03f, 0.85f } }; + }; +} + +TEST_F(TestStewartGoughKinematics, home_legs_are_equal) +{ + ExpectLegs(hexapod.Inverse(home), { 0.980189f, 0.980189f, 0.980189f, 0.980189f, 0.980189f, 0.980189f }); +} + +TEST_F(TestStewartGoughKinematics, raising_the_platform_lengthens_every_leg) +{ + ExpectLegs(hexapod.Inverse(Transform{ Matrix3::Identity(), Vector3{ 0.0f, 0.0f, 0.9f } }), { 1.063376f, 1.063376f, 1.063376f, 1.063376f, 1.063376f, 1.063376f }); +} + +TEST_F(TestStewartGoughKinematics, inverse_of_a_general_pose) +{ + ExpectLegs(hexapod.Inverse(tilted), { 0.955703f, 1.037816f, 1.049122f, 1.083232f, 0.981789f, 1.042429f }); +} + +TEST_F(TestStewartGoughKinematics, leg_jacobian_matches_finite_difference) +{ + math::Vector6 twist{ 0.1f, -0.2f, 0.05f, 0.3f, -0.1f, 0.2f }; + math::Vector6 rotation{}; + constexpr float epsilon{ 1e-3f }; + for (std::size_t i = 3; i < 6; ++i) + rotation[i] = twist[i] * epsilon; + const Transform perturbed{ Matrix3{ Transform::Exp(rotation, 1.0f).rotation * tilted.rotation }, + Vector3{ tilted.translation + Vector3{ twist[0], twist[1], twist[2] } * epsilon } }; + + const math::Vector6 predicted{ hexapod.LegJacobian(tilted) * twist }; + + const auto ahead{ hexapod.Inverse(perturbed) }; + const auto here{ hexapod.Inverse(tilted) }; + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(predicted[i], (ahead[i] - here[i]) / epsilon, 1e-3f); +} + +TEST_F(TestStewartGoughKinematics, forward_recovers_the_pose) +{ + const auto result{ hexapod.Forward(hexapod.Inverse(tilted), Transform{ RotationZ(0.15f), Vector3{ 0.0f, 0.0f, 0.8f } }) }; + + EXPECT_TRUE(result.converged); + EXPECT_LE(result.iterations, std::size_t{ 6 }); + ExpectPose(result.pose, tilted, 1e-4f); +} + +TEST_F(TestStewartGoughKinematics, assembly_mode_is_selected_by_the_guess) +{ + const auto lengths{ hexapod.Inverse(home) }; + + const auto upper{ hexapod.Forward(lengths, Transform{ Matrix3::Identity(), Vector3{ 0.0f, 0.0f, 0.7f } }) }; + const auto lower{ hexapod.Forward(lengths, Transform{ Matrix3::Identity(), Vector3{ 0.0f, 0.0f, -0.7f } }) }; + + ExpectPose(upper.pose, home, 1e-4f); + ExpectPose(lower.pose, Transform{ Matrix3::Identity(), Vector3{ 0.0f, 0.0f, -0.8f } }, 1e-4f); +} + +TEST_F(TestStewartGoughKinematics, singular_pose_step_stays_finite) +{ + const Hexapod damped{ geometry, { 1e-5f, 1e-3f, 50 } }; + const auto lengths{ damped.Inverse(Transform{ RotationZ(pi / 2.0f), Vector3{ 0.0f, 0.0f, 0.8f } }) }; + + const auto result{ damped.Forward(lengths, Transform{ RotationZ(80.0f * pi / 180.0f), Vector3{ 0.0f, 0.0f, 0.8f } }) }; + + for (std::size_t r = 0; r < 3; ++r) + { + EXPECT_TRUE(std::isfinite(result.pose.translation[r])); + for (std::size_t c = 0; c < 3; ++c) + EXPECT_TRUE(std::isfinite(result.pose.rotation.at(r, c))); + } + const auto reached{ damped.Inverse(result.pose) }; + float residual{ 0.0f }; + for (std::size_t i = 0; i < 6; ++i) + residual += (reached[i] - lengths[i]) * (reached[i] - lengths[i]); + EXPECT_LT(std::sqrt(residual), 1e-4f); +} diff --git a/robotics/planning/CMakeLists.txt b/robotics/planning/CMakeLists.txt index ceedbe8..d2379eb 100644 --- a/robotics/planning/CMakeLists.txt +++ b/robotics/planning/CMakeLists.txt @@ -12,16 +12,20 @@ target_link_libraries(robotics.planning ${ROBOTICS_VISIBILITY} ) target_sources(robotics.planning PRIVATE + ConvexCollision.hpp GridSearch.hpp PotentialField.hpp PrimitiveDistance.hpp + RrtConnect.hpp SelfCollision.hpp ) robotics_add_coverage_sources(robotics.planning + ConvexCollision.cpp GridSearch.cpp PotentialField.cpp PrimitiveDistance.cpp + RrtConnect.cpp SelfCollision.cpp ) diff --git a/robotics/planning/ConvexCollision.cpp b/robotics/planning/ConvexCollision.cpp new file mode 100644 index 0000000..e31aa56 --- /dev/null +++ b/robotics/planning/ConvexCollision.cpp @@ -0,0 +1,8 @@ +#include "robotics/planning/ConvexCollision.hpp" + +namespace planning +{ + template struct ConvexShape; + template class ConvexCollision; + template class ConvexCollision; +} diff --git a/robotics/planning/ConvexCollision.hpp b/robotics/planning/ConvexCollision.hpp new file mode 100644 index 0000000..639a794 --- /dev/null +++ b/robotics/planning/ConvexCollision.hpp @@ -0,0 +1,582 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CholeskyDecomposition.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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning +{ + enum class ShapeKind : std::uint8_t + { + Sphere, + Capsule, + Box, + Hull + }; + + template + struct ConvexShape + { + static_assert(std::is_floating_point_v, "ConvexShape supports floating-point types"); + static_assert(MaxHullVertices > 0, "Hulls need at least one vertex slot"); + + using Vector3 = math::Vector3; + + ShapeKind kind{ ShapeKind::Sphere }; + math::SE3Transform pose{}; + T radius{}; + Vector3 halfExtents{}; + T halfLength{}; + std::array vertices{}; + std::size_t vertexCount{ 0 }; + + static ConvexShape Sphere(const Vector3& centre, T radius); + static ConvexShape Capsule(const Vector3& start, const Vector3& end, T radius); + static ConvexShape Box(const math::SE3Transform& pose, const Vector3& halfExtents); + static ConvexShape Hull(const math::SE3Transform& pose, std::span points); + + Vector3 CoreSupport(const Vector3& direction) const; + Vector3 Support(const Vector3& direction) const; + }; + + template + struct ConvexProximity + { + static_assert(std::is_floating_point_v, "ConvexProximity supports floating-point types"); + + T distance{}; + math::Vector3 onFirst{}; + math::Vector3 onSecond{}; + math::Vector3 normal{}; + bool converged{ false }; + std::size_t iterations{ 0 }; + }; + + template + class ConvexCollision + { + static_assert(std::is_floating_point_v, "ConvexCollision supports floating-point types"); + static_assert(MaxFaces >= 4 && MaxFaces / 2 + 4 <= 255, "Face budget must fit 8-bit vertex indices"); + + public: + using Shape = ConvexShape; + + OPTIMIZE_FOR_SPEED static ConvexProximity Query(const Shape& a, const Shape& b); + + private: + using Vector3 = math::Vector3; + + static constexpr std::size_t maxGjkIterations{ 32 }; + static constexpr std::size_t maxVertices{ MaxFaces / 2 + 4 }; + static constexpr std::size_t maxEdges{ 3 * MaxFaces }; + static constexpr T relativeTolerance{ T{ 1e-6f } }; + + enum class SupportKind : std::uint8_t + { + Core, + Full + }; + + struct Vertex + { + Vector3 w{}; + Vector3 onA{}; + Vector3 onB{}; + }; + + struct Simplex + { + std::array vertices{}; + std::array weights{}; + std::size_t size{ 0 }; + }; + + struct GjkResult + { + Simplex simplex{}; + Vector3 closest{}; + bool intersecting{ false }; + std::size_t iterations{ 0 }; + }; + + struct Face + { + std::array index{}; + Vector3 normal{}; + T distance{}; + }; + + struct Edge + { + std::uint8_t from{}; + std::uint8_t to{}; + }; + + struct Polytope + { + std::array vertices{}; + std::size_t vertexCount{ 0 }; + std::array faces{}; + std::size_t faceCount{ 0 }; + Vector3 centre{}; + }; + + static T Dot(const Vector3& lhs, const Vector3& rhs); + static Vertex MinkowskiSupport(const Shape& a, const Shape& b, const Vector3& direction, SupportKind kind); + static GjkResult Gjk(const Shape& a, const Shape& b, SupportKind kind); + static bool ClosestOnSimplex(Simplex& simplex, Vector3& closest); + static std::optional> AffineWeights(const std::array& points, std::size_t count); + static ConvexProximity Separated(const GjkResult& core, const Shape& a, const Shape& b); + static ConvexProximity Epa(const Shape& a, const Shape& b, const Simplex& seed); + static bool BuildTetrahedron(const Shape& a, const Shape& b, const Simplex& seed, Polytope& polytope); + static bool RaisesRank(const Polytope& polytope, const Vector3& point); + static void AppendIfIndependent(Polytope& polytope, const Vertex& vertex); + static void ToggleHorizonEdge(std::array& horizon, std::size_t& edgeCount, const Edge& edge); + static bool AddFace(Polytope& polytope, std::uint8_t i, std::uint8_t j, std::uint8_t k); + static bool Expand(Polytope& polytope, const Vertex& support, T scale); + static std::size_t ClosestFace(const Polytope& polytope); + static ConvexProximity Contact(const Polytope& polytope, std::size_t face, bool converged, std::size_t iterations); + }; + + template + auto ConvexShape::Sphere(const Vector3& centre, T radius) -> ConvexShape + { + ConvexShape shape{}; + shape.kind = ShapeKind::Sphere; + shape.pose.translation = centre; + shape.radius = radius; + return shape; + } + + template + auto ConvexShape::Capsule(const Vector3& start, const Vector3& end, T radius) -> ConvexShape + { + ConvexShape shape{}; + shape.kind = ShapeKind::Capsule; + shape.radius = radius; + shape.pose.translation = Vector3{ (start + end) * T{ 0.5f } }; + + const Vector3 axis{ end - start }; + const T length{ math::VectorNorm(axis) }; + if (length <= std::numeric_limits::epsilon()) + return shape; + + shape.halfLength = length / T{ 2 }; + const Vector3 z{ axis * (T{ 1 } / length) }; + const Vector3 helper{ math::Abs(z[0]) < T{ 0.9f } ? Vector3{ T{ 1 }, T{}, T{} } : Vector3{ T{}, T{ 1 }, T{} } }; + const Vector3 projected{ helper - z * (helper[0] * z[0] + helper[1] * z[1] + helper[2] * z[2]) }; + const Vector3 x{ projected * (T{ 1 } / math::VectorNorm(projected)) }; + const Vector3 y{ math::CrossProduct(z, x) }; + shape.pose.rotation = math::Matrix3{ { x[0], y[0], z[0] }, { x[1], y[1], z[1] }, { x[2], y[2], z[2] } }; + return shape; + } + + template + auto ConvexShape::Box(const math::SE3Transform& pose, const Vector3& halfExtents) -> ConvexShape + { + ConvexShape shape{}; + shape.kind = ShapeKind::Box; + shape.pose = pose; + shape.halfExtents = halfExtents; + return shape; + } + + template + auto ConvexShape::Hull(const math::SE3Transform& pose, std::span points) -> ConvexShape + { + really_assert(!points.empty() && points.size() <= MaxHullVertices); + + ConvexShape shape{}; + shape.kind = ShapeKind::Hull; + shape.pose = pose; + shape.vertexCount = points.size(); + for (std::size_t i = 0; i < points.size(); ++i) + shape.vertices[i] = points[i]; + return shape; + } + + template + auto ConvexShape::CoreSupport(const Vector3& direction) const -> Vector3 + { + const Vector3 local{ pose.rotation.Transpose() * direction }; + Vector3 offset{}; + if (kind == ShapeKind::Capsule) + offset = Vector3{ T{}, T{}, local[2] >= T{} ? halfLength : -halfLength }; + else if (kind == ShapeKind::Box) + for (std::size_t i = 0; i < 3; ++i) + offset[i] = local[i] >= T{} ? halfExtents[i] : -halfExtents[i]; + else if (kind == ShapeKind::Hull) + { + std::size_t best{ 0 }; + T bestProjection{ -std::numeric_limits::infinity() }; + for (std::size_t v = 0; v < vertexCount; ++v) + { + const T projection{ vertices[v][0] * local[0] + vertices[v][1] * local[1] + vertices[v][2] * local[2] }; + if (projection > bestProjection) + { + best = v; + bestProjection = projection; + } + } + offset = vertices[best]; + } + + return Vector3{ pose.translation + pose.rotation * offset }; + } + + template + auto ConvexShape::Support(const Vector3& direction) const -> Vector3 + { + const T norm{ math::VectorNorm(direction) }; + if (norm <= T{} || radius == T{}) + return CoreSupport(direction); + + return Vector3{ CoreSupport(direction) + direction * (radius / norm) }; + } + + template + OPTIMIZE_FOR_SPEED ConvexProximity ConvexCollision::Query(const Shape& a, const Shape& b) + { + const auto core{ Gjk(a, b, SupportKind::Core) }; + if (!core.intersecting) + return Separated(core, a, b); + + return Epa(a, b, Gjk(a, b, SupportKind::Full).simplex); + } + + template + T ConvexCollision::Dot(const Vector3& lhs, const Vector3& rhs) + { + return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2]; + } + + template + auto ConvexCollision::MinkowskiSupport(const Shape& a, const Shape& b, const Vector3& direction, SupportKind kind) -> Vertex + { + const Vector3 opposite{ direction * T{ -1 } }; + const Vector3 onA{ kind == SupportKind::Core ? a.CoreSupport(direction) : a.Support(direction) }; + const Vector3 onB{ kind == SupportKind::Core ? b.CoreSupport(opposite) : b.Support(opposite) }; + return Vertex{ Vector3{ onA - onB }, onA, onB }; + } + + template + auto ConvexCollision::Gjk(const Shape& a, const Shape& b, SupportKind kind) -> GjkResult + { + GjkResult result{}; + result.simplex.vertices[0] = MinkowskiSupport(a, b, Vector3{ T{ 1 }, T{}, T{} }, kind); + result.simplex.weights[0] = T{ 1 }; + result.simplex.size = 1; + result.closest = result.simplex.vertices[0].w; + result.intersecting = Dot(result.closest, result.closest) <= T{ 1e-12f }; + + while (!result.intersecting && result.iterations < maxGjkIterations) + { + const T squared{ Dot(result.closest, result.closest) }; + ++result.iterations; + const auto support{ MinkowskiSupport(a, b, Vector3{ result.closest * T{ -1 } }, kind) }; + bool duplicate{ false }; + for (std::size_t i = 0; i < result.simplex.size; ++i) + duplicate = duplicate || (support.w[0] == result.simplex.vertices[i].w[0] && support.w[1] == result.simplex.vertices[i].w[1] && support.w[2] == result.simplex.vertices[i].w[2]); + if (duplicate || squared - Dot(result.closest, support.w) <= relativeTolerance * squared) + return result; + + result.simplex.vertices[result.simplex.size++] = support; + result.intersecting = !ClosestOnSimplex(result.simplex, result.closest) || result.simplex.size == 4; + } + + return result; + } + + template + bool ConvexCollision::ClosestOnSimplex(Simplex& simplex, Vector3& closest) + { + T bestSquared{ std::numeric_limits::infinity() }; + Simplex best{}; + + for (std::size_t mask = 1; mask < (std::size_t{ 1 } << simplex.size); ++mask) + { + std::array subset{}; + std::size_t count{ 0 }; + for (std::size_t i = 0; i < simplex.size; ++i) + if ((mask >> i) & 1u) + subset[count++] = simplex.vertices[i]; + + const auto weights{ AffineWeights(subset, count) }; + if (!weights) + continue; + + Vector3 point{}; + for (std::size_t i = 0; i < count; ++i) + point = point + subset[i].w * (*weights)[i]; + + const T squared{ Dot(point, point) }; + if (squared < bestSquared) + { + bestSquared = squared; + best = Simplex{ subset, *weights, count }; + closest = point; + } + } + + simplex = best; + return bestSquared > T{ 1e-12f }; + } + + template + auto ConvexCollision::AffineWeights(const std::array& points, std::size_t count) -> std::optional> + { + std::array weights{ T{ 1 } }; + if (count == 1) + return weights; + + math::SquareMatrix gram{}; + math::Vector rhs{}; + for (std::size_t r = 1; r < count; ++r) + { + const Vector3 dr{ points[r].w - points[0].w }; + rhs[r - 1] = -Dot(dr, points[0].w); + for (std::size_t c = 1; c < count; ++c) + gram.at(r - 1, c - 1) = Dot(dr, Vector3{ points[c].w - points[0].w }); + } + for (std::size_t d = count - 1; d < 3; ++d) + gram.at(d, d) = T{ 1 }; + + const auto mu{ math::CholeskyDecomposition::Solve(gram, rhs).value_or(math::Vector{ T{ -1 }, T{ -1 }, T{ -1 } }) }; + + T sum{}; + for (std::size_t i = 1; i < count; ++i) + { + weights[i] = mu[i - 1]; + sum += weights[i]; + } + weights[0] = T{ 1 } - sum; + + for (std::size_t i = 0; i < count; ++i) + if (weights[i] < T{}) + return std::nullopt; + return weights; + } + + template + ConvexProximity ConvexCollision::Separated(const GjkResult& core, const Shape& a, const Shape& b) + { + Vector3 onA{}; + Vector3 onB{}; + for (std::size_t i = 0; i < core.simplex.size; ++i) + { + onA = onA + core.simplex.vertices[i].onA * core.simplex.weights[i]; + onB = onB + core.simplex.vertices[i].onB * core.simplex.weights[i]; + } + + const Vector3 gap{ onB - onA }; + const T coreDistance{ math::VectorNorm(gap) }; + const Vector3 normal{ gap * (T{ 1 } / coreDistance) }; + return ConvexProximity{ coreDistance - a.radius - b.radius, Vector3{ onA + normal * a.radius }, Vector3{ onB - normal * b.radius }, normal, true, core.iterations }; + } + + template + ConvexProximity ConvexCollision::Epa(const Shape& a, const Shape& b, const Simplex& seed) + { + Polytope polytope{}; + if (!BuildTetrahedron(a, b, seed, polytope)) + return ConvexProximity{}; + + T scale{ T{ 1 } }; + for (std::size_t i = 0; i < polytope.vertexCount; ++i) + scale = std::max(scale, math::VectorNorm(polytope.vertices[i].w)); + + std::size_t iteration{ 0 }; + bool converged{ false }; + bool expanded{ true }; + while (!converged && expanded && iteration < MaxFaces) + { + ++iteration; + const auto& closest{ polytope.faces[ClosestFace(polytope)] }; + const auto support{ MinkowskiSupport(a, b, closest.normal, SupportKind::Full) }; + converged = Dot(support.w, closest.normal) - closest.distance < relativeTolerance * scale; + expanded = converged || Expand(polytope, support, scale); + } + + return Contact(polytope, ClosestFace(polytope), converged, iteration); + } + + template + bool ConvexCollision::BuildTetrahedron(const Shape& a, const Shape& b, const Simplex& seed, Polytope& polytope) + { + for (std::size_t i = 0; i < seed.size; ++i) + AppendIfIndependent(polytope, seed.vertices[i]); + + constexpr T diagonal{ T{ 1 } / std::numbers::sqrt3_v }; + const std::array directions{ Vector3{ T{ 1 }, T{}, T{} }, Vector3{ T{ -1 }, T{}, T{} }, Vector3{ T{}, T{ 1 }, T{} }, Vector3{ T{}, T{ -1 }, T{} }, + Vector3{ T{}, T{}, T{ 1 } }, Vector3{ T{}, T{}, T{ -1 } }, Vector3{ diagonal, diagonal, diagonal }, Vector3{ diagonal, diagonal, -diagonal }, Vector3{ diagonal, -diagonal, diagonal }, + Vector3{ diagonal, -diagonal, -diagonal }, Vector3{ -diagonal, diagonal, diagonal }, Vector3{ -diagonal, diagonal, -diagonal }, Vector3{ -diagonal, -diagonal, diagonal }, + Vector3{ -diagonal, -diagonal, -diagonal } }; + for (const auto& direction : directions) + { + if (polytope.vertexCount == 4) + break; + AppendIfIndependent(polytope, MinkowskiSupport(a, b, direction, SupportKind::Full)); + } + + if (polytope.vertexCount < 4) + return false; + + polytope.centre = Vector3{ (polytope.vertices[0].w + polytope.vertices[1].w + polytope.vertices[2].w + polytope.vertices[3].w) * T{ 0.25f } }; + return AddFace(polytope, 0, 1, 2) && AddFace(polytope, 0, 1, 3) && AddFace(polytope, 0, 2, 3) && AddFace(polytope, 1, 2, 3); + } + + template + bool ConvexCollision::RaisesRank(const Polytope& polytope, const Vector3& point) + { + constexpr T tolerance{ T{ 1e-6f } }; + const auto& p{ polytope.vertices }; + if (polytope.vertexCount == 0) + return true; + + const Vector3 d1{ point - p[0].w }; + if (polytope.vertexCount == 1) + return math::VectorNorm(d1) > tolerance; + + const Vector3 e1{ p[1].w - p[0].w }; + const Vector3 normal{ math::CrossProduct(e1, d1) }; + if (polytope.vertexCount == 2) + return math::VectorNorm(normal) > tolerance * math::VectorNorm(e1); + + const Vector3 plane{ math::CrossProduct(e1, Vector3{ p[2].w - p[0].w }) }; + return math::Abs(Dot(plane, d1)) > tolerance * math::VectorNorm(plane); + } + + template + bool ConvexCollision::AddFace(Polytope& polytope, std::uint8_t i, std::uint8_t j, std::uint8_t k) + { + const Vector3& pi{ polytope.vertices[i].w }; + Vector3 normal{ math::CrossProduct(Vector3{ polytope.vertices[j].w - pi }, Vector3{ polytope.vertices[k].w - pi }) }; + const T norm{ math::VectorNorm(normal) }; + if (norm <= std::numeric_limits::min()) + return false; + + normal = normal * (T{ 1 } / norm); + if (Dot(normal, Vector3{ pi - polytope.centre }) < T{}) + { + normal = normal * T{ -1 }; + std::swap(j, k); + } + + polytope.faces[polytope.faceCount++] = Face{ { i, j, k }, normal, Dot(normal, pi) }; + return true; + } + + template + bool ConvexCollision::Expand(Polytope& polytope, const Vertex& support, T scale) + { + std::array visible{}; + std::array horizon{}; + std::size_t edgeCount{ 0 }; + std::size_t kept{ 0 }; + const std::size_t faceCount{ polytope.faceCount }; + for (std::size_t f = 0; f < faceCount; ++f) + { + const auto& face{ polytope.faces[f] }; + visible[f] = Dot(face.normal, Vector3{ support.w - polytope.vertices[face.index[0]].w }) > -relativeTolerance * scale; + if (!visible[f]) + { + ++kept; + continue; + } + + for (std::size_t e = 0; e < 3; ++e) + ToggleHorizonEdge(horizon, edgeCount, Edge{ face.index[e], face.index[(e + 1) % 3] }); + } + + if (polytope.vertexCount == maxVertices || kept + edgeCount > MaxFaces) + return false; + + std::size_t slot{ 0 }; + for (std::size_t f = 0; f < faceCount; ++f) + if (!visible[f]) + polytope.faces[slot++] = polytope.faces[f]; + polytope.faceCount = slot; + + const auto added{ static_cast(polytope.vertexCount) }; + polytope.vertices[polytope.vertexCount++] = support; + for (std::size_t h = 0; h < edgeCount; ++h) + if (!AddFace(polytope, horizon[h].from, horizon[h].to, added)) + return false; + + return true; + } + + template + void ConvexCollision::AppendIfIndependent(Polytope& polytope, const Vertex& vertex) + { + if (polytope.vertexCount < 4 && RaisesRank(polytope, vertex.w)) + polytope.vertices[polytope.vertexCount++] = vertex; + } + + template + void ConvexCollision::ToggleHorizonEdge(std::array& horizon, std::size_t& edgeCount, const Edge& edge) + { + std::size_t match{ edgeCount }; + for (std::size_t h = 0; h < edgeCount; ++h) + if (horizon[h].from == edge.to && horizon[h].to == edge.from) + match = h; + + if (match < edgeCount) + horizon[match] = horizon[--edgeCount]; + else + horizon[edgeCount++] = edge; + } + + template + std::size_t ConvexCollision::ClosestFace(const Polytope& polytope) + { + std::size_t closest{ 0 }; + for (std::size_t f = 1; f < polytope.faceCount; ++f) + if (polytope.faces[f].distance < polytope.faces[closest].distance) + closest = f; + return closest; + } + + template + ConvexProximity ConvexCollision::Contact(const Polytope& polytope, std::size_t face, bool converged, std::size_t iterations) + { + const auto& closest{ polytope.faces[face] }; + const auto& v0{ polytope.vertices[closest.index[0]] }; + const auto& v1{ polytope.vertices[closest.index[1]] }; + const auto& v2{ polytope.vertices[closest.index[2]] }; + + const Vector3 point{ closest.normal * closest.distance }; + const Vector3 e1{ v1.w - v0.w }; + const Vector3 e2{ v2.w - v0.w }; + const Vector3 rel{ point - v0.w }; + const T d11{ Dot(e1, e1) }; + const T d12{ Dot(e1, e2) }; + const T d22{ Dot(e2, e2) }; + const T determinant{ d11 * d22 - d12 * d12 }; + const T u{ (d22 * Dot(rel, e1) - d12 * Dot(rel, e2)) / determinant }; + const T v{ (d11 * Dot(rel, e2) - d12 * Dot(rel, e1)) / determinant }; + const T w{ T{ 1 } - u - v }; + + return ConvexProximity{ -closest.distance, Vector3{ v0.onA * w + v1.onA * u + v2.onA * v }, Vector3{ v0.onB * w + v1.onB * u + v2.onB * v }, closest.normal, converged, iterations }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template struct ConvexShape; + extern template class ConvexCollision; + extern template class ConvexCollision; +#endif +} diff --git a/robotics/planning/RrtConnect.cpp b/robotics/planning/RrtConnect.cpp new file mode 100644 index 0000000..b6d3f53 --- /dev/null +++ b/robotics/planning/RrtConnect.cpp @@ -0,0 +1,12 @@ +#include "robotics/planning/RrtConnect.hpp" + +namespace planning +{ + template class RrtConnect; + template class RrtConnect; + template PlanStatus RrtConnect::Plan (&)(), bool (&)(const math::Vector&)>( + const math::Vector&, const math::Vector&, math::Vector (&)(), bool (&)(const math::Vector&)); + template PlanStatus RrtConnect::Plan (&)(), bool (&)(const math::Vector&)>( + const math::Vector&, const math::Vector&, math::Vector (&)(), bool (&)(const math::Vector&)); + template void RrtConnect::Shortcut&)>(std::size_t, float (&)(), bool (&)(const math::Vector&)); +} diff --git a/robotics/planning/RrtConnect.hpp b/robotics/planning/RrtConnect.hpp new file mode 100644 index 0000000..91d5fb1 --- /dev/null +++ b/robotics/planning/RrtConnect.hpp @@ -0,0 +1,282 @@ +#pragma once + +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning +{ + template + struct RrtConnectParameters + { + static_assert(std::is_floating_point_v, "RrtConnectParameters supports floating-point types"); + + T stepSize{}; + T collisionResolution{}; + std::size_t maxIterations{}; + }; + + enum class PlanStatus : std::uint8_t + { + Found, + StartInCollision, + GoalInCollision, + PoolExhausted, + IterationLimit + }; + + template + class RrtConnect + { + static_assert(std::is_floating_point_v, "RrtConnect supports floating-point types"); + static_assert(MaxNodes > 0 && MaxNodes < 65535, "Node indices are 16-bit"); + + public: + using JointVector = math::Vector; + + explicit RrtConnect(const RrtConnectParameters& parameters); + + template + PlanStatus Plan(const JointVector& start, const JointVector& goal, Sample&& sample, IsFree&& isFree); + + template + void Shortcut(std::size_t attempts, Uniform&& uniform, IsFree&& isFree); + + std::span Path() const; + std::size_t Iterations() const; + std::size_t NodeCount() const; + + private: + enum class Growth : std::uint8_t + { + Trapped, + Advanced, + Reached, + Full + }; + + struct Tree + { + std::array configuration{}; + std::array parent{}; + std::size_t size{ 0 }; + }; + + static constexpr std::uint16_t root{ std::numeric_limits::max() }; + + static T Distance(const JointVector& a, const JointVector& b); + std::size_t PathIndex(T fraction) const; + + template + bool EdgeFree(const JointVector& from, const JointVector& to, IsFree& isFree) const; + template + Growth Extend(Tree& tree, const JointVector& target, IsFree& isFree) const; + template + Growth Connect(Tree& tree, const JointVector& target, IsFree& isFree) const; + + static void Seed(Tree& tree, const JointVector& configuration); + void BuildPath(); + + RrtConnectParameters parameters; + std::array trees{}; + std::array path{}; + std::size_t pathLength{ 0 }; + std::size_t iterations{ 0 }; + }; + + template + RrtConnect::RrtConnect(const RrtConnectParameters& parameters) + : parameters{ parameters } + {} + + template + template + PlanStatus RrtConnect::Plan(const JointVector& start, const JointVector& goal, Sample&& sample, IsFree&& isFree) + { + pathLength = 0; + iterations = 0; + Seed(trees[0], start); + Seed(trees[1], goal); + + if (!isFree(start)) + return PlanStatus::StartInCollision; + if (!isFree(goal)) + return PlanStatus::GoalInCollision; + + if (EdgeFree(start, goal, isFree)) + { + path[0] = start; + path[1] = goal; + pathLength = 2; + return PlanStatus::Found; + } + + std::size_t grow{ 0 }; + while (iterations < parameters.maxIterations) + { + ++iterations; + Tree& active{ trees[grow] }; + Tree& other{ trees[1 - grow] }; + + const auto extended{ Extend(active, sample(), isFree) }; + const auto growth{ (extended == Growth::Advanced || extended == Growth::Reached) ? Connect(other, active.configuration[active.size - 1], isFree) : extended }; + if (growth == Growth::Full) + return PlanStatus::PoolExhausted; + if (growth == Growth::Reached) + { + BuildPath(); + return PlanStatus::Found; + } + + grow = 1 - grow; + } + + return PlanStatus::IterationLimit; + } + + template + template + void RrtConnect::Shortcut(std::size_t attempts, Uniform&& uniform, IsFree&& isFree) + { + for (std::size_t attempt = 0; attempt < attempts && pathLength > 2; ++attempt) + { + auto first{ PathIndex(uniform()) }; + auto last{ PathIndex(uniform()) }; + if (last < first) + std::swap(first, last); + if (last - first < 2 || !EdgeFree(path[first], path[last], isFree)) + continue; + + std::copy(path.begin() + static_cast(last), path.begin() + static_cast(pathLength), path.begin() + static_cast(first + 1)); + pathLength -= last - first - 1; + } + } + + template + auto RrtConnect::Path() const -> std::span + { + return std::span{ path.data(), pathLength }; + } + + template + std::size_t RrtConnect::Iterations() const + { + return iterations; + } + + template + std::size_t RrtConnect::NodeCount() const + { + return trees[0].size + trees[1].size; + } + + template + std::size_t RrtConnect::PathIndex(T fraction) const + { + return std::min(static_cast(fraction * static_cast(pathLength)), pathLength - 1); + } + + template + T RrtConnect::Distance(const JointVector& a, const JointVector& b) + { + T sum{}; + for (std::size_t i = 0; i < Dof; ++i) + sum += (b[i] - a[i]) * (b[i] - a[i]); + return math::Sqrt(sum); + } + + template + template + bool RrtConnect::EdgeFree(const JointVector& from, const JointVector& to, IsFree& isFree) const + { + const auto checks{ static_cast(std::ceil(Distance(from, to) / parameters.collisionResolution)) }; + for (std::size_t k = 1; k <= checks; ++k) + if (!isFree(JointVector{ from + JointVector{ to - from } * (static_cast(k) / static_cast(checks)) })) + return false; + return true; + } + + template + template + auto RrtConnect::Extend(Tree& tree, const JointVector& target, IsFree& isFree) const -> Growth + { + std::size_t nearest{ 0 }; + T nearestDistance{ Distance(tree.configuration[0], target) }; + for (std::size_t i = 1; i < tree.size; ++i) + { + const T distance{ Distance(tree.configuration[i], target) }; + if (distance < nearestDistance) + { + nearest = i; + nearestDistance = distance; + } + } + + const bool reaches{ nearestDistance <= parameters.stepSize }; + const JointVector candidate{ reaches ? target : JointVector{ tree.configuration[nearest] + JointVector{ target - tree.configuration[nearest] } * (parameters.stepSize / nearestDistance) } }; + if (!EdgeFree(tree.configuration[nearest], candidate, isFree)) + return Growth::Trapped; + if (tree.size == MaxNodes) + return Growth::Full; + + tree.configuration[tree.size] = candidate; + tree.parent[tree.size] = static_cast(nearest); + ++tree.size; + return reaches ? Growth::Reached : Growth::Advanced; + } + + template + template + auto RrtConnect::Connect(Tree& tree, const JointVector& target, IsFree& isFree) const -> Growth + { + auto growth{ Extend(tree, target, isFree) }; + while (growth == Growth::Advanced) + growth = Extend(tree, target, isFree); + return growth; + } + + template + void RrtConnect::Seed(Tree& tree, const JointVector& configuration) + { + tree.configuration[0] = configuration; + tree.parent[0] = root; + tree.size = 1; + } + + template + void RrtConnect::BuildPath() + { + const auto& fromStart{ trees[0] }; + const auto& fromGoal{ trees[1] }; + + std::size_t depth{ 0 }; + for (auto node{ static_cast(fromStart.size - 1) }; node != root; node = fromStart.parent[node]) + ++depth; + + std::size_t slot{ depth }; + for (auto node{ static_cast(fromStart.size - 1) }; node != root; node = fromStart.parent[node]) + path[--slot] = fromStart.configuration[node]; + + pathLength = depth; + for (auto node{ fromGoal.parent[fromGoal.size - 1] }; node != root; node = fromGoal.parent[node]) + path[pathLength++] = fromGoal.configuration[node]; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class RrtConnect; + extern template class RrtConnect; + extern template PlanStatus RrtConnect::Plan (&)(), bool (&)(const math::Vector&)>( + const math::Vector&, const math::Vector&, math::Vector (&)(), bool (&)(const math::Vector&)); + extern template PlanStatus RrtConnect::Plan (&)(), bool (&)(const math::Vector&)>( + const math::Vector&, const math::Vector&, math::Vector (&)(), bool (&)(const math::Vector&)); + extern template void RrtConnect::Shortcut&)>(std::size_t, float (&)(), bool (&)(const math::Vector&)); +#endif +} diff --git a/robotics/planning/test/CMakeLists.txt b/robotics/planning/test/CMakeLists.txt index 42bb705..447a63c 100644 --- a/robotics/planning/test/CMakeLists.txt +++ b/robotics/planning/test/CMakeLists.txt @@ -8,8 +8,10 @@ target_link_libraries(robotics.planning_test PUBLIC ) target_sources(robotics.planning_test PRIVATE + TestConvexCollision.cpp TestGridSearch.cpp TestPotentialField.cpp TestPrimitiveDistance.cpp + TestRrtConnect.cpp TestSelfCollision.cpp ) diff --git a/robotics/planning/test/TestConvexCollision.cpp b/robotics/planning/test/TestConvexCollision.cpp new file mode 100644 index 0000000..d89bc53 --- /dev/null +++ b/robotics/planning/test/TestConvexCollision.cpp @@ -0,0 +1,263 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/planning/ConvexCollision.hpp" +#include "robotics/planning/PrimitiveDistance.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Transform = math::SE3Transform; + using Collision = planning::ConvexCollision; + using Shape = planning::ConvexShape; + using Distance = planning::PrimitiveDistance; + + constexpr float pi{ std::numbers::pi_v }; + + Matrix3 Rotation(const Vector3& axis, float angle) + { + const float norm{ std::sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]) }; + return math::RotationAboutAxis(Vector3{ axis * (1.0f / norm) }, angle); + } + + float Dot(const Vector3& a, const Vector3& b) + { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + } + + Vector3 Column(const Matrix3& m, std::size_t c) + { + return Vector3{ m.at(0, c), m.at(1, c), m.at(2, c) }; + } + + std::array Corners(const Shape& box) + { + std::array corners{}; + for (std::size_t i = 0; i < 8; ++i) + { + const Vector3 local{ (i & 1u) != 0u ? box.halfExtents[0] : -box.halfExtents[0], (i & 2u) != 0u ? box.halfExtents[1] : -box.halfExtents[1], + (i & 4u) != 0u ? box.halfExtents[2] : -box.halfExtents[2] }; + corners[i] = Vector3{ box.pose.translation + box.pose.rotation * local }; + } + return corners; + } + + float ExactBoxBox(const Shape& a, const Shape& b) + { + const planning::Box boxA{ a.pose, a.halfExtents }; + const planning::Box boxB{ b.pose, b.halfExtents }; + const auto cornersA{ Corners(a) }; + const auto cornersB{ Corners(b) }; + float best{ std::numeric_limits::infinity() }; + for (std::size_t i = 0; i < 8; ++i) + { + best = std::min(best, Distance::PointBox(cornersA[i], boxB).distance); + best = std::min(best, Distance::PointBox(cornersB[i], boxA).distance); + } + + constexpr std::array, 12> edges{ { { 0, 1 }, { 2, 3 }, { 4, 5 }, { 6, 7 }, { 0, 2 }, { 1, 3 }, { 4, 6 }, { 5, 7 }, { 0, 4 }, { 1, 5 }, { 2, 6 }, { 3, 7 } } }; + for (const auto& first : edges) + for (const auto& second : edges) + best = std::min(best, Distance::SegmentSegment(planning::Segment{ cornersA[first[0]], cornersA[first[1]] }, planning::Segment{ cornersB[second[0]], cornersB[second[1]] }).distance); + return best; + } + + float SatDepth(const Shape& a, const Shape& b) + { + std::array axes{}; + std::size_t count{ 0 }; + for (std::size_t i = 0; i < 3; ++i) + { + axes[count++] = Column(a.pose.rotation, i); + axes[count++] = Column(b.pose.rotation, i); + } + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + { + const Vector3 cross{ math::CrossProduct(Column(a.pose.rotation, i), Column(b.pose.rotation, j)) }; + const float norm{ std::sqrt(Dot(cross, cross)) }; + if (norm > 1e-6f) + axes[count++] = Vector3{ cross * (1.0f / norm) }; + } + + const Vector3 offset{ b.pose.translation - a.pose.translation }; + float depth{ std::numeric_limits::infinity() }; + for (std::size_t k = 0; k < count; ++k) + { + float radius{ 0.0f }; + for (std::size_t i = 0; i < 3; ++i) + radius += a.halfExtents[i] * std::abs(Dot(axes[k], Column(a.pose.rotation, i))) + b.halfExtents[i] * std::abs(Dot(axes[k], Column(b.pose.rotation, i))); + depth = std::min(depth, radius - std::abs(Dot(axes[k], offset))); + } + return depth; + } + + class TestConvexCollision + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected, float tolerance = 1e-5f) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + static bool Finite(const planning::ConvexProximity& result) + { + return std::isfinite(result.distance) && std::isfinite(result.normal[0]) && std::isfinite(result.normal[1]) && std::isfinite(result.normal[2]); + } + + float Uniform() + { + state ^= state << 13; + state ^= state >> 17; + state ^= state << 5; + return static_cast(static_cast(state) / 4294967296.0); + } + + float Between(float low, float high) + { + return low + (high - low) * Uniform(); + } + + Shape RandomBox() + { + const Vector3 axis{ Between(-1.0f, 1.0f), Between(-1.0f, 1.0f), Between(-1.0f, 1.0f) + 1e-3f }; + const Matrix3 rotation{ Rotation(axis, Between(0.0f, pi)) }; + const Vector3 centre{ Between(-0.6f, 0.6f), Between(-0.6f, 0.6f), Between(-0.6f, 0.6f) }; + return Shape::Box(Transform{ rotation, centre }, Vector3{ Between(0.2f, 1.0f), Between(0.2f, 1.0f), Between(0.2f, 1.0f) }); + } + + std::uint32_t state{ 2463534242u }; + Shape boxA{ Shape::Box(Transform{ Matrix3::Identity(), Vector3{} }, Vector3{ 0.5f, 0.4f, 0.3f }) }; + Shape boxB{ Shape::Box(Transform{ Rotation(Vector3{ 1.0f, 1.0f, 0.0f }, 0.7f), Vector3{ 1.4f, 0.6f, 0.5f } }, Vector3{ 0.3f, 0.3f, 0.2f }) }; + Shape cube{ Shape::Box(Transform{ Matrix3::Identity(), Vector3{} }, Vector3{ 1.0f, 1.0f, 1.0f }) }; + }; +} + +TEST_F(TestConvexCollision, rounded_shapes_match_the_closed_forms) +{ + const auto spheres{ Collision::Query(Shape::Sphere(Vector3{}, 1.0f), Shape::Sphere(Vector3{ 3.0f, 1.0f, 0.0f }, 0.5f)) }; + const auto capsules{ Collision::Query(Shape::Capsule(Vector3{}, Vector3{ 1.0f, 2.0f, 3.0f }, 0.1f), Shape::Capsule(Vector3{ 2.0f, 0.0f, 1.0f }, Vector3{ 0.0f, 1.0f, -1.0f }, 0.2f)) }; + const auto sphereBox{ Collision::Query(Shape::Sphere(Vector3{ 2.0f, 1.0f, 0.0f }, 0.3f), + Shape::Box(Transform{ Rotation(Vector3{ 0.0f, 0.0f, 1.0f }, pi / 6.0f), Vector3{ 1.0f, 0.0f, 0.0f } }, Vector3{ 0.5f, 0.25f, 0.1f })) }; + + EXPECT_NEAR(spheres.distance, 1.662278f, 1e-5f); + ExpectNear(spheres.onFirst, Vector3{ 0.948683f, 0.316228f, 0.0f }); + EXPECT_NEAR(capsules.distance, 0.648683f, 1e-5f); + EXPECT_NEAR(sphereBox.distance, 0.573763f, 1e-5f); + EXPECT_NEAR(Collision::Query(Shape::Capsule(Vector3{ 3.0f, 1.0f, 0.0f }, Vector3{ 3.0f, 1.0f, 0.0f }, 0.5f), Shape::Sphere(Vector3{}, 1.0f)).distance, 1.662278f, 1e-5f); +} + +TEST_F(TestConvexCollision, box_pair_distance_matches_the_exact_value) +{ + const auto result{ Collision::Query(boxA, boxB) }; + + EXPECT_NEAR(result.distance, 0.511063f, 1e-5f); + EXPECT_NEAR(result.distance, ExactBoxBox(boxA, boxB), 1e-5f); + ExpectNear(result.onFirst, Vector3{ 0.5f, 0.391106f, 0.3f }); + ExpectNear(result.onSecond, Vector3{ 1.008894f, 0.391106f, 0.347032f }); +} + +TEST_F(TestConvexCollision, capsule_box_and_hull_pairs) +{ + const std::array tetrahedron{ Vector3{}, Vector3{ 1.0f, 0.0f, 0.0f }, Vector3{ 0.0f, 1.0f, 0.0f }, Vector3{ 0.0f, 0.0f, 1.0f } }; + + const auto capsuleBox{ Collision::Query(Shape::Capsule(Vector3{ 1.0f, -0.5f, 0.8f }, Vector3{ 1.0f, 0.5f, 0.8f }, 0.1f), boxA) }; + const auto hullSphere{ Collision::Query(Shape::Hull(Transform{ Matrix3::Identity(), Vector3{ 2.0f, 0.0f, 0.0f } }, tetrahedron), Shape::Sphere(Vector3{}, 1.0f)) }; + + EXPECT_NEAR(capsuleBox.distance, 0.607107f, 1e-5f); + EXPECT_NEAR(hullSphere.distance, 1.0f, 1e-5f); +} + +TEST_F(TestConvexCollision, rounded_overlap_with_separated_cores_is_exact) +{ + const auto result{ Collision::Query(cube, Shape::Sphere(Vector3{ 1.8f, 0.0f, 0.0f }, 1.0f)) }; + + EXPECT_NEAR(result.distance, -0.2f, 1e-5f); + ExpectNear(result.normal, Vector3{ 1.0f, 0.0f, 0.0f }); + EXPECT_TRUE(result.converged); + + const auto coresIntersect{ Collision::Query(cube, Shape::Sphere(Vector3{ 0.5f, 0.0f, 0.0f }, 0.25f)) }; + EXPECT_NEAR(coresIntersect.distance, -0.75f, 1e-3f); + ExpectNear(coresIntersect.normal, Vector3{ 1.0f, 0.0f, 0.0f }, 1e-2f); +} + +TEST_F(TestConvexCollision, epa_resolves_axis_aligned_boxes) +{ + const auto result{ Collision::Query(cube, Shape::Box(Transform{ Matrix3::Identity(), Vector3{ 1.5f, 0.0f, 0.0f } }, Vector3{ 1.0f, 1.0f, 1.0f })) }; + + EXPECT_NEAR(result.distance, -0.5f, 1e-5f); + ExpectNear(result.normal, Vector3{ 1.0f, 0.0f, 0.0f }); +} + +TEST_F(TestConvexCollision, epa_matches_separating_axes_for_rotated_boxes) +{ + const auto overlapping{ Shape::Box(Transform{ Rotation(Vector3{ 1.0f, 1.0f, 0.0f }, 0.7f), Vector3{ 0.6f, 0.3f, 0.2f } }, Vector3{ 0.3f, 0.3f, 0.2f }) }; + + const auto result{ Collision::Query(boxA, overlapping) }; + + EXPECT_NEAR(result.distance, -0.291106f, 1e-4f); + EXPECT_NEAR(result.distance, -SatDepth(boxA, overlapping), 1e-4f); + ExpectNear(result.normal, Vector3{ 1.0f, 0.0f, 0.0f }, 1e-4f); +} + +TEST_F(TestConvexCollision, symmetric_and_tied_configurations_stay_finite) +{ + const auto identical{ Collision::Query(boxA, boxA) }; + + EXPECT_NEAR(identical.distance, -0.6f, 1e-5f); + EXPECT_NEAR(std::abs(identical.normal[2]), 1.0f, 1e-5f); + EXPECT_TRUE(identical.converged); + + constexpr std::array extents{ 0.25f, 0.5f, 0.75f }; + constexpr std::array offsets{ -0.25f, 0.0f, 0.25f }; + for (std::size_t sample = 0; sample < 50; ++sample) + { + const auto pick{ [this](const std::array& values) + { + return values[std::min(static_cast(Uniform() * 3.0f), 2)]; + } }; + const auto first{ Shape::Box(Transform{ Matrix3::Identity(), Vector3{ pick(offsets), pick(offsets), pick(offsets) } }, Vector3{ pick(extents), pick(extents), pick(extents) }) }; + const auto second{ Shape::Box(Transform{ Matrix3::Identity(), Vector3{ pick(offsets), pick(offsets), pick(offsets) } }, Vector3{ pick(extents), pick(extents), pick(extents) }) }; + + const auto result{ Collision::Query(first, second) }; + + ASSERT_TRUE(Finite(result)) << "sample " << sample; + EXPECT_NEAR(-result.distance, SatDepth(first, second), 1e-5f) << "sample " << sample; + } + + const std::array triangle{ Vector3{}, Vector3{ 1.0f, 0.0f, 0.0f }, Vector3{ 0.0f, 1.0f, 0.0f } }; + const auto flat{ Collision::Query(Shape::Hull(Transform::Identity(), triangle), Shape::Sphere(Vector3{ 0.2f, 0.2f, 0.0f }, 0.0f)) }; + EXPECT_TRUE(Finite(flat)); + EXPECT_FALSE(flat.converged); + + const auto budget{ planning::ConvexCollision::Query(cube, Shape::Sphere(Vector3{ 0.5f, 0.0f, 0.0f }, 0.25f)) }; + EXPECT_TRUE(Finite(budget)); + EXPECT_FALSE(budget.converged); +} + +TEST_F(TestConvexCollision, random_box_pairs_agree_with_exact_references) +{ + for (std::size_t sample = 0; sample < 300; ++sample) + { + const auto first{ RandomBox() }; + const auto second{ RandomBox() }; + const float depth{ SatDepth(first, second) }; + + const auto result{ Collision::Query(first, second) }; + + EXPECT_TRUE(result.converged) << "sample " << sample; + if (depth < 0.0f) + EXPECT_NEAR(result.distance, ExactBoxBox(first, second), 1e-5f) << "sample " << sample; + else + EXPECT_NEAR(-result.distance, depth, 1e-4f) << "sample " << sample; + } +} diff --git a/robotics/planning/test/TestRrtConnect.cpp b/robotics/planning/test/TestRrtConnect.cpp new file mode 100644 index 0000000..4c18444 --- /dev/null +++ b/robotics/planning/test/TestRrtConnect.cpp @@ -0,0 +1,150 @@ +#include "robotics/planning/RrtConnect.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + using planning::PlanStatus; + using Joints = math::Vector; + + constexpr float pi{ std::numbers::pi_v }; + constexpr float lowerBound{ 7.496376f }; + + std::uint32_t generatorState{ 1u }; + + float Uniform() + { + generatorState ^= generatorState << 13; + generatorState ^= generatorState >> 17; + generatorState ^= generatorState << 5; + return static_cast(static_cast(generatorState) / 4294967296.0); + } + + Joints Sample() + { + const float first{ -pi + 2.0f * pi * Uniform() }; + return Joints{ first, -pi + 2.0f * pi * Uniform() }; + } + + bool IsFree(const Joints& q) + { + return !(std::abs(q[0]) <= 0.3f && q[1] <= 1.0f); + } + + class TestRrtConnect + : public ::testing::Test + { + protected: + static float Length(std::span path) + { + float length{ 0.0f }; + for (std::size_t i = 1; i < path.size(); ++i) + length += std::hypot(path[i][0] - path[i - 1][0], path[i][1] - path[i - 1][1]); + return length; + } + + static void ExpectValidPath(std::span path, const Joints& start, const Joints& goal) + { + ASSERT_GE(path.size(), std::size_t{ 2 }); + EXPECT_EQ(path.front()[0], start[0]); + EXPECT_EQ(path.front()[1], start[1]); + EXPECT_EQ(path.back()[0], goal[0]); + EXPECT_EQ(path.back()[1], goal[1]); + for (std::size_t i = 1; i < path.size(); ++i) + { + const float length{ std::hypot(path[i][0] - path[i - 1][0], path[i][1] - path[i - 1][1]) }; + const auto checks{ static_cast(std::ceil(length / 0.05f)) }; + for (std::size_t k = 0; k <= checks; ++k) + { + const float fraction{ checks == 0 ? 0.0f : static_cast(k) / static_cast(checks) }; + EXPECT_TRUE(IsFree(Joints{ path[i - 1] + Joints{ path[i] - path[i - 1] } * fraction })); + } + } + } + + planning::RrtConnect planner{ { 0.3f, 0.05f, 500 } }; + Joints start{ -2.0f, -2.0f }; + Joints goal{ 2.0f, -2.0f }; + }; +} + +TEST_F(TestRrtConnect, finds_a_valid_path_around_the_wall) +{ + generatorState = 1u; + + const auto status{ planner.Plan(start, goal, Sample, IsFree) }; + + ASSERT_EQ(status, PlanStatus::Found); + ExpectValidPath(planner.Path(), start, goal); + EXPECT_GE(Length(planner.Path()), lowerBound); + EXPECT_LE(planner.Iterations(), std::size_t{ 500 }); + EXPECT_GT(planner.NodeCount(), std::size_t{ 2 }); +} + +TEST_F(TestRrtConnect, results_are_reproducible_for_a_seed) +{ + generatorState = 7u; + ASSERT_EQ(planner.Plan(start, goal, Sample, IsFree), PlanStatus::Found); + std::array first{}; + const auto firstPath{ planner.Path() }; + std::copy(firstPath.begin(), firstPath.end(), first.begin()); + + generatorState = 7u; + ASSERT_EQ(planner.Plan(start, goal, Sample, IsFree), PlanStatus::Found); + + ASSERT_EQ(planner.Path().size(), firstPath.size()); + for (std::size_t i = 0; i < firstPath.size(); ++i) + { + EXPECT_EQ(planner.Path()[i][0], first[i][0]); + EXPECT_EQ(planner.Path()[i][1], first[i][1]); + } +} + +TEST_F(TestRrtConnect, shortcut_shortens_and_stays_valid) +{ + generatorState = 1u; + ASSERT_EQ(planner.Plan(start, goal, Sample, IsFree), PlanStatus::Found); + const float raw{ Length(planner.Path()) }; + + generatorState = 99u; + planner.Shortcut(100, Uniform, IsFree); + + ExpectValidPath(planner.Path(), start, goal); + EXPECT_LT(Length(planner.Path()), raw); + EXPECT_GE(Length(planner.Path()), lowerBound); +} + +TEST_F(TestRrtConnect, trivial_case_returns_the_straight_edge) +{ + const Joints left{ -2.0f, 2.0f }; + const Joints right{ 2.0f, 2.0f }; + + ASSERT_EQ(planner.Plan(left, right, Sample, IsFree), PlanStatus::Found); + + ASSERT_EQ(planner.Path().size(), std::size_t{ 2 }); + EXPECT_EQ(planner.Path()[0][0], left[0]); + EXPECT_EQ(planner.Path()[1][0], right[0]); + EXPECT_EQ(planner.Iterations(), std::size_t{ 0 }); +} + +TEST_F(TestRrtConnect, invalid_endpoints_are_reported) +{ + EXPECT_EQ(planner.Plan(Joints{ 0.0f, 0.0f }, Joints{ 2.0f, 2.0f }, Sample, IsFree), PlanStatus::StartInCollision); + EXPECT_EQ(planner.Plan(Joints{ 2.0f, 2.0f }, Joints{ 0.0f, 0.0f }, Sample, IsFree), PlanStatus::GoalInCollision); +} + +TEST_F(TestRrtConnect, small_pool_is_exhausted) +{ + planning::RrtConnect small{ { 0.3f, 0.05f, 500 } }; + generatorState = 1u; + + EXPECT_EQ(small.Plan(start, goal, Sample, IsFree), PlanStatus::PoolExhausted); + + planning::RrtConnect noBudget{ { 0.3f, 0.05f, 0 } }; + EXPECT_EQ(noBudget.Plan(start, goal, Sample, IsFree), PlanStatus::IterationLimit); +}