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 crate::geometry::{get_frame_error, invert_frame, new_ref_point, Frame, Introspection, Twist};
use crate::joint::{Joint, JointType};
use serde::{Deserialize, Serialize};

/// This is a placeholder, as we are interested only in kinematics, we are not
/// implementing dynamics yet.
pub type RigidBodyInertia = f64;

/// Implementation of a kinematic chian segment. This is the joint together with
/// the link.
///
/// It is represented by the joint, a frame that transforms from the joint to the
/// end of the link, and an inertia parameter.
#[derive(Copy, Clone, Debug, Deserialize, Serialize)]
pub struct Segment {
    joint: Joint,
    f_tip: Frame,
    inertia: RigidBodyInertia,
}

impl Segment {
    /// Default segment, with the default joint and no link.
    pub fn default() -> Segment {
        Segment {
            joint: Joint::default(),
            f_tip: Frame::identity(),
            inertia: 0.0,
        }
    }

    /// Constructs a segment from the given `Joint`, link transformation
    /// and inertia
    pub fn new(joint: Joint, f_tip: Frame, inertia: RigidBodyInertia) -> Segment {
        let frame = invert_frame(joint.pose(0.0)) * f_tip;
        Segment {
            joint: joint,
            f_tip: frame,
            inertia: inertia,
        }
    }

    /// Creates a new segment with the given joint
    pub fn set_joint(&self, new_joint: Joint) -> Segment {
        Segment {
            joint: new_joint,
            f_tip: self.f_tip,
            inertia: self.inertia,
        }
    }

    /// Creates a new segment with the given link transformation
    pub fn set_tip_frame(&self, new_f_tip: Frame) -> Segment {
        let frame = invert_frame(self.joint.pose(0.0)) * new_f_tip;
        Segment {
            joint: self.joint,
            f_tip: frame,
            inertia: self.inertia,
        }
    }

    /// Creates a new segment with the given inertia
    ///
    /// Note that right now this method is useless because dynamics are not
    /// yet implemented
    pub fn set_inertia(&self, new_inertia: RigidBodyInertia) -> Segment {
        Segment {
            joint: self.joint,
            f_tip: self.f_tip,
            inertia: new_inertia,
        }
    }

    /// Returns the type of the internal joint
    pub fn get_joint_type(&self) -> JointType {
        self.joint.get_joint_type()
    }

    /// Returns the link transfrom
    pub fn get_link_transform(&self) -> Frame {
        self.f_tip
    }

    /// Get the pose at the tip of the segment given the state of the joint
    pub fn pose(&self, joint_state: f64) -> Frame {
        self.joint.pose(joint_state) * self.f_tip
    }

    /// Get the twist at the tip of the segment given the state of the joint
    pub fn twist(&self, joint_state: f64, qdot: f64) -> Twist {
        new_ref_point(
            self.joint.twist(qdot),
            self.joint.pose(joint_state).get_rotation() * self.f_tip.get_translation(),
        )
    }
}

impl PartialEq for Segment {
    fn eq(&self, other: &Self) -> bool {
        let threshold = 0.0000001;
        let similar_tf = get_frame_error(self.f_tip, other.f_tip) < threshold;
        (self.joint == other.joint) && (self.inertia == other.inertia) && similar_tf
    }
}

#[cfg(test)]
mod tests {
    use crate::geometry::{get_frame_error, get_twist_error, Frame, Twist};
    use crate::joint::{Joint, JointType};
    use crate::segment::Segment;
    use nalgebra::geometry::{Isometry3, Translation3, UnitQuaternion};

    #[test]
    fn calculate_pose() {
        let joint = Joint::default()
            .set_type(JointType::RotZ)
            .set_offset(2.36)
            .set_scale(3.5);
        let f_tip = Isometry3::from_parts(
            Translation3::new(0.1, -2.0, 0.5),
            UnitQuaternion::from_euler_angles(1.0, 0.5, -0.4),
        )
        .to_homogeneous();
        let joint_state = 2.1;
        let segment = Segment::new(joint, f_tip, 0.0);
        let result = Frame::new(
            0.689601, -0.0171617, 0.723987, 1.79963, 0.542773, 0.674079, -0.501015, -0.878265,
            -0.479426, 0.73846, 0.47416, 0.5, 0.0, 0.0, 0.0, 1.0,
        );
        let error = get_frame_error(result, segment.pose(joint_state));
        assert!(error < 0.00001);
    }

    #[test]
    fn calculate_twist() {
        let joint = Joint::default()
            .set_type(JointType::RotZ)
            .set_offset(2.36)
            .set_scale(3.5);
        let f_tip = Isometry3::from_parts(
            Translation3::new(0.1, -2.0, 0.5),
            UnitQuaternion::from_euler_angles(1.0, 0.5, -0.4),
        )
        .to_homogeneous();
        let joint_state = 2.1;
        let qdot = 0.3;
        let segment = Segment::new(joint, f_tip, 0.0);
        let result = Twist::new(0.92217, 1.88961, 0.0, 0.0, 0.0, 1.05);
        let error = get_twist_error(result, segment.twist(joint_state, qdot));
        assert!(error < 0.001);
    }
}