rs_opw_kinematics/
parallelogram.rs

1//! Support for parallelogram mechanism some robots have. Like Tool and Base, Parallelogram
2//! both wraps arround some instance of Kinematics and implements Kinematics itself.
3
4use std::sync::Arc;
5use crate::constraints::Constraints;
6use crate::kinematic_traits::{Joints, Kinematics, Pose, Singularity, Solutions};
7
8/// Parallelogram Mechanism:
9/// The parallelogram mechanism introduces a geometric dependency between two specific joints,
10/// typically to maintain the orientation of the end-effector as the robot arm moves.
11/// This is useful in tasks that require a constant tool orientation, such as welding
12/// or handling objects, ensuring that the tool or end-effector remains level.
13///
14/// The mechanism links two joints, referred to as `joints[driven]` and `joints[coupled]`. The movement
15/// of the driven joint influences the coupled joint, maintaining the orientation of the end-effector
16/// during motion. The scaling factor determines the proportional influence of the driven joint on the
17/// coupled joint.
18///
19/// - **Forward Kinematics**: The coupled joint (`joints[coupled]`) is adjusted based on the driven joint:
20///   `joints[coupled]' = joints[coupled] - scaling * joints[driven]`. This adjustment maintains the correct
21///   alignment of the end-effector.
22/// - **Inverse Kinematics**: The dependency is reversed, adding the influence of the driven joint to the
23///   coupled joint: `joints[coupled]' = joints[coupled] + scaling * joints[driven]`. This ensures accurate
24///   calculation of joint angles to achieve the desired pose and orientation.
25///
26/// The `Parallelogram` structure automatically adjusts `joints[coupled]` based on `joints[driven]` using
27/// a scaling factor to account for the parallelogram mechanism.
28///
29/// # Fields:
30/// - `robot`: The underlying robot's kinematics model used to compute forward and inverse kinematics.
31/// - `scaling`: The factor that determines how much influence `joints[driven]` has on `joints[coupled]`.
32/// - `driven`: The index of the driven joint in the parallelogram mechanism (typically the primary joint).
33/// - `coupled`: The index of the coupled joint in the parallelogram mechanism (the secondary joint influenced by the driven joint).
34///
35/// # Example:
36/// ```rust
37/// use std::sync::Arc;
38/// // As J1 = 0, J2 = 1 and J3 = 2, so it is more clear with J-constants:
39/// use rs_opw_kinematics::kinematic_traits::{J2, J3};
40///
41/// use rs_opw_kinematics::kinematics_impl::OPWKinematics;
42/// use rs_opw_kinematics::parallelogram::Parallelogram;
43/// use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
44///
45/// // Assuming a robot that implements the Kinematics trait
46/// let robot_kinematics = Arc::new(OPWKinematics::new(Parameters::irb2400_10()));
47///
48/// // Create the Parallelogram structure with a scaling factor of 0.5,
49/// // where joints[1] is the driven joint and joints[2] is the coupled joint.
50/// let parallelogram = Parallelogram {
51///     robot: robot_kinematics,
52///     scaling: 1.0, // typically there is 1-to-1 influence between driven and coupled joints
53///     driven: J2,   // Joint 2 is most often the driven joint. 
54///     coupled: J3,  // Joint 3 is most often the coupled joint
55/// };
56/// ```
57/// As Parallelogram accepts and itself implements Kinematics, it is possible to chain multiple 
58/// parallelograms if the robot has more than one.
59/// 
60#[derive(Clone)]
61pub struct Parallelogram {
62    /// The underlying robot's kinematics used for forward and inverse kinematics calculations.
63    pub robot: Arc<dyn Kinematics>,
64
65    /// The scaling factor that determines the proportional influence of `joints[driven]` on `joints[coupled]`.
66    pub scaling: f64,
67
68    /// The index of the driven joint in the parallelogram mechanism (`joints[driven]`).
69    pub driven: usize,
70
71    /// The index of the coupled joint in the parallelogram mechanism (`joints[coupled]`).
72    pub coupled: usize,
73}
74impl Kinematics for Parallelogram {
75    fn inverse(&self, tcp: &Pose) -> Solutions {
76        let mut solutions = self.robot.inverse(tcp);
77
78        // Reversing the influence of driven joint in inverse kinematics
79        solutions.iter_mut().for_each(|x| x[self.coupled] += 
80            self.scaling * x[self.driven]); 
81        solutions
82    }
83
84    fn inverse_5dof(&self, tcp: &Pose, j6: f64) -> Solutions {
85        let mut solutions = self.robot.inverse_5dof(tcp, j6);
86
87        // Reversing the influence of driven joint in inverse kinematics        
88        solutions.iter_mut().for_each(|x| x[self.coupled] += 
89            self.scaling * x[self.driven]); 
90        solutions
91    }
92
93    fn inverse_continuing_5dof(&self, tcp: &Pose, previous: &Joints) -> Solutions {
94        let mut solutions = self.robot.inverse_continuing_5dof(tcp, previous);
95        
96        // Reversing the influence of driven joint in inverse kinematics
97        solutions.iter_mut().for_each(|x| x[self.coupled] += 
98            self.scaling * x[self.driven]); 
99        solutions
100    }
101
102    fn inverse_continuing(&self, tcp: &Pose, previous: &Joints) -> Solutions {
103        let mut solutions = self.robot.inverse_continuing(tcp, previous);
104        
105        // Reversing the influence of driven joint in inverse kinematics
106        solutions.iter_mut().for_each(|x| x[self.coupled] += 
107            self.scaling * x[self.driven]); 
108        solutions
109    }
110
111    fn forward(&self, qs: &Joints) -> Pose {
112        let mut joints = *qs;
113        // Adjusting coupled joint based on driven joint in forward kinematics
114        joints[self.coupled] -= self.scaling * joints[self.driven]; 
115        self.robot.forward(&joints)
116    }
117
118    fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] {
119        let mut joints = *joints; 
120        // Adjusting coupled joint based on driven joint in forward kinematics
121        joints[self.coupled] -= self.scaling * joints[self.driven]; 
122        self.robot.forward_with_joint_poses(&joints)
123    }
124
125    fn kinematic_singularity(&self, qs: &Joints) -> Option<Singularity> {
126        self.robot.kinematic_singularity(qs)
127    }
128
129    fn constraints(&self) -> &Option<Constraints> {
130        self.robot.constraints()
131    }
132}
rs_opw_kinematics/parallelogram.rs

rs_opw_kinematics/
parallelogram.rs
