arcos-kdl 0.3.3

// Copyright (c) 2019 Autonomous Robots and Cognitive Systems Laboratory
// Author: Daniel Garcia-Vaglio <degv364@gmail.com>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

use nalgebra::geometry::{Isometry3, IsometryMatrix3, Rotation3, Translation3, UnitQuaternion};
use nalgebra::{Matrix3, Matrix4, Vector3, Vector6};

/// Homogeneous matrix type. This is a 4x4 matrix, of wich the top left
/// 3x3 matrix is the rotation, and the last column represents the
/// position and scale.
pub type Frame = Matrix4<f64>;

/// Hexadimensional velocity. The first 3 entries represent the
/// translational velocity and the last 3 the rotational velocity
pub type Twist = Vector6<f64>;

/// Rotation 3x3 matrix type.
pub type Rotation = Matrix3<f64>;

/// Vector 3D.
pub type Vector = Vector3<f64>;

/// Different rotation representations
pub trait RotationRepresentations {
    /// Convert rotation matrix to a vector representing the rotation axis and
    /// its norm representing its angle
    fn to_scaled_axis(&self) -> Vector;
    /// Create a rotation matrix from an rotation axis with angle in magnitude
    fn from_scaled_axis(scaled_axis: Vector) -> Rotation;
    /// Convert rotation matrix into Euler angles
    fn to_euler(&self) -> (f64, f64, f64);
    /// Create rotation matrix from Euler angles
    fn from_euler(roll: f64, pitch: f64, yaw: f64) -> Rotation;
}

impl RotationRepresentations for Rotation {
    fn to_scaled_axis(&self) -> Vector {
        Rotation3::from_matrix(self).scaled_axis()
    }
    fn from_scaled_axis(scaled_axis: Vector) -> Rotation {
        *Rotation3::from_scaled_axis(scaled_axis).matrix()
    }
    fn to_euler(&self) -> (f64, f64, f64) {
        Rotation3::from_matrix(self).euler_angles()
    }
    fn from_euler(roll: f64, pitch: f64, yaw: f64) -> Rotation {
        *Rotation3::from_euler_angles(roll, pitch, yaw).matrix()
    }
}

/// Get the internal components of a Homogeneous transformation
pub trait Introspection {
    /// Get the translational part
    fn get_translation(&self) -> Vector;
    /// Get the rotational part
    fn get_rotation(&self) -> Rotation;
    /// Get the Euler angles
    fn get_euler(&self) -> (f64, f64, f64);
}

impl Introspection for Frame {
    fn get_translation(&self) -> Vector {
        Vector::new(self[(0, 3)], self[(1, 3)], self[(2, 3)])
    }
    fn get_rotation(&self) -> Rotation {
        Rotation::new(
            self[(0, 0)],
            self[(0, 1)],
            self[(0, 2)],
            self[(1, 0)],
            self[(1, 1)],
            self[(1, 2)],
            self[(2, 0)],
            self[(2, 1)],
            self[(2, 2)],
        )
    }

    fn get_euler(&self) -> (f64, f64, f64) {
        Rotation3::from_matrix(&Matrix3::new(
            self[(0, 0)],
            self[(0, 1)],
            self[(0, 2)],
            self[(1, 0)],
            self[(1, 1)],
            self[(1, 2)],
            self[(2, 0)],
            self[(2, 1)],
            self[(2, 2)],
        ))
        .euler_angles()
    }
}
/// Build homogeneous transformations from Euler angles, and translations
pub trait EulerBuild {
    /// Build only rotation without translation
    fn from_euler(roll: f64, pitch: f64, yaw: f64) -> Frame;
    /// Build only translation without rotation
    fn from_translation(x: f64, y: f64, z: f64) -> Frame;
    /// Build from translation and rotation
    fn from_translation_euler(x: f64, y: f64, z: f64, roll: f64, pitch: f64, yaw: f64) -> Frame;
    /// Build from axis-angle.
    ///
    /// Receives vector components, that determines the axis, and\
    /// its norm the angle
    fn from_axisangle(a: f64, b: f64, c: f64) -> Frame;
    /// Build from translation and axis-angle
    fn from_translation_axisangle(x: f64, y: f64, z: f64, a: f64, b: f64, c: f64) -> Frame;
    /// Build from a Rotation matrix and a translation Vector
    fn from_rotation_vector(rot: Rotation, vec: Vector) -> Frame;
}

