arcos-kdl 0.3.3

// Copyright (c) 2020 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 std::f64::consts::PI;

use nalgebra::Dyn;

use crate::chains::Chain;
use crate::forward_diff_kinematics::{reference_frame, ForwardDiffKinematicsSolver};
use crate::forward_kinematics::ForwardKinematicsSolver;
use crate::geometry::{add_delta, diff, get_frame_error, Frame, Twist};
use crate::inverse_diff_kinematics::{
    InverseDiffKinematicsSolver, WeightJointSpace, WeightTaskSpace,
};
use crate::joint::{Joint, JointType};
use crate::segment::Segment;

/// Calculate if the difference between two frames is acceptable
fn is_frame_error_acceptable(end: Frame, init: Frame, tolerance: f64, rot_tolerance: f64) -> bool {
    let delta = diff(end, init, 1.0);
    let translation = (delta[0].powi(2) + delta[1].powi(2) + delta[2].powi(2)).sqrt();
    let rotation = (delta[3].powi(2) + delta[4].powi(2) + delta[5].powi(2)).sqrt();
    if translation < tolerance && rotation < rot_tolerance {
        true
    } else {
        false
    }
}

/// Calculate the twist to use for positional control convergence\
/// goal: Pose to reach\
/// pose: Starting pose\
/// speed: Maximum translational speed\
/// rot_speed: Maximum rotational speed\
/// dt: Time differential\
fn compute_twist(goal: Frame, pose: Frame, speed: f64, rot_speed: f64, dt: f64) -> Twist {
    let delta = diff(goal, pose, dt);
    let translation = (delta[0].powi(2) + delta[1].powi(2) + delta[2].powi(2)).sqrt();
    let rotation = (delta[3].powi(2) + delta[4].powi(2) + delta[5].powi(2)).sqrt();

    let translation_scale = if translation == 0.0 {
        0.0
    } else if translation > speed {
        speed / translation
    } else {
        1.0
    };

    let rotation_scale = if rotation == 0.0 {
        0.0
    } else if rotation > rot_speed {
        rot_speed / rotation
    } else {
        1.0
    };

    Twist::new(
        delta[0] * translation_scale,
        delta[1] * translation_scale,
        delta[2] * translation_scale,
        delta[3] * rotation_scale,
        delta[4] * rotation_scale,
        delta[5] * rotation_scale,
    )
}

/// Implementation of a simple segment description
#[derive(Clone, Copy, Debug)]
pub struct SegmentDescription {
    /// Type of the described joint
    pub joint_type: JointType,
    /// Transform of the respective link
    pub joint_tf: Frame,
    /// Limits of the joint
    pub limits: (f64, f64),
    /// Speed limit of the joint
    pub speed_limit: f64,
}

impl SegmentDescription {
    /// Create a shoulder
    pub fn shoulder(tf: Frame) -> Self {
        Self {
            joint_type: JointType::NoJoint,
            joint_tf: tf,
            limits: (0.0, 0.0),
            speed_limit: 0.0,
        }
    }
}

impl PartialEq for SegmentDescription {
    fn eq(&self, other: &Self) -> bool {
        let threshold = 0.0000001;
        let type_eq = self.joint_type == other.joint_type;
        let limits_eq = self.limits == other.limits;
        let speed_eq = self.speed_limit == other.speed_limit;
        let similar_tf = get_frame_error(self.joint_tf, other.joint_tf) < threshold;
        similar_tf && type_eq && limits_eq && speed_eq
    }
}

