Pose Transform 2D — Robotics/Planar Kinematics

Pose Transform 2D

Robotics / Planar Kinematics

The three rigid-body operations a planar pose supports, selected by Operation. A pose is (x, y, θ) as a [3,1] column: the position and heading of one frame expressed in another, with θ in radians and never wrapped.

• Compose – Tc = Ta·Tb, with Tb read in Ta's frame: xc = xa + cosθa·xb − sinθa·yb, yc = ya + sinθa·xb + cosθa·yb, θc = θa + θb.
• Apply to point – p = Ta·p_local, the same two position rows against a [2,1] point instead of a pose: x = xa + cosθa·px − sinθa·py, y = ya + sinθa·px + cosθa·py.
• Invert – Tc = Ta⁻¹, the pose that undoes Ta: xc = −(cosθa·xa + sinθa·ya), yc = sinθa·xa − cosθa·ya, θc = −θa.

The rotation is active and counter-clockwise, the same sense Rotation 2D uses, so composing Ta = (0, 0, +90°) with the pure translation Tb = (0.1, 0, 0) gives (0, +0.1, +90°).

Ports

The input port count follows Operation, so it is editable and the block checks it rather than changing it underneath you. Add or remove an input to match.

• Ta – the pose that acts, a [3,1] column (x, y, θ). Present in all three operations.
• Tb – in Compose the second pose, [3,1], read in Ta's frame; in Apply to point the point (x, y), [2,1], likewise in Ta's frame. Invert reads Ta alone: it needs only one input port, and a second one is ignored if it is left in place. Remove it for a diagram that says what it means.
• Tc – the result. [3,1] under Compose and Invert, [2,1] under Apply to point. Its size is fixed by the operation, not inherited.

Parameters

• Operation – which of the three the block computes. It selects a different computation AND a different port list, so it is not a tuning knob:
  • Compose (Tc = Ta * Tb) – two [3,1] inputs, a [3,1] output. The default.
  • Apply to point (p = Ta * p_local) – a [3,1] and a [2,1] in, a [2,1] out.
  • Invert (Tc = Ta inverse) – reads Ta only, a [3,1] output. One input port is enough; a second is ignored.
• 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. There is no tunable parameter on the generated core: Operation chooses which body is emitted, so it is a structural choice baked in at export time rather than something retuned afterwards.

The three HDL targets are simulation-only: they carry the arithmetic in real and quantize only at the port boundary. A sine and a cosine per sample have no Q16.16 form. Compose's angle row and Invert's would each be exact in fixed point on their own, and are still carried in real with the rest – a body mixing the two domains would buy no accuracy the port boundary does not take straight back.

Simulink bridge

None, measured rather than assumed: SE(2) pose algebra ships in the Robotics System Toolbox and the Navigation Toolbox, and neither is installed on this machine (robotics, nav and shared_robotics are all absent from matlabroot/toolbox). The same arithmetic is easy to rebuild there from Trigonometric Function, Product and Sum blocks – keeping the active, counter-clockwise sense this block uses, and the argument order Ta then Tb.

Notes

• Algebraic and stateless.
• θ is RAW in every operation, deliberately – including Compose, which ADDS two angles and so can leave (−π, π] in one step. Unicycle Odometry and Planar Arm Forward Kinematics both publish a raw heading and both chain into this block; wrapping here would contradict them and would hide the wrap from the diagram. Put an Angle Wrap where a bounded angle is wanted.
• Compose does not commute, and the order is the contract: Ta is the parent-frame pose and Tb is read in Ta's frame, so Tc = Ta·Tb reads left to right the way a frame chain does. Swapping the two inputs is a different answer wherever the two headings differ.
• The inverse is exact algebra, not a solve: composing a pose with its own inverse returns (0, 0, 0) to the last bit of the arithmetic.
• Bilinear, so deliberately no state space – each operation is linear in its second input for a fixed θa, but θa arrives as a signal.
• For the 3-D case use Quaternion Rotate Vector with a translation, or Quaternion To Rotation Matrix with Matrix Multiply.