Skip to main content

crx_kinematics/
lib.rs

1pub mod ik;
2
3// The embedded link geometry contains approximately 460 KB of binary data and requires a decoder.
4// Callers that need only the kinematics can disable it through this feature.
5#[cfg(feature = "meshes")]
6pub mod meshes;
7
8use crate::na::{Matrix3, Matrix4, Translation, Translation3, UnitQuaternion, try_convert};
9pub use nalgebra as na;
10use std::error::Error;
11use std::f64::consts::PI;
12
13/// A rigid-body transformation used to express every pose in this library.
14pub type Iso3 = na::Isometry3<f64>;
15
16/// A point in three dimensions.
17pub type Point3 = na::Point3<f64>;
18
19/// A vector in three dimensions.
20pub type Vector3 = na::Vector3<f64>;
21
22/// The result type used throughout the crate.
23pub type Result<T> = std::result::Result<T, Box<dyn Error>>;
24
25/// Wrap an angle in radians to the range [-pi, pi).
26///
27/// Joint angles recovered with `atan2` can be compared with angles from other sources after both
28/// values are wrapped to a common range.
29///
30/// # Arguments
31///
32/// * `radians`: the angle to wrap
33///
34/// returns: f64
35pub fn wrap_pi(radians: f64) -> f64 {
36    (radians + PI).rem_euclid(2.0 * PI) - PI
37}
38
39#[cfg(feature = "meshes")]
40pub use meshes::{LinkMesh, LinkMeshes};
41
42/// The different models in the FANUC CRX series of collaborative robots.
43///
44/// The derived traits let callers copy and compare model values or use them as hashed lookup keys.
45#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
46pub enum CrxModel {
47    Crx3iA,
48    Crx5iA,
49    Crx10iA,
50    Crx10iAL,
51    Crx20iAL,
52    Crx30iA,
53}
54
55/// Entity for the CRX series of collaborative robots. These are non-spherical wrist robots with
56/// three parallel axes. The entire series (as of Q1 2025) has the same kinematic structure and
57/// differs only in the lengths of the different links.
58///
59/// The CRX series also has the linked J2/J3 quirk common to the FANUC robots.  This means that
60/// when J2 actuates in the positive direction, J3 actuates the same amount to keep the forearm
61/// parallel to the robot base.  This means that to use any kinematics model for robots in this
62/// series, the J2/J3 angles must be modified on their way in and out.
63pub struct Crx {
64    z1: f64,
65    x1: f64,
66    x2: f64,
67    y1: f64,
68    h: [Vector3; 6],
69}
70
71impl Crx {
72    /// Returns the height from the J2 axis to the J3 axis (e.g. 410mm on the CRX-5iA).
73    pub fn z1(&self) -> f64 {
74        self.z1
75    }
76
77    /// Returns the length from the J3 axis to the J5 axis (e.g. 430mm on the CRX-5iA).
78    pub fn x1(&self) -> f64 {
79        self.x1
80    }
81
82    /// Returns the length from the J5 axis to the robot flange (e.g. 145mm on the CRX-5iA).
83    pub fn x2(&self) -> f64 {
84        self.x2
85    }
86
87    /// Returns the offset from the J1 axis to the J2 axis (e.g. 130mm on the CRX-5iA).
88    pub fn y1(&self) -> f64 {
89        self.y1
90    }
91
92    /// Returns the rotation axis of the joint at `index`, as a direction in the frame of the link
93    /// that carries the joint.
94    pub(crate) fn axis(&self, index: usize) -> Vector3 {
95        self.h[index]
96    }
97
98    /// Creates a robot from its four link lengths.
99    ///
100    /// Every robot in the CRX family shares one kinematic layout and differs only in these four
101    /// lengths, which fully define the model. Use this function for a variant that does not yet
102    /// have its own constructor or to explore how the solver handles a layout that FANUC has not
103    /// built.
104    ///
105    /// # Arguments
106    ///
107    /// * `z1`: The height from the J2 axis to the J3 axis (410mm on the CRX-5iA datasheet).
108    /// * `x1`: The length from the J3 axis to the J5 axis (430mm on the CRX-5iA datasheet).
109    /// * `x2`: The length from the J5 axis to the robot flange (145mm on the CRX-5iA datasheet).
110    /// * `y1`: The offset from the J1 axis to the J2 axis (130mm on the CRX-5iA datasheet).
111    ///
112    /// returns: Crx
113    pub fn from_params(z1: f64, x1: f64, x2: f64, y1: f64) -> Self {
114        // The h vectors are the directions of the rotation axes associated with each joint.
115        let h = [
116            Vector3::z(),
117            Vector3::y(),
118            -Vector3::y(),
119            -Vector3::x(),
120            -Vector3::y(),
121            -Vector3::x(),
122        ];
123
124        Self { z1, x1, x2, y1, h }
125    }
126
127    /// Creates a new robot of the specified model.
128    pub fn from_model(model: CrxModel) -> Self {
129        match model {
130            CrxModel::Crx3iA => Self::new_3ia(),
131            CrxModel::Crx5iA => Self::new_5ia(),
132            CrxModel::Crx10iA => Self::new_10ia(),
133            CrxModel::Crx10iAL => Self::new_10ia_l(),
134            CrxModel::Crx20iAL => Self::new_20ia_l(),
135            CrxModel::Crx30iA => Self::new_30ia(),
136        }
137    }
138
139    /// Creates a new CRX-3iA robot
140    pub fn new_3ia() -> Self {
141        Self::from_params(280.0, 280.0, 123.0, 111.0)
142    }
143
144    /// Creates a new CRX-5iA robot
145    pub fn new_5ia() -> Self {
146        Self::from_params(410.0, 430.0, 145.0, 130.0)
147    }
148
149    /// Creates a new CRX-10iA robot
150    pub fn new_10ia() -> Self {
151        Self::from_params(540.0, 540.0, 160.0, 150.0)
152    }
153
154    /// Creates a new CRX-10iA/L robot
155    pub fn new_10ia_l() -> Self {
156        Self::from_params(710.0, 540.0, 160.0, 150.0)
157    }
158
159    /// Creates a new CRX-20iA/L robot
160    pub fn new_20ia_l() -> Self {
161        Self::from_params(710.0, 540.0, 160.0, 150.0)
162    }
163
164    /// Creates a new CRX-30iA robot
165    pub fn new_30ia() -> Self {
166        Self::from_params(950.0, 750.0, 180.0, 185.0)
167    }
168
169    /// Compute the forward kinematics of a series of joint angles for the CRX series of robots.
170    /// The joints should be provided in degrees as they would appear in the robot controller. The
171    /// output will be an ` Iso3 ` object representing the position and orientation of the robot's
172    /// flange in relation to the robot origin.
173    ///
174    /// The output frame will match the FANUC controller in position and orientation.
175    ///
176    /// # Arguments
177    ///
178    /// * `joints`: The joint angles for the robot in degrees. This should be an array of 6 values
179    ///   representing the angles for each joint in the order of J1, J2, J3, J4, J5, and J6.
180    ///
181    /// returns: Isometry<f64, Unit<Quaternion<f64>>, 3>
182    pub fn fk(&self, joints: &[f64; 6]) -> Iso3 {
183        self.fk_all(joints)[5]
184    }
185
186    /// Compute the forward kinematics of a series of joint angles for the CRX series of robots,
187    /// returning the full kinematic chain for each joint in the robot. This will return an array
188    /// of `Iso3` objects representing the position and orientation of each joint in relation
189    /// to the robot origin. This can be useful for visualizing the full kinematic chain of the
190    /// robot and understanding how each joint contributes to the overall position and
191    /// orientation of the robot's flange.
192    ///
193    /// The final frame in the array will represent the position and orientation of the robot's
194    /// flange, and will be identical to the result of the `forward` method, matching the expected
195    /// value of the actual robot controller. The other frames in the array will be at the
196    /// kinematic link origins, and do not have any corresponding values in the actual robot.
197    ///
198    /// # Arguments
199    ///
200    /// * `joints`: The joint angles for the robot in degrees. This should be an array of 6 values
201    ///   representing the angles for each joint in the order of J1, J2, J3, J4, J5, and J6.
202    ///
203    /// returns: [Isometry<f64, Unit<Quaternion<f64>>, 3>; 6]
204    pub fn fk_all(&self, joints: &[f64; 6]) -> [Iso3; 6] {
205        let joints = joints_to_rad(joints);
206
207        // The first link is at the origin, rotated by the first joint angle
208        let f1 = Iso3::rotation(self.h[0] * joints[0]);
209
210        // J1->J2 has no origin shift
211        let f2 = f1 * Iso3::rotation(self.h[1] * joints[1]);
212
213        // J2->J3 shifts up by the z1 value
214        let f3 = f2
215            * Iso3::from_parts(
216                Translation::<f64, 3>::new(0.0, 0.0, self.z1),
217                UnitQuaternion::new(self.h[2] * joints[2]),
218            );
219
220        // J3->J4 has no origin shift
221        let f4 = f3 * Iso3::rotation(self.h[3] * joints[3]);
222
223        // J4->J5 shifts by x1, -y1
224        let f5 = f4
225            * Iso3::from_parts(
226                Translation::<f64, 3>::new(self.x1, -self.y1, 0.0),
227                UnitQuaternion::new(self.h[4] * joints[4]),
228            );
229
230        // J5->J6 shifts by x2, then gets re-oriented by the FANUC end
231        // effector adjustment
232        let f6 =
233            f5 * Iso3::from_parts(
234                Translation::<f64, 3>::new(self.x2, 0.0, 0.0),
235                UnitQuaternion::new(self.h[5] * joints[5]),
236            ) * end_adjust();
237
238        [f1, f2, f3, f4, f5, f6]
239    }
240}
241
242/// This is the transformation which rotates the world XYZ coordinate system to the FANUC flange
243/// convention where Z is pointing directly out of the flange, Y is inverted from the world Y axis,
244/// and X is pointing straight up.
245fn end_adjust() -> Iso3 {
246    Iso3::rotation(Vector3::new(2.221441469079183, 0.0, 2.221441469079183))
247}
248
249/// Convert FANUC joint angles from degrees to radians, including the J2/J3 interaction quirk.
250///
251/// # Arguments
252///
253/// * `joints`: a slice of 6 joint angles in degrees as they would be in the FANUC controller
254///
255/// returns: [f64; 6]
256fn joints_to_rad(joints: &[f64]) -> [f64; 6] {
257    let mut rad_joints = [0.0; 6];
258    for (i, j) in joints.iter().enumerate() {
259        rad_joints[i] = j.to_radians();
260    }
261    rad_joints[2] += rad_joints[1];
262    rad_joints
263}
264
265pub fn parts_to_iso(rot: Matrix3<f64>, trans: Vector3) -> Iso3 {
266    let r = UnitQuaternion::from_matrix(&rot);
267    let t = Translation3::from(trans);
268
269    Iso3::from_parts(t, r)
270}
271
272pub fn iso_to_parts(iso: &Iso3) -> (Matrix3<f64>, Vector3) {
273    let t = iso.translation.vector;
274    let r = iso.rotation.to_rotation_matrix();
275
276    (r.into(), t)
277}
278
279pub fn row_slice_to_iso(slice: &[f64]) -> Result<Iso3> {
280    if slice.len() != 16 {
281        return Err("Slice length must be 16".into());
282    }
283
284    let m = Matrix4::new(
285        slice[0], slice[1], slice[2], slice[3], slice[4], slice[5], slice[6], slice[7], slice[8],
286        slice[9], slice[10], slice[11], slice[12], slice[13], slice[14], slice[15],
287    );
288
289    try_convert(m).ok_or("Failed to convert matrix to isometry".into())
290}
291
292#[cfg(test)]
293pub mod tests {
294    use super::*;
295    use approx::assert_relative_eq;
296    use rand::RngExt;
297
298    /// Returns one instance of each robot model in the CRX series.
299    pub fn all_robots() -> [Crx; 6] {
300        [
301            Crx::from_model(CrxModel::Crx3iA),
302            Crx::from_model(CrxModel::Crx5iA),
303            Crx::from_model(CrxModel::Crx10iA),
304            Crx::from_model(CrxModel::Crx10iAL),
305            Crx::from_model(CrxModel::Crx20iAL),
306            Crx::from_model(CrxModel::Crx30iA),
307        ]
308    }
309
310    /// Generates a random set of joint angles where each value is between -180 and 180 degrees.
311    pub fn random_joints() -> [f64; 6] {
312        let mut rng = rand::rng();
313        std::array::from_fn(|_| rng.random_range(-180.0..=180.0))
314    }
315
316    #[test]
317    fn wrap_pi_brings_angles_into_range() {
318        assert_relative_eq!(wrap_pi(0.0), 0.0, epsilon = 1e-15);
319        // The range excludes its upper bound, so a half turn wraps to -pi.
320        assert_relative_eq!(wrap_pi(PI), -PI, epsilon = 1e-15);
321        assert_relative_eq!(wrap_pi(-PI), -PI, epsilon = 1e-15);
322        assert_relative_eq!(wrap_pi(3.0 * PI), -PI, epsilon = 1e-15);
323        assert_relative_eq!(wrap_pi(1.5 * PI), -0.5 * PI, epsilon = 1e-15);
324        assert_relative_eq!(wrap_pi(-1.5 * PI), 0.5 * PI, epsilon = 1e-15);
325    }
326
327    #[test]
328    fn from_params_matches_a_named_model() {
329        // The four link lengths fully define the model, so manual construction produces the same
330        // arm as construction by model name.
331        let named = Crx::new_10ia();
332        let by_hand = Crx::from_params(540.0, 540.0, 160.0, 150.0);
333        let joints = random_joints();
334
335        assert_relative_eq!(named.fk(&joints), by_hand.fk(&joints), epsilon = 1e-15);
336    }
337
338    #[test]
339    fn zero_position() -> Result<()> {
340        let j = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
341        let expected = row_slice_to_iso(&[
342            0.0, 0.0, 1.0, 575.0, 0.0, -1.0, 0.0, -130.0, 1.0, 0.0, 0.0, 410.0, 0.0, 0.0, 0.0, 1.0,
343        ])?;
344        let robot = Crx::new_5ia();
345        let fwd = robot.fk(&j);
346
347        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
348        Ok(())
349    }
350
351    #[test]
352    fn only_j1() -> Result<()> {
353        let j = [10.0, 0.0, 0.0, 0.0, 0.0, 0.0];
354        let expected = row_slice_to_iso(&[
355            0.0,
356            0.1736481776669304,
357            0.984807753012208,
358            588.8387210787206,
359            0.0,
360            -0.984807753012208,
361            0.1736481776669304,
362            -28.17730573310209,
363            1.0,
364            0.0,
365            0.0,
366            410.0,
367            0.0,
368            0.0,
369            0.0,
370            1.0,
371        ])?;
372        let robot = Crx::new_5ia();
373        let fwd = robot.fk(&j);
374
375        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
376        Ok(())
377    }
378
379    #[test]
380    fn only_j2() -> Result<()> {
381        let j = [0.0, 10.0, 0.0, 0.0, 0.0, 0.0];
382        let expected = row_slice_to_iso(&[
383            0.0,
384            0.0,
385            1.0,
386            646.1957528434414,
387            0.0,
388            -1.0,
389            0.0,
390            -130.0,
391            1.0,
392            0.0,
393            0.0,
394            403.7711787350052,
395            0.0,
396            0.0,
397            0.0,
398            1.0,
399        ])?;
400        let robot = Crx::new_5ia();
401        let fwd = robot.fk(&j);
402
403        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
404        Ok(())
405    }
406
407    #[test]
408    fn only_j3() -> Result<()> {
409        let j = [0.0, 0.0, 10.0, 0.0, 0.0, 0.0];
410        let expected = row_slice_to_iso(&[
411            -0.17364817766693028,
412            0.0,
413            0.984807753012208,
414            566.2644579820196,
415            0.0,
416            -1.0,
417            0.0,
418            -130.0,
419            0.984807753012208,
420            0.0,
421            0.17364817766693028,
422            509.84770215848494,
423            0.0,
424            0.0,
425            0.0,
426            1.0,
427        ])?;
428        let robot = Crx::new_5ia();
429        let fwd = robot.fk(&j);
430
431        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
432        Ok(())
433    }
434
435    #[test]
436    fn only_j4() -> Result<()> {
437        let j = [0.0, 0.0, 0.0, 10.0, 0.0, 0.0];
438        let expected = row_slice_to_iso(&[
439            0.0,
440            0.0,
441            1.0,
442            575.0,
443            0.17364817766693028,
444            -0.984807753012208,
445            0.0,
446            -128.02500789158705,
447            0.984807753012208,
448            0.17364817766693028,
449            0.0,
450            432.5742630967009,
451            0.0,
452            0.0,
453            0.0,
454            1.0,
455        ])?;
456        let robot = Crx::new_5ia();
457        let fwd = robot.fk(&j);
458
459        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
460        Ok(())
461    }
462
463    #[test]
464    fn only_j5() -> Result<()> {
465        let j = [0.0, 0.0, 0.0, 0.0, 10.0, 0.0];
466        let expected = row_slice_to_iso(&[
467            -0.17364817766693028,
468            0.0,
469            0.984807753012208,
470            572.7971241867701,
471            0.0,
472            -1.0,
473            0.0,
474            -130.0,
475            0.984807753012208,
476            0.0,
477            0.17364817766693028,
478            435.1789857617049,
479            0.0,
480            0.0,
481            0.0,
482            1.0,
483        ])?;
484        let robot = Crx::new_5ia();
485        let fwd = robot.fk(&j);
486
487        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
488        Ok(())
489    }
490
491    #[test]
492    fn only_j6() -> Result<()> {
493        let j = [0.0, 0.0, 0.0, 0.0, 0.0, 10.0];
494        let expected = row_slice_to_iso(&[
495            0.0,
496            0.0,
497            1.0,
498            575.0,
499            0.17364817766693028,
500            -0.984807753012208,
501            0.0,
502            -130.0,
503            0.984807753012208,
504            0.17364817766693028,
505            0.0,
506            410.0,
507            0.0,
508            0.0,
509            0.0,
510            1.0,
511        ])?;
512        let robot = Crx::new_5ia();
513        let fwd = robot.fk(&j);
514
515        assert_relative_eq!(expected, fwd, epsilon = 1e-6);
516        Ok(())
517    }
518
519    #[test]
520    fn crx5ia_bulk() -> Result<()> {
521        let bytes = include_bytes!("../tests/data/fanuc_crx_5ia.json");
522        let data: Vec<([f64; 6], [f64; 16])> = serde_json::from_slice(bytes)?;
523
524        let robot = Crx::new_5ia();
525        for (joints, expected) in data {
526            let fwd = robot.fk(&joints);
527            let expected = row_slice_to_iso(&expected)?;
528            assert_relative_eq!(fwd, expected, epsilon = 1e-6);
529        }
530
531        Ok(())
532    }
533
534    #[test]
535    fn crx10ia_bulk() -> Result<()> {
536        let bytes = include_bytes!("../tests/data/fanuc_crx_10ia.json");
537        let data: Vec<([f64; 6], [f64; 16])> = serde_json::from_slice(bytes)?;
538
539        let robot = Crx::new_10ia();
540        for (joints, expected) in data {
541            let fwd = robot.fk(&joints);
542            let expected = row_slice_to_iso(&expected)?;
543            assert_relative_eq!(fwd, expected, epsilon = 1e-6);
544        }
545
546        Ok(())
547    }
548}