/// Implementation of kinematic arms
///
/// These are the subset of kinematic chains that are used as robotic\
/// arms. With an improved API for ease of used compared to the one\
/// we have for kinematic chains. It is different from the chain because\
/// it has a shoulder segment (segment with no joint) at the beginning \
/// and then N mobile segments. Also has a base transform that represents\
/// the robotic body to which the arm is attached.
///
/// base_transform: Pose of the base of the shoulder\
/// chain: The kinematic chain that describes the arm\
/// forward_solver: Forward kinematics solver\
/// forward_diff_solver: Forward Differential kinematics solver\
/// inverse_diff_solver: Inverse differential kinematics solver\
/// joint_limits: Mechanical joint limits (radians)\
/// joint_speed_limits: max speed of each joint (radians/s)\
/// weights: the weight of each joint for inverse kinematics\
/// state: current state of each joint (radians)\
/// speed: current speed of each joint (radians/s)\
#[derive(Clone, Debug)]
pub struct KinematicArm {
    base_transform: Frame,
    base_twist: Twist,
    chain: Chain,
    forward_solver: ForwardKinematicsSolver,
    forward_diff_solver: ForwardDiffKinematicsSolver,
    inverse_diff_solver: InverseDiffKinematicsSolver,
    joint_limits: Vec<(f64, f64)>,
    joint_speed_limits: Vec<f64>,
    weights: Vec<f64>,
    normal_weights: Vec<f64>,
    min_weights: Vec<f64>,
    state: Vec<f64>,
    speed: Vec<f64>,
}

impl KinematicArm {
    /// Default empty kinematic arm
    pub fn default() -> Self {
        let shoulder = Segment::default();
        let mut in_chain = Chain::default();
        in_chain.add_segment(shoulder);
        Self {
            base_transform: Frame::identity(),
            base_twist: Twist::zeros(),
            chain: in_chain.clone(),
            forward_solver: ForwardKinematicsSolver::default(),
            forward_diff_solver: ForwardDiffKinematicsSolver::default(),
            inverse_diff_solver: InverseDiffKinematicsSolver::new(in_chain.clone()),
            joint_limits: Vec::new(),
            joint_speed_limits: Vec::new(),
            weights: Vec::new(),
            normal_weights: Vec::new(),
            min_weights: Vec::new(),
            state: Vec::new(),
            speed: Vec::new(),
        }
    }

    fn update_solvers(&mut self) {
        self.forward_solver = ForwardKinematicsSolver::new(self.chain.clone());
        self.forward_diff_solver = ForwardDiffKinematicsSolver::new(self.chain.clone());
        self.inverse_diff_solver = InverseDiffKinematicsSolver::new(self.chain.clone());
    }

    /// Get how many joints the kinematic arm has
    pub fn get_joint_num(&self) -> usize {
        self.chain.get_num_joints()
    }

    /// Get how many segments the kinematic arm has
    pub fn get_segment_num(&self) -> usize {
        // number of joints plus the shoulder
        self.joint_limits.len() + 1
    }
    /// Get the limits of each joint
    pub fn get_limits(&self) -> Vec<(f64, f64)> {
        self.joint_limits.clone()
    }

    /// Get the speed limits of each joint
    pub fn get_speed_limits(&self) -> Vec<f64> {
        self.joint_speed_limits.clone()
    }

    /// Set the base pose transformation.\
    /// `transform`: A Frame holding the base transform to set
    pub fn set_base_transform(&mut self, transform: Frame) {
        self.base_transform = transform;
    }

    /// Set the base velocity.\
    /// `twist`: the twist of the base to set
    pub fn set_base_twist(&mut self, twist: Twist) {
        self.base_twist = twist;
    }

    /// Add a shoulder to the kinematic arm. This will remove the current\
    /// chain and add a new one with this shoulder\
    /// `shoulder`: a segment holding the shoulder
    pub fn set_shoulder(&mut self, shoulder: Segment) {
        self.chain = Chain::default();
        self.chain.add_segment(shoulder);
        self.update_solvers();
    }

    /// Add a shoulder to the kinematic arm. This will remove the current\
    /// chain and add a new one with this shoulder.\
    /// `shoulder_tf`: Frame holding the shoulder transform
    pub fn set_shoulder_transform(&mut self, shoulder_tf: Frame) {
        let shoulder = Segment::new(Joint::default(), shoulder_tf, 0.0);
        self.set_shoulder(shoulder);
    }