impl EulerBuild for Frame {
    fn from_euler(roll: f64, pitch: f64, yaw: f64) -> Frame {
        Frame::from_euler_angles(roll, pitch, yaw)
    }
    fn from_translation(x: f64, y: f64, z: f64) -> Frame {
        Isometry3::from_parts(
            Translation3::new(x, y, z),
            UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0),
        )
        .to_homogeneous()
    }
    fn from_translation_euler(x: f64, y: f64, z: f64, roll: f64, pitch: f64, yaw: f64) -> Frame {
        Isometry3::from_parts(
            Translation3::new(x, y, z),
            UnitQuaternion::from_euler_angles(roll, pitch, yaw),
        )
        .to_homogeneous()
    }
    fn from_axisangle(a: f64, b: f64, c: f64) -> Frame {
        Isometry3::rotation(Vector::new(a, b, c)).to_homogeneous()
    }
    fn from_translation_axisangle(x: f64, y: f64, z: f64, a: f64, b: f64, c: f64) -> Frame {
        Isometry3::new(Vector::new(x, y, z), Vector::new(a, b, c)).to_homogeneous()
    }
    fn from_rotation_vector(rot: Rotation, vec: Vector) -> Frame {
        Frame::new(
            rot[(0, 0)],
            rot[(0, 1)],
            rot[(0, 2)],
            vec[0],
            rot[(1, 0)],
            rot[(1, 1)],
            rot[(1, 2)],
            vec[1],
            rot[(2, 0)],
            rot[(2, 1)],
            rot[(2, 2)],
            vec[2],
            0.0,
            0.0,
            0.0,
            1.0,
        )
    }
}

/// Add a differential to homogeneous transformation for integrating poses
///
/// Given a pose P, a twist v and a delta time dt the result is
///
/// R = P + v*dt
///
/// (Note that the above formula is not the real algorithm but a
/// simplification for understanding this function)
pub fn add_delta(pose: Frame, twist: Twist, dt: f64) -> Frame {
    let trans = pose.get_translation() + twist.fixed_rows::<3>(0) * dt;
    let rot_axis = twist.fixed_rows::<3>(3);
    let rot = pose.get_rotation() * Rotation3::new(dt * pose.get_rotation().transpose() * rot_axis);
    IsometryMatrix3::from_parts(
        Translation3::from(trans),
        Rotation3::from_matrix_unchecked(rot),
    )
    .to_homogeneous()
}

/// Differentiate a homogeneous transformation for derivating poses
///
/// Given an inittial pose P, a final pose F, and a delta time dt
/// The resulting twist r is given by:
///
/// r = (P-F)/dt
///
/// (Note that the above formula is not the real algorithm but a
/// simplification for understanding this function)
pub fn diff(end: Frame, init: Frame, dt: f64) -> Twist {
    let tra = (end.get_translation() - init.get_translation()) / dt;
    let rot_mat = init.get_rotation().transpose() * end.get_rotation();
    let rot = init.get_rotation() * Rotation3::from_matrix_unchecked(rot_mat).scaled_axis() / dt;
    Twist::new(tra[0], tra[1], tra[2], rot[0], rot[1], rot[2])
}

/// Calculate the absolute difference between two twists
pub fn get_twist_error(left: Twist, right: Twist) -> f64 {
    let error = (left - right).abs();
    error.fold(0.0, |a: f64, b: f64| a + b)
}

