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::{EulerBuild, Frame, Twist, Vector};
use serde::{Deserialize, Serialize};

/// Defines what types of joints are suported by this library.
///
/// `NoJoint` means a Joint without movement \
/// The ones that start with `Rot` represent rotational joints \
/// The ones that start with `Trans` represent prismatic joints
#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq)]
pub enum JointType {
    /// Joint without movement. Same as Orocos-kdl None type
    NoJoint, // Same as None in Orocos-kdl
    /// Rotational joint with axis in `x`
    RotX,
    /// Rotational joint with axis in `y`
    RotY,
    /// Rotational joint with axis in `z`
    RotZ,
    /// Prismatic joint with axis in `x`
    TransX,
    /// Prismatic joint with axis in `y`
    TransY,
    /// Prismatic joint with axis in `z`
    TransZ,
}

/// Defines a robotic Joint
///
/// joint_type: `JointType` that defines the type of the joint\
/// scale: received `cmd` mut moves `cmd*scale` units\
/// offset: received `cmd` mut moves `cmd+offset`\
/// inertia: Meassurement of inertia for Dynamic purposes\
/// damping: Used for Impedance control\
/// stiffness: Used for Impedance control\
/// axis: Rotational or translational axis of this joint
#[derive(Copy, Clone, Debug, Deserialize, Serialize)]
pub struct Joint {
    joint_type: JointType,
    scale: f64,
    offset: f64,
    inertia: f64,
    damping: f64,
    stiffness: f64,
    axis: Vector,
}

impl Joint {
    /// Returns the default Joint, of type `NoJoint`, no scale, offset
    /// or any Dynamic parameter.
    pub fn default() -> Joint {
        Joint {
            joint_type: JointType::NoJoint,
            scale: 1.0,
            offset: 0.0,
            inertia: 0.0,
            damping: 0.0,
            stiffness: 0.0,
            axis: Vector::z(),
        }
    }

    // TODO: Change this to be mutating methods
    /// Create a new Joint with the given Type, and sets the appropiate axis
    pub fn set_type(&self, new_joint_type: JointType) -> Joint {
        Joint {
            joint_type: new_joint_type,
            scale: self.scale,
            offset: self.offset,
            inertia: self.inertia,
            damping: self.damping,
            stiffness: self.stiffness,
            axis: axis_from_type(new_joint_type),
        }
    }

    /// Create a new Joint with the given scale
    pub fn set_scale(&self, new_scale: f64) -> Joint {
        Joint {
            joint_type: self.joint_type,
            scale: new_scale,
            offset: self.offset,
            inertia: self.inertia,
            damping: self.damping,
            stiffness: self.stiffness,
            axis: self.axis,
        }
    }

    /// Create a new Joint with the given offset
    pub fn set_offset(&self, new_offset: f64) -> Joint {
        Joint {
            joint_type: self.joint_type,
            scale: self.scale,
            offset: new_offset,
            inertia: self.inertia,
            damping: self.damping,
            stiffness: self.stiffness,
            axis: self.axis,
        }
    }

    /// Create a new Joint with the given inertial parameter
    pub fn set_inertia(&self, new_inertia: f64) -> Joint {
        Joint {
            joint_type: self.joint_type,
            scale: self.scale,
            offset: self.offset,
            inertia: new_inertia,
            damping: self.damping,
            stiffness: self.stiffness,
            axis: self.axis,
        }
    }

    /// Create a new Joint with the given damping parameter
    pub fn set_damping(&self, new_damping: f64) -> Joint {
        Joint {
            joint_type: self.joint_type,
            scale: self.scale,
            offset: self.offset,
            inertia: self.inertia,
            damping: new_damping,
            stiffness: self.stiffness,
            axis: self.axis,
        }
    }

    /// Create a new Joint with the given stiffness parameter
    pub fn set_stiffness(&self, new_stiffness: f64) -> Joint {
        Joint {
            joint_type: self.joint_type,
            scale: self.scale,
            offset: self.offset,
            inertia: self.inertia,
            damping: self.damping,
            stiffness: new_stiffness,
            axis: self.axis,
        }
    }