    /// Add a new segment to the description of the kinematic arm.\
    /// It will be added to the end of the current chain\
    /// `segment`: The new segment to add\
    /// `limits`: mechanical limits of the joint (radians)\
    /// `speed_limits`: speed limit of the joint (radians/s)\
    pub fn add_segment(&mut self, new_segment: Segment, limits: (f64, f64), speed_limit: f64) {
        self.chain.add_segment(new_segment);
        self.joint_limits.push(limits);
        self.joint_speed_limits.push(speed_limit);
        self.update_solvers();
        if new_segment.get_joint_type() != JointType::NoJoint {
            self.state.push(0.0);
            self.speed.push(0.0);
            self.weights.push(1.0);
            self.normal_weights.push(1.0);
            self.min_weights.push(0.2);
        }
    }

    /// Add a new joint by Type and transform. See 'add_segment' for more info\
    /// `joint_type`: type of joint\
    /// `transform`: transform of the segment\
    /// `limits`: mechanical limits of the joint (radians)\
    /// `speed_limits`: speed limit of the joint (radians/s)\
    pub fn add_segment_by_type(
        &mut self,
        joint_type: JointType,
        transform: Frame,
        limits: (f64, f64),
        speed_limit: f64,
    ) {
        let joint = Joint::default().set_type(joint_type);
        let segment = Segment::new(joint, transform, 0.0);
        self.add_segment(segment, limits, speed_limit);
    }

    /// Set the weight ranges for each joint. The algorithm will use the highest\
    /// weight for each joint, but when it gets close to the limit, it will\
    /// switch to the minimum weight to avoid a collision (to the joint limit)\
    /// `weights`: Weight for normal operation of the arm\
    /// `min_weights`: Weight to use when the joint approaches its limit.
    pub fn set_joint_weight_ranges(&mut self, weights: Vec<f64>, min_weights: Vec<f64>) {
        if weights.len() != self.chain.get_num_joints() {
            panic!("The length of the weights does not match the amount of movable joints");
        }
        if min_weights.len() != weights.len() {
            panic!("The normal weights vector and the minimum weights vector must have the same length");
        }

        self.normal_weights = weights.clone();
        self.min_weights = min_weights.clone();
    }

    /// Set the weights of each joint. This tells the algorithm how much to move\
    /// each joint for IK. The weight is a floating value between 0 and 1, where\
    /// 1 means free movement and 0 means to stay put.\
    /// `weights`: The weight for each movable joint\
    pub fn set_joint_weights(&mut self, weights: Vec<f64>) {
        if weights.len() != self.chain.get_num_joints() {
            panic!("The length of the weights does not match the amount of movable joints");
        }
        self.weights = weights.clone();
        let num_joints = weights.len();
        let js_weights_size = Dyn(num_joints);
        let task_space_weights = WeightTaskSpace::identity();
        let joint_space_weights = WeightJointSpace::from_partial_diagonal_generic(
            js_weights_size,
            js_weights_size,
            &weights[..],
        );
        self.inverse_diff_solver
            .set_weights(task_space_weights, joint_space_weights);
    }

    /// Set how much to avoid singularities for inverse kinematics.\
    /// It is a scalar from 0 to 1 where\
    /// 0 means low avoidance and high accuracy and 1 means high avoidance\
    /// and low accuracy.\
    /// `lambda`: singularity avoidance\
    pub fn set_singularity_avoidance_ratio(&mut self, lambda: f64) {
        self.inverse_diff_solver.set_lambda(lambda);
    }

    /// Configure the behaviour of the inverse kinematics differential\
    /// solver. By changing the epsilon and maximum iterations parameters.\
    /// Lower epsilon means more accuracy but slower execution. Higher \
    /// maxiter means higher robustness but slower executions.\
    /// `epsilon`: IK SVD solution threshold
    /// `max_iter`: IK SVD solver maximum iterations
    pub fn set_ik_convergence_parameters(&mut self, epsilon: f64, max_iter: usize) {
        self.inverse_diff_solver.set_convergence(epsilon, max_iter);
    }

    /// Get the current joint speeds (radians/s)\
    pub fn get_joint_speeds(&mut self) -> Vec<f64> {
        self.speed.clone()
    }

