Planar 2R Inverse Kinematics — Robotics/Planar Kinematics

Robotics/Planar_Kinematics/Planar_2R_Inverse_Kinematics · 2 input / 3 output port(s) at insert · exports to Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog, PLC Structured Text

Description#

The block's own DESCRIPTION_HTML, rendered verbatim — the same text the config dialog's info panel and the library navigator show. Fix a wrong sentence in the block's .cpp (R-D9), never here.

Planar 2R Inverse Kinematics

Robotics / Planar Kinematics

The closed-form inverse kinematics of a two-link planar arm: given a target point (x, y), the two joint angles that put the tool there.

c₂ = (x² + y² − (l₁² + l₂²)) / (2·l₁·l₂), clamped to [−1, +1]
q₂ = ±acos(c₂) – the sign is the elbow choice
q₁ = atan2(y, x) − atan2(±l₂·sin(q₂), l₁ + l₂·c₂)

Feed q₁ and q₂ into Planar Arm Forward Kinematics with the same link lengths and the tool returns to the target, whenever the target was reachable.

Ports

x, y – the target point, two [1,1] scalars in the diagram's length unit, measured from the base joint.
q1 – the base joint angle, [1,1] radians in (−2π, 2π): it is a difference of two atan2 results and is not wrapped.
q2 – the elbow joint angle, [1,1] radians, relative to link 1 exactly as Planar Arm Forward Kinematics reads it. In [0, π] for elbow Up and [−π, 0] for Down.
valid – 1 when the target was reachable, 0 when it was not. Gate on this: when it is 0 the angles are still finite and still the closest the arm can get, but they do not reach the target.

Parameters

