repose_kinematics/
lib.rs

1//! Deterministic, allocation-free serial robot kinematics (FK, Jacobian, IK, Servo) for `copper-rs`.
2
3#![cfg_attr(not(feature = "std"), no_std)]
4
5extern crate alloc;
6
7mod chain;
8mod ik;
9mod joint;
10mod robots;
11pub mod serde_arrays;
12mod task;
13
14pub use chain::CuJointAxis;
15pub use chain::CuJointSpec;
16pub use chain::CuJointType;
17pub use chain::CuKinematicChain;
18pub use ik::CuDiffIkParams;
19pub use ik::CuDiffIkStatus;
20pub use ik::CuIkParams;
21pub use ik::CuIkSolver;
22pub use ik::CuIkStatus;
23pub use ik::CuTwist6D;
24pub use ik::CuTwistFrame;
25pub use joint::CuJointAcceleration;
26pub use joint::CuJointJerk;
27pub use joint::CuJointLimits;
28pub use joint::CuJointMetric;
29pub use joint::CuJointPosition;
30pub use joint::CuJointResolution;
31pub use joint::CuJointVelocity;
32pub use robots::CuChainSpec;
33pub use robots::CuRobotPreset;
34pub use robots::igus_rebel_chain;
35pub use task::CuForwardKinematicsParams;
36pub use task::CuForwardKinematicsTask;
37pub use task::CuIkSolutionPayload;
38pub use task::CuInverseKinematicsParams;
39pub use task::CuInverseKinematicsTask;
40pub use task::CuJointPositionsPayload;
41pub use task::CuJointStatePayload;
42pub use task::CuJointVelocitiesPayload;
43pub use task::CuRobotStatus;
44pub use task::CuServoParams;
45pub use task::CuServoTask;
46
47#[cfg(test)]
48mod tests {
49    use crate::CuDiffIkParams;
50    use crate::CuDiffIkStatus;
51    use crate::CuIkParams;
52    use crate::CuIkSolver;
53    use crate::CuInverseKinematicsTask;
54    use crate::CuJointAxis;
55    use crate::CuJointLimits;
56    use crate::CuJointPosition;
57    use crate::CuJointPositionsPayload;
58    use crate::CuJointSpec;
59    use crate::CuJointVelocity;
60    use crate::CuKinematicChain;
61    use crate::CuServoTask;
62    use crate::CuTwist6D;
63    use crate::CuTwistFrame;
64    use crate::igus_rebel_chain;
65    use cu_spatial_payloads::Transform3D;
66    use cu29::prelude::*;
67    use cu29::units::si::angle::radian;
68    use cu29::units::si::angular_velocity::radian_per_second;
69    use cu29::units::si::f64::Angle as Angle64;
70    use cu29::units::si::f64::AngularVelocity;
71    use cu29::units::si::f64::Length;
72    use cu29::units::si::f64::Velocity;
73    use cu29::units::si::length::meter;
74    use cu29::units::si::velocity::meter_per_second;
75
76    fn m3(v: [f64; 3]) -> [Length; 3] {
77        v.map(Length::new::<meter>)
78    }
79    fn rad3(v: [f64; 3]) -> [Angle64; 3] {
80        v.map(Angle64::new::<radian>)
81    }
82    fn rad_limits(lo: f64, hi: f64, v: f64) -> CuJointLimits {
83        CuJointLimits::revolute(
84            Angle64::new::<radian>(lo),
85            Angle64::new::<radian>(hi),
86            AngularVelocity::new::<radian_per_second>(v),
87        )
88    }
89    fn mk_twist(lin: [f64; 3], ang: [f64; 3], frame: CuTwistFrame) -> CuTwist6D {
90        CuTwist6D::new(
91            lin.map(Velocity::new::<meter_per_second>),
92            ang.map(AngularVelocity::new::<radian_per_second>),
93            frame,
94        )
95    }
96
97    #[test]
98    fn mixed_joint_units_fixed_slots_and_metric_are_preserved() {
99        let chain = CuKinematicChain::new([
100            CuJointSpec::revolute(
101                m3([0.0; 3]),
102                rad3([0.0; 3]),
103                CuJointAxis::PosZ,
104                rad_limits(-3.0, 3.0, 1.0),
105            ),
106            CuJointSpec {
107                joint_type: crate::CuJointType::Fixed,
108                ..Default::default()
109            },
110            CuJointSpec::prismatic(
111                m3([0.0; 3]),
112                rad3([0.0; 3]),
113                CuJointAxis::Arbitrary([2.0, 0.0, 0.0]),
114                CuJointLimits::prismatic(
115                    Length::new::<meter>(0.0),
116                    Length::new::<meter>(1.0),
117                    Velocity::new::<meter_per_second>(0.2),
118                ),
119            ),
120        ]);
121        let q = [
122            Angle64::new::<radian>(0.0).into(),
123            CuJointPosition::Fixed,
124            cu29::units::si::f64::Length::new::<cu29::units::si::length::meter>(0.2).into(),
125        ];
126        let mut pose = Transform3D::<f64>::identity();
127        chain.forward_kinematics(&q, &mut pose).unwrap();
128        assert!((pose.to_matrix()[0][3] - 0.2).abs() < 1e-12);
129        let mut jac = [[0.0; 3]; 6];
130        chain.geometric_jacobian(&q, &mut jac).unwrap();
131        assert_eq!(jac[0][2], 1.0);
132        assert!(jac.iter().all(|row| row[1] == 0.0));
133        assert_eq!(crate::CuJointMetric::default().normalized(q[2]), 0.2);
134        let wrong = [q[0], q[1], q[0]];
135        assert!(!chain.is_within_limits(&wrong));
136        assert!(chain.forward_kinematics(&wrong, &mut pose).is_err());
137    }
138
139    #[test]
140    fn test_igus_rebel_zero_pose_fk() {
141        let chain = igus_rebel_chain();
142        let q_zero = [Angle64::new::<radian>(0.0).into(); 6];
143        let mut ee = Transform3D::<f64>::identity();
144        chain.forward_kinematics(&q_zero, &mut ee).unwrap();
145
146        let mat = ee.to_matrix();
147        // At q = [0; 6], y offsets (+0.0265 and -0.0265, -0.0182 and +0.0182) cancel out.
148        // Total height z = 0.1462 + 0.106 + 0.24152 + 0.157479407401356 + 0.142 + 0.0768 = 0.869999407401356 m.
149        // Because joint 1 has rpy = [0, 0, pi], the x offset of J_motor4 (+0.00134498...) points in -X in base_link.
150        assert!((mat[0][3] - (-0.0013449814287804)).abs() < 1e-12);
151        assert!(mat[1][3].abs() < 1e-12);
152        assert!((mat[2][3] - 0.869999407401356).abs() < 1e-12);
153    }
154
155    #[test]
156    fn test_geometric_jacobian_matches_finite_differences() {
157        let chain = igus_rebel_chain();
158        let q = [
159            Angle64::new::<radian>(0.25).into(),
160            Angle64::new::<radian>(-0.4).into(),
161            Angle64::new::<radian>(0.55).into(),
162            Angle64::new::<radian>(-0.3).into(),
163            Angle64::new::<radian>(0.45).into(),
164            Angle64::new::<radian>(0.15).into(),
165        ];
166
167        let mut jac = [[0.0_f64; 6]; 6];
168        chain.geometric_jacobian(&q, &mut jac).unwrap();
169
170        let eps = 1e-7;
171        for col in 0..6 {
172            let mut q_plus = q;
173            let mut q_minus = q;
174            q_plus[col] = Angle64::new::<radian>(q[col].si_value() + eps).into();
175            q_minus[col] = Angle64::new::<radian>(q[col].si_value() - eps).into();
176
177            let mut pose_plus = Transform3D::<f64>::identity();
178            let mut pose_minus = Transform3D::<f64>::identity();
179            chain.forward_kinematics(&q_plus, &mut pose_plus).unwrap();
180            chain.forward_kinematics(&q_minus, &mut pose_minus).unwrap();
181
182            let mp = pose_plus.to_matrix();
183            let mm = pose_minus.to_matrix();
184            for row in 0..3 {
185                let fd_vel = (mp[row][3] - mm[row][3]) / (2.0 * eps);
186                assert!(
187                    (jac[row][col] - fd_vel).abs() < 1e-5,
188                    "linear Jacobian mismatch at ({row}, {col}): analytic={} fd={fd_vel}",
189                    jac[row][col]
190                );
191            }
192        }
193    }
194
195    #[test]
196    fn test_body_jacobian_matches_finite_differences() {
197        let chain = igus_rebel_chain();
198        let q = [
199            Angle64::new::<radian>(0.25).into(),
200            Angle64::new::<radian>(-0.4).into(),
201            Angle64::new::<radian>(0.55).into(),
202            Angle64::new::<radian>(-0.3).into(),
203            Angle64::new::<radian>(0.45).into(),
204            Angle64::new::<radian>(0.15).into(),
205        ];
206
207        let mut ee = Transform3D::<f64>::identity();
208        chain.forward_kinematics(&q, &mut ee).unwrap();
209        let ee_inv = ee.inverse();
210
211        let mut body_jac = [[0.0_f64; 6]; 6];
212        chain.body_jacobian(&q, &mut body_jac).unwrap();
213
214        let eps = 1e-7;
215        for col in 0..6 {
216            let mut q_plus = q;
217            let mut q_minus = q;
218            q_plus[col] = Angle64::new::<radian>(q[col].si_value() + eps).into();
219            q_minus[col] = Angle64::new::<radian>(q[col].si_value() - eps).into();
220
221            let mut pose_plus = Transform3D::<f64>::identity();
222            let mut pose_minus = Transform3D::<f64>::identity();
223            chain.forward_kinematics(&q_plus, &mut pose_plus).unwrap();
224            chain.forward_kinematics(&q_minus, &mut pose_minus).unwrap();
225
226            // Relative transforms in end-effector frame: T_ee^-1 * T(q +- eps)
227            let rel_plus = ee_inv.compose(pose_plus).to_matrix();
228            let rel_minus = ee_inv.compose(pose_minus).to_matrix();
229
230            for row in 0..3 {
231                let fd_vel = (rel_plus[row][3] - rel_minus[row][3]) / (2.0 * eps);
232                assert!(
233                    (body_jac[row][col] - fd_vel).abs() < 1e-5,
234                    "body linear Jacobian mismatch at ({row}, {col}): analytic={} fd={fd_vel}",
235                    body_jac[row][col]
236                );
237            }
238
239            // Angular velocity components from skew-symmetric part of dR_rel / dq
240            let wx_fd = ((rel_plus[2][1] - rel_plus[1][2]) - (rel_minus[2][1] - rel_minus[1][2]))
241                / (4.0 * eps);
242            let wy_fd = ((rel_plus[0][2] - rel_plus[2][0]) - (rel_minus[0][2] - rel_minus[2][0]))
243                / (4.0 * eps);
244            let wz_fd = ((rel_plus[1][0] - rel_plus[0][1]) - (rel_minus[1][0] - rel_minus[0][1]))
245                / (4.0 * eps);
246
247            assert!((body_jac[3][col] - wx_fd).abs() < 1e-5);
248            assert!((body_jac[4][col] - wy_fd).abs() < 1e-5);
249            assert!((body_jac[5][col] - wz_fd).abs() < 1e-5);
250        }
251    }
252
253    #[test]
254    fn test_differential_ik_tracks_base_and_body_twists() {
255        let chain = igus_rebel_chain();
256        let q = [
257            Angle64::new::<radian>(0.2).into(),
258            Angle64::new::<radian>(0.45).into(),
259            Angle64::new::<radian>(-0.6).into(),
260            Angle64::new::<radian>(0.3).into(),
261            Angle64::new::<radian>(-0.5).into(),
262            Angle64::new::<radian>(0.2).into(),
263        ];
264
265        let cfg = CuDiffIkParams {
266            base_damping: 1e-4,
267            max_singularity_damping: 0.0,
268            singularity_threshold_sq: 0.0,
269            limit_margin: Angle64::new::<radian>(0.0),
270            linear_limit_margin: Default::default(),
271            enforce_velocity_limits: true,
272        };
273
274        let twist_base = mk_twist([0.05, -0.03, 0.04], [0.1, -0.05, 0.08], CuTwistFrame::Base);
275        let mut qd = [CuJointVelocity::default(); 6];
276        let status = chain
277            .differential_ik(&q, &twist_base, &cfg, &mut qd)
278            .unwrap();
279        assert!(matches!(status, CuDiffIkStatus::Nominal { .. }));
280
281        // Verify J_s * qd reproduces twist_base
282        let mut jac = [[0.0_f64; 6]; 6];
283        chain.geometric_jacobian(&q, &mut jac).unwrap();
284        let expected = twist_base.to_raw_array();
285        for (r, exp_r) in expected.iter().enumerate() {
286            let mut achieved = 0.0;
287            for (c, qd_c) in qd.iter().enumerate() {
288                achieved += jac[r][c] * qd_c.si_value();
289            }
290            assert!((achieved - exp_r).abs() < 1e-3);
291        }
292
293        // Verify body-frame twist tracking
294        let twist_body = mk_twist([0.04, 0.02, -0.03], [-0.05, 0.06, 0.04], CuTwistFrame::Body);
295        let mut qd_body = [CuJointVelocity::default(); 6];
296        chain
297            .differential_ik(&q, &twist_body, &cfg, &mut qd_body)
298            .unwrap();
299
300        let mut bjac = [[0.0_f64; 6]; 6];
301        chain.body_jacobian(&q, &mut bjac).unwrap();
302        let expected_body = twist_body.to_raw_array();
303        for (r, exp_r) in expected_body.iter().enumerate() {
304            let mut achieved = 0.0;
305            for (c, qd_c) in qd_body.iter().enumerate() {
306                achieved += bjac[r][c] * qd_c.si_value();
307            }
308            assert!((achieved - exp_r).abs() < 1e-3);
309        }
310    }
311
312    #[test]
313    fn test_differential_ik_singularity_and_velocity_scaling() {
314        let chain = igus_rebel_chain();
315        // Zero pose has wrist joint 5 = 0 (wrist singularity!)
316        let q_singular = [Angle64::new::<radian>(0.0).into(); 6];
317        let w_sq = chain.manipulability_squared(&q_singular).unwrap();
318        assert!(w_sq < 1e-6);
319
320        let cfg = CuDiffIkParams::default();
321        let twist = mk_twist([0.05, 0.0, 0.0], [0.0, 0.0, 0.0], CuTwistFrame::Base);
322        let mut qd = [CuJointVelocity::default(); 6];
323        let status = chain
324            .differential_ik(&q_singular, &twist, &cfg, &mut qd)
325            .unwrap();
326        assert!(matches!(status, CuDiffIkStatus::SingularityDamped { .. }));
327
328        // High twist triggers uniform velocity scaling
329        let fast_twist = mk_twist([5.0, 5.0, 5.0], [5.0, 5.0, 5.0], CuTwistFrame::Base);
330        let status_scaled = chain
331            .differential_ik(&q_singular, &fast_twist, &cfg, &mut qd)
332            .unwrap();
333        assert!(matches!(
334            status_scaled,
335            CuDiffIkStatus::VelocityScaled { .. }
336        ));
337        for (j, qd_j) in qd.iter().enumerate() {
338            let v = qd_j.si_value().abs();
339            let max_v = chain.joints()[j].limits.max_velocity().si_value();
340            assert!(v <= max_v + 1e-9);
341        }
342    }
343
344    #[test]
345    fn test_dls_ik_roundtrip_on_igus_rebel() {
346        let chain = igus_rebel_chain();
347        let q_goal = [
348            Angle64::new::<radian>(0.3).into(),
349            Angle64::new::<radian>(0.45).into(),
350            Angle64::new::<radian>(-0.35).into(),
351            Angle64::new::<radian>(0.25).into(),
352            Angle64::new::<radian>(-0.4).into(),
353            Angle64::new::<radian>(0.5).into(),
354        ];
355
356        let mut target_pose = Transform3D::<f64>::identity();
357        chain.forward_kinematics(&q_goal, &mut target_pose).unwrap();
358
359        let q_seed = [
360            Angle64::new::<radian>(0.1).into(),
361            Angle64::new::<radian>(0.2).into(),
362            Angle64::new::<radian>(-0.1).into(),
363            Angle64::new::<radian>(0.0).into(),
364            Angle64::new::<radian>(-0.1).into(),
365            Angle64::new::<radian>(0.1).into(),
366        ];
367
368        let cfg = CuIkParams::default();
369        let mut q_sol = [CuJointPosition::default(); 6];
370        let status = chain
371            .inverse_kinematics_dls(&target_pose, &q_seed, &cfg, &mut q_sol)
372            .unwrap();
373
374        assert!(status.is_converged(), "IK did not converge: {status:?}");
375        assert!(chain.is_within_limits(&q_sol));
376
377        let mut solved_pose = Transform3D::<f64>::identity();
378        chain.forward_kinematics(&q_sol, &mut solved_pose).unwrap();
379
380        let tgt = target_pose.to_matrix();
381        let sol = solved_pose.to_matrix();
382        for r in 0..3 {
383            assert!((tgt[r][3] - sol[r][3]).abs() < 1e-4);
384        }
385    }
386
387    #[test]
388    fn test_pseudoinverse_solver_converges() {
389        let chain = igus_rebel_chain();
390        let q_goal = [
391            Angle64::new::<radian>(0.2).into(),
392            Angle64::new::<radian>(-0.1).into(),
393            Angle64::new::<radian>(0.0).into(),
394            Angle64::new::<radian>(-0.1).into(),
395            Angle64::new::<radian>(0.1).into(),
396            Angle64::new::<radian>(0.0).into(),
397        ];
398        let mut target_pose = Transform3D::<f64>::identity();
399        chain.forward_kinematics(&q_goal, &mut target_pose).unwrap();
400        let cfg = CuIkParams {
401            solver: CuIkSolver::Pseudoinverse,
402            ..CuIkParams::default()
403        };
404        // Non-singular seed (the zero pose is singular for the undamped solve).
405        let q_seed = [
406            Angle64::new::<radian>(0.1).into(),
407            Angle64::new::<radian>(0.2).into(),
408            Angle64::new::<radian>(-0.1).into(),
409            Angle64::new::<radian>(0.0).into(),
410            Angle64::new::<radian>(-0.1).into(),
411            Angle64::new::<radian>(0.1).into(),
412        ];
413        let mut q_sol = [CuJointPosition::default(); 6];
414        let status = chain
415            .inverse_kinematics_dls(&target_pose, &q_seed, &cfg, &mut q_sol)
416            .unwrap();
417        assert!(
418            status.is_converged(),
419            "pseudoinverse IK did not converge: {status:?}"
420        );
421    }
422
423    #[test]
424    fn test_ik_and_servo_tasks() {
425        let (ctx, _mock) = CuContext::new_mock_clock();
426        let mut ik_task =
427            CuInverseKinematicsTask::<6>::from_params(igus_rebel_chain(), CuIkParams::default())
428                .unwrap();
429        let mut servo_task =
430            CuServoTask::<6>::from_params(igus_rebel_chain(), CuDiffIkParams::default()).unwrap();
431
432        let q_goal = [
433            Angle64::new::<radian>(0.2).into(),
434            Angle64::new::<radian>(0.35).into(),
435            Angle64::new::<radian>(-0.3).into(),
436            Angle64::new::<radian>(0.15).into(),
437            Angle64::new::<radian>(-0.35).into(),
438            Angle64::new::<radian>(0.25).into(),
439        ];
440        let mut target_pose = Transform3D::<f64>::identity();
441        ik_task
442            .chain()
443            .forward_kinematics(&q_goal, &mut target_pose)
444            .unwrap();
445
446        let seed_msg = CuMsg::new(Some(CuJointPositionsPayload {
447            positions: [Angle64::new::<radian>(0.05).into(); 6],
448        }));
449        let pose_msg = CuMsg::new(Some(target_pose));
450        let mut ik_out = CuMsg::new(None);
451
452        ik_task
453            .process(&ctx, &(&pose_msg, &seed_msg), &mut ik_out)
454            .unwrap();
455        let sol = ik_out.payload().unwrap();
456        assert!(sol.status.is_converged());
457
458        let twist_msg = CuMsg::new(Some(mk_twist(
459            [0.02, 0.0, -0.01],
460            [0.0, 0.05, 0.0],
461            CuTwistFrame::Base,
462        )));
463        let joints_msg = CuMsg::new(Some(CuJointPositionsPayload { positions: q_goal }));
464        let mut servo_out = CuMsg::new(None);
465        servo_task
466            .process(&ctx, &(&twist_msg, &joints_msg), &mut servo_out)
467            .unwrap();
468        assert!(servo_out.payload().is_some());
469    }
470
471    #[test]
472    fn test_two_link_planar_arm() {
473        let chain = CuKinematicChain::new([
474            CuJointSpec::revolute(
475                m3([0.0, 0.0, 0.0]),
476                rad3([0.0, 0.0, 0.0]),
477                CuJointAxis::PosZ,
478                rad_limits(-core::f64::consts::PI, core::f64::consts::PI, 1.0),
479            ),
480            CuJointSpec::revolute(
481                m3([1.0, 0.0, 0.0]),
482                rad3([0.0, 0.0, 0.0]),
483                CuJointAxis::PosZ,
484                rad_limits(-core::f64::consts::PI, core::f64::consts::PI, 1.0),
485            ),
486        ]);
487        let mut ee = Transform3D::<f64>::identity();
488        chain
489            .forward_kinematics(
490                &[
491                    Angle64::new::<radian>(core::f64::consts::FRAC_PI_2).into(),
492                    Angle64::new::<radian>(0.0).into(),
493                ],
494                &mut ee,
495            )
496            .unwrap();
497        let m = ee.to_matrix();
498        assert!(m[0][3].abs() < 1e-12);
499        assert!((m[1][3] - 1.0).abs() < 1e-12);
500    }
501}
repose_kinematics/lib.rs

repose_kinematics/
lib.rs