/// Calculate the absolute difference between two poses
pub fn get_frame_error(left: Frame, right: Frame) -> f64 {
    let error = (left - right).abs();
    error.fold(0.0, |a: f64, b: f64| a + b)
}

/// Change the reference point of a twist
pub fn new_ref_point(twist: Twist, new_ref: Vector) -> Twist {
    let rot = twist.fixed_rows::<3>(3);
    let trans = twist.fixed_rows::<3>(0) + rot.cross(&new_ref);
    Twist::new(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2])
}

/// Change the reference rotation of a twist
pub fn new_ref_base(twist: Twist, new_ref: Matrix3<f64>) -> Twist {
    let trans = new_ref * twist.fixed_rows::<3>(0);
    let rot = new_ref * twist.fixed_rows::<3>(3);
    Twist::new(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2])
}

/// Change the reference frame of a twist
pub fn new_ref_frame(twist: Twist, new_ref: Frame) -> Twist {
    let rot = new_ref.get_rotation() * twist.fixed_rows::<3>(3);
    let trans =
        new_ref.get_rotation() * twist.fixed_rows::<3>(0) + new_ref.get_translation().cross(&rot);
    Twist::new(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2])
}

fn determinant(mx: Matrix3<f64>) -> f64 {
    let det00 = mx[(1, 1)] * mx[(2, 2)] - mx[(2, 1)] * mx[(1, 2)];
    let det01 = mx[(1, 0)] * mx[(2, 2)] - mx[(2, 0)] * mx[(1, 2)];
    let det02 = mx[(1, 0)] * mx[(2, 1)] - mx[(2, 0)] * mx[(1, 1)];
    mx[(0, 0)] * (det00) - mx[(0, 1)] * (det01) + mx[(0, 2)] * (det02)
}

fn remove_row_col(row: usize, col: usize, mx: Frame) -> Matrix3<f64> {
    mx.remove_row(row).remove_column(col)
}

/// Optimized algorithm for inverting Homogeneous transformations
pub fn invert_frame(frame: Frame) -> Frame {
    let rot_mat = remove_row_col(3, 3, frame);
    // Get the translation inversion from adjunct matrix
    let adj_x = determinant(remove_row_col(3, 0, frame)) / determinant(rot_mat);
    let adj_y = determinant(remove_row_col(3, 1, frame)) / determinant(rot_mat);
    let adj_z = determinant(remove_row_col(3, 2, frame)) / determinant(rot_mat);
    // Rotation in inverted by transposing
    Frame::new(
        frame[(0, 0)],
        frame[(1, 0)],
        frame[(2, 0)],
        -adj_x,
        frame[(0, 1)],
        frame[(1, 1)],
        frame[(2, 1)],
        adj_y,
        frame[(0, 2)],
        frame[(1, 2)],
        frame[(2, 2)],
        -adj_z,
        0.0,
        0.0,
        0.0,
        1.0,
    )
}

/// Truncate the norm of a twist to a certain limit
pub fn truncate_twist(twist: Twist, max_norm: f64) -> Twist {
    if twist.norm() > max_norm {
        twist / twist.norm() * (max_norm - 0.0001)
    } else {
        twist
    }
}

#[cfg(test)]
mod tests {
    use crate::geometry::{
        add_delta, diff, get_frame_error, get_twist_error, invert_frame, new_ref_base,
        new_ref_frame, new_ref_point, truncate_twist, EulerBuild, Frame, Introspection, Twist,
        Vector,
    };
    use nalgebra::geometry::{Isometry3, Translation3, UnitQuaternion};
    use nalgebra::Matrix3;
    use rand::Rng;
    use std::f64::consts::PI;

