franka/robot/target_control/
rotation.rs

1//! The rotation arithmetic of the Cartesian interface: column-major poses, unit quaternions
2//! in `[x, y, z, w]` order and rotation vectors (unit axis times angle), on nalgebra.
3
4use nalgebra::{Matrix3, Matrix4, Quaternion, Rotation3, UnitQuaternion, Vector3};
5
6use crate::error::{FrankaError, FrankaResult};
7use crate::math_utils::{linear_of, orthonormalized_rotation, pose_to_array};
8use crate::rate_limiting::scaled_axis;
9
10/// How far from orthonormal (the largest entry of `|R^T R - I|`, or `|det R - 1|`) the
11/// rotation block of a target pose may be: within this it is re-orthonormalised, beyond it
12/// refused.
13pub const ORTHONORMAL_TOLERANCE: f64 = 1e-3;
14
15/// How far from unit length a target quaternion may be before it is refused; within this it
16/// is normalised.
17pub const UNIT_QUATERNION_TOLERANCE: f64 = 1e-3;
18
19/// The rotation block of a column-major 4x4 pose, as it is (not orthonormalised).
20pub(super) fn rotation_of(pose: &[f64; 16]) -> Matrix3<f64> {
21    linear_of(&Matrix4::from_column_slice(pose))
22}
23
24/// The translation of a column-major 4x4 pose.
25pub(super) fn translation_of(pose: &[f64; 16]) -> [f64; 3] {
26    [pose[12], pose[13], pose[14]]
27}
28
29/// A column-major 4x4 pose from a rotation and a translation.
30pub(super) fn pose_from(rotation: &Matrix3<f64>, translation: &[f64; 3]) -> [f64; 16] {
31    pose_to_array(rotation, &Vector3::from(*translation))
32}
33
34/// How far `m` is from a rotation: the largest entry of `|m^T m - I|`, or `|det m - 1|` if
35/// that is larger (a reflection is orthonormal but no rotation).
36pub(super) fn orthonormality_error(m: &Matrix3<f64>) -> f64 {
37    let gram = m.transpose() * m - Matrix3::identity();
38    let worst = gram.iter().fold(0.0f64, |worst, x| worst.max(x.abs()));
39    worst.max((m.determinant() - 1.0).abs())
40}
41
42/// The unit quaternion of a rotation matrix, `[x, y, z, w]` with `w >= 0`.
43pub(super) fn to_quaternion(rotation: &Matrix3<f64>) -> [f64; 4] {
44    let q = UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked(*rotation));
45    let sign = if q.w < 0.0 { -1.0 } else { 1.0 };
46    [sign * q.i, sign * q.j, sign * q.k, sign * q.w]
47}
48
49/// The rotation matrix of a quaternion `[x, y, z, w]`, normalised on the way.
50pub(super) fn from_quaternion(q: &[f64; 4]) -> Matrix3<f64> {
51    UnitQuaternion::from_quaternion(Quaternion::new(q[3], q[0], q[1], q[2]))
52        .to_rotation_matrix()
53        .into_inner()
54}
55
56/// The rotation vector of a rotation matrix (quaternion-based: accurate for small angles).
57pub(super) fn log(rotation: &Matrix3<f64>) -> [f64; 3] {
58    let v = scaled_axis(rotation);
59    [v[0], v[1], v[2]]
60}
61
62/// The rotation matrix of a rotation vector (Rodrigues); the identity for the zero vector.
63pub(super) fn exp(v: &[f64; 3]) -> Matrix3<f64> {
64    Rotation3::from_scaled_axis(Vector3::from(*v)).into_inner()
65}
66
67/// The angle, rad, that takes `a` to `b`.
68pub(super) fn angle_between(a: &Matrix3<f64>, b: &Matrix3<f64>) -> f64 {
69    norm(&log(&(b * a.transpose())))
70}
71
72pub(super) fn norm(v: &[f64; 3]) -> f64 {
73    (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
74}
75
76pub(super) fn distance(a: &[f64; 3], b: &[f64; 3]) -> f64 {
77    norm(&[a[0] - b[0], a[1] - b[1], a[2] - b[2]])
78}
79
80/// `q` as a unit quaternion.
81///
82/// # Errors
83/// [`FrankaError::InvalidArgument`] if a component is not finite or the norm is further than
84/// [`UNIT_QUATERNION_TOLERANCE`] from one.
85pub(super) fn unit_quaternion(q: [f64; 4]) -> FrankaResult<[f64; 4]> {
86    if q.iter().any(|x| !x.is_finite()) {
87        return Err(FrankaError::InvalidArgument(format!(
88            "target control: the orientation must be finite, got {q:?}"
89        )));
90    }
91    let norm = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
92    if (norm - 1.0).abs() > UNIT_QUATERNION_TOLERANCE {
93        return Err(FrankaError::InvalidArgument(format!(
94            "target control: the orientation {q:?} is not a unit quaternion (norm {norm}); \
95             the order is [x, y, z, w]"
96        )));
97    }
98    Ok(q.map(|x| x / norm))
99}
100
101/// The translation and the re-orthonormalised rotation of a column-major pose.
102///
103/// # Errors
104/// [`FrankaError::InvalidArgument`] if an entry is not finite, the last row is not
105/// `[0, 0, 0, 1]` or the rotation block is further from orthonormal than the tolerance.
106pub(super) fn checked_pose(pose: &[f64; 16]) -> FrankaResult<([f64; 3], Matrix3<f64>)> {
107    if pose.iter().any(|x| !x.is_finite()) {
108        return Err(FrankaError::InvalidArgument(format!(
109            "target control: the pose must be finite, got {pose:?}"
110        )));
111    }
112    let last_row = [pose[3], pose[7], pose[11], pose[15]];
113    if last_row
114        .iter()
115        .zip(&[0.0, 0.0, 0.0, 1.0])
116        .any(|(a, b)| (a - b).abs() > 1e-6)
117    {
118        return Err(FrankaError::InvalidArgument(format!(
119            "target control: the pose must be a column-major homogeneous transform with the \
120             last row [0, 0, 0, 1], got {last_row:?}"
121        )));
122    }
123    let rotation = rotation_of(pose);
124    let error = orthonormality_error(&rotation);
125    if error > ORTHONORMAL_TOLERANCE {
126        return Err(FrankaError::InvalidArgument(format!(
127            "target control: the rotation block of the pose is not orthonormal (error {error:.3e} \
128             against a tolerance of {ORTHONORMAL_TOLERANCE:.0e}); a rotation matrix's columns \
129             must be unit length and mutually perpendicular, and the pose column-major"
130        )));
131    }
132    Ok((translation_of(pose), orthonormalized_rotation(&rotation)))
133}
franka/robot/target_control/rotation.rs

franka/robot/target_control/
rotation.rs