Link Lengths – a [2,1] column [l₁; l₂], base link first. Both must be strictly positive: a zero-length link makes the arm's reachable set degenerate and the formula's divisor zero.
Elbow Configuration – Up or Down, the two mirror solutions that reach the same point.
- Up – q₂ ≥ 0, the elbow turns counter-clockwise from link 1 (the ledger's positive sense).
- Down – q₂ ≤ 0, the mirror image.
The two coincide exactly on the reach boundary, where q₂ is 0 or ±π.
Sampling Time (s) – zero or less inherits the solver's rate; a positive value runs the block at that period.

Code export

All ten targets: Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog and PLC Structured Text. Link lengths and the elbow sign are baked into the emitted body – both are structural rather than tunable.

l₁² + l₂² and 1/(2·l₁·l₂) are folded once at config load, so no generated core performs a division per sample.

The three HDL targets are simulation-only real arithmetic: one acos and two atan2 every sample.

PLC Structured Text has no atan2, so both are rebuilt from ATAN with the quadrant reconstruction – the same one Cartesian To Spherical uses.

Simulink bridge

None, and the reason is measured rather than assumed: Simulink's inverse kinematics ship in the Robotics System Toolbox, which is not installed on this machine. A model carrying this block exports with the block reported rather than silently dropped.

Notes

Algebraic and stateless.
The block is TOTAL – it never returns NaN. An unreachable target is clamped to the nearest reachable configuration and flagged by valid, rather than producing a NaN (six targets), a complex number (MATLAB) or an aborting assertion (VHDL). The unreachable set has two parts and they mean different things: too far (r > l₁ + l₂) and too close (r < |l₁ − l₂|, the dead hole an unequal-link arm cannot fold into).
q₁ is not wrapped, matching the family's ledger. Put an Angle Wrap after it if a wrapped angle is wanted.
Only the two-link case has this closed form. For more links the problem is redundant and has no unique answer – which is why this block is 2R and not NR, and why Planar Arm Jacobian is what a general-N solver would iterate on instead.
Nonlinear, so deliberately no state space.

Code facts#

Fact	Value
registered type	`Robotics/Planar_Kinematics/Planar_2R_Inverse_Kinematics`
family	`Robotics/Planar_Kinematics`
solver environment class	`ICoreBlock_0_Robotics_1_Planar_Kinematics_2_Planar_2R_Inverse_Kinematics`
source	`src/ICoreSDK/ICoreBlockLibrary/Blocks/Robotics/Planar_Kinematics/Planar_2R_Inverse_Kinematics/ICoreBlock_0_Robotics_1_Planar_Kinematics_2_Planar_2R_Inverse_Kinematics.cpp`
header	`src/ICoreSDK/ICoreBlockLibrary/Blocks/Robotics/Planar_Kinematics/Planar_2R_Inverse_Kinematics/ICoreBlock_0_Robotics_1_Planar_Kinematics_2_Planar_2R_Inverse_Kinematics.h`
default size on canvas	140 × 100 px
ports at insert	2 in, 3 out
code generators implemented	`Python`, `MATLAB`, `Java`, `Rust`, `C`, `C++`, `VHDL`, `Verilog`, `SystemVerilog`, `PLC Structured Text`

Ports#

#	Direction	Signal type	Description label
1	in	`ICoreDouble`	x
2	in	`ICoreDouble`	y
3	out	`ICoreDouble`	q1
4	out	`ICoreDouble`	q2
5	out	`ICoreDouble`	valid

Ports the constructor creates. A block whose port list changes with its configuration adds or removes ports at load time; the count above is the one a freshly inserted block has.

Configuration variables#

Config variable	Default	Simulink parameter
`Link Lengths`	`[0.65; 0.4]`	—
`Elbow Configuration`	`Up%~%Down~~Up`	—

Every block also carries Sampling Time (s) from ICoreBlockSolverEnvironment: zero or less inherits the solver's rate, a positive value runs the block at that period.

Simulink bridge#


support	`Support::None`
Simulink path	—
port-count rule	`PortsParam::None`
`SampleTime` parameter	yes

Caveat (shown to the user): no Simulink equivalent available: inverse kinematics ship in the Robotics System Toolbox, which is not installed. Rebuild the closed form on the Simulink side from Trigonometric Function blocks, keeping the RELATIVE elbow-angle convention this block shares with Planar Arm Forward Kinematics and clamping the arccosine argument as this block does

Catalog contract: src/ICoreSDK/ICoreCoder/ICoreCommandSystem/SimulinkBridge/ICoreSimulinkBlockCatalog.h

Description vs code#

The checker has a blind spot here — it could not resolve something (a grouped port bullet, a computed config name), which is reported and never counted as a pass. A reader has to settle it:

B0 Ports lists 4 entries for 5 ports (2 in, 3 out) — grouped, or one undocumented? a reader must say

The verdict above is tools/docs/check_block_descriptions.py (P7.1), which compares LISTS. It cannot read a sentence: "stateless" on a block with a state, an initial-value semantic the recursion does not implement, a "not synthesizable" caveat the HDL banner contradicts. That is the agent audit (P7.3) on BLOCK_DESCRIPTION_AUDIT.md, and this tool's green is not a substitute for one.

The top comment of the block's .cpp — the maths, the realization and the export strategy, addressed to whoever changes it. It must not contradict the description above (P7.5).

Planar 2R Inverse Kinematics — a target (x, y) to the two joint angles, with a valid flag r2 = x*x + y*y c2 = (r2 - lenSq) * inv2L lenSq = l1^2 + l2^2, inv2L = 1/(2*l1*l2) cc = clamp(c2, -1, +1) q2 = s * acos(cc) s = +1 elbow Up, -1 elbow Down q1 = atan2(y, x) - atan2(s*l2*sin(acos(cc)), l1 + l2*cc) valid = 1 when c2 was already inside [-1, +1], else 0

The inverse of Planar_Arm_Forward_Kinematics at N = 2, and checked against it by composition -- see the header for why that is a real test here and not two blocks agreeing.

⚠ THE EXPRESSION FORM IS THE CONTRACT, and three choices in it are arbitrary-but-fixed, so every one of the ten backends spells them identically:

cos(q2) is cc itself, never cos(acos(cc));
sin(q2) is sin(acos(cc)), never sqrt(1 - cc*cc);
the clamp compares STRICTLY (> 1, < -1), so c2 exactly at a boundary is NOT clamped

and IS valid -- which is what lets an arm with equal links reach its own origin. Each pair is equal in exact arithmetic and need not be bit-identical in floating point, so picking one and holding it is the whole point.

⚠ lenSq AND inv2L ARE FOLDED AT CONFIG LOAD, so no generated core divides per sample. Standard_Scaler's move. It does not buy synthesizable HDL here (acos and two atan2 rule that out regardless) but it costs nothing and removes a divider from ten cores.

⚠ THE GENERATORS BAKE THE LINK LENGTHS AND THE ELBOW SIGN IN, so loadBlockConfig() MUST be called from initializePortSignalSize() (§3): code export never calls it, and a generator reading an unfilled config map logs an error and stops the run.

Sample results#

t	in ICoreDouble-Out-0	in ICoreDouble-Out-0	out ICoreDouble-Out-0	out ICoreDouble-Out-1	out ICoreDouble-Out-2
0	-2	-2	-2.356	0	0
0.4	0.5	0.5	0.1928	1.73	1
0.8	-2	-2	-2.356	0	0
1.2	0.5	0.5	0.1928	1.73	1
1.6	-2	-2	-2.356	0	0
2	0.5	0.5	0.1928	1.73	1
2.4	-2	-2	-2.356	0	0
2.8	0.5	0.5	0.1928	1.73	1
3.2	-2	-2	-2.356	0	0
3.6	0.5	0.5	0.1928	1.73	1
4	-2	-2	-2.356	0	0
4.4	0.5	0.5	0.1928	1.73	1
4.8	-2	-2	-2.356	0	0
5.2	0.5	0.5	0.1928	1.73	1

Every 4th of 60 samples, from the table stimulus.

The same rig also ran:

Stimulus	What it is	Output range
`impulse`	Impulse: one sample of 1 at k = 5, 0 elsewhere (Repeating Sequence Stair)	-1.959e-16 … 0.7854
`ramp`	Ramp: slope 1 from t = 0	-1.959e-16 … 0.7854
`sine`	Sine Wave: amplitude 1, 2 rad/s, no phase, no bias	-3.019 … 0.7854
`step`	Step: 0 -> 1 at t = 1 s	-1.959e-16 … 0.7854

Plotted: table — Repeating Sequence Stair: [-2 -1 -0.5 0 0.5 1 2 3], one entry per sample

Category static · sample time 0.1 · 60 steps · commit ccf005c8 · produced by docsSample --out <folder> --steps 60 · data docs/generated/samples/Robotics__Planar_Kinematics__Planar_2R_Inverse_Kinematics.json · the SVG is generated from those numbers by tools/docs/plot_svg.py, so it is a run and not a drawing (R-D10).

Planar 2R Inverse Kinematics — Robotics/Planar Kinematics

Description#