Reachy-remote / math.js
gportillo222's picture
Upload 9 files
9e61743 verified
Raw History Blame Contribute Delete
1.22 kB
// Local copies of the SDK's pose math, used only by demo mode so it runs
// without loading the SDK. Same conventions as the SDK: angles in degrees,
// ZYX order (yaw, then pitch, then roll), 4×4 matrix as 16 floats row-major.
const D2R = Math.PI / 180;
export function rpyToMatrix(roll, pitch, yaw) {
const [cr, sr] = [Math.cos(roll * D2R), Math.sin(roll * D2R)];
const [cp, sp] = [Math.cos(pitch * D2R), Math.sin(pitch * D2R)];
const [cy, sy] = [Math.cos(yaw * D2R), Math.sin(yaw * D2R)];
return [
cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, 0,
sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, 0,
-sp, cp * sr, cp * cr, 0,
0, 0, 0, 1,
];
}
export function matrixToRpy(m) {
const f = flattenMatrix(m);
const pitch = Math.asin(Math.max(-1, Math.min(1, -f[8])));
const roll = Math.atan2(f[9], f[10]);
const yaw = Math.atan2(f[4], f[0]);
return { roll: roll / D2R, pitch: pitch / D2R, yaw: yaw / D2R };
}
/** Accepts a nested 4×4 array or a flat 16-array and returns a flat copy. */
export function flattenMatrix(m) {
return Array.isArray(m?.[0]) ? m.flat() : Array.from(m);
}