    /// Get the current joint state (radians)\
    pub fn get_joint_states(&mut self) -> Vec<f64> {
        self.state.clone()
    }

    /// Set the speeds of each joint.\
    /// `joint_speeds`: vector with the speed to set (radians/s). Must be the same\
    /// length as movable joints in the arm\
    pub fn set_joint_speeds(&mut self, joint_speeds: Vec<f64>) {
        if joint_speeds.len() != self.chain.get_num_joints() {
            panic!("The length of the speeds does not match the amount of movable joints");
        }
        for (idx, speed) in joint_speeds.iter().enumerate() {
            self.speed[idx] = if speed.abs() > self.joint_speed_limits[idx] {
                self.joint_speed_limits[idx] * speed.signum()
            } else {
                *speed
            };
        }
    }

    /// Set the state of each joint. If the joint approaches a limit, it will
    /// also handle the weights for the inverse kinematic solver.\
    /// `joint_states`: vector with the state to set (radians). Must be the same
    /// length as movable joints in the arm\
    pub fn set_joint_states(&mut self, joint_states: Vec<f64>) {
        if joint_states.len() != self.chain.get_num_joints() {
            panic!("The length of the states does not match the amount of movable joints");
        }
        for idx in 0..joint_states.len() {
            self.state[idx] = if joint_states[idx] < self.joint_limits[idx].0 {
                // Reached the lower limit
                self.weights[idx] = 0.0;
                self.joint_limits[idx].0
            } else if (joint_states[idx] - self.joint_limits[idx].0).abs() < 5.0 * PI / 180.0 {
                // Close to low limit
                self.weights[idx] = self.min_weights[idx];
                joint_states[idx]
            } else if joint_states[idx] > self.joint_limits[idx].1 {
                // Reached higher limit
                self.weights[idx] = 0.0;
                self.joint_limits[idx].1
            } else if (joint_states[idx] - self.joint_limits[idx].1).abs() < 5.0 * PI / 180.0 {
                // Close to high limit
                self.weights[idx] = self.min_weights[idx];
                joint_states[idx]
            } else {
                self.weights[idx] = self.normal_weights[idx];
                joint_states[idx]
            };
        }
        self.set_joint_weights(self.weights.clone());
    }

    /// Get the end effector pose. Takes into consideration the pose of
    /// the base of the arm.
    pub fn get_global_pose(&mut self) -> Frame {
        let local_pose = self.forward_solver.solve(&self.state);
        self.base_transform * local_pose
    }

    /// Get the end effector twist. Takes into consideration the pose and
    /// twist of the base of the arm
    pub fn get_global_twist(&mut self) -> Twist {
        let local_twist = self.forward_diff_solver.solve(&self.state, &self.speed);
        let local_pose = self.forward_solver.solve(&self.state);
        reference_frame(
            self.base_twist,
            local_twist,
            self.base_transform,
            local_pose,
        )
    }

    /// Get the end effector local pose, from the shoulder to the end
    /// effector.
    pub fn get_pose(&mut self) -> Frame {
        self.forward_solver.solve(&self.state)
    }

    /// Get the end effector local twist, from the shoulder to the
    // end effector.
    pub fn get_twist(&mut self) -> Twist {
        self.forward_diff_solver.solve(&self.state, &self.speed)
    }

    /// Move the base.\
    pub fn move_base(&mut self, base_twist: Twist, dt: f64) {
        self.set_base_twist(base_twist);
        self.set_base_transform(add_delta(self.base_transform, base_twist, dt));
    }

    /// Move the end effector a differential\
    /// `twist`: Required or commanded Twist\
    /// `dt`: time differential\
    pub fn cartesian_diff_move(&mut self, twist: Twist, dt: f64) {
        let result = self.inverse_diff_solver.solve(&twist, &self.state);
        let solution = if result.is_err() {
            // If there is singularity with the inverse kinematics, the arm
            // will continue to move as it was moving, to try come out of
            // singularity.
            self.speed.clone()
        } else {
            result.unwrap()
        };
        self.set_joint_speeds(solution);
        let mut new_state = Vec::new();
        for idx in 0..self.state.len() {
            new_state.push(self.state[idx] + self.speed[idx] * dt);
        }
        self.set_joint_states(new_state);
    }

