1pub mod ik;
2
3#[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
13pub type Iso3 = na::Isometry3<f64>;
15
16pub type Point3 = na::Point3<f64>;
18
19pub type Vector3 = na::Vector3<f64>;
21
22pub type Result<T> = std::result::Result<T, Box<dyn Error>>;
24
25pub 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#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
46pub enum CrxModel {
47 Crx3iA,
48 Crx5iA,
49 Crx10iA,
50 Crx10iAL,
51 Crx20iAL,
52 Crx30iA,
53}
54
55pub struct Crx {
64 z1: f64,
65 x1: f64,
66 x2: f64,
67 y1: f64,
68 h: [Vector3; 6],
69}
70
71impl Crx {
72 pub fn z1(&self) -> f64 {
74 self.z1
75 }
76
77 pub fn x1(&self) -> f64 {
79 self.x1
80 }
81
82 pub fn x2(&self) -> f64 {
84 self.x2
85 }
86
87 pub fn y1(&self) -> f64 {
89 self.y1
90 }
91
92 pub(crate) fn axis(&self, index: usize) -> Vector3 {
95 self.h[index]
96 }
97
98 pub fn from_params(z1: f64, x1: f64, x2: f64, y1: f64) -> Self {
114 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 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 pub fn new_3ia() -> Self {
141 Self::from_params(280.0, 280.0, 123.0, 111.0)
142 }
143
144 pub fn new_5ia() -> Self {
146 Self::from_params(410.0, 430.0, 145.0, 130.0)
147 }
148
149 pub fn new_10ia() -> Self {
151 Self::from_params(540.0, 540.0, 160.0, 150.0)
152 }
153
154 pub fn new_10ia_l() -> Self {
156 Self::from_params(710.0, 540.0, 160.0, 150.0)
157 }
158
159 pub fn new_20ia_l() -> Self {
161 Self::from_params(710.0, 540.0, 160.0, 150.0)
162 }
163
164 pub fn new_30ia() -> Self {
166 Self::from_params(950.0, 750.0, 180.0, 185.0)
167 }
168
169 pub fn fk(&self, joints: &[f64; 6]) -> Iso3 {
183 self.fk_all(joints)[5]
184 }
185
186 pub fn fk_all(&self, joints: &[f64; 6]) -> [Iso3; 6] {
205 let joints = joints_to_rad(joints);
206
207 let f1 = Iso3::rotation(self.h[0] * joints[0]);
209
210 let f2 = f1 * Iso3::rotation(self.h[1] * joints[1]);
212
213 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 let f4 = f3 * Iso3::rotation(self.h[3] * joints[3]);
222
223 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 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
242fn end_adjust() -> Iso3 {
246 Iso3::rotation(Vector3::new(2.221441469079183, 0.0, 2.221441469079183))
247}
248
249fn 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 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 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 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 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}