diff --git a/.idea/rs-opw-kinematics.iml b/.idea/rs-opw-kinematics.iml index c254557..7025ac1 100644 --- a/.idea/rs-opw-kinematics.iml +++ b/.idea/rs-opw-kinematics.iml @@ -2,6 +2,7 @@ + diff --git a/Cargo.lock b/Cargo.lock index fa4b4bf..a609beb 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -404,7 +404,7 @@ checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rs-opw-kinematics" -version = "1.2.1" +version = "1.3.0-rc" dependencies = [ "clap", "nalgebra", diff --git a/Cargo.toml b/Cargo.toml index a2aa39f..62d2aae 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.2.1" +version = "1.3.0" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." @@ -29,7 +29,7 @@ default = ["allow_filesystem"] allow_filesystem = ["yaml-rust2", "sxd-document", "regex", "clap"] # To disable filesystem: -#rs-opw-kinematics = { version = "1.2.1", default-features = false } +#rs-opw-kinematics = { version = "1.3.0", default-features = false } [dev-dependencies] rand = "0.8.5" diff --git a/README.md b/README.md index ce2fa45..2fd1e21 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,9 @@ This documentation also incorporates the robot diagram from that project. - For kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 values close to the previous positions of these joints (and not arbitrary that may result in a large jerk of the real robot) +- Jacobian, torgues and velocities +- The robot can be equipped with the tool and placed on the base, planning for the desired location and orientation + of the tool center point (TCP) rather than any part of the robot. - Experimental support for parameter extraction from URDF. The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for the two @@ -77,20 +80,44 @@ if _from_ = 5° and _to_ = 15°, values 6°, 8°, and 11° are va Constraints are tested for the range from -2π to 2π, but as angles repeat with period of 2π, the constraint from -π to π already permits free rotation, covering any angle. +# Jacobian: torgues and velocities +Since 1.3.0, it is possible to obtain the Jacobian that represents the relationship between the joint velocities +and the end-effector velocities. The computed Jacobian object provides: +- Joint velocities required to achieve a desired end-effector velocity. +- Joint torques required to achieve a desired end-effector force/torque. + +The same Joints structure is reused, the six values now representing either angular velocities in radians per second +or torgues in Newton meters. For the end effector, it is possible to use either nalgebra Isometry3 or Vector6, +both defining velocities in m/s or rotations in N m. + +These values are useful when path planning for a robot that needs to move very swiftly, to prevent +overspeed or overtorgue of individual joints. + +# The tool and the base +Since 1.3.0, robot can be equipped with the tool, defined as Isometry3. The tool isometry defines both +additional translation and additional rotation. The "pose" as defined in forward and inverse kinematics +now becomes the pose of the tool center point, not any part of the robot. The robot can also be placed +on a base, further supporting the conditions much closer to the real industrial environment. + +"Robot with the tool" and "Robot on the base" can be constructed around any Kinematics trait, and implement the +Kinematics trait themselves. It is possible to cascade them. + # Example Cargo.toml: ```toml [dependencies] -rs-opw-kinematics = ">=1.1.1, <2.0.0" +rs-opw-kinematics = ">=1.3.0, <2.0.0" ``` main.rs: ```Rust use std::f64::consts::PI; -use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, +use std::sync::Arc; +use nalgebra::{Isometry3, Translation3, UnitQuaternion, Vector3}; +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO, CONSTRAINT_CENTERED}; use rs_opw_kinematics::kinematics_impl::OPWKinematics; use rs_opw_kinematics::parameters::opw_kinematics::Parameters; @@ -98,57 +125,108 @@ use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; fn main() { - let robot = OPWKinematics::new(Parameters::irb2400_10()); - let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] - println!("Initial joints with singularity J5 = 0: "); - dump_joints(&joints); + let robot = OPWKinematics::new(Parameters::irb2400_10()); + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + println!("Initial joints with singularity J5 = 0: "); + dump_joints(&joints); - println!("Solutions (original angle set is lacking due singularity there: "); - let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 + println!("Solutions (original angle set is lacking due singularity there: "); + let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 - let solutions = robot.inverse(&pose); // Solutions is alias of Vec - dump_solutions(&solutions); + let solutions = robot.inverse(&pose); // Solutions is alias of Vec + dump_solutions(&solutions); - println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); - let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from); - dump_solutions(&solutions); + println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); + let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from); + dump_solutions(&solutions); - println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); - let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); + println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); - println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0 \ + println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0 \ The solution appears and the needed rotation is now equally distributed between J4 and J6."); - let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); - dump_solutions(&solutions); + let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); + dump_solutions(&solutions); - let robot = OPWKinematics::new_with_constraints( - Parameters::irb2400_10(), Constraints::new( - [-0.1, 0.0, 0.0, 0.0, -PI, -PI], - [ PI, PI, 2.0*PI, PI, PI, PI], - BY_PREV, - )); + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_PREV, + )); - println!("If we do not have the previous pose yet, we can now ask to prefer the pose \ + println!("If we do not have the previous pose yet, we can now ask to prever the pose \ closer to the center of constraints."); - let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED); - dump_solutions(&solutions); - - println!("With constraints, sorted by proximity to the previous pose"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); - - let robot = OPWKinematics::new_with_constraints( - Parameters::irb2400_10(), Constraints::new( - [-0.1, 0.0, 0.0, 0.0, -PI, -PI], - [ PI, PI, 2.0*PI, PI, PI, PI], - BY_CONSTRAINS, - )); - println!("With constraints, sorted by proximity to the center of constraints"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); + let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED); + dump_solutions(&solutions); + + + println!("With constraints, sorted by proximity to the previous pose"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_CONSTRAINS, + )); + println!("With constraints, sorted by proximity to the center of constraints"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + // This requires YAML library + let parameters = Parameters::irb2400_10(); + println!("Reading:\n{}", ¶meters.to_yaml()); + + // Jacobian, velocities and forces: + let jakobian = rs_opw_kinematics::jakobian::Jacobian::new(&robot, &joints, 1E-6); + let desired_velocity_isometry = + Isometry3::new(Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0)); + let joint_velocities = jakobian.velocities(&desired_velocity_isometry); + println!("Computed joint velocities: {:?}", joint_velocities.unwrap()); + + let desired_force_torque = + Isometry3::new(Vector3::new(0.0, 0.0, 0.0), + Vector3::new(0.0, 0.0, 1.234)); + + let joint_torques = jakobian.torques(&desired_force_torque); + println!("Computed joint torques: {:?}", joint_torques); + + // Robot with the tool, standing on a base: + let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + // 1 meter high pedestal + let pedestal = 0.5; + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, pedestal).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = rs_opw_kinematics::tool::Base { + robot: Arc::new(robot_alone), + base: base_translation, + }; + + // Tool extends 1 meter in the Z direction, envisioning something like sword + let sword = 1.0; + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, sword).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_complete = rs_opw_kinematics::tool::Tool { + robot: Arc::new(robot_with_base), + tool: tool_translation, + }; + + let tcp_pose: Pose = robot_complete.forward(&joints); + println!("The sword tip is at: {:?}", tcp_pose); } ``` @@ -191,7 +269,7 @@ values. In general, always test in simulator before feeding the output of any so For security and performance, some users prefer smaller libraries with less dependencies. If YAML and URDF readers are not in use, the filesystem access can be completely disabled in your Cargo.toml, importing the library like: -rs-opw-kinematics = { version = ">=1.2.1, <2.0.0", default-features = false } +rs-opw-kinematics = { version = ">=1.3.0, <2.0.0", default-features = false } In this case, import of URDF and YAML files will be unaccessible, and used dependencies will be limited to the single _nalgebra_ crate. diff --git a/examples/basic.rs b/examples/basic.rs new file mode 100644 index 0000000..6b8c3bf --- /dev/null +++ b/examples/basic.rs @@ -0,0 +1,32 @@ +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO}; +use rs_opw_kinematics::kinematics_impl::OPWKinematics; +use rs_opw_kinematics::parameters::opw_kinematics::Parameters; +use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; + +fn main() { + let robot = OPWKinematics::new(Parameters::irb2400_10()); + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + println!("Initial joints with singularity J5 = 0: "); + dump_joints(&joints); + + println!("Solutions (original angle set is lacking due singularity there: "); + let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 + + let solutions = robot.inverse(&pose); // Solutions is alias of Vec + dump_solutions(&solutions); + + println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); + let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from); + dump_solutions(&solutions); + + println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0 \ + The solution appears and the needed rotation is now equally distributed between J4 and J6."); + let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); + dump_solutions(&solutions); +} \ No newline at end of file diff --git a/examples/constraints.rs b/examples/constraints.rs new file mode 100644 index 0000000..4c9d768 --- /dev/null +++ b/examples/constraints.rs @@ -0,0 +1,39 @@ +use std::f64::consts::PI; +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, CONSTRAINT_CENTERED}; +use rs_opw_kinematics::kinematics_impl::OPWKinematics; +use rs_opw_kinematics::parameters::opw_kinematics::Parameters; +use rs_opw_kinematics::utils::{dump_solutions}; +use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; + +fn main() { + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_PREV, + )); + + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + + let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 + + println!("If we do not have the previous pose yet, we can now ask to prever the pose \ + closer to the center of constraints."); + let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED); + dump_solutions(&solutions); + + println!("With constraints, sorted by proximity to the previous pose"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_CONSTRAINS, + )); + println!("With constraints, sorted by proximity to the center of constraints"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); +} \ No newline at end of file diff --git a/examples/jacobian.rs b/examples/jacobian.rs new file mode 100644 index 0000000..70e5633 --- /dev/null +++ b/examples/jacobian.rs @@ -0,0 +1,62 @@ +use std::f64::consts::PI; +use std::sync::Arc; +use nalgebra::{Isometry3, Translation3, UnitQuaternion, Vector3}; +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose}; +use rs_opw_kinematics::kinematics_impl::OPWKinematics; +use rs_opw_kinematics::parameters::opw_kinematics::Parameters; +use rs_opw_kinematics::constraints::{BY_CONSTRAINS, Constraints}; + +fn main() { + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_CONSTRAINS, + )); + + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + let jakobian = rs_opw_kinematics::jacobian::Jacobian::new(&robot, &joints, 1E-6); + let desired_velocity_isometry = + Isometry3::new(Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0)); + let joint_velocities = jakobian.velocities(&desired_velocity_isometry); + println!("Computed joint velocities: {:?}", joint_velocities.unwrap()); + + let desired_force_torque = + Isometry3::new(Vector3::new(0.0, 0.0, 0.0), + Vector3::new(0.0, 0.0, 1.234)); + + let joint_torques = jakobian.torques(&desired_force_torque); + println!("Computed joint torques: {:?}", joint_torques); + + // Robot with the tool, standing on a base: + let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + // 1 meter high pedestal + let pedestal = 0.5; + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, pedestal).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = rs_opw_kinematics::tool::Base { + robot: Arc::new(robot_alone), + base: base_translation, + }; + + // Tool extends 1 meter in the Z direction, envisioning something like sword + let sword = 1.0; + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, sword).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_complete = rs_opw_kinematics::tool::Tool { + robot: Arc::new(robot_with_base), + tool: tool_translation, + }; + + let tcp_pose: Pose = robot_complete.forward(&joints); + println!("The sword tip is at: {:?}", tcp_pose); +} \ No newline at end of file diff --git a/examples/tool_and_base.rs b/examples/tool_and_base.rs new file mode 100644 index 0000000..5413710 --- /dev/null +++ b/examples/tool_and_base.rs @@ -0,0 +1,47 @@ +use std::sync::Arc; +use nalgebra::{Isometry3, Translation3, UnitQuaternion}; +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose}; +use rs_opw_kinematics::kinematics_impl::OPWKinematics; +use rs_opw_kinematics::parameters::opw_kinematics::Parameters; +use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; + +fn main() { + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5]; // Joints are alias of [f64; 6] + dump_joints(&joints); + + // Robot with the tool, standing on a base: + let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + // 1 meter high pedestal + let pedestal = 0.5; + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, pedestal).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = rs_opw_kinematics::tool::Base { + robot: Arc::new(robot_alone), + base: base_translation, + }; + + // Tool extends 1 meter in the Z direction, envisioning something like sword + let sword = 1.0; + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, sword).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_on_base_with_tool = rs_opw_kinematics::tool::Tool { + robot: Arc::new(robot_with_base), + tool: tool_translation, + }; + + let tcp_pose: Pose = robot_on_base_with_tool.forward(&joints); + println!("The sword tip is at: {:?}", tcp_pose); + + // robot_complete implements Kinematics so have the usual inverse kinematics methods available. + let inverse = robot_on_base_with_tool.inverse_continuing(&tcp_pose, &joints); + dump_solutions(&inverse); + +} \ No newline at end of file diff --git a/src/constraints.rs b/src/constraints.rs index 3b0a376..1301ebf 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -5,7 +5,7 @@ use std::f64::INFINITY; use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; use crate::utils::deg; -#[derive(Clone, Debug)] +#[derive(Clone, Debug, Copy)] pub struct Constraints { /// Normalized lower limit. If more than upper limit, the range wraps-around through 0 pub from: [f64; 6], diff --git a/src/jacobian.rs b/src/jacobian.rs new file mode 100644 index 0000000..7db2bc2 --- /dev/null +++ b/src/jacobian.rs @@ -0,0 +1,402 @@ +//! This package contains support for Jacobian matrix. +//! +//! Jacobian matrix, as understood here, represents the relationship between the joint velocities +//! and the end-effector velocities: +//! +//! | ∂vx/∂θ1 ∂vx/∂θ2 ∂vx/∂θ3 ∂vx/∂θ4 ∂vx/∂θ5 ∂vx/∂θ6 | +//! | ∂vy/∂θ1 ∂vy/∂θ2 ∂vy/∂θ3 ∂vy/∂θ4 ∂vy/∂θ5 ∂vy/∂θ6 | +//! | ∂vz/∂θ1 ∂vz/∂θ2 ∂vz/∂θ3 ∂vz/∂θ4 ∂vz/∂θ5 ∂vz/∂θ6 | +//! | ∂ωx/∂θ1 ∂ωx/∂θ2 ∂ωx/∂θ3 ∂ωx/∂θ4 ∂ωx/∂θ5 ∂ωx/∂θ6 | +//! | ∂ωy/∂θ1 ∂ωy/∂θ2 ∂ωy/∂θ3 ∂ωy/∂θ4 ∂ωy/∂θ5 ∂ωy/∂θ6 | +//! | ∂ωz/∂θ1 ∂ωz/∂θ2 ∂ωz/∂θ3 ∂ωz/∂θ4 ∂ωz/∂θ5 ∂ωz/∂θ6 | +//! +//! The first three rows to the linear velocities: vx, vy, vz. +//! The last three rows correspond to the angular velocities: roll wx, pitch wy, yaw wz). +//! θ1, θ2, θ3, θ4, θ5, θ6 are the joint angles +//! ∂ means partial derivative. + +extern crate nalgebra as na; + +use na::{Matrix6, Vector6, Isometry3}; +use na::linalg::SVD; +use crate::kinematic_traits::{Joints, Kinematics}; +use crate::utils::vector6_to_joints; + +/// Struct representing the Jacobian matrix +pub struct Jacobian { + /// A 6x6 matrix representing the Jacobian + /// + /// The Jacobian matrix maps the joint velocities to the end-effector velocities. + /// Each column corresponds to a joint, and each row corresponds to a degree of freedom + /// of the end-effector (linear and angular velocities). + matrix: Matrix6, + + /// The disturbance value used for computing the Jacobian + epsilon: f64, +} + +impl Jacobian { + /// Constructs a new Jacobian struct by computing the Jacobian matrix for the given robot and joint configuration + /// + /// # Arguments + /// + /// * `robot` - A reference to the robot implementing the Kinematics trait + /// * `qs` - A reference to the joint configuration + /// * `epsilon` - A small value used for numerical differentiation + /// + /// # Returns + /// + /// A new instance of `Jacobian` + pub fn new(robot: &impl Kinematics, qs: &Joints, epsilon: f64) -> Self { + let matrix = compute_jacobian(robot, qs, epsilon); + Self { matrix, epsilon } + } + + /// Computes the joint velocities required to achieve a desired end-effector velocity: + /// + /// Q' = J⁻¹ x' + /// + /// where Q' are joint velocities, J⁻¹ is the inverted Jacobian matrix and x' is the vector + /// of velocities of the tool center point. First 3 components are velocities along x,y and z + /// axis, the other 3 are angular rotation velocities around x (roll), y (pitch) and z (yaw) axis + /// + /// # Arguments + /// + /// * `desired_end_effector_velocity` - An Isometry3 representing the desired linear and + /// angular velocity of the end-effector. The x' vector is extracted from the isometry. + /// + /// # Returns + /// + /// `Result` - Joint positions, with values representing joint velocities rather than angles, + /// or an error message if the computation fails. + /// + /// This method extracts the linear and angular velocities from the provided Isometry3 + /// and combines them into a single 6D vector. It then computes the joint velocities required + /// to achieve the desired end-effector velocity using the `velocities_from_vector` method. + pub fn velocities(&self, desired_end_effector_velocity: &Isometry3) -> Result { + // Extract the linear velocity (translation) and angular velocity (rotation) + let linear_velocity = desired_end_effector_velocity.translation.vector; + let angular_velocity = desired_end_effector_velocity.rotation.scaled_axis(); + + // Combine into a single 6D vector + let desired_velocity = Vector6::new( + linear_velocity.x, linear_velocity.y, linear_velocity.z, + angular_velocity.x, angular_velocity.y, angular_velocity.z, + ); + + // Compute the joint velocities from the 6D vector + self.velocities_from_vector(&desired_velocity) + } + + /// Computes the joint velocities required to achieve a desired end-effector velocity: + /// + /// Q' = J⁻¹ x' + /// + /// where Q' are joint velocities, J⁻¹ is the inverted Jacobian matrix and x' is the vector + /// of velocities of the tool center point. First 3 components are velocities along x,y and z + /// axis. The remaining 3 are angular rotation velocities are assumed to be zero. + /// + /// # Arguments + /// + /// * `vx, vy, vz` - x, y and z components of end effector velocity (linear). + /// + /// # Returns + /// + /// `Result` - Joint positions, with values representing + /// joint velocities rather than angles or an error message if the computation fails. + pub fn velocities_fixed(&self, vx: f64, vy: f64, vz: f64) -> Result { + + // Combine into a single 6D vector with 0 rotational part + let desired_velocity = Vector6::new( + vx, vy, vz, + 0.0, 0.0, 0.0, + ); + + // Compute the joint velocities from the 6D vector + self.velocities_from_vector(&desired_velocity) + } + + /// Computes the joint velocities required to achieve a desired end-effector velocity: + /// + /// Q' = J⁻¹ X' + /// + /// where Q' are joint velocities, J⁻¹ is the inverted Jacobian matrix and x' is the vector + /// of velocities of the tool center point. First 3 components are velocities along x,y and z + /// axis, the other 3 are angular rotation velocities around x (roll), y (pitch) and z (yaw) axis + /// + /// # Arguments + /// + /// * `X'` - A 6D vector representing the desired linear and angular velocity of the + /// end-effector as defined above. + /// + /// # Returns + /// + /// `Result` - Joint positions, with values representing joint velocities rather than angles, + /// or an error message if the computation fails. + /// + /// This method tries to compute the joint velocities using the inverse of the Jacobian matrix. + /// If the Jacobian matrix is not invertible, it falls back to using the pseudoinverse. + #[allow(non_snake_case)] // Standard Math notation calls for single uppercase name + pub fn velocities_from_vector(&self, X: &Vector6) -> Result { + // Try to calculate the joint velocities using the inverse of the Jacobian matrix + let joint_velocities: Vector6; + if let Some(jacobian_inverse) = self.matrix.try_inverse() { + joint_velocities = jacobian_inverse * X; + } else { + // If the inverse does not exist, use the pseudoinverse + let svd = SVD::new(self.matrix.clone(), true, true); + match svd.pseudo_inverse(self.epsilon) { + Ok(jacobian_pseudoinverse) => { + joint_velocities = jacobian_pseudoinverse * X; + } + Err(_) => { + return Err("Unable to compute the pseudoinverse of the Jacobian matrix"); + } + } + } + // Convert the resulting Vector6 to Joints + Ok(vector6_to_joints(joint_velocities)) + } + + /// Computes the joint torques required to achieve a desired end-effector force/torque + /// This function computes + /// + /// t = JᵀF + /// + /// where Jᵀ is transposed Jacobian as defined above and f is the desired force vector that + /// is extracted from the passed Isometry3. + /// + /// # Arguments + /// + /// * `desired_force_torque` - isometry structure representing forces (in Newtons, N) and torgues + /// (in Newton - meters, Nm) rather than dimensions and angles. + /// + /// # Returns + /// + /// Joint positions, with values representing joint torques, + /// or an error message if the computation fails. + pub fn torques(&self, desired_force_isometry: &Isometry3) -> Joints { + + // Extract the linear velocity (translation) and angular velocity (rotation) + let linear_force = desired_force_isometry.translation.vector; + let angular_torgue = desired_force_isometry.rotation.scaled_axis(); + + // Combine into a single 6D vector + let desired_force_torgue_vector = Vector6::new( + linear_force.x, linear_force.y, linear_force.z, + angular_torgue.x, angular_torgue.y, angular_torgue.z, + ); + + let joint_torques = self.matrix.transpose() * desired_force_torgue_vector; + vector6_to_joints(joint_torques) + } + + /// Computes the joint torques required to achieve a desired end-effector force/torque + /// This function computes + /// + /// t = JᵀF + /// + /// where Jᵀ is transposed Jacobian as defined above and f is the desired force and torgue + /// vector. The first 3 components are forces along x, y and z in Newtons, the other 3 + /// components are rotations around x (roll), y (pitch) and z (yaw) axis in Newton meters. + /// + /// # Arguments + /// + /// * `F` - A 6D vector representing the desired force and torque at the end-effector + /// as explained above. + /// + /// # Returns + /// + /// Joint positions, with values representing joint torques, + /// or an error message if the computation fails. + #[allow(non_snake_case)] // Standard Math notation calls for single uppercase name + pub fn torques_from_vector(&self, F: &Vector6) -> Joints { + let joint_torques = self.matrix.transpose() * F; + vector6_to_joints(joint_torques) + } +} + +/// Function to compute the Jacobian matrix for a given robot and joint configuration +/// +/// # Arguments +/// +/// * `robot` - A reference to the robot implementing the Kinematics trait +/// * `qs` - A reference to the joint configuration +/// * `epsilon` - A small value used for numerical differentiation +/// +/// # Returns +/// +/// A 6x6 matrix representing the Jacobian +/// +/// The Jacobian matrix maps the joint velocities to the end-effector velocities. +/// Each column corresponds to a joint, and each row corresponds to a degree of freedom +/// of the end-effector (linear and angular velocities). +pub(crate) fn compute_jacobian(robot: &impl Kinematics, joints: &Joints, epsilon: f64) -> Matrix6 { + let mut jacobian = Matrix6::zeros(); + let current_pose = robot.forward(joints); + let current_position = current_pose.translation.vector; + let current_orientation = current_pose.rotation; + + // Parallelize the loop using rayon + let jacobian_columns: Vec<_> = (0..6).into_iter().map(|i| { + let mut perturbed_qs = *joints; + perturbed_qs[i] += epsilon; + let perturbed_pose = robot.forward(&perturbed_qs); + let perturbed_position = perturbed_pose.translation.vector; + let perturbed_orientation = perturbed_pose.rotation; + + let delta_position = (perturbed_position - current_position) / epsilon; + let delta_orientation = (perturbed_orientation * current_orientation.inverse()).scaled_axis() / epsilon; + + (delta_position, delta_orientation) + }).collect(); + + for (i, (delta_position, delta_orientation)) in jacobian_columns.into_iter().enumerate() { + jacobian.fixed_view_mut::<3, 1>(0, i).copy_from(&delta_position); + jacobian.fixed_view_mut::<3, 1>(3, i).copy_from(&delta_orientation); + } + + jacobian +} + +#[cfg(test)] +mod tests { + use nalgebra::{Translation3, UnitQuaternion, Vector3}; + use crate::kinematic_traits::{Pose, Singularity, Solutions}; + use super::*; + + const EPSILON: f64 = 1e-6; + + /// Example implementation of the Kinematics trait for a single rotary joint robot + /// When the first joint rotates, it affects the Y-position and the Z-orientation of the end-effector. + /// The derivative of the Y-position with respect to the first joint should be 1. + /// The derivative of the Z-orientation with respect to the first joint should be 1. + /// No other joint affects the end-effector in this simple robot model. + pub struct SingleRotaryJointRobot; + + impl Kinematics for SingleRotaryJointRobot { + fn inverse(&self, _pose: &Pose) -> Solutions { + panic!() // Should not be used + } + + /// Simple inverse kinematics for a single rotary joint of the length 1. + fn inverse_continuing(&self, pose: &Pose, _previous: &Joints) -> Solutions { + let angle = pose.translation.vector[1].atan2(pose.translation.vector[0]); + vec![[angle, 0.0, 0.0, 0.0, 0.0, 0.0]] + } + + fn forward(&self, qs: &Joints) -> Pose { + // Forward kinematics for a single rotary joint robot + let angle = qs[0]; + let rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, angle); + let translation = Translation3::new(angle.cos(), angle.sin(), 0.0); + Isometry3::from_parts(translation, rotation) + } + + fn kinematic_singularity(&self, _qs: &Joints) -> Option { + None + } + } + + + fn assert_matrix_approx_eq(left: &Matrix6, right: &Matrix6, epsilon: f64) { + for i in 0..6 { + for j in 0..6 { + assert!((left[(i, j)] - right[(i, j)]).abs() < epsilon, "left[{0},{1}] = {2} is not approximately equal to right[{0},{1}] = {3}", i, j, left[(i, j)], right[(i, j)]); + } + } + } + + #[test] + fn test_forward_kinematics() { + let robot = SingleRotaryJointRobot; + let joints: Joints = [std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0, 0.0]; + let pose = robot.forward(&joints); + assert!((pose.translation.vector[0] - 0.0).abs() < EPSILON); + assert!((pose.translation.vector[1] - 1.0).abs() < EPSILON); + } + + #[test] + fn test_inverse_kinematics() { + let robot = SingleRotaryJointRobot; + let pose = Isometry3::new(Vector3::new(0.0, 1.0, 0.0), na::zero()); + let previous: Joints = [0.0; 6]; + let solutions = robot.inverse_continuing(&pose, &previous); + assert_eq!(solutions.len(), 1); + assert!((solutions[0][0] - std::f64::consts::FRAC_PI_2).abs() < EPSILON); + } + + #[test] + fn test_compute_jacobian() { + let robot = SingleRotaryJointRobot; + + // This loop was used to profile rayon performance. No improvement was found so not used. + for _e in 0..2 { + let joints: Joints = [0.0; 6]; + let jacobian = compute_jacobian(&robot, &joints, EPSILON); + let mut expected_jacobian = Matrix6::zeros(); + + expected_jacobian[(0, 0)] = 0.0; // No effect on X position + expected_jacobian[(1, 0)] = 1.0; // Y position is affected by the first joint + expected_jacobian[(2, 0)] = 0.0; // No effect on Z position + + expected_jacobian[(3, 0)] = 0.0; // No effect on X orientation + expected_jacobian[(4, 0)] = 0.0; // No effect on Y orientation + expected_jacobian[(5, 0)] = 1.0; // Z orientation is affected by the first joint + + assert_matrix_approx_eq(&jacobian, &expected_jacobian, EPSILON); + } + } + + #[test] + fn test_velocities_from_iso() { + let robot = SingleRotaryJointRobot; + let initial_qs = [0.0; 6]; + let jacobian = Jacobian::new(&robot, &initial_qs, EPSILON); + + // Given an end effector located 1 meter away from the axis of rotation, + // with the joint rotating at a speed of 1 radian per second, the tip velocity is + // one meter per second. Given we start from the angle 0, it all goes to the y component. + let desired_velocity_isometry = + Isometry3::new(Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0)); + let result = jacobian.velocities(&desired_velocity_isometry); + + assert!(result.is_ok()); + let joint_velocities = result.unwrap(); + println!("Computed joint velocities: {:?}", joint_velocities); + + // Add assertions to verify the expected values + assert!((joint_velocities[0] - 1.0).abs() < EPSILON); + assert_eq!(joint_velocities[1], 0.0); + assert_eq!(joint_velocities[2], 0.0); + assert_eq!(joint_velocities[3], 0.0); + assert_eq!(joint_velocities[4], 0.0); + assert_eq!(joint_velocities[5], 0.0); + } + + #[test] + fn test_compute_joint_torques() { + let robot = SingleRotaryJointRobot; + let initial_qs = [0.0; 6]; + let jacobian = Jacobian::new(&robot, &initial_qs, EPSILON); + + // For a single joint robot, that we want on the torgue is what we need to put + let desired_force_torque = + Isometry3::new(Vector3::new(0.0, 0.0, 0.0), + Vector3::new(0.0, 0.0, 1.234)); + + let joint_torques = jacobian.torques(&desired_force_torque); + println!("Computed joint torques: {:?}", joint_torques); + + assert_eq!(joint_torques[0], 1.234); + assert_eq!(joint_torques[1], 0.0); + assert_eq!(joint_torques[2], 0.0); + assert_eq!(joint_torques[3], 0.0); + assert_eq!(joint_torques[4], 0.0); + assert_eq!(joint_torques[5], 0.0); + } +} + + + diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index f68560d..26c03d5 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -49,7 +49,8 @@ pub const CONSTRAINT_CENTERED: Joints = [NAN, 0.0, 0.0, 0.0, 0.0, 0.0]; /// given point). pub type Solutions = Vec; -pub trait Kinematics { +/// Defines agreed functionality of direct and inverse kinematics and singularity detection. +pub trait Kinematics: Send + Sync { /// Find inverse kinematics (joint position) for this pose /// This function is faster but does not handle the singularity J5 = 0 well. /// All returned solutions are cross-checked with forward kinematics and diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index bfc122b..1a43c47 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -10,7 +10,7 @@ use crate::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; const DEBUG: bool = false; -#[derive(Debug)] +#[derive(Debug, Copy, Clone)] pub struct OPWKinematics { /// The parameters that were used to construct this solver. parameters: Parameters, @@ -182,19 +182,13 @@ impl Kinematics for OPWKinematics { let cy0 = cx1 * f64::sin(q1) + cy1 * f64::cos(q1); let cz0 = cz1 + p.c1; - let s1 = f64::sin(q1); - let s2 = f64::sin(q2); - let s3 = f64::sin(q3); - let s4 = f64::sin(q4); - let s5 = f64::sin(q5); - let s6 = f64::sin(q6); - - let c1 = f64::cos(q1); - let c2 = f64::cos(q2); - let c3 = f64::cos(q3); - let c4 = f64::cos(q4); - let c5 = f64::cos(q5); - let c6 = f64::cos(q6); + // Precompute sines and cosines + let (s1, c1) = (q1.sin(), q1.cos()); + let (s2, c2) = (q2.sin(), q2.cos()); + let (s3, c3) = (q3.sin(), q3.cos()); + let (s4, c4) = (q4.sin(), q4.cos()); + let (s5, c5) = (q5.sin(), q5.cos()); + let (s6, c6) = (q6.sin(), q6.cos()); let r_0c = Matrix3::new( c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3, diff --git a/src/lib.rs b/src/lib.rs index 7b6c8bb..c8b829f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -13,6 +13,10 @@ pub mod kinematics_impl; pub mod constraints; +pub mod tool; + +pub mod jacobian; + #[cfg(feature = "allow_filesystem")] pub mod urdf; #[cfg(feature = "allow_filesystem")] @@ -29,3 +33,5 @@ mod tests; + + diff --git a/src/main.rs b/src/main.rs index f9ebbac..341d1ee 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,4 +1,4 @@ -const VERSION: &str = "1.2.1"; +const VERSION: &str = "1.3.0"; #[cfg(feature = "allow_filesystem")] fn main() { @@ -17,8 +17,11 @@ fn main() { \nWhile both parameters and constraints are printed out, constraints are not \ \npart of the OPW parameters. \ \n\nThis tool is Free software under BSD 3, hosted in repository \ - \nhttps://github.com/bourumir-wyngs/rs-opw-kinematics\n"); - println!("Usage: rs-opw-kinematics urdf_file.urdf"); + \nhttps://github.com/bourumir-wyngs/rs-opw-kinematics \ + \nData from ROS industrial (https://github.com/ros-industrial) were used for testing,\ + \nlicense files in tests/data contain exact origins\n"); + + println!("\nUsage: rs-opw-kinematics urdf_file.urdf"); } fn read_file(file_name: &str) -> io::Result { diff --git a/src/parameters.rs b/src/parameters.rs index d656927..04846d1 100644 --- a/src/parameters.rs +++ b/src/parameters.rs @@ -4,7 +4,7 @@ pub mod opw_kinematics { use crate::utils::deg; /// Parameters for the robot. See parameters_robots.rs for examples for concrete robot models. - #[derive(Debug, Clone)] + #[derive(Debug, Clone, Copy)] pub struct Parameters { pub a1: f64, pub a2: f64, diff --git a/src/parameters_from_file.rs b/src/parameters_from_file.rs index 2ca45c1..72781e2 100644 --- a/src/parameters_from_file.rs +++ b/src/parameters_from_file.rs @@ -50,6 +50,7 @@ impl Parameters { Yaml::String(s) => Self::parse_degrees(s), Yaml::Real(s) => s.parse::() .map_err(|_| ParameterError::ParseError("Failed to parse angle".into())), + Yaml::Integer(s) => Ok(*s as f64), _ => Err(ParameterError::ParseError( "Offset entry is not a number or deg() function".into())), }) diff --git a/src/parameters_robots.rs b/src/parameters_robots.rs index 4050c9e..14ce60b 100644 --- a/src/parameters_robots.rs +++ b/src/parameters_robots.rs @@ -6,7 +6,6 @@ pub mod opw_kinematics { #[allow(dead_code)] impl Parameters { - // Provides default values pub fn new() -> Self { Parameters { @@ -21,7 +20,7 @@ pub mod opw_kinematics { sign_corrections: [1; 6], } } - + pub fn irb2400_10() -> Self { Parameters { a1: 0.100, @@ -39,8 +38,8 @@ pub mod opw_kinematics { // See https://www.staubli.com/content/dam/robotics/products/robots/tx2/TX2-140-160-datasheet-EN.pdf. // These three Staubli robots have spherical wrist and mostly identical plan, with only - // two parameters being different. - pub fn staubli_tx2() -> Self { + // two parameters being different. This function does not create usable parameters alone. + fn staubli_tx2() -> Self { Parameters { a1: 0.150, a2: 0.000, diff --git a/src/tests/data/fanuc/m20ia_macro.xacro b/src/tests/data/fanuc/m20ia_macro.xacro new file mode 100644 index 0000000..a0bb429 --- /dev/null +++ b/src/tests/data/fanuc/m20ia_macro.xacro @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tests/data/fanuc/opw_parameters_m20ia.yaml b/src/tests/data/fanuc/opw_parameters_m20ia.yaml new file mode 100644 index 0000000..5b36223 --- /dev/null +++ b/src/tests/data/fanuc/opw_parameters_m20ia.yaml @@ -0,0 +1,20 @@ +# +# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist) +# kinematic configurations, as described in the paper "An Analytical Solution +# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an +# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur +# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop +# 2014, 22-23 May, 2014, Linz, Austria). +# +# The moveit_opw_kinematics_plugin package provides such a solver. +# +opw_kinematics_geometric_parameters: + a1: 0.15 + a2: -0.25 + b: 0.0 + c1: 0.525 + c2: 0.79 + c3: 0.835 + c4: 0.10 +opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)] +opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] diff --git a/src/tests/data/kuka/kr6r900_2_macro.xacro b/src/tests/data/kuka/kr6r900_2_macro.xacro new file mode 100644 index 0000000..377b753 --- /dev/null +++ b/src/tests/data/kuka/kr6r900_2_macro.xacro @@ -0,0 +1,180 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tests/data/kuka/opw_parameters_kr6r900_2.yaml b/src/tests/data/kuka/opw_parameters_kr6r900_2.yaml new file mode 100644 index 0000000..ef9ea57 --- /dev/null +++ b/src/tests/data/kuka/opw_parameters_kr6r900_2.yaml @@ -0,0 +1,20 @@ +# +# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist) +# kinematic configurations, as described in the paper "An Analytical Solution +# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an +# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur +# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop +# 2014, 22-23 May, 2014, Linz, Austria). +# +# The moveit_opw_kinematics_plugin package provides such a solver. +# +opw_kinematics_geometric_parameters: + a1: 0.025 + a2: -0.025 + b: 0.000 + c1: 0.400 + c2: 0.455 + c3: 0.420 + c4: 0.090 +opw_kinematics_joint_offsets: [0.0, deg(-90.0), 0.0, 0.0, 0.0, 0.0] +opw_kinematics_joint_sign_corrections: [-1, 1, 1, -1, 1, -1] diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs index 848d677..ee7fd3c 100644 --- a/src/tests/testcases.rs +++ b/src/tests/testcases.rs @@ -167,9 +167,12 @@ fn are_isometries_approx_equal(a: &Isometry3, b: &Isometry3, tolerance mod tests { use std::collections::HashMap; use std::f64::consts::PI; + use std::sync::Arc; use crate::kinematic_traits::{Joints, Kinematics, Singularity, Solutions}; use crate::parameters::opw_kinematics::Parameters; use crate::kinematics_impl::OPWKinematics; + use crate::tool::{Base, Tool}; + use crate::utils::{dump_joints, dump_solutions_degrees}; use super::*; /// Check if 'expected' exists in the given vector of solutions. This function is also @@ -464,4 +467,63 @@ mod tests { assert_eq!(expected.offsets, loaded.offsets); assert_eq!(expected.sign_corrections, loaded.sign_corrections); } + + + #[test] + fn test_complex_robot_reversible() { + let filename = "src/tests/data/cases.yaml"; + let result = load_yaml(filename); + assert!(result.is_ok(), "Failed to load or parse the YAML file: {}", result.unwrap_err()); + let cases = result.expect("Expected a valid Cases struct after parsing"); + let all_parameters = create_parameter_map(); + println!("Forward IK: {} test cases", cases.len()); + + for case in cases.iter() { + let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { + panic!("Parameters for the robot [{}] are unknown", &case.parameters) + }); + let robot_alone = OPWKinematics::new(parameters.clone()); + + // 1 meter high pedestal + let pedestal = 0.5; + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, pedestal).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = Base { + robot: Arc::new(robot_alone), + base: base_translation, + }; + + // Tool extends 1 meter in the Z direction, envisioning something like sword + let sword = 1.0; + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, sword).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let kinematics = Tool { + robot: Arc::new(robot_with_base), + tool: tool_translation, + }; + + + // Try forward on the initial data set first. + let joints = case.joints_in_radians(); + let pose = kinematics.forward(&joints); + let solutions = kinematics.inverse_continuing(&pose, &joints); + + // It must be the matching solution, it must be the first in solutions. + if !matches!(found_joints_approx_equal(&solutions, &joints, + 0.001_f64.to_radians()), Some(0)) { + println!("Not found or not first:"); + dump_joints(&joints); + dump_solutions_degrees(&solutions); + panic!(); + } + } + } + } diff --git a/src/tests/urdf_extractor.rs b/src/tests/urdf_extractor.rs index 9bf8340..19548dc 100644 --- a/src/tests/urdf_extractor.rs +++ b/src/tests/urdf_extractor.rs @@ -6,15 +6,15 @@ use crate::urdf::URDFParameters; fn read_urdf(path: &str) -> URDFParameters { let opw_parameters = urdf::from_urdf(read_to_string(path) - .expect("Failed to read test data file"), &None).expect("Faile to interpret URDF"); + .expect("Failed to read test data file"), &None).expect("Faile to interpret URDF"); // Output the results or further process println!("{:?}", opw_parameters); - opw_parameters + opw_parameters } #[test] fn test_extraction_m10ia() { - let opw_parameters= read_urdf("src/tests/data/fanuc/m10ia_macro.xacro"); + let opw_parameters = read_urdf("src/tests/data/fanuc/m10ia_macro.xacro"); // opw_kinematics_geometric_parameters: // a1: 0.15 @@ -52,9 +52,10 @@ fn test_extraction_m10ia() { assert_eq!(opw_parameters.to[i], val, "Mismatch in to at index {}", i); } } + #[test] fn test_extraction_lrmate200ib() { - let opw_parameters= read_urdf("src/tests/data/fanuc/lrmate200ib_macro.xacro"); + let opw_parameters = read_urdf("src/tests/data/fanuc/lrmate200ib_macro.xacro"); // opw_kinematics_geometric_parameters: // a1: 0.15 @@ -95,7 +96,7 @@ fn test_extraction_lrmate200ib() { #[test] fn test_extraction_m6ib() { - let opw_parameters= read_urdf("src/tests/data/fanuc/m6ib_macro.xacro"); + let opw_parameters = read_urdf("src/tests/data/fanuc/m6ib_macro.xacro"); // opw_kinematics_geometric_parameters: // a1: 0.15 @@ -115,22 +116,6 @@ fn test_extraction_m6ib() { assert_eq!(opw_parameters.c4, 0.10, "c4 parameter mismatch"); } -#[test] -fn test_extraction_kr6r700sixx() { - let urdf = - read_urdf("src/tests/data/kuka/kr6r700sixx_macro.xacro"); - - let yaml = Parameters::kuka_kr6_r700_sixx(); - - assert_eq!(urdf.a1, yaml.a1, "a1 parameter mismatch"); - assert_eq!(urdf.a2, yaml.a2, "a2 parameter mismatch"); - assert_eq!(urdf.b, yaml.b, "b parameter mismatch"); - assert_eq!(urdf.c1, yaml.c1, "c1 parameter mismatch"); - assert_eq!(urdf.c2, yaml.c2, "c2 parameter mismatch"); - assert_eq!(urdf.c3, yaml.c3, "c3 parameter mismatch"); - assert_eq!(urdf.c4, yaml.c4, "c4 parameter mismatch"); -} - fn assert_parameter_extraction(yaml: Parameters, urdf: URDFParameters, robot: &str) { assert_eq!(urdf.a1, yaml.a1, "a1 parameter mismatch for {}", robot); assert_eq!(urdf.a2, yaml.a2, "a2 parameter mismatch for {}", robot); @@ -141,6 +126,15 @@ fn assert_parameter_extraction(yaml: Parameters, urdf: URDFParameters, robot: &s assert_eq!(urdf.c4, yaml.c4, "c4 parameter mismatch for {}", robot); } +#[test] +fn test_extraction_kr6r700sixx() { + let urdf = + read_urdf("src/tests/data/kuka/kr6r700sixx_macro.xacro"); + + let params = Parameters::kuka_kr6_r700_sixx(); + assert_parameter_extraction(params, urdf, "_kr6r700sixx"); +} + #[test] fn test_extraction_kr150() { let yaml = Parameters::from_yaml_file("\ @@ -161,4 +155,22 @@ fn test_extraction_kr10r1420() { assert_parameter_extraction(yaml, urdf, "kr10r1420"); } +#[test] +fn test_extraction_kr5_arc() { + let yaml = Parameters::from_yaml_file("\ + src/tests/data/kuka/opw_parameters_kr6r900_2.yaml") + .expect("Failed to read or parse URDF"); + let urdf = read_urdf("src/tests/data/kuka/kr6r900_2_macro.xacro"); + + assert_parameter_extraction(yaml, urdf, "kr6r900_2"); +} +#[test] +fn test_extraction_m20ia() { + let yaml = Parameters::from_yaml_file("\ + src/tests/data/fanuc/opw_parameters_m20ia.yaml") + .expect("Failed to read or parse URDF"); + let urdf = read_urdf("src/tests/data/fanuc/m20ia_macro.xacro"); + + assert_parameter_extraction(yaml, urdf, "m20ia"); +} diff --git a/src/tool.rs b/src/tool.rs new file mode 100644 index 0000000..d680f0e --- /dev/null +++ b/src/tool.rs @@ -0,0 +1,385 @@ +extern crate nalgebra as na; + +use std::sync::Arc; +use na::{Isometry3}; +use nalgebra::Translation3; +use crate::kinematic_traits::{Joints, Kinematics, Pose, Singularity, Solutions}; + + +#[derive(Clone)] +pub struct Tool { + pub robot: Arc, // The robot + + /// Transformation from the robot's tip joint to the tool's TCP. + pub tool: Isometry3, +} + +#[derive(Clone)] +pub struct Base { + pub robot: Arc, // The robot + + /// Transformation from the world origin to the robots base. + pub base: Isometry3, +} + +impl Kinematics for Tool { + fn inverse(&self, tcp: &Pose) -> Solutions { + self.robot.inverse(&(tcp * self.tool.inverse())) + } + + fn inverse_continuing(&self, tcp: &Pose, previous: &Joints) -> Solutions { + self.robot.inverse_continuing(&(tcp * self.tool.inverse()), previous) + } + + fn forward(&self, qs: &Joints) -> Pose { + // Calculate the pose of the tip joint using the robot's kinematics + let tip_joint = self.robot.forward(qs); + let tcp = tip_joint * self.tool; + tcp + } + + fn kinematic_singularity(&self, qs: &Joints) -> Option { + self.robot.kinematic_singularity(qs) + } +} + +impl Kinematics for Base { + fn inverse(&self, tcp: &Pose) -> Solutions { + self.robot.inverse(&(self.base.inverse() * tcp)) + } + + fn inverse_continuing(&self, tcp: &Pose, previous: &Joints) -> Solutions { + self.robot.inverse_continuing(&(self.base.inverse() * tcp), &previous) + } + + fn forward(&self, joints: &Joints) -> Pose { + self.base * self.robot.forward(joints) + } + + fn kinematic_singularity(&self, qs: &Joints) -> Option { + self.robot.kinematic_singularity(qs) + } +} + +// Define the Cart (linear axis, prismatic joint) structure that can hold the robot. +// The cart is moving in parallel to Cartesian axis (x, y, z) and provides the forward kinematics. +// Same as joint positions for the robot, cart prismatic joints are not stored +// in this structure. The linear axis can be itself placed on the base. +#[derive(Clone)] +pub struct LinearAxis { + robot: Arc, + axis: u32, + /// The base of the axis (not the robot on the axis) + pub base: Isometry3, +} + +/// A platform for a robot that can ride in x, y and z directions. This way it is less +/// restricted than LinearAxis but tasks focusing with moving in one dimension only +/// may prefer abstracting which dimension it is. +#[derive(Clone)] +pub struct Gantry { + robot: Arc, + /// The base of the gantry crane (not the robot on the gantry crane) + pub base: Isometry3, +} + + +impl LinearAxis { + // Compute the forward transformation including the cart's offset and the robot's kinematics + pub fn forward(&self, distance: f64, joint_angles: &[f64; 6]) -> Isometry3 { + let cart_translation = match self.axis { + 0 => Translation3::new(distance, 0.0, 0.0), + 1 => Translation3::new(0.0, distance, 0.0), + 2 => Translation3::new(0.0, 0.0, distance), + _ => panic!("Invalid axis index; must be 0 (x), 1 (y), or 2 (z)"), + }; + let robot_pose = self.robot.forward(joint_angles); + self.base * cart_translation * robot_pose + } +} + +impl Gantry { + // Compute the forward transformation including the cart's offset and the robot's kinematics + pub fn forward(&self, translation: &Translation3, joint_angles: &[f64; 6]) -> Isometry3 { + let robot_pose = self.robot.forward(joint_angles); + self.base * translation * robot_pose + } +} + + +#[cfg(test)] +mod tests { + use std::f64::consts::PI; + use super::*; + use nalgebra::{Isometry3, Translation3, UnitQuaternion}; + use crate::kinematics_impl::OPWKinematics; + use crate::parameters::opw_kinematics::Parameters; + + /// Asserts that two `Translation3` instances are approximately equal within a given tolerance. + pub(crate) fn assert_diff(a: &Translation3, b: &Translation3, expected_diff: [f64; 3], epsilon: f64) { + let actual_diff = a.vector - b.vector; + + assert!( + (actual_diff.x - expected_diff[0]).abs() <= epsilon, + "X difference is not as expected: actual difference = {}, expected difference = {}", + actual_diff.x, expected_diff[0] + ); + assert!( + (actual_diff.y - expected_diff[1]).abs() <= epsilon, + "Y difference is not as expected: actual difference = {}, expected difference = {}", + actual_diff.y, expected_diff[1] + ); + assert!( + (actual_diff.z - expected_diff[2]).abs() <= epsilon, + "Z difference is not as expected: actual difference = {}, expected difference = {}", + actual_diff.z, expected_diff[2] + ); + } + + fn diff(robot_without: &dyn Kinematics, robot_with: &dyn Kinematics, joints: &[f64; 6]) -> (Pose, Pose) { + let tcp_without_tool = robot_without.forward(&joints); + let tcp_with_tool = robot_with.forward(&joints); + (tcp_without_tool, tcp_with_tool) + } + + #[test] + fn test_tool() { + // Parameters for Staubli TX40 robot are assumed to be correctly set in OPWKinematics::new + let robot_without_tool = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + // Tool extends 1 meter in the Z direction + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, 1.0).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_with_tool = Tool { + robot: Arc::new(robot_without_tool), + tool: tool_translation, + }; + + // Joints are all at zero position + let joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, [0., 0., 1.], 1E-6); + + // Rotating J6 by any angle should not change anything. + // Joints are all at zero position + let joints = [0.0, 0.0, 0.0, 0.0, 0.0, PI / 6.0]; + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, [0., 0., 1.], 1E-6); + + // Rotating J5 by 90 degrees result in offset + let joints = [0.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, [1.0, 0.0, 0.], 1E-6); + + // Rotate base joint around, sign must change. + let joints = [PI, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, [-1.0, 0.0, 0.], 1E-6); + + // Rotate base joint 90 degrees, must become Y + let joints = [PI / 2.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, [0.0, 1.0, 0.], 1E-6); + + // Rotate base joint 45 degrees, must divide between X and Y + let joints = [PI / 4.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let catet = 45.0_f64.to_radians().sin(); + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, + [catet, catet, 0.], 1E-6); + + // Rotate base joint 45 degrees, must divide between X and Y, and also raise 45 deg up + let joints = [PI / 4.0, 0.0, 0.0, 0.0, PI / 4.0, 0.0]; + let (tcp_without_tool, tcp_with_tool) = diff(&robot_without_tool, &robot_with_tool, &joints); + assert_diff(&tcp_with_tool.translation, &tcp_without_tool.translation, + [0.5, 0.5, 2.0_f64.sqrt() / 2.0], 1E-6); + } + + #[test] + fn test_base() { + // Parameters for Staubli TX40 robot are assumed to be correctly set in OPWKinematics::new + let robot_without_base = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + // 1 meter high pedestal + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, 1.0).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = Base { + robot: Arc::new(robot_without_base), + base: base_translation, + }; + + // Joints are all at zero position + let joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + + let (tcp_without_base, tcp_with_base) = diff(&robot_without_base, &robot_with_base, &joints); + assert_diff(&tcp_with_base.translation, &tcp_without_base.translation, [0., 0., 1.], 1E-6); + + // Rotate base joint around, sign must not change. + let joints = [PI / 3.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_without_base, tcp_with_base) = diff(&robot_without_base, &robot_with_base, &joints); + assert_diff(&tcp_with_base.translation, &tcp_without_base.translation, [0.0, 0.0, 1.0], 1E-6); + } + + #[test] + fn test_linear_axis_forward() { + let robot_without_base = OPWKinematics::new(Parameters::staubli_tx2_160l()); + let base_translation = Isometry3::from_parts( + Translation3::new(0.1, 0.2, 0.3).into(), + UnitQuaternion::identity(), + ); + + let cart = LinearAxis { + robot: Arc::new(robot_without_base), + axis: 1, + base: base_translation, + }; + + let joint_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + let result = cart.forward(7.0, &joint_angles); + + assert_eq!(result.translation.vector.y, + robot_without_base.forward(&joint_angles).translation.vector.y + 7.0 + 0.2); + } + + #[test] + fn test_gantry_forward() { + let robot_without_base = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + let base_translation = Isometry3::from_parts( + Translation3::new(0.1, 0.2, 0.3).into(), + UnitQuaternion::identity(), + ); + + let gantry = Gantry { + robot: Arc::new(robot_without_base), + base: base_translation, + }; + + let joint_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + let r = gantry.forward( + &Translation3::new(7.0, 8.0, 9.0), + &joint_angles).translation; + + let alone = robot_without_base.forward(&joint_angles).translation; + + // Gantry riding plus gantry base. + assert_eq!(r.x, alone.x + 7.1); + assert_eq!(r.y, alone.y + 8.2); + assert_eq!(r.z, alone.z + 9.3); + } + + /// Complete test that includes robot on linear axis, standing on the base and equipped + /// witht he tool. + #[test] + fn test_complete_robot() { + fn diff(alone: &dyn Kinematics, riding: &LinearAxis, axis: f64, joints: &[f64; 6]) -> (Pose, Pose) { + let tcp_alone = alone.forward(&joints); + let tcp = riding.forward(axis, &joints); + (tcp_alone, tcp) + } + + let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l()); + + // 1 meter high pedestal + let pedestal = 0.5; + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, pedestal).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = Base { + robot: Arc::new(robot_alone), + base: base_translation, + }; + + // Tool extends 1 meter in the Z direction, envisioning something like sword + let sword = 1.0; + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, sword).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_complete = Tool { + robot: Arc::new(robot_with_base), + tool: tool_translation, + }; + + + // Gantry is based with 0.75 horizontal offset along y + let gantry_base = 0.75; + let gantry_translation = Isometry3::from_parts( + Translation3::new(0.0, gantry_base, 0.0).into(), + UnitQuaternion::identity(), + ); + + let riding_robot = LinearAxis { + robot: Arc::new(robot_complete), + axis: 0, + base: gantry_translation, + }; + + // Joints are all at zero position + let joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + let axis = 0.0; + + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [0., gantry_base, pedestal + sword], 1E-6); + + // Rotating J6 by any angle should not change anything. + // Joints are all at zero position + let joints = [0.0, 0.0, 0.0, 0.0, 0.0, PI / 6.0]; + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [0., gantry_base, pedestal + sword], 1E-6); + + // Rotating J5 by 90 degrees result in offset horizontally for the sword. + let joints = [0.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [sword, gantry_base, pedestal], 1E-6); + + // Rotate base joint around, sign for the sword must change. + let joints = [PI, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [-sword, gantry_base, pedestal], 1E-6); + + // Rotate base joint 90 degrees, swords contributes to Y now + let joints = [PI / 2.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [0.0, gantry_base + sword, pedestal], 1E-6); + + // Rotate base joint 45 degrees, the effect of sword must divide between X and Y + let joints = [PI / 4.0, 0.0, 0.0, 0.0, PI / 2.0, 0.0]; + let catet = 45.0_f64.to_radians().sin(); + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [catet, catet + gantry_base, pedestal], 1E-6); + + // Rotate base joint 45 degrees, must divide between X and Y, and also raise 45 deg up + let joints = [PI / 4.0, 0.0, 0.0, 0.0, PI / 4.0, 0.0]; + let (tcp_alone, tcp) = diff(&robot_alone, &riding_robot, axis, &joints); + assert_diff(&tcp.translation, &tcp_alone.translation, + [sword * 0.5, sword * 0.5 + gantry_base, sword * 2.0_f64.sqrt() / 2.0 + pedestal], 1E-6); + + + // Ride the gantry 10 meters along x. + let ride = 10.0; + let tcp_translation = riding_robot.forward(ride, &joints).translation; + assert_diff(&tcp_translation, &tcp_alone.translation, + [sword * 0.5 + ride, sword * 0.5 + gantry_base, + sword * 2.0_f64.sqrt() / 2.0 + pedestal], 1E-6); + } +} + diff --git a/src/utils.rs b/src/utils.rs index 1c8b744..866b28d 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,5 +1,6 @@ //! Helper functions +use nalgebra::Vector6; use crate::kinematic_traits::{Joints, Solutions}; /// Checks the solution for validity. This is only internally needed as all returned @@ -69,6 +70,16 @@ pub(crate) fn deg(x: &f64) -> String { format!("deg({:.4})", x.to_degrees()) } +/// Converts nalgebra::Vector6 to Joints ([f64; 6]) +pub fn vector6_to_joints(v: Vector6) -> Joints { + [v[0], v[1], v[2], v[3], v[4], v[5]] +} + +/// Converts Joints ([f64; 6]) to a Vector6 +pub fn joints_to_vector6(j: Joints) -> nalgebra::Vector6 { + Vector6::new(j[0], j[1], j[2], j[3], j[4], j[5]) +} + #[cfg(test)] mod tests { use std::f64::consts::PI;