rs_opw_kinematics/kinematic_traits.rs
1//! Defines traits for direct and inverse kinematics.
2
3extern crate nalgebra as na;
4
5use std::f64::NAN;
6use na::{Isometry3};
7use crate::constraints::Constraints;
8
9/// Pose is used a pose of the robot tcp. It contains both Cartesian position and rotation quaternion
10/// ```
11/// extern crate nalgebra as na;
12/// use na::{Isometry3, Translation3, UnitQuaternion, Vector3};
13///
14/// type Pose = Isometry3<f64>;
15///
16/// let translation = Translation3::new(1.0, 0.0, 0.0);
17/// // The quaternion should be normalized to represent a valid rotation.
18/// let rotation = UnitQuaternion::from_quaternion(na::Quaternion::new(1.0, 0.0, 0.0, 1.0).normalize());
19/// let transform = Pose::from_parts(translation, rotation);
20/// ```
21pub type Pose = Isometry3<f64>;
22
23/// Defines kinematic singularity. A is a singularity when J5 = 0 (this is possible with
24/// any robot). The structure is reserved for other possible singularies but these require
25/// b = 0 and a1 = a2 so not possible with most of the robots.
26/// Joints are counted from 1 to 6 in this comment.
27#[derive(PartialEq, Debug)]
28pub enum Singularity {
29 /// Represents singularity when J5 = 0, possible with any robot.
30 A,
31}
32
33/// Six rotary joints of the robot with angles in radians.
34pub type Joints = [f64; 6];
35
36// Define indices for easier reading (numbering in array starts from 0 and this one-off is
37// contra - intuitive)
38#[allow(dead_code)]
39pub const J1: usize = 0;
40#[allow(dead_code)]
41pub const J2: usize = 1;
42#[allow(dead_code)]
43pub const J3: usize = 2;
44#[allow(dead_code)]
45pub const J4: usize = 3;
46#[allow(dead_code)]
47pub const J5: usize = 4;
48#[allow(dead_code)]
49pub const J6: usize = 5;
50
51/// The number for the robot tool in collision report
52pub const J_TOOL: usize = 100;
53
54/// The robot base joint
55pub const J_BASE: usize = 101;
56
57/// Starting index for collision_environment entries in collision pairs
58pub const ENV_START_IDX: usize = 1000;
59
60/// For providing singularity - proof solution when the previous value is not known.
61/// Joints that take arbitrary angles will take angles as close to 0 as possible:
62/// let solutions = kinematics.inverse_continuing(&pose, &JOINTS_AT_ZERO);
63#[allow(dead_code)]
64pub const JOINTS_AT_ZERO: Joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
65
66/// Special value that can be used with inverse_continuing, indicating that there
67/// are not previous joint value, but returned joints must be sorted to be as
68/// close as possible to the centers of the constraints. If no constraitns are set,
69/// zeroes are assumed.
70pub const CONSTRAINT_CENTERED: Joints = [NAN, 0.0, 0.0, 0.0, 0.0, 0.0];
71
72/// For providing solutions. As invalid solutions are discarded,
73/// this is a variable length vector (may be empty if robot cannot reach the
74/// given point).
75pub type Solutions = Vec<Joints>;
76
77/// Defines agreed functionality of direct and inverse kinematics and singularity detection.
78pub trait Kinematics: Send + Sync {
79 /// Find inverse kinematics (joint position) for this pose
80 /// This function is faster but does not handle the singularity J5 = 0 well.
81 /// All returned solutions are cross-checked with forward kinematics and
82 /// valid.
83 fn inverse(&self, pose: &Pose) -> Solutions;
84
85 /// Find inverse kinematics (joint position) for this pose
86 /// This function handles the singularity J5 = 0 by keeping the previous values
87 /// the values J4 and J6 from the previous solution
88 /// Use CONSTRAINT_CENTERED as previous if there is no previous position but we prefer
89 /// to be as close to the center of constraints (or zeroes if not set) as
90 /// possible. "Previous" can be in a wide range, say 90000 degrees.
91 fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions;
92
93 /// Find forward kinematics (pose from joint positions).
94 /// For 5 DOF robot, the rotation of the joint 6 should normally be 0.0
95 /// but some other value can be given, meaning the tool is mounted with
96 /// fixed rotation offset.
97 fn forward(&self, qs: &Joints) -> Pose;
98
99 /// Calculates the inverse kinematics for a robot while ignoring the rotation
100 /// around joint 6. The position of the tool center point remains precise,
101 /// but the rotation is approximate (rotation around the tool axis is ignored).
102 /// The return value for joint 6 is set according to the provided parameter.
103 /// This method is significantly faster
104 fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions;
105
106
107 /// Calculates the inverse kinematics for a robot while ignoring the rotation
108 /// around joint 6. The position of the tool center point remains precise,
109 /// but the rotation is approximate (rotation around the tool axis is ignored).
110 /// The return value for joint 6 is set based on the previous joint values.
111 /// This method is significantly faster
112 fn inverse_continuing_5dof(&self, pose: &Pose, prev: &Joints) -> Solutions;
113
114 /// Returns constraints under what the solver is operating.
115 /// Constraints are remembered here and can be used for generating random
116 /// joint angles needed by RRT, or say providing limits of sliders in GUI.
117 fn constraints(&self) -> &Option<Constraints>;
118
119 /// Detect the singularity. Returns either A type singularity or None if
120 /// no singularity detected.
121 fn kinematic_singularity(&self, qs: &Joints) -> Option<Singularity>;
122
123 /// Computes the forward kinematics for a 6-DOF robotic arm and returns an array of poses
124 /// representing the position and orientation of each joint, including the final end-effector.
125 ///
126 /// The function calculates the transformation for each joint in the robotic arm using the joint
127 /// angles (in radians) and the kinematic parameters of the robot (link lengths and offsets).
128 /// It returns an array of 6 poses: one for each joint and the end-effector.
129 ///
130 /// # Parameters
131 /// - `self`: A reference to the kinematics model containing the geometric and joint-specific parameters.
132 /// - `joints`: A reference to an array of joint angles (in radians) for the 6 joints of the robot.
133 ///
134 /// # Returns
135 /// - An array of 6 `Pose` structures:
136 /// - Pose 1: The position and orientation of the base link.
137 /// - Pose 2: The position and orientation of link 1 (first joint).
138 /// - Pose 3: The position and orientation of link 2 (second joint).
139 /// - Pose 4: The position and orientation of link 3 (third joint).
140 /// - Pose 5: The position and orientation of link 4 (fourth joint), before applying any wrist offset.
141 /// - Pose 6: The position and orientation of the end-effector, including the wrist offset (`c4`).
142 ///
143 /// # Example
144 /// ```
145 /// use rs_opw_kinematics::kinematic_traits::Kinematics;
146 /// use rs_opw_kinematics::kinematics_impl::OPWKinematics;
147 /// use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
148 /// let parameters = Parameters {
149 /// a1: 0.150,
150 /// a2: 0.000,
151 /// b: 0.000,
152 /// c1: 0.550,
153 /// c2: 0.550,
154 /// c3: 0.600,
155 /// c4: 0.110,
156 /// offsets: [0.0; 6], // No joint offsets
157 /// ..Parameters::new()
158 /// };
159 ///
160 /// let joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; // All joints straight up
161 /// let kinematics = OPWKinematics::new(parameters);
162 ///
163 /// let poses = kinematics.forward_with_joint_poses(&joints);
164 ///
165 /// assert_eq!(poses.len(), 6); // Should return 6 poses
166 /// ```
167 ///
168 /// # Notes
169 /// - The function applies the geometric parameters (like link lengths and joint offsets) and computes
170 /// each joint's position and orientation relative to the base frame.
171 /// - The final pose (Pose 6) includes the `c4` offset, which accounts for the wrist length.
172 fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6];
173
174}
175