    /// Returns the type of this Joint
    pub fn get_joint_type(&self) -> JointType {
        self.joint_type
    }

    /// Calculates the pose of the Joint, given its current state
    pub fn pose(&self, joint_state: f64) -> Frame {
        match self.joint_type {
            JointType::NoJoint => Frame::identity(),
            JointType::RotX => Frame::from_euler(self.scale * joint_state + self.offset, 0.0, 0.0),
            JointType::RotY => Frame::from_euler(0.0, self.scale * joint_state + self.offset, 0.0),
            JointType::RotZ => Frame::from_euler(0.0, 0.0, self.scale * joint_state + self.offset),
            JointType::TransX => {
                Frame::from_translation(self.scale * joint_state + self.offset, 0.0, 0.0)
            }
            JointType::TransY => {
                Frame::from_translation(0.0, self.scale * joint_state + self.offset, 0.0)
            }
            JointType::TransZ => {
                Frame::from_translation(0.0, 0.0, self.scale * joint_state + self.offset)
            }
        }
    }

    /// Calculates the velocity of the Joint, given its current joint-speed
    pub fn twist(&self, qdot: f64) -> Twist {
        match self.joint_type {
            JointType::NoJoint => Twist::zeros(),
            JointType::TransX => Twist::new(self.scale * qdot, 0.0, 0.0, 0.0, 0.0, 0.0),
            JointType::TransY => Twist::new(0.0, self.scale * qdot, 0.0, 0.0, 0.0, 0.0),
            JointType::TransZ => Twist::new(0.0, 0.0, self.scale * qdot, 0.0, 0.0, 0.0),
            JointType::RotX => Twist::new(0.0, 0.0, 0.0, self.scale * qdot, 0.0, 0.0),
            JointType::RotY => Twist::new(0.0, 0.0, 0.0, 0.0, self.scale * qdot, 0.0),
            JointType::RotZ => Twist::new(0.0, 0.0, 0.0, 0.0, 0.0, self.scale * qdot),
        }
    }

    /// Get the action-axis of this Joint
    pub fn joint_axis(&self) -> Vector {
        axis_from_type(self.joint_type)
    }
}

fn axis_from_type(joint_type: JointType) -> Vector {
    match joint_type {
        JointType::NoJoint => Vector::zeros(),
        JointType::TransX => Vector::x(),
        JointType::TransY => Vector::y(),
        JointType::TransZ => Vector::z(),
        JointType::RotX => Vector::x(),
        JointType::RotY => Vector::y(),
        JointType::RotZ => Vector::z(),
    }
}

impl PartialEq for Joint {
    fn eq(&self, other: &Self) -> bool {
        (self.joint_type == other.joint_type)
            && (self.scale == other.scale)
            && (self.offset == other.offset)
            && (self.inertia == other.inertia)
            && (self.damping == other.damping)
            && (self.stiffness == other.stiffness)
            && (self.axis == other.axis)
    }
}

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

    #[test]
    fn calculate_pose() {
        let joint = Joint::default()
            .set_type(JointType::RotZ)
            .set_offset(2.36)
            .set_scale(3.5);
        let angle = 1.23;
        let pose = Frame::new(
            0.92799, -0.372605, 0.0, 0.0, 0.372605, 0.92799, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
            0.0, 0.0, 1.0,
        );
        let error = get_frame_error(pose, joint.pose(angle));
        assert!(error < 0.000001);
    }

    #[test]
    fn calculate_twist() {
        let joint = Joint::default()
            .set_type(JointType::RotZ)
            .set_offset(2.36)
            .set_scale(3.5);
        let angle_rate = 1.23;
        let twist = Twist::new(0.0, 0.0, 0.0, 0.0, 0.0, 4.305);
        let error = get_twist_error(twist, joint.twist(angle_rate));
        assert!(error < 0.000001);
    }
}