    /// Move to the goal pose in a straight line from the current pose
    /// to the goal.\
    ///
    /// `goal`: the goal pose\
    /// `speed`: the speed at which the end effector should move\
    /// `rot_speed`: the speed at which the end effector should rotate\
    /// `dt`: time differential\
    /// `timeout`: time after which the goal is unreachable\
    /// `translation_tolerance`: allowed translational error\
    /// `rotation_tolerance`: allowed rotational error\
    /// returns final state if reachable\
    pub fn cartesian_move(
        &mut self,
        goal: Frame,
        speed: f64,
        rot_speed: f64,
        dt: f64,
        timeout: f64,
        translation_tolerance: f64,
        rotation_tolerance: f64,
    ) -> Result<Vec<f64>, &str> {
        let mut spent_time = 0.0;
        while spent_time < timeout
            && !is_frame_error_acceptable(
                goal,
                self.get_pose(),
                translation_tolerance,
                rotation_tolerance,
            )
        {
            let twist = compute_twist(goal, self.get_pose(), speed, rot_speed, dt);
            self.cartesian_diff_move(twist, dt);
            spent_time += dt;
        }

        if spent_time < timeout {
            Ok(self.get_joint_states())
        } else {
            Err("Goal is unreachable")
        }
    }

    /// Build the arm from a kinematic description.\
    pub fn build_from_description(description: Vec<SegmentDescription>) -> Self {
        if description[0].joint_type != JointType::NoJoint {
            panic!("The first joint of the arm must be a shoulder (JointType::NoJoint)");
        }
        let mut arm = Self::default();
        let mut has_shoulder = false;
        for segment in description {
            if segment.joint_type == JointType::NoJoint && !has_shoulder {
                // it is a shoulder
                arm.set_shoulder_transform(segment.joint_tf);
                has_shoulder = true;
            } else {
                arm.add_segment_by_type(
                    segment.joint_type,
                    segment.joint_tf,
                    segment.limits,
                    segment.speed_limit,
                );
            }
        }
        arm
    }

    /// Return the description of the current arm
    pub fn get_description(&self) -> Vec<SegmentDescription> {
        let mut description = Vec::<SegmentDescription>::new();
        for idx in 0..self.chain.get_num_segments() {
            let current_segment = self.chain.get_segment(idx);
            let tf = current_segment.get_link_transform();
            let joint_type = current_segment.get_joint_type();
            let limits = if joint_type == JointType::NoJoint {
                ((0.0, 0.0), 0.0)
            } else {
                // the chain has only one shoulder, and the current segment
                // is not a shoulder.
                (self.joint_limits[idx - 1], self.joint_speed_limits[idx - 1])
            };
            description.push(SegmentDescription {
                joint_type: joint_type,
                joint_tf: tf,
                limits: limits.0,
                speed_limit: limits.1,
            });
        }
        description
    }
}

impl PartialEq for KinematicArm {
    fn eq(&self, other: &Self) -> bool {
        let limits_eq = self.joint_limits == other.joint_limits;
        let speeds_eq = self.joint_speed_limits == other.joint_speed_limits;
        let chains_eq = self.chain == other.chain;
        chains_eq && speeds_eq && limits_eq
    }
}

#[cfg(test)]
///Testing module for arms
pub mod tests {
    use crate::geometry::{EulerBuild, Frame};
    use crate::joint::JointType;
    use crate::kinematic_arm::KinematicArm;
    use crate::kinematic_arm::SegmentDescription as Desc;
    use rand::prelude::*;
    use std::f64::consts::PI;

