rs_opw_kinematics/
frame.rs

1//! Supports concept of the frame that is common in robotics.
2//! Transform frame allows to compute or prepare by hand joint positions of the robot once,
3//! for some "standard" location. If the target the robot needs to work with moves somewhere
4//! else, it is possible to define a "Frame", specifying this target displacement
5//! (most often specifying 3 points both before and after displacement). The common example
6//! is robot picking boxes from the transportation pallets: the same box picking program
7//! can be used for picking from different pallets by specifying a new transport frame
8//! each time.
9//!
10//! The main functionality of the Frame is to use the "standard" joint positions for finding
11//! the new joint position, as they would be required after applying the frame. The Frame in
12//! this package also implements the Kinematics trait if such would be required.
13
14use nalgebra::{Isometry3, Point3, Rotation3, Translation3, UnitQuaternion};
15use std::error::Error;
16use std::fmt;
17use std::sync::Arc;
18use crate::constraints::Constraints;
19use crate::kinematic_traits::{Joints, Kinematics, Pose, Singularity, Solutions};
20
21
22/// Defines the frame that transforms the robot working area, moving and rotating it
23/// (not stretching). The frame can be created from 3 pairs of points, one defining
24/// the points before transforming and another after, or alternatively 1 pair is enough
25/// if we assume there is no rotation.
26pub struct Frame {
27    pub robot: Arc<dyn Kinematics>,  // The robot
28
29    /// The frame transform, normally computed with either Frame::translation or Frame::isometry
30    pub frame: Isometry3<f64>,
31}
32
33impl Frame {
34    /// Compute the frame transform that may include only shift (but not rotation)
35    pub fn translation(p: Point3<f64>, q: Point3<f64>) -> Isometry3<f64> {
36        let translation = q - p;
37        Isometry3::from_parts(Translation3::from(translation), nalgebra::UnitQuaternion::identity())
38    }
39
40    /// Compute the frame transform that may include shift and rotation (but not stretching)
41    pub fn frame(
42        p1: Point3<f64>,
43        p2: Point3<f64>,
44        p3: Point3<f64>,
45        q1: Point3<f64>,
46        q2: Point3<f64>,
47        q3: Point3<f64>,
48    ) -> Result<Isometry3<f64>, Box<dyn Error>> {
49        const NON_ISOMETRY_TOLERANCE: f64 = 0.005; // Tolerance how much the isometry can be actually not
50        // an isometry (distance between points differs). 5 mm looks like a reasonable check.
51        if !is_valid_isometry(&p1, &p2, &p3, &q1, &q2, &q3, NON_ISOMETRY_TOLERANCE) {
52            return Err(Box::new(NotIsometry::new(p1, p2, p3, q1, q2, q3)));
53        }
54        // Vectors between points
55        let v1 = p2 - p1;
56        let v2 = p3 - p1;
57
58        if v1.cross(&v2).norm() == 0.0 {
59            return Err(Box::new(ColinearPoints::new(p1, p2, p3, true)));
60        }
61
62        // Destination vectors
63        let w1 = q2 - q1;
64        let w2 = q3 - q1;
65
66        if w1.cross(&w2).norm() == 0.0 {
67            return Err(Box::new(ColinearPoints::new(q1, q2, q3, false)));
68        }
69
70        // Create orthonormal bases
71        let b1 = v1.normalize();
72        let b2 = v1.cross(&v2).normalize();
73        let b3 = b1.cross(&b2);
74
75        let d1 = w1.normalize();
76        let d2 = w1.cross(&w2).normalize();
77        let d3 = d1.cross(&d2);
78
79        let rotation_matrix = nalgebra::Matrix3::from_columns(&[
80            d1, d2, d3,
81        ]) * nalgebra::Matrix3::from_columns(&[
82            b1, b2, b3,
83        ]).transpose();
84
85        let rotation_matrix = Rotation3::from_matrix_unchecked(rotation_matrix);
86        let rotation = UnitQuaternion::from_rotation_matrix(&rotation_matrix);
87        let translation = q1 - rotation.transform_point(&p1);
88
89        Ok(Isometry3::from_parts(translation.into(), rotation))
90    }
91
92    /// This function calculates the required joint values for a robot after applying a transformation
93    /// to the tool center point (TCP) location, as defined by a specified frame. It uses the provided
94    /// joint positions (qs) and the previous joint positions to compute the new positions. Depending on
95    /// how the frame is defined, there can be no solutions or multiple solutions; hence, the function
96    /// returns a `Solutions` structure to account for this variability. Additionally, this method
97    /// returns the transformed tool center pose.
98    ///
99    /// # Arguments
100    ///
101    /// * `qs` - A reference to the `Joints` structure representing the initial joint positions
102    ///   before the transformation.
103    /// * `previous` - A reference to the `Joints` structure representing the previous joint positions.
104    ///
105    /// # Returns
106    ///
107    /// A tuple containing:
108    /// * `Solutions` - A structure containing the possible joint positions after transformation.
109    /// * `Pose` - The transformed tool center pose.
110    ///
111    /// # Example
112    ///
113    /// ```
114    /// use std::sync::Arc;
115    /// use nalgebra::Point3;
116    /// use rs_opw_kinematics::frame::Frame;
117    /// use rs_opw_kinematics::kinematics_impl::OPWKinematics;
118    /// use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
119    ///
120    /// let robot = OPWKinematics::new(Parameters::irb2400_10());
121    /// let frame_transform = Frame::translation(
122    ///   Point3::new(0.0, 0.0, 0.0), Point3::new(0.01, 0.00, 0.0));
123    /// let framed = rs_opw_kinematics::frame::Frame {
124    ///   robot: Arc::new(robot),
125    ///   frame: frame_transform,
126    /// };
127    /// let joints_no_frame: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; // without frame
128    /// let (solutions, _transformed_pose) = framed.forward_transformed(
129    ///   &joints_no_frame, &joints_no_frame /* prioritize closest to this value */);
130    /// // Use solutions and transformed_pose as needed
131    /// ```
132    pub fn forward_transformed(&self, qs: &Joints, previous: &Joints) -> (Solutions, Pose) {
133        // Calculate the pose of the tip joint using the robot's kinematics
134        let tcp_no_frame = self.robot.forward(qs);
135
136        // Apply the frame transformation to the tool center point (TCP)
137        let tcp_frame = self.frame * tcp_no_frame;
138
139        // Compute the transformed joint values based on the transformed TCP pose and 
140        // previous joint positions
141        let transformed_joints = self.robot.inverse_continuing(&tcp_frame, previous);
142
143        // Return the possible joint solutions and the transformed TCP pose
144        (transformed_joints, tcp_frame)
145    }
146}
147
148impl Kinematics for Frame {
149    fn inverse(&self, tcp: &Pose) -> Solutions {
150        self.robot.inverse(&(tcp * self.frame.inverse()))
151    }
152
153    fn inverse_5dof(&self, tcp: &Pose, j6: f64) -> Solutions {
154        self.robot.inverse_5dof(&(tcp * self.frame.inverse()), j6)
155    }
156
157    fn inverse_continuing_5dof(&self, tcp: &Pose, previous: &Joints) -> Solutions {
158        self.robot.inverse_continuing_5dof(&(tcp * self.frame.inverse()), previous)
159    }
160
161    fn inverse_continuing(&self, tcp: &Pose, previous: &Joints) -> Solutions {
162        self.robot.inverse_continuing(&(tcp * self.frame.inverse()), previous)
163    }
164
165    fn forward(&self, qs: &Joints) -> Pose {
166        // Calculate the pose of the tip joint using the robot's kinematics
167        let tip_joint = self.robot.forward(qs);
168        let tcp = tip_joint * self.frame;
169        tcp
170    }
171
172    fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] {
173        // Compute the forward kinematics for the robot itself
174        let mut poses = self.robot.forward_with_joint_poses(joints);
175
176        // Apply the tool transformation only to the last pose (TCP pose)
177        poses[5] = poses[5] * self.frame;
178
179        poses
180    }    
181
182    fn kinematic_singularity(&self, qs: &Joints) -> Option<Singularity> {
183        self.robot.kinematic_singularity(qs)
184    }
185
186    fn constraints(&self) -> &Option<Constraints> {
187        self.robot.constraints()
188    }
189}
190
191
192/// Defines error when points specified as source or target for creating the frame are colinear (on the same line).
193/// Such points cannot be used to create the frame. The exact values of the points in question
194/// are included in the error structure and printed in the error message.
195#[derive(Debug)]
196pub struct ColinearPoints {
197    pub p1: Point3<f64>,
198    pub p2: Point3<f64>,
199    pub p3: Point3<f64>,
200    pub source: bool,
201}
202
203/// Struct to hold six points that still do not represent a valid isometry.
204/// It implements Error, containing at the same time six 3D points 
205/// from whom the isometry cannot be constructed.
206#[derive(Debug)]
207pub struct NotIsometry {
208    pub a1: Point3<f64>,
209    pub a2: Point3<f64>,
210    pub a3: Point3<f64>,
211    pub b1: Point3<f64>,
212    pub b2: Point3<f64>,
213    pub b3: Point3<f64>,
214}
215
216impl NotIsometry {
217    /// Creates a new NotIsometry instance, containing 6 points that do not
218    /// represent a valid isometry.
219    pub fn new(a1: Point3<f64>, a2: Point3<f64>, a3: Point3<f64>,
220               b1: Point3<f64>, b2: Point3<f64>, b3: Point3<f64>) -> Self {
221        NotIsometry { a1, a2, a3, b1, b2, b3 }
222    }
223}
224
225impl fmt::Display for NotIsometry {
226    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
227        write!(f, "Not isometry: (  a1: {:?},  a2: {:?},  a3: {:?},  b1: {:?},  b2: {:?},  b3: {:?})",
228               self.a1, self.a2, self.a3, self.b1, self.b2, self.b3)
229    }
230}
231
232impl ColinearPoints {
233    pub fn new(p1: Point3<f64>, p2: Point3<f64>, p3: Point3<f64>, source: bool) -> Self {
234        Self { p1, p2, p3, source }
235    }
236}
237
238impl fmt::Display for ColinearPoints {
239    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
240        write!(
241            f, "Cannot create a frame from colinear {} points: p1 = {:?}, p2 = {:?}, p3 = {:?}",
242            if self.source { "source" } else { "target" }, self.p1, self.p2, self.p3
243        )
244    }
245}
246
247impl Error for ColinearPoints {}
248
249impl Error for NotIsometry {}
250
251fn distances_match(a1: &Point3<f64>, a2: &Point3<f64>, a3: &Point3<f64>,
252                   b1: &Point3<f64>, b2: &Point3<f64>, b3: &Point3<f64>,
253                   tolerance: f64) -> bool {
254    let dist_a1_a2 = (a1 - a2).norm();
255    let dist_a1_a3 = (a1 - a3).norm();
256    let dist_a2_a3 = (a2 - a3).norm();
257
258    let dist_b1_b2 = (b1 - b2).norm();
259    let dist_b1_b3 = (b1 - b3).norm();
260    let dist_b2_b3 = (b2 - b3).norm();
261
262    (dist_a1_a2 - dist_b1_b2).abs() < tolerance &&
263        (dist_a1_a3 - dist_b1_b3).abs() < tolerance &&
264        (dist_a2_a3 - dist_b2_b3).abs() < tolerance
265}
266
267/// Function to check if 3 pairs of points define a valid isometry.
268pub fn is_valid_isometry(a1: &Point3<f64>, a2: &Point3<f64>, a3: &Point3<f64>,
269                         b1: &Point3<f64>, b2: &Point3<f64>, b3: &Point3<f64>,
270                         tolerance: f64) -> bool {
271    distances_match(a1, a2, a3, b1, b2, b3, tolerance)
272}
273
274#[cfg(test)]
275mod tests {
276    use super::*;
277    use nalgebra::{Translation3, Vector3};
278    use crate::kinematics_impl::OPWKinematics;
279    use crate::parameters::opw_kinematics::Parameters;
280    use crate::utils::{dump_joints, dump_solutions};
281
282    #[test]
283    fn test_find_isometry3_rotation_translation() {
284        // Define points before transformation
285        let p1 = Point3::new(0.0, 0.0, 0.0);
286        let p2 = Point3::new(1.0, 0.0, 0.0);
287        let p3 = Point3::new(0.0, 1.0, 0.0);
288
289        // Define points after 90-degree rotation around Z axis and translation by (1, 2, 0)
290        let q1 = Point3::new(1.0, 2.0, 0.0);
291        let q2 = Point3::new(1.0, 3.0, 0.0);
292        let q3 = Point3::new(0.0, 2.0, 0.0);
293
294        // Define additional points before transformation
295        let p4 = Point3::new(1.0, 1.0, 0.0);
296        let p5 = Point3::new(2.0, 1.0, 0.0);
297        let p6 = Point3::new(1.0, 2.0, 0.0);
298
299        // Define expected points after transformation
300        let q4 = Point3::new(0.0, 3.0, 0.0); // p4 rotated 90 degrees and translated
301        let q5 = Point3::new(0.0, 4.0, 0.0); // p5 rotated 90 degrees and translated
302        let q6 = Point3::new(-1.0, 3.0, 0.0); // p6 rotated 90 degrees and translated
303
304        // Find the isometry using our function
305        let result = Frame::frame(p1, p2, p3, q1, q2, q3)
306            .expect("These points are not colinear and must be ok");
307
308        // Expected translation
309        let expected_translation = Translation3::new(1.0, 2.0, 0.0);
310
311        // Expected rotation (90 degrees around the Z axis)
312        let expected_rotation =
313            UnitQuaternion::from_axis_angle(&Vector3::z_axis(), std::f64::consts::FRAC_PI_2);
314
315        // Compare the result with the expected translation and rotation
316        assert!((result.translation.vector - expected_translation.vector).norm() < 1e-6);
317
318        // Convert Unit<Vector3<f64>> to Vector3<f64> for comparison
319        let result_axis = result.rotation.axis().unwrap().into_inner();
320        let expected_axis = expected_rotation.axis().unwrap().into_inner();
321        assert!((result_axis - expected_axis).norm() < 1e-6);
322        assert!((result.rotation.angle() - expected_rotation.angle()).abs() < 1e-6);
323
324        // Check if the additional points are transformed correctly
325        assert!((result.transform_point(&p4) - q4).norm() < 1e-6);
326        assert!((result.transform_point(&p5) - q5).norm() < 1e-6);
327        assert!((result.transform_point(&p6) - q6).norm() < 1e-6);
328    }
329
330    #[test]
331    fn test_find_translation() {
332        // Define points
333        let p = Point3::new(1.0, 2.0, 3.0);
334        let q = Point3::new(4.0, 5.0, 6.0);
335
336        // Expected translation vector
337        let expected_translation = Translation3::new(3.0, 3.0, 3.0);
338
339        // Get the isometry
340        let isometry = Frame::translation(p, q);
341
342        // Check if the translation part is correct
343        assert!((isometry.translation.vector - expected_translation.vector).norm() < 1e-6);
344
345        // Check if the rotation part is identity
346        let identity_rotation = UnitQuaternion::identity();
347        assert!((isometry.rotation.quaternion() - identity_rotation.quaternion()).norm() < 1e-6);
348    }
349
350    #[test]
351    fn test_restore_pose() {
352        let robot = OPWKinematics::new(Parameters::irb2400_10());
353
354        // The frame is shifted by these offsets. Putting joints as reported
355        // by the computed frame should result these values again.
356        let dx = 0.011;
357        let dy = 0.022;
358        let dz = 0.033;
359
360        // Shift not too much to have values close to previous
361        let frame_transform = Frame::translation(
362            Point3::new(0.0, 0.0, 0.0),
363            Point3::new(dx, dy, dz));
364
365        let framed = Frame {
366            robot: Arc::new(robot),
367            frame: frame_transform,
368        };
369        let joints_no_frame: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; // without frame
370
371        println!("No frame transform:");
372        dump_joints(&joints_no_frame);
373
374        println!("Possible joint values after the frame transform:");
375        let (solutions, _transformed_pose) = framed.forward_transformed(
376            &joints_no_frame, &joints_no_frame);
377        dump_solutions(&solutions);
378
379        let framed = robot.forward(&solutions[0]).translation;
380        let unframed = robot.forward(&joints_no_frame).translation;
381
382        println!("Distance between framed and not framed pose {:.3} {:.3} {:.3}",
383                 framed.x - unframed.x, framed.y - unframed.y, framed.z - unframed.z);
384
385        let actual_dx = framed.x - unframed.x;
386        let actual_dy = framed.y - unframed.y;
387        let actual_dz = framed.z - unframed.z;
388
389        assert!((actual_dx - dx).abs() < 1e-6, "dx should be approximately {:.6}", dx);
390        assert!((actual_dy - dy).abs() < 1e-6, "dy should be approximately {:.6}", dy);
391        assert!((actual_dz - dz).abs() < 1e-6, "dz should be approximately {:.6}", dz);
392    }
393
394    #[test]
395    fn test_restore_pose_isometry_shift() -> Result<(), Box<dyn Error>> {
396        let robot = OPWKinematics::new(Parameters::irb2400_10());
397
398        // The frame is shifted by these offsets. Putting joints as reported
399        // by the computed frame should result these values again.
400        let dx = 0.011;
401        let dy = 0.022;
402        let dz = 0.033;
403
404        let angle = 0.0_f64.to_radians();
405        let axis = Vector3::z_axis();
406        let quaternion = UnitQuaternion::from_axis_angle(&axis, angle);
407
408        // Shift not too much to have values close to previous. Rotate 30 degrees.
409        let frame_transform = Frame::frame(
410            Point3::new(0.0, 0.0, 0.0),
411            Point3::new(1.0, 1.1, 1.2),
412            Point3::new(2.0, 2.2, 2.3),
413            quaternion * Point3::new(0.0 + dx, 0.0 + dy, 0.0 + dz),
414            quaternion * Point3::new(1.0 + dx, 1.1 + dy, 1.2 + dz),
415            quaternion * Point3::new(2.0 + dx, 2.2 + dy, 2.3 + dz),
416        )?;
417
418        let framed = Frame {
419            robot: Arc::new(robot),
420            frame: frame_transform,
421        };
422        let joints_no_frame: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; // without frame
423
424        println!("No frame transform:");
425        dump_joints(&joints_no_frame);
426
427        println!("Possible joint values after the frame transform:");
428        let (solutions, _transformed_pose) = framed.forward_transformed(
429            &joints_no_frame, &joints_no_frame);
430        dump_solutions(&solutions);
431
432        let framed = robot.forward(&solutions[0]).translation;
433        let unframed = robot.forward(&joints_no_frame).translation;
434
435        println!("Distance between framed and not framed pose {:.3} {:.3} {:.3}",
436                 framed.x - unframed.x, framed.y - unframed.y, framed.z - unframed.z);
437
438        let actual_dx = framed.x - unframed.x;
439        let actual_dy = framed.y - unframed.y;
440        let actual_dz = framed.z - unframed.z;
441
442        assert!((actual_dx - dx).abs() < 1e-6, "dx should be approximately {:.6}", dx);
443        assert!((actual_dy - dy).abs() < 1e-6, "dy should be approximately {:.6}", dy);
444        assert!((actual_dz - dz).abs() < 1e-6, "dz should be approximately {:.6}", dz);
445
446        Ok(())
447    }
448
449    #[test]
450    fn test_restore_pose_isometry_rotate() -> Result<(), Box<dyn Error>> {
451        let robot = OPWKinematics::new(Parameters::irb2400_10());
452
453        let angle = 90_f64.to_radians(); // We will rotate 90 degrees around z axis.
454        let axis = Vector3::z_axis();
455        let quaternion = UnitQuaternion::from_axis_angle(&axis, angle);
456
457        // Shift not too much to have values close to previous. Rotate 30 degrees.
458        let p1 = Point3::new(0.0, 0.0, 0.0);
459        let p2 = Point3::new(1.0, 1.1, 1.2);
460        let p3 = Point3::new(2.0, 2.2, 2.3);
461        let frame_transform = Frame::frame(
462            p1, p2, p3,
463            quaternion * p1, quaternion * p2, quaternion * p3,
464        )?;
465
466        let framed = Frame {
467            robot: Arc::new(robot),
468            frame: frame_transform,
469        };
470        let joints_no_frame: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; // without frame
471
472        println!("No frame transform:");
473        dump_joints(&joints_no_frame);
474
475        println!("Possible joint values after the frame transform:");
476        let (solutions, _transformed_pose) = framed.forward_transformed(
477            &joints_no_frame, &joints_no_frame);
478        dump_solutions(&solutions);
479
480        let framed = robot.forward(&solutions[0]).translation;
481        let unframed = robot.forward(&joints_no_frame).translation;
482
483        println!("Distance between framed and not framed pose {:.3} {:.3} {:.3} vs {:.3} {:.3} {:.3}",
484                 framed.x, framed.y, framed.z, unframed.x, unframed.y, unframed.z, );
485
486        let actual_dx = framed.x - -unframed.y; // x becomes -y
487        let actual_dy = framed.y - unframed.x; // y becomes x
488        let actual_dz = framed.z - unframed.z; // z does not change as we rotate around z.
489
490        assert!((actual_dx - 0.0).abs() < 1e-6, "dx should be approximately {:.6}", 0.0);
491        assert!((actual_dy - 0.0).abs() < 1e-6, "dy should be approximately {:.6}", 0.0);
492        assert!((actual_dz - 0.0).abs() < 1e-6, "dz should be approximately {:.6}", 0.0);
493
494        Ok(())
495    }
496}
497
rs_opw_kinematics/frame.rs

rs_opw_kinematics/
frame.rs
