From 24ca7db20c7dbe9bef8d0d0b07cb927478d78281 Mon Sep 17 00:00:00 2001
From: Marlin Strub <mstrub@gravisrobotics.com>
Date: Tue, 28 Oct 2025 11:30:45 +0100
Subject: [PATCH] Accumulate successive fixed transforms left-to-right.

Origin tags in URDFs specify the pose of the child frame in parent-frame
coordinates and should be accumulated in order of the kinematic chain, i.e., left-to-right.
---
 crates/optik/src/kinematics.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/crates/optik/src/kinematics.rs b/crates/optik/src/kinematics.rs
index 09e50d9..26bd179 100644
--- a/crates/optik/src/kinematics.rs
+++ b/crates/optik/src/kinematics.rs
@@ -67,14 +67,14 @@ impl KinematicChain {
                 match joint.typ {
                     JointType::Fixed => {
                         // Accumulate the transforms from successive fixed joints.
-                        (joints, joint.origin * collapsed_tfm)
+                        (joints, collapsed_tfm * joint.origin)
                     }
                     _ => {
                         // When we encounter an articulated joint, apply the
                         // accumulated fixed joint transforms (if any) and then
                         // reset the accumulation.
                         let new_joint = Joint {
-                            origin: joint.origin * collapsed_tfm,
+                            origin: collapsed_tfm * joint.origin,
                             ..joint
                         };
                         joints.push(new_joint);