    fn create_kuka_description() -> Vec<Desc> {
        vec![
            Desc::shoulder(Frame::from_translation_euler(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)),
            Desc {
                // segment 1
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.2, -PI / 2., 0.0, 0.0),
                limits: (-169.5 * PI / 180., 169.5 * PI / 180.),
                speed_limit: 110. * PI / 180.,
            },
            Desc {
                // segment 2
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, -0.2, 0.0, PI / 2., 0.0, 0.0),
                limits: (-119.5 * PI / 180., 119.5 * PI / 180.0),
                speed_limit: 110.0 * PI / 180.0,
            },
            Desc {
                // segment 3
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.2, PI / 2.0, 0.0, 0.0),
                limits: (-169.5 * PI / 180.0, 169.5 * PI / 180.0),
                speed_limit: 128.0 * PI / 180.0,
            },
            Desc {
                //segment 4
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.2, 0.0, -PI / 2.0, 0.0, 0.0),
                limits: (-119.5 * PI / 180.0, 119.0 * PI / 180.0),
                speed_limit: 128.0 * PI / 180.0,
            },
            Desc {
                //segment 5
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.19, -PI / 2., 0.0, 0.0),
                limits: (-169.5 * PI * 180.0, 169.5 * PI / 180.0),
                speed_limit: 204.0 * PI / 180.0,
            },
            Desc {
                //segment 6
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, -0.078, 0.0, PI / 2.0, 0.0, 0.0),
                limits: (-119.5 * PI / 180.0, 119.5 * PI / 180.0),
                speed_limit: 184.0 * PI / 180.0,
            },
            Desc {
                //segment 7
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.1, 0.0, 0.0, 0.0),
                limits: (-169.5 * PI / 180.0, 169.5 * PI / 180.0),
                speed_limit: 184.5 * PI / 180.0,
            },
        ]
    }

    // This is the same Kuka robot but one of the Joints
    // Has been substituted with a NoJoint
    fn create_defective_kuka_description() -> Vec<Desc> {
        vec![
            Desc::shoulder(Frame::from_translation_euler(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)),
            Desc {
                // segment 1
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.2, -PI / 2., 0.0, 0.0),
                limits: (-169.5 * PI / 180., 169.5 * PI / 180.),
                speed_limit: 110. * PI / 180.,
            },
            Desc {
                // segment 2
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, -0.2, 0.0, PI / 2., 0.0, 0.0),
                limits: (-119.5 * PI / 180., 119.5 * PI / 180.0),
                speed_limit: 110.0 * PI / 180.0,
            },
            Desc {
                // segment 3
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.2, PI / 2.0, 0.0, 0.0),
                limits: (-169.5 * PI / 180.0, 169.5 * PI / 180.0),
                speed_limit: 128.0 * PI / 180.0,
            },
            Desc {
                //segment 4
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.2, 0.0, -PI / 2.0, 0.0, 0.0),
                limits: (-119.5 * PI / 180.0, 119.0 * PI / 180.0),
                speed_limit: 128.0 * PI / 180.0,
            },
            Desc {
                //segment 5
                joint_type: JointType::NoJoint,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.19, -PI / 2., 0.0, 0.0),
                limits: (-169.5 * PI * 180.0, 169.5 * PI / 180.0),
                speed_limit: 204.0 * PI / 180.0,
            },
            Desc {
                //segment 6
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, -0.078, 0.0, PI / 2.0, 0.0, 0.0),
                limits: (-119.5 * PI / 180.0, 119.5 * PI / 180.0),
                speed_limit: 184.0 * PI / 180.0,
            },
            Desc {
                //segment 7
                joint_type: JointType::RotZ,
                joint_tf: Frame::from_translation_euler(0.0, 0.0, 0.1, 0.0, 0.0, 0.0),
                limits: (-169.5 * PI / 180.0, 169.5 * PI / 180.0),
                speed_limit: 184.5 * PI / 180.0,
            },
        ]
    }

    #[test]
    fn move_the_arm() {
        let goal = Frame::from_translation_euler(0.5, 0.0, 0.5, 0.0, 0.0, 0.0);
        let kuka_description = create_kuka_description();
        let mut testing_arm = KinematicArm::build_from_description(kuka_description);

        let result = testing_arm.cartesian_move(
            goal,
            0.5,
            15.0 * PI / 180.0,
            1.0 / 60.0,
            10.0,
            0.05,
            5.0 * PI / 180.0,
        );
        assert!(result.is_ok());
    }

    // Try to reach a pose from different initial poses, if all fail, then
    // fail.
    fn reach_pose_with_retry(
        mut arm: KinematicArm,
        goal: Frame,
        speed: f64,
        rot_speed: f64,
        dt: f64,
        timeout: f64,
        tolerance: f64,
        rot_tolerance: f64,
        retries: u32,
    ) -> bool {
        let mut rng = rand::rng();
        let mut joints = Vec::<f64>::new();
        let mut fails = 0;
        for _retry in 0..retries {
            joints.clear();
            for _j in 0..7 {
                joints.push(-PI / 2.0 + rng.random::<f64>() * PI);
            }
            arm.set_joint_states(joints.clone());
            let result = arm.cartesian_move(
                goal,
                speed,
                rot_speed,
                dt,
                timeout,
                tolerance,
                rot_tolerance,
            );
            if result.is_err() {
                fails = fails + 1;
            }
        }
        if fails == retries {
            false
        } else {
            true
        }
    }

    #[test]
    fn move_to_many_reachable_goals() {
        let mut testing_arm = KinematicArm::build_from_description(create_kuka_description());
        let mut generator_arm = KinematicArm::build_from_description(create_kuka_description());
        testing_arm.set_ik_convergence_parameters(1e-200, 100);
        let mut rng = rand::rng();
        let mut joints = Vec::<f64>::new();
        for _i in 0..10 {
            // compute random joints for goal
            joints.clear();
            for _j in 0..7 {
                joints.push(-PI / 2.0 + rng.random::<f64>() * PI);
            }
            generator_arm.set_joint_states(joints.clone());
            // We know that this goal is reachable because the arm was just there
            let goal = generator_arm.get_pose();

            let reached = reach_pose_with_retry(
                testing_arm.clone(),
                goal,
                0.5,                // speed
                150.0 * PI / 180.0, // Rot Speed
                1.0 / 60.0,         // dt
                10.0,               // timeout
                0.5,                // tolerance
                5.0 * PI / 180.0,   // rot tolerance
                10,                 // retries
            );

            assert!(reached);
        }
    }

    #[test]
    fn move_to_unreachable() {
        let goal = Frame::from_translation_euler(3.0, 0.0, 3.0, 0.0, 0.0, 0.0);
        let kuka_description = create_kuka_description();
        let mut testing_arm = KinematicArm::build_from_description(kuka_description);
        let result = testing_arm.cartesian_move(
            goal,
            0.5,
            150.0 * PI / 180.0,
            1.0 / 60.0,
            10.0,
            0.05,
            5.0 * PI / 180.0,
        );
        assert!(result.is_err());
        testing_arm.set_joint_weight_ranges(
            std::vec![1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0],
            std::vec![0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
        );
        // Retry with more permissive weights
        let result = testing_arm.cartesian_move(
            goal,
            0.5,
            150.0 * PI / 180.0,
            1.0 / 60.0,
            10.0,
            0.05,
            5.0 * PI / 180.0,
        );
        assert!(result.is_err());
    }

    #[test]
    fn handle_descriptions() {
        let kuka_description = create_kuka_description();
        let testing_arm = KinematicArm::build_from_description(kuka_description.clone());
        let generated_description = testing_arm.get_description();
        assert_eq!(generated_description, kuka_description);
    }

    #[test]
    fn middle_unmovable_joints() {
        let goal = Frame::from_translation_euler(0.5, 0.0, 0.5, 0.0, 0.0, 0.0);
        let defective_kuka_description = create_defective_kuka_description();
        let mut testing_arm = KinematicArm::build_from_description(defective_kuka_description);

        let result = testing_arm.cartesian_move(
            goal,
            0.5,
            15.0 * PI / 180.0,
            1.0 / 60.0,
            10.0,
            0.05,
            5.0 * PI / 180.0,
        );
        assert!(result.is_ok());
        // Check that this is not panicking
        testing_arm.set_joint_states(std::vec![0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
    }
}