    #[test]
    fn transformation_by_zero() {
        let frame = Frame::identity();
        let twist = Twist::zeros();
        let new_axis = add_delta(frame, twist, 0.1);
        assert_eq!(new_axis, frame);
    }
    #[test]
    fn add_delta_traslation() {
        let result = Frame::from_translation(0.1, 0.0, 0.0);
        let prev = Frame::from_translation(0.1, 0.5, 0.5);
        let twist = Twist::new(0.0, -5.0, -5.0, 0.0, 0.0, 0.0);
        assert_eq!(add_delta(prev, twist, 0.1), result);
    }
    #[test]
    fn add_delta_rotation() {
        let prev = Frame::identity();
        let twist = Twist::new(0.0, 0.0, 0.0, 1.0, 2.0, 3.0);
        let result = Frame::from_translation_axisangle(0.0, 0.0, 0.0, 0.1, 0.2, 0.3);
        let error = get_frame_error(result, add_delta(prev, twist, 0.1));
        assert!(error < 0.000001);
    }
    #[test]
    fn add_delta_example() {
        let prev = Frame::from_translation_euler(0.1, 0.2, 0.3, 1.0, 0.5, 0.5);
        let twist = Twist::new(0.2, 0.5, 0.7, 0.1, 0.5, 1.2);
        // This was computed using orocos-KDL
        let result = Frame::new(
            0.689222, 0.0517462, 0.7227, 0.12, 0.513383, 0.668978, -0.5375, 0.25, -0.511284,
            0.741479, 0.434509, 0.37, 0.0, 0.0, 0.0, 1.0,
        );

        let error = get_frame_error(result, add_delta(prev, twist, 0.1));
        assert!(error < 0.00001);
    }
    #[test]
    fn diff_translation() {
        let init = Isometry3::translation(0.1, 0.0, 0.0).to_homogeneous();
        let end = Isometry3::translation(0.4, 0.3, 1.0).to_homogeneous();
        let result = Twist::new(3.0, 3.0, 10.0, 0.0, 0.0, 0.0);
        let error = get_twist_error(result, diff(end, init, 0.1));
        assert!(error < 0.00001);
    }
    #[test]
    fn diff_rotation() {
        let init = Isometry3::from_parts(
            Translation3::new(0.0, 0.0, 0.0),
            UnitQuaternion::from_euler_angles(1.0, 0.5, 0.5),
        )
        .to_homogeneous();
        let end = Isometry3::from_parts(
            Translation3::new(0.0, 0.0, 0.0),
            UnitQuaternion::from_euler_angles(2.0, 0.3, 0.4),
        )
        .to_homogeneous();
        let result = Twist::new(0.0, 0.0, 0.0, 9.13642, 2.37322, -4.82489);
        let error = get_twist_error(result, diff(end, init, 0.1));
        assert!(error < 0.0001);
    }
    #[test]
    fn diff_example() {
        let init = Isometry3::from_parts(
            Translation3::new(0.3, 4.0, 2.1),
            UnitQuaternion::from_euler_angles(1.0, 0.5, 0.5),
        )
        .to_homogeneous();
        let end = Isometry3::from_parts(
            Translation3::new(8.2, 5.0, 0.4),
            UnitQuaternion::from_euler_angles(2.5, 1.3, 6.4),
        )
        .to_homogeneous();
        let result = Twist::new(79.0, 10.0, -17.0, 7.47766517, 9.43106542, -15.38060189);
        let error = get_twist_error(result, diff(end, init, 0.1));
        assert!(error < 0.0001);
    }
    // Now here come the tests with random matrices
    #[test]
    fn add_delta_inverts_diff() {
        let mut rng = rand::rng();
        for _index in 1..500 {
            let dt = rng.random_range(0.01..2.0);
            let init = Isometry3::from_parts(
                Translation3::new(rng.random(), rng.random(), rng.random()),
                UnitQuaternion::from_euler_angles(
                    rng.random_range(-PI..PI),
                    rng.random_range(-PI..PI),
                    rng.random_range(-PI..PI),
                ),
            )
            .to_homogeneous();
            let end = Isometry3::from_parts(
                Translation3::new(rng.random(), rng.random(), rng.random()),
                UnitQuaternion::from_euler_angles(
                    rng.random_range(-PI..PI),
                    rng.random_range(-PI..PI),
                    rng.random_range(-PI..PI),
                ),
            )
            .to_homogeneous();
            let error = get_frame_error(end, add_delta(init, diff(end, init, dt), dt));
            assert!(error < 0.001);
        }
    }
    #[test]
    fn diff_inverts_add_delta() {
        let mut rng = rand::rng();
        for _index in 1..500 {
            let dt = rng.random_range(0.01..2.0);
            let init = Isometry3::from_parts(
                Translation3::new(rng.random(), rng.random(), rng.random()),
                UnitQuaternion::from_euler_angles(
                    rng.random_range(-PI..PI),
                    rng.random_range(-PI..PI),
                    rng.random_range(-PI..PI),
                ),
            )
            .to_homogeneous();
            let free_twist = Twist::new(
                rng.random(),
                rng.random(),
                rng.random(),
                rng.random_range(-PI..PI),
                rng.random_range(-PI..PI),
                rng.random_range(-PI..PI),
            );
            let twist = truncate_twist(free_twist, PI / 2.0);
            let error = get_twist_error(twist, diff(add_delta(init, twist, dt), init, dt));
            println!("{}", twist);
            println!("{}", diff(add_delta(init, twist, dt), init, dt));
            assert!(error < 0.001);
        }
    }

