diff --git a/.idea/GrepConsole.xml b/.idea/GrepConsole.xml
index 0e1af88..dd20438 100644
--- a/.idea/GrepConsole.xml
+++ b/.idea/GrepConsole.xml
@@ -66,6 +66,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/README.md b/README.md
index d8c72ee..cc781c3 100644
--- a/README.md
+++ b/README.md
@@ -36,6 +36,7 @@ This documentation also incorporates the robot diagram from that project.
- 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.
- 5 DOF inverse kinematics.
+- Individual link positions now available
- Experimental support for parameter extraction from URDF (for 5 DOF robot, specify the tool center point as joint 6)
The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for the two
@@ -140,6 +141,10 @@ Once constructed by specifying original and transformed points, the Frame object
and calculated joint angles for the transformed (shifted and rotated) trajector. See the
[frame](https://docs.rs/rs-opw-kinematics/1.5.0/rs_opw_kinematics/frame/index.html) documentation for details.
+# Individual link positions
+It is now possible to obtain positions of individual links in forward kinematics. This would be needed for
+collision avoidance and graphical rendering of the robot.
+
# 5 DOF inverse kinematics
For tools that are not sensitive to axis rotation (such as welding torches or paint sprayers), inverse kinematics can be
diff --git a/src/frame.rs b/src/frame.rs
index 2b08eb5..ff1b35c 100644
--- a/src/frame.rs
+++ b/src/frame.rs
@@ -168,6 +168,16 @@ impl Kinematics for Frame {
tcp
}
+ fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] {
+ // Compute the forward kinematics for the robot itself
+ let mut poses = self.robot.forward_with_joint_poses(joints);
+
+ // Apply the tool transformation only to the last pose (TCP pose)
+ poses[5] = poses[5] * self.frame;
+
+ poses
+ }
+
fn kinematic_singularity(&self, qs: &Joints) -> Option {
self.robot.kinematic_singularity(qs)
}
diff --git a/src/jacobian.rs b/src/jacobian.rs
index c1a9032..e4a6d4f 100644
--- a/src/jacobian.rs
+++ b/src/jacobian.rs
@@ -307,6 +307,10 @@ mod tests {
panic!() // Not used in this test
}
+ fn forward_with_joint_poses(&self, _joints: &Joints) -> [Pose; 6] {
+ panic!() // Not used in this test
+ }
+
/// 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]);
diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs
index 85616c8..ee6ee61 100644
--- a/src/kinematic_traits.rs
+++ b/src/kinematic_traits.rs
@@ -90,5 +90,56 @@ pub trait Kinematics: Send + Sync {
/// Detect the singularity. Returns either A type singlularity or None if
/// no singularity detected.
fn kinematic_singularity(&self, qs: &Joints) -> Option;
+
+ /// Computes the forward kinematics for a 6-DOF robotic arm and returns an array of poses
+ /// representing the position and orientation of each joint, including the final end-effector.
+ ///
+ /// The function calculates the transformation for each joint in the robotic arm using the joint
+ /// angles (in radians) and the kinematic parameters of the robot (link lengths and offsets).
+ /// It returns an array of 6 poses: one for each joint and the end-effector.
+ ///
+ /// # Parameters
+ /// - `self`: A reference to the kinematics model containing the geometric and joint-specific parameters.
+ /// - `joints`: A reference to an array of joint angles (in radians) for the 6 joints of the robot.
+ ///
+ /// # Returns
+ /// - An array of 6 `Pose` structures:
+ /// - Pose 1: The position and orientation of the base link.
+ /// - Pose 2: The position and orientation of link 1 (first joint).
+ /// - Pose 3: The position and orientation of link 2 (second joint).
+ /// - Pose 4: The position and orientation of link 3 (third joint).
+ /// - Pose 5: The position and orientation of link 4 (fourth joint), before applying any wrist offset.
+ /// - Pose 6: The position and orientation of the end-effector, including the wrist offset (`c4`).
+ ///
+ /// # Example
+ /// ```
+ /// use rs_opw_kinematics::kinematic_traits::Kinematics;
+ /// use rs_opw_kinematics::kinematics_impl::OPWKinematics;
+ /// use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
+ /// let parameters = Parameters {
+ /// a1: 0.150,
+ /// a2: 0.000,
+ /// b: 0.000,
+ /// c1: 0.550,
+ /// c2: 0.550,
+ /// c3: 0.600,
+ /// c4: 0.110,
+ /// offsets: [0.0; 6], // No joint offsets
+ /// ..Parameters::new()
+ /// };
+ ///
+ /// let joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; // All joints straight up
+ /// let kinematics = OPWKinematics::new(parameters);
+ ///
+ /// let poses = kinematics.forward_with_joint_poses(&joints);
+ ///
+ /// assert_eq!(poses.len(), 6); // Should return 6 poses
+ /// ```
+ ///
+ /// # Notes
+ /// - The function applies the geometric parameters (like link lengths and joint offsets) and computes
+ /// each joint's position and orientation relative to the base frame.
+ /// - The final pose (Pose 6) includes the `c4` offset, which accounts for the wrist length.
+ fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6];
}
diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs
index cce39a3..17968bb 100644
--- a/src/kinematics_impl.rs
+++ b/src/kinematics_impl.rs
@@ -1,5 +1,5 @@
//! Provides implementation of inverse and direct kinematics.
-
+
use std::f64::{consts::PI};
use crate::kinematic_traits::{Kinematics, Solutions, Pose, Singularity, Joints, JOINTS_AT_ZERO};
use crate::parameters::opw_kinematics::{Parameters};
@@ -41,6 +41,7 @@ impl OPWKinematics {
}
}
+
const MM: f64 = 0.001;
const DISTANCE_TOLERANCE: f64 = 0.001 * MM;
const ANGULAR_TOLERANCE: f64 = 1E-6;
@@ -64,7 +65,6 @@ const J5: usize = 4;
const J6: usize = 5;
impl Kinematics for OPWKinematics {
-
/// Return the solution that is constraint compliant anv values are valid
/// (no NaNs, etc) but otherwise not sorted.
/// If this is 5 degree of freedom robot only, the 6 joint is set to 0.0
@@ -74,7 +74,7 @@ impl Kinematics for OPWKinematics {
// For 5 DOF robot, we can only do 5 DOF approximate inverse.
self.inverse_intern_5_dof(pose, f64::NAN)
} else {
- self.filter_constraints_compliant(self.inverse_intern(&pose))
+ self.filter_constraints_compliant(self.inverse_intern(&pose))
}
}
@@ -83,9 +83,9 @@ impl Kinematics for OPWKinematics {
// The rotation of pose in this case is only approximate.
fn inverse_continuing(&self, pose: &Pose, prev: &Joints) -> Solutions {
if self.parameters.dof == 5 {
- return self.inverse_intern_5_dof(pose, prev[5]);
+ return self.inverse_intern_5_dof(pose, prev[5]);
}
-
+
let previous;
if prev[0].is_nan() {
// Special value CONSTRAINT_CENTERED has been used
@@ -177,6 +177,7 @@ impl Kinematics for OPWKinematics {
fn forward(&self, joints: &Joints) -> Pose {
let p = &self.parameters;
+ // Apply sign corrections and offsets
let q1 = joints[0] * p.sign_corrections[0] as f64 - p.offsets[0];
let q2 = joints[1] * p.sign_corrections[1] as f64 - p.offsets[1];
let q3 = joints[2] * p.sign_corrections[2] as f64 - p.offsets[2];
@@ -187,15 +188,20 @@ impl Kinematics for OPWKinematics {
let psi3 = f64::atan2(p.a2, p.c3);
let k = f64::sqrt(p.a2 * p.a2 + p.c3 * p.c3);
- let cx1 = p.c2 * f64::sin(q2) + k * f64::sin(q2 + q3 + psi3) + p.a1;
+ // Precompute q23_psi3 for better readability and reuse
+ let q23_psi3 = q2 + q3 + psi3;
+ let sin_q23_psi3 = q23_psi3.sin();
+ let cos_q23_psi3 = q23_psi3.cos();
+
+ let cx1 = p.c2 * f64::sin(q2) + k * sin_q23_psi3 + p.a1;
let cy1 = p.b;
- let cz1 = p.c2 * f64::cos(q2) + k * f64::cos(q2 + q3 + psi3);
+ let cz1 = p.c2 * f64::cos(q2) + k * cos_q23_psi3;
let cx0 = cx1 * f64::cos(q1) - cy1 * f64::sin(q1);
let cy0 = cx1 * f64::sin(q1) + cy1 * f64::cos(q1);
let cz0 = cz1 + p.c1;
- // Precompute sines and cosines
+ // Precompute sines and cosines for efficiency
let (s1, c1) = (q1.sin(), q1.cos());
let (s2, c2) = (q2.sin(), q2.cos());
let (s3, c3) = (q3.sin(), q3.cos());
@@ -203,27 +209,123 @@ impl Kinematics for OPWKinematics {
let (s5, c5) = (q5.sin(), q5.cos());
let (s6, c6) = (q6.sin(), q6.cos());
+ // Compute rotation matrix r_0c
let r_0c = Matrix3::new(
c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3,
s1 * c2 * c3 - s1 * s2 * s3, c1, s1 * c2 * s3 + s1 * s2 * c3,
-s2 * c3 - c2 * s3, 0.0, -s2 * s3 + c2 * c3,
);
+ // Compute rotation matrix r_ce
let r_ce = Matrix3::new(
c4 * c5 * c6 - s4 * s6, -c4 * c5 * s6 - s4 * c6, c4 * s5,
s4 * c5 * c6 + c4 * s6, -s4 * c5 * s6 + c4 * c6, s4 * s5,
-s5 * c6, s5 * s6, c5,
);
+ // Compute the final rotation matrix r_oe
let r_oe = r_0c * r_ce;
+ // Calculate the final translation
let translation = Vector3::new(cx0, cy0, cz0) + p.c4 * r_oe * *self.unit_z;
+
+ // Convert the rotation matrix to a quaternion
let rotation = Rotation3::from_matrix_unchecked(r_oe);
- Pose::from_parts(Translation3::from(translation),
- UnitQuaternion::from_rotation_matrix(&rotation))
+ // Return the pose combining translation and rotation
+ Pose::from_parts(
+ Translation3::from(translation),
+ UnitQuaternion::from_rotation_matrix(&rotation),
+ )
}
+ fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] {
+ let p = &self.parameters;
+
+ // Use joint angles directly as radians (no conversion needed)
+ let q1 = joints[0] * p.sign_corrections[0] as f64 - p.offsets[0];
+ let q2 = joints[1] * p.sign_corrections[1] as f64 - p.offsets[1];
+ let q3 = joints[2] * p.sign_corrections[2] as f64 - p.offsets[2];
+ let q4 = joints[3] * p.sign_corrections[3] as f64 - p.offsets[3];
+ let q5 = joints[4] * p.sign_corrections[4] as f64 - p.offsets[4];
+ let q6 = joints[5] * p.sign_corrections[5] as f64 - p.offsets[5];
+
+ let psi3 = f64::atan2(p.a2, p.c3);
+ let k = f64::sqrt(p.a2 * p.a2 + p.c3 * p.c3);
+
+ // Precompute q23_psi3 for better readability and reuse
+ let q23_psi3 = q2 + q3 + psi3;
+ let sin_q23_psi3 = q23_psi3.sin();
+ let cos_q23_psi3 = q23_psi3.cos();
+
+ // Precompute sines and cosines for efficiency
+ 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());
+
+ // Pose 1: Base link position
+ let joint1_pos = Vector3::new(0.0, 0.0, p.c1); // Z-offset from the base height
+ let joint1_rot = Rotation3::identity(); // No rotation for the base frame
+ let pose1 = Pose::from_parts(Translation3::from(joint1_pos), UnitQuaternion::from_rotation_matrix(&joint1_rot));
+
+ // Pose 2: Link 1 position (translation along x = a1, rotation around q1)
+ let joint2_pos = Vector3::new(p.a1 * c1, p.a1 * s1, p.c1); // a1 affects X, base height remains Z
+ let joint2_rot = Rotation3::from_euler_angles(q1, 0.0, 0.0); // Rotation around Z-axis (q1)
+ let pose2 = Pose::from_parts(Translation3::from(joint2_pos), UnitQuaternion::from_rotation_matrix(&joint2_rot));
+
+ // Pose 3: Link 2 position
+ let cx1 = p.c2 * f64::sin(q2) + p.a1;
+ let cy1 = p.b; // Typically 0 for most robots
+ let cz1 = p.c2 * f64::cos(q2); // Move in Z by link length
+ let joint3_pos = Vector3::new(
+ cx1 * c1 - cy1 * s1,
+ cx1 * s1 + cy1 * c1,
+ cz1 + p.c1 // Add the base height
+ );
+ let joint3_rot = Rotation3::from_euler_angles(q1, q2, 0.0); // Rotation around Z and Y
+ let pose3 = Pose::from_parts(Translation3::from(joint3_pos), UnitQuaternion::from_rotation_matrix(&joint3_rot));
+
+ // Pose 4: Link 3 position (additional translation along Z)
+ let cx2 = p.c2 * f64::sin(q2) + k * sin_q23_psi3 + p.a1;
+ let cz2 = p.c2 * f64::cos(q2) + k * cos_q23_psi3;
+ let joint4_pos = Vector3::new(
+ cx2 * c1 - cy1 * s1,
+ cx2 * s1 + cy1 * c1,
+ cz2 + p.c1 // Continue translating along Z
+ );
+ let joint4_rot = Rotation3::from_euler_angles(q1, q2, q3); // Rotation around Z, Y, and additional Y (q3)
+ let pose4 = Pose::from_parts(Translation3::from(joint4_pos), UnitQuaternion::from_rotation_matrix(&joint4_rot));
+
+ // Pose 5: Link 4 position (No c4 applied here, just joint4_pos)
+ let joint5_pos = joint4_pos; // Do not apply c4 here
+ let joint5_rot = Rotation3::from_euler_angles(q1, q2, q3 + q4); // Adding q4 for rotation around X-axis
+ let pose5 = Pose::from_parts(Translation3::from(joint5_pos), UnitQuaternion::from_rotation_matrix(&joint5_rot));
+
+ // Pose 6: End-effector position (including c4 offset)
+ let r_0c = Matrix3::new(
+ c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3,
+ s1 * c2 * c3 - s1 * s2 * s3, c1, s1 * c2 * s3 + s1 * s2 * c3,
+ -s2 * c3 - c2 * s3, 0.0, -s2 * s3 + c2 * c3,
+ );
+ let r_ce = Matrix3::new(
+ c4 * c5 * c6 - s4 * s6, -c4 * c5 * s6 - s4 * c6, c4 * s5,
+ s4 * c5 * c6 + c4 * s6, -s4 * c5 * s6 + c4 * c6, s4 * s5,
+ -s5 * c6, s5 * s6, c5,
+ );
+ let r_oe = r_0c * r_ce;
+
+ // Apply the c4 offset for the final end-effector position
+ let end_effector_pos = joint5_pos + r_oe * (p.c4 * *self.unit_z); // Apply c4 offset here for the wrist
+ let end_effector_rot = Rotation3::from_matrix_unchecked(r_oe); // Final rotation
+ let pose6 = Pose::from_parts(Translation3::from(end_effector_pos), UnitQuaternion::from_rotation_matrix(&end_effector_rot));
+
+ // Return all 6 Poses, with Pose 6 including c4 offset
+ [pose1, pose2, pose3, pose4, pose5, pose6]
+ }
+
fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions {
self.filter_constraints_compliant(self.inverse_intern_5_dof(&pose, j6))
}
@@ -617,7 +719,7 @@ impl OPWKinematics {
let mut sols: [[f64; 6]; 8] = [[f64::NAN; 6]; 8];
for si in 0..sols.len() {
- for ji in 0.. 5 {
+ for ji in 0..5 {
sols[si][ji] = (theta[si][ji] + params.offsets[ji]) *
params.sign_corrections[ji] as f64;
}
@@ -663,7 +765,7 @@ impl OPWKinematics {
fn compare_xyz_only(pose_translation: &Translation3, check_xyz: &Translation3, tolerance: f64) -> bool {
(pose_translation.vector - check_xyz.vector).norm() <= tolerance
}
-
+
fn filter_constraints_compliant(&self, solutions: Solutions) -> Solutions {
match &self.constraints {
Some(constraints) => constraints.filter(&solutions),
diff --git a/src/tests/mod.rs b/src/tests/mod.rs
index 8ad4e61..8e922e4 100644
--- a/src/tests/mod.rs
+++ b/src/tests/mod.rs
@@ -5,3 +5,4 @@ mod urdf_extractor;
mod tool_base_test;
mod test_utils;
mod test_from_yaml;
+mod test_individual_link_positions;
diff --git a/src/tests/test_individual_link_positions.rs b/src/tests/test_individual_link_positions.rs
new file mode 100644
index 0000000..48e9e9a
--- /dev/null
+++ b/src/tests/test_individual_link_positions.rs
@@ -0,0 +1,279 @@
+use std::sync::Arc;
+use nalgebra::{Isometry3, Translation3, UnitQuaternion};
+use crate::frame::Frame;
+use crate::kinematic_traits::{Kinematics, Pose};
+use crate::kinematics_impl::OPWKinematics;
+use crate::parameters::opw_kinematics::Parameters;
+use crate::tool::{Base, Tool};
+
+const SMALL: f64 = 1e-6;
+
+#[test]
+fn test_forward_kinematics_straight_up() {
+ // Define the joint angles for pointing the robot straight up (all 0)
+ let joints: [f64; 6] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
+ let parameters: Parameters = create_parameters();
+ let opw_kinematics: OPWKinematics = OPWKinematics::new(parameters);
+ let poses = opw_kinematics.forward_with_joint_poses(&joints);
+
+ // Expected positions
+ let expected_positions = [
+ (0.0, 0.0, parameters.c1), // Pose 1: Base link Z height
+ (parameters.a1, 0.0, parameters.c1), // Pose 2: Link 1 along X by a1, same Z
+ (parameters.a1, 0.0, parameters.c1 + parameters.c2), // Pose 3: Add link 2 length along Z
+ (parameters.a1, 0.0, parameters.c1 + parameters.c2 + parameters.c3), // Pose 4: Add link 3 length along Z
+ (parameters.a1, 0.0, parameters.c1 + parameters.c2 + parameters.c3), // Pose 5: Same Z as Pose 4, no c4 yet
+ (parameters.a1, 0.0, parameters.c1 + parameters.c2 + parameters.c3 + parameters.c4), // Pose 6: Add wrist length c4
+ ];
+
+ // Check all poses for correct X, Y, and Z translation
+ check_xyz(poses, expected_positions);
+
+ let standing: UnitQuaternion = UnitQuaternion::identity();
+
+ // Check that the quaternion (rotation) is identity quaternion for all poses
+ for (i, pose) in poses.iter().enumerate() {
+ check_rotation(standing, i, &pose.rotation);
+ }
+}
+
+#[test]
+fn test_forward_kinematics_j2_rotated_90_degrees() {
+ // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0
+ let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0];
+ let parameters: Parameters = create_parameters();
+ let opw_kinematics: OPWKinematics = OPWKinematics::new(parameters);
+ let poses = opw_kinematics.forward_with_joint_poses(&joints);
+
+ // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2
+ let expected_positions = [
+ (0.0, 0.0, parameters.c1),
+ (parameters.a1, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4, 0.0, parameters.c1),
+ ];
+
+ // Check all poses for correct X, Y, and Z translation
+ check_xyz(poses, expected_positions);
+
+ let standing: UnitQuaternion = UnitQuaternion::identity();
+ let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0);
+
+ // Check quaternions for each pose (after J2 rotation)
+ for (i, pose) in poses.iter().enumerate() {
+ // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied)
+ if i <= 1 {
+ check_rotation(standing, i, &pose.rotation);
+ } else {
+ // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis)
+ check_rotation(lying, i, &pose.rotation);
+ }
+ }
+}
+
+#[test]
+fn test_tool_forward_kinematics() {
+ let tool_offset = 1.0;
+
+ // Create an instance of OPWKinematics
+ let parameters: Parameters = create_parameters();
+ let robot_without_tool = OPWKinematics::new(parameters);
+
+ // Tool extends 1 meter in the Z direction
+ let tool_translation = Isometry3::from_parts(
+ Translation3::new(0.0, 0.0, tool_offset).into(),
+ UnitQuaternion::identity(),
+ );
+
+ // Create the Tool instance with the transformation
+ let robot_with_tool = Tool {
+ robot: Arc::new(robot_without_tool),
+ tool: tool_translation,
+ };
+
+ // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0
+ let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0];
+
+ // Compute forward kinematics for the given joint angles
+ let poses = robot_with_tool.forward_with_joint_poses(&joints);
+
+ // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2, plus the tool offset in Z direction
+ let expected_positions = [
+ (0.0, 0.0, parameters.c1),
+ (parameters.a1, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4 + tool_offset, 0.0, parameters.c1), // Tool adds 1 meter in X
+ ];
+
+ // Check all poses for correct X, Y, and Z translation
+ check_xyz(poses, expected_positions);
+
+ let standing: UnitQuaternion = UnitQuaternion::identity();
+ let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0);
+
+ // Check quaternions for each pose (after J2 rotation)
+ for (i, pose) in poses.iter().enumerate() {
+ // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied)
+ if i <= 1 {
+ check_rotation(standing, i, &pose.rotation);
+ } else {
+ // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis)
+ check_rotation(lying, i, &pose.rotation);
+ }
+ }
+}
+
+#[test]
+fn test_frame_forward_kinematics() {
+ let frame_offset = 1.0;
+
+ // Create an instance of OPWKinematics
+ let parameters: Parameters = create_parameters();
+ let robot_without_tool = OPWKinematics::new(parameters);
+
+ // Tool extends 1 meter in the Z direction
+ let frame_translation = Isometry3::from_parts(
+ Translation3::new(0.0, 0.0, frame_offset).into(),
+ UnitQuaternion::identity(),
+ );
+
+ // Create the Tool instance with the transformation
+ let robot_with_tool = Frame {
+ robot: Arc::new(robot_without_tool),
+ frame: frame_translation,
+ };
+
+ // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0
+ let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0];
+
+ // Compute forward kinematics for the given joint angles
+ let poses = robot_with_tool.forward_with_joint_poses(&joints);
+
+ // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2, plus the tool offset in Z direction
+ let expected_positions = [
+ (0.0, 0.0, parameters.c1),
+ (parameters.a1, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1),
+ (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4 + frame_offset, 0.0, parameters.c1), // Frame adds 1 meter in X
+ ];
+
+ // Check all poses for correct X, Y, and Z translation
+ check_xyz(poses, expected_positions);
+
+ let standing: UnitQuaternion = UnitQuaternion::identity();
+ let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0);
+
+ // Check quaternions for each pose (after J2 rotation)
+ for (i, pose) in poses.iter().enumerate() {
+ // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied)
+ if i <= 1 {
+ check_rotation(standing, i, &pose.rotation);
+ } else {
+ // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis)
+ check_rotation(lying, i, &pose.rotation);
+ }
+ }
+}
+
+#[test]
+fn test_base_forward_kinematics() {
+ let base_height = 1.0;
+
+ let parameters: Parameters = create_parameters();
+ let robot_without_base = OPWKinematics::new(parameters);
+
+ // 1 meter high pedestal
+ let base_translation = Isometry3::from_parts(
+ Translation3::new(0.0, 0.0, base_height).into(),
+ UnitQuaternion::identity(),
+ );
+
+ // Create the Base instance with the transformation
+ let robot_with_base = Base {
+ robot: Arc::new(robot_without_base),
+ base: base_translation,
+ };
+
+ // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0
+ let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0];
+
+ // Compute forward kinematics for the given joint angles
+ let poses = robot_with_base.forward_with_joint_poses(&joints);
+
+ // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2, plus the base offset in Z direction
+ let expected_positions = [
+ (0.0, 0.0, parameters.c1 + base_height), // Base adds 1 meter in Z
+ (parameters.a1, 0.0, parameters.c1 + base_height),
+ (parameters.a1 + parameters.c2, 0.0, parameters.c1 + base_height),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1 + base_height),
+ (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1 + base_height),
+ (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4, 0.0, parameters.c1 + base_height),
+ ];
+
+ // Check all poses for correct X, Y, and Z translation
+ check_xyz(poses, expected_positions);
+
+ let standing: UnitQuaternion = UnitQuaternion::identity();
+ let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0);
+
+ // Check quaternions for each pose (after J2 rotation)
+ for (i, pose) in poses.iter().enumerate() {
+ // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied)
+ if i <= 1 {
+ check_rotation(standing, i, &pose.rotation);
+ } else {
+ // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis)
+ check_rotation(lying, i, &pose.rotation);
+ }
+ }
+}
+
+fn create_parameters() -> Parameters {
+ let parameters = Parameters {
+ a1: 0.150,
+ a2: 0.000,
+ b: 0.000,
+ c1: 0.550,
+ c2: 0.550, // Example model-specific value
+ c3: 0.600, // Example model-specific value
+ c4: 0.110,
+ ..Parameters::new() // Any other required fields
+ };
+ parameters
+}
+
+fn check_xyz(poses: [Pose; 6], expected_positions: [(f64, f64, f64); 6]) {
+ for (i, &(expected_x, expected_y, expected_z)) in expected_positions.iter().enumerate() {
+ let translation = poses[i].translation.vector;
+
+ // X, Y, and Z coordinates should match the expected positions
+ assert!((translation[0] as f64 - expected_x).abs() < SMALL, "Pose {} X- expected {}, got {}", i + 1, expected_x, translation[0]);
+ assert!((translation[1] as f64 - expected_y).abs() < SMALL, "Pose {} Y- expected {}, got {}", i + 1, expected_y, translation[1]);
+ assert!((translation[2] as f64 - expected_z).abs() < SMALL, "Pose {} Z- expected {}, got {}", i + 1, expected_z, translation[2]);
+ }
+}
+
+fn check_rotation(standing: UnitQuaternion, i: usize, quaternion: &UnitQuaternion) {
+ assert!(
+ (quaternion.w as f64 - standing.w).abs() < SMALL,
+ "Pose {} quaternion w mismatch", i + 1
+ );
+ assert!(
+ (quaternion.i as f64 - standing.i).abs() < SMALL,
+ "Pose {} quaternion i mismatch", i + 1
+ );
+ assert!(
+ (quaternion.j as f64 - standing.j).abs() < SMALL,
+ "Pose {} quaternion j mismatch", i + 1
+ );
+ assert!(
+ (quaternion.k as f64 - standing.k).abs() < SMALL,
+ "Pose {} quaternion k mismatch", i + 1
+ );
+}
diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs
index b51ad28..6f8c5ec 100644
--- a/src/tests/testcases.rs
+++ b/src/tests/testcases.rs
@@ -2,7 +2,7 @@
mod tests {
use std::f64::consts::PI;
use nalgebra::{UnitQuaternion, Vector3};
- use crate::kinematic_traits::{ Kinematics, Singularity };
+ use crate::kinematic_traits::{Kinematics, Singularity};
use crate::parameters::opw_kinematics::Parameters;
use crate::kinematics_impl::OPWKinematics;
use crate::tests::test_utils;
@@ -55,6 +55,38 @@ mod tests {
}
}
+ #[test]
+ fn test_forward_ik_with_joint_poses() {
+ let filename = "src/tests/data/cases.yaml";
+ let result = test_utils::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 = test_utils::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 kinematics = OPWKinematics::new(parameters.clone());
+
+ // This test only checks the final pose so far.
+ let ik = kinematics.forward_with_joint_poses(&case.joints_in_radians())[5];
+ let pose = test_utils::Pose::from_isometry(&ik);
+
+ if !test_utils::are_isometries_approx_equal(&ik, &case.pose.to_isometry(), 0.00001) {
+ println!("Seems not equal");
+ println!("joints: {:?} ", &case.joints);
+ println!("case: {:?} ", &pose);
+ println!("IK : {:?} ", &case.pose);
+ println!("{}", parameters.to_yaml());
+
+
+ panic!("Forward kinematics of the primary pose seems not equal");
+ }
+ }
+ }
+
#[test]
fn test_inverse_ik() {
let filename = "src/tests/data/cases.yaml";
@@ -75,7 +107,7 @@ mod tests {
// Try forward on the initial data set first.
let solutions = kinematics.inverse(&case.pose.to_isometry());
if test_utils::found_joints_approx_equal(&solutions, &case.joints_in_radians(),
- 0.001_f64.to_radians()).is_none() {
+ 0.001_f64.to_radians()).is_none() {
println!("**** No valid solution for case {} on {} ****", case.id, case.parameters);
let joints_str = &case.joints.iter()
.map(|&val| format!("{:5.2}", val))
@@ -121,7 +153,7 @@ mod tests {
&case.pose.to_isometry(), &case.joints_in_radians());
let found_matching =
test_utils::found_joints_approx_equal(&solutions, &case.joints_in_radians(),
- 0.001_f64.to_radians());
+ 0.001_f64.to_radians());
if !matches!(found_matching, Some(0)) {
println!("**** No valid solution: {:?} for case {} on {} ****",
found_matching, case.id, case.parameters);
@@ -284,5 +316,4 @@ mod tests {
);
}
}
-
}
diff --git a/src/tool.rs b/src/tool.rs
index 4ad8754..089c85a 100644
--- a/src/tool.rs
+++ b/src/tool.rs
@@ -100,6 +100,16 @@ impl Kinematics for Tool {
tcp
}
+ fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] {
+ // Compute the forward kinematics for the robot itself
+ let mut poses = self.robot.forward_with_joint_poses(joints);
+
+ // Apply the tool transformation only to the last pose (TCP pose)
+ poses[5] = poses[5] * self.tool;
+
+ poses
+ }
+
fn kinematic_singularity(&self, qs: &Joints) -> Option {
self.robot.kinematic_singularity(qs)
}
@@ -126,6 +136,18 @@ impl Kinematics for Base {
self.base * self.robot.forward(joints)
}
+ fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] {
+ // Compute the forward kinematics for the robot itself
+ let mut poses = self.robot.forward_with_joint_poses(joints);
+
+ // Apply the base transformation to each pose
+ for pose in poses.iter_mut() {
+ *pose = self.base * *pose;
+ }
+
+ poses
+ }
+
fn kinematic_singularity(&self, qs: &Joints) -> Option {
self.robot.kinematic_singularity(qs)
}