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::chains::Chain;
use crate::geometry::{Frame, Introspection, Twist};
use crate::joint::JointType;

/// Calculate the global twist of a segment given the global pose and twist
/// of the previous segment, and the local pose and twist of the current
/// segment
pub fn reference_frame(
    total_twist: Twist,
    segment_twist: Twist,
    total_pose: Frame,
    segment_pose: Frame,
) -> Twist {
    let rotation = total_pose.get_rotation();
    let rot = rotation * segment_twist.fixed_rows::<3>(3);
    let tmp = rotation * segment_pose.get_translation();
    let trans =
        rotation * segment_twist.fixed_rows::<3>(0) + total_twist.fixed_rows::<3>(3).cross(&tmp);

    Twist::new(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2])
}

/// Solver for forward differential kinematics. Calculate the velocity at any
/// given joint from the current joint configuration and rate.
#[derive(Clone, Debug)]
pub struct ForwardDiffKinematicsSolver {
    chain: Chain,
}

impl ForwardDiffKinematicsSolver {
    /// Create a default empty solver
    pub fn default() -> Self {
        Self {
            chain: Chain::default(),
        }
    }

    /// Create a new solver from a given chain
    pub fn new(in_chain: Chain) -> Self {
        Self { chain: in_chain }
    }

    /// Solver for forward differential kinematics at an arbitrary segment.\
    /// Computes the twist at the segment given the current chain state.
    ///
    /// `chain`: Kinematic chain to use by the solver\
    /// `angles`: vector with the state at each joint\
    /// `qdots`: vector with the differential state at each joint\
    /// `segment_index`: at which segment to compute the velocity\
    pub fn solve_at_segment(
        &self,
        angles: &Vec<f64>,
        qdots: &Vec<f64>,
        segment_index: usize,
    ) -> Twist {
        let num_joints = self.chain.get_num_joints();
        let num_segments = self.chain.get_num_segments();
        if num_joints != angles.len() {
            panic!(
                "Got {} angles, but the chain has {} joints",
                angles.len(),
                num_joints
            );
        }

        if num_joints != qdots.len() {
            panic!(
                "Got {} angle differentials, but the chain has {} joints",
                qdots.len(),
                num_joints
            );
        }

        if segment_index >= num_segments {
            panic!(
                "Asked for pose at segment {}, but the chain has {} segments",
                segment_index, num_segments
            );
        }
        let mut counter = 0;
        let mut result_pose = Frame::identity();
        let mut result_twist = Twist::zeros();

        for i in 0..segment_index + 1 {
            let segment = self.chain.get_segment(i);
            let pose = segment.pose(angles[counter]);
            let twist = segment.twist(angles[counter], qdots[counter]);
            result_twist += reference_frame(result_twist, twist, result_pose, pose);
            result_pose *= pose;
            if segment.get_joint_type() != JointType::NoJoint {
                counter += 1;
            }
        }
        result_twist
    }

    /// Solver for forward differential kinematics at the end-effector\
    /// Uses the arbitrary segment solver with end-effector as default segment
    pub fn solve(&self, angles: &Vec<f64>, qdots: &Vec<f64>) -> Twist {
        self.solve_at_segment(angles, qdots, self.chain.get_num_segments() - 1)
    }
}

#[cfg(test)]
mod test {
    use crate::chains::tests::create_testing_chain;
    use crate::forward_diff_kinematics::ForwardDiffKinematicsSolver;
    use crate::geometry::{get_twist_error, Twist};

    #[test]
    fn base_twist_correct() {
        let chain = create_testing_chain();
        let angles = vec![0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47];
        let qdots = vec![1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24];
        let solver = ForwardDiffKinematicsSolver::new(chain);
        let base_twist = solver.solve_at_segment(&angles, &qdots, 0);
        assert_eq!(base_twist, Twist::zeros())
    }

    #[test]
    fn end_twist_correct() {
        let chain = create_testing_chain();
        let angles = vec![0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47];
        let qdots = vec![1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24];
        let solver = ForwardDiffKinematicsSolver::new(chain);
        let end_twist = solver.solve(&angles, &qdots);
        let error = get_twist_error(
            end_twist,
            Twist::new(-0.423878, -1.01074, 0.123592, -2.28297, 0.209941, 1.80897),
        );
        assert!(error < 0.00001)
    }

    #[test]
    fn middle_twist_correct() {
        let chain = create_testing_chain();
        let angles = vec![0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47];
        let qdots = vec![1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24];
        let solver = ForwardDiffKinematicsSolver::new(chain);
        let middle_twist = solver.solve_at_segment(&angles, &qdots, 4);
        let error = get_twist_error(
            middle_twist,
            Twist::new(-0.344006, 0.591387, 0.0751308, -1.1035, -0.0710371, 1.45781),
        );
        assert!(error < 0.00001)
    }
}