Accumulate successive fixed-joint transforms left-to-right.

[marlinstrub]

Hi Kyle

Thanks for building and maintaining this library, your time and effort is sincerely appreciated.

This PR fixes a bug in the fixed-joint folding optimization where transforms were composed in the wrong order (right-to-left instead of left-to-right), causing incorrect results for robots with sequential fixed joints.

The origin tag of a URDF joint defines the pose of the child link in the coordinate frame of the parent link. To find the pose of a (great-...) grandchild, we have to accumulate the poses in the same order as the kinematic chain.

For example, this chain with two fixed joints:

BASE → J_FIXED_1[t=(1, 0, 0), R=R_y(0.5)] → LINK → J_FIXED_2[t=(2, 0, 0), R=I] → EE

The EE origin in BASE coordinates is at

EE.position = [1, 0, 0] + R_y(0.5) * [2, 0, 0]
            = [1, 0, 0] + [2*cos(0.5), 0, 2*sin(0.5)]
            = [1, 0, 0] + [1.755, 0, 0.959]
            = [2.755, 0, 0.959]

Which is T_FIXED_1 * T_FIXED_2, not the other way around.