    #[test]
    fn change_ref_point() {
        let vector = Vector::new(1.0, 2.0, 3.0);
        let twist = Twist::new(0.5, 1.0, 1.5, -1.5, -1.0, -0.5);
        let result = Twist::new(-1.5, 5.0, -0.5, -1.5, -1.0, -0.5);
        let error = get_twist_error(result, new_ref_point(twist, vector));
        assert!(error < 0.001);
    }

    #[test]
    fn change_ref_base() {
        let rotation = Matrix3::new(
            -0.950561, 0.264988, -0.161906, 0.276619, 0.485602, -0.82926, -0.14112, -0.83305,
            -0.534895,
        );
        let twist = Twist::new(0.5, 1.0, 1.5, -1.5, -1.0, -0.5);
        let result = Twist::new(-0.453157, -0.619979, -1.70595, 1.24181, -0.485901, 1.31218);
        let error = get_twist_error(result, new_ref_base(twist, rotation));
        assert!(error < 0.001);
    }

    #[test]
    fn change_ref_frame() {
        let frame = Frame::new(
            -0.950561, 0.264988, -0.161906, 1.0, 0.276619, 0.485602, -0.82926, 2.0, -0.14112,
            -0.83305, -0.534895, 3.0, 0.0, 0.0, 0.0, 1.0,
        );
        let twist = Twist::new(0.5, 1.0, 1.5, -1.5, -1.0, -0.5);
        let result = Twist::new(3.6289, 1.79327, -4.67547, 1.24181, -0.485901, 1.31218);
        let error = get_twist_error(result, new_ref_frame(twist, frame));
        assert!(error < 0.001);
    }

    #[test]
    fn inversion() {
        let mut rng = rand::rng();
        for _index in 1..10000 {
            let frame = Frame::from_translation_euler(
                rng.random(),
                rng.random(),
                rng.random(),
                rng.random_range(-PI..PI),
                rng.random_range(-PI..PI),
                rng.random_range(-PI..PI),
            );
            let inverted = invert_frame(frame);
            let error_left = get_frame_error(Frame::identity(), inverted * frame);
            let error_right = get_frame_error(Frame::identity(), frame * inverted);
            assert!(error_left < 0.001);
            assert!(error_right < 0.001);
        }
    }
    #[test]
    fn build_destroy_rot_vec() {
        let frame = Frame::from_translation_euler(1.0, 2.0, -1.0, 0.2, -0.3, 1.2);
        let rotation = frame.get_rotation();
        let translation = frame.get_translation();
        let frame2 = Frame::from_rotation_vector(rotation, translation);
        assert_eq!(frame, frame2);
    }
}