1use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
2use nalgebra::{Matrix3, Owned, Rotation3, SMatrix, SVector, Vector3, U12, U6};
3use ndarray_einsum_beta::einsum;
4use nshare::{RefNdarray2, ToNalgebra};
5
6use crate::{conversion, InverseSolutionErrorKind, Orbita3dKinematicsModel};
7
8impl Orbita3dKinematicsModel {
9 pub fn compute_forward_kinematics(&self, thetas: [f64; 3]) -> Rotation3<f64> {
18 let thetas = Vector3::from_row_slice(&[
19 thetas[0],
20 thetas[1] + 120.0_f64.to_radians(),
21 thetas[2] - 120.0_f64.to_radians(),
22 ]);
23
24 let st1 = thetas[0].sin();
27 let st2 = thetas[1].sin();
28 let st3 = thetas[2].sin();
29
30 let ct1 = thetas[0].cos();
31 let ct2 = thetas[1].cos();
32 let ct3 = thetas[2].cos();
33
34 let phis = self.compute_phis(thetas).unwrap();
35 let sp1_n = phis[0];
36 let cp1_n = phis[1];
37
38 let sp2_n = phis[2];
39 let cp2_n = phis[3];
40
41 let sp3_n = phis[4];
42 let cp3_n = phis[5];
43
44 let mut v1sol = self.v_i(cp1_n, sp1_n, ct1, st1);
46 let mut v2sol = self.v_i(cp2_n, sp2_n, ct2, st2);
47 let mut v3sol = self.v_i(cp3_n, sp3_n, ct3, st3);
48
49 let roffset = Rotation3::from_euler_angles(0., 0., self.beta);
50 v1sol = roffset * v1sol;
51 v2sol = roffset * v2sol;
52 v3sol = roffset * v3sol;
53
54 let v_mat = Matrix3::from_columns(&[v1sol, v2sol, v3sol]);
55
56 let mut b1 =
57 Vector3::from_row_slice(&[0.0_f64.to_radians().cos(), 0.0_f64.to_radians().sin(), 0.]);
58
59 let mut b2 = Vector3::from_row_slice(&[
60 120.0_f64.to_radians().cos(),
61 120.0_f64.to_radians().sin(),
62 0.,
63 ]);
64
65 let mut b3 = Vector3::from_row_slice(&[
66 (-120.0_f64).to_radians().cos(),
67 (-120.0_f64).to_radians().sin(),
68 0.,
69 ]);
70
71 b1 = roffset * b1;
72 b2 = roffset * b2;
73 b3 = roffset * b3;
74
75 let off = Rotation3::from_euler_angles(0.0, 0.0, self.offset);
76 b1 = off * b1;
77 b2 = off * b2;
78 b3 = off * b3;
79
80 let b_mat = Matrix3::from_columns(&[b1, b2, b3]);
81 align_vectors(v_mat.transpose(), b_mat.transpose())
82 }
83
84 pub fn compute_forward_kinematics_rpy_multiturn(
85 &self,
86 thetas: [f64; 3],
87 ) -> Result<[f64; 3], InverseSolutionErrorKind> {
88 let rot = self.compute_forward_kinematics([thetas[0], thetas[1], thetas[2]]); let rpy = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
99 let ik = self.compute_inverse_kinematics_rpy_multiturn(rpy);
100
101 match ik {
104 Ok(disks) => {
105 if (thetas[0] - disks[0]).abs() >= 0.01_f64.to_radians()
106 || (thetas[1] - disks[1]).abs() >= 0.01_f64.to_radians()
107 || (thetas[2] - disks[2]).abs() >= 0.01_f64.to_radians()
108 {
109 log::debug!("IK/FK mismatch. Probable >180° rotation of disks");
110
111 let mut rpy = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
113 log::debug!("=> rpy: {:?}", rpy);
114 let rot_noyaw =
115 conversion::intrinsic_roll_pitch_yaw_to_matrix(rpy[0], rpy[1], 0.0);
116 let ik_noyaw = self.compute_inverse_kinematics(rot_noyaw);
119 match ik_noyaw {
120 Ok(disks_noyaw) => {
121 let disk_yaw_comp: [f64; 3] = [
122 thetas[0] - disks_noyaw[0],
123 thetas[1] - disks_noyaw[1],
124 thetas[2] - disks_noyaw[2],
125 ];
126
127 let mut disk_yaw_avg: f64 = disk_yaw_comp.iter().sum::<f64>();
132 disk_yaw_avg /= 3.0;
133 log::debug!(
134 "AVERAGE YAW: {} YAW COMPONENT: {:?} NO_YAW: {:?}",
135 disk_yaw_avg,
136 disk_yaw_comp,
137 disks_noyaw
138 );
139
140 if rpy[2].signum() != disk_yaw_avg.signum() {
141 log::debug!("bad yaw sign");
142 if rpy[2] < 0.0 {
143 log::debug!("\t+TAU");
144 rpy[2] += std::f64::consts::TAU;
145 } else {
146 log::debug!("\t-TAU");
147 rpy[2] -= std::f64::consts::TAU;
148 }
149 }
150 log::debug!("=> RPY with yaw sign {:?}", rpy);
151
152 let nb_turns = (disk_yaw_avg / std::f64::consts::TAU).trunc(); log::debug!("=> nb_turns {:?}", nb_turns);
159
160 if (disk_yaw_avg.abs() >= std::f64::consts::PI)
161 && (disk_yaw_avg.abs() < std::f64::consts::TAU)
162 {
163 if nb_turns.abs() > 0.0 || nb_turns == -1.0
165 {
167 log::debug!(
168 "Adding offset {}",
169 disk_yaw_avg.signum() * std::f64::consts::TAU
170 );
171 rpy[2] += disk_yaw_avg.signum() * std::f64::consts::TAU;
172 }
173
174 log::debug!("180<|yaw|<360: {}", rpy[2]);
175 } else {
176 rpy[2] += nb_turns * std::f64::consts::TAU;
179 log::debug!("|yaw|>360: nb_turns {nb_turns} {}", rpy[2]);
180 }
181 log::debug!("=> Out RPY {:?}", rpy);
182 return Ok(rpy);
183 }
184 Err(e) => log::error!("IK error? {e}"),
185 }
186 }
187 log::debug!("IK/FK match");
188 log::debug!("No extra yaw THETAS: {:?} thetas: {:?}", thetas, disks);
190 log::debug!("=> rpy: {:?}", rpy);
192
193 Ok(rpy)
194 }
195 Err(e) => {
196 log::error!("IK error for rpy: {:?}", rpy);
197 Err(e)
198 }
199 }
200 }
201
202 fn v_i(&self, cpi: f64, spi: f64, cti: f64, sti: f64) -> Vector3<f64> {
203 let sa1 = self.alpha.sin();
204 let sa2 = self.beta.sin();
205
206 let ca1 = self.alpha.cos();
207 let ca2 = self.beta.cos();
208
209 Vector3::from_row_slice(&[
210 -ca1 * cpi * sa2 * sti + ca2 * sa1 * sti + cti * sa2 * spi,
211 ca1 * cpi * cti * sa2 - ca2 * cti * sa1 + sa2 * spi * sti,
212 -ca1 * ca2 - cpi * sa1 * sa2,
213 ])
214 }
215 fn w_i(&self, cti: f64, sti: f64) -> Vector3<f64> {
216 let sa1 = self.alpha.sin();
217 let ca1 = self.alpha.cos();
218
219 Vector3::from_row_slice(&[sa1 * sti, -cti * sa1, -ca1])
220 }
221
222 fn compute_phis(&self, thetas: Vector3<f64>) -> Option<SVector<f64, 9>> {
223 let lm = LevenbergMarquardt::new().with_xtol(f64::EPSILON);
225
226 let problem = Orbita3dForwardProblem {
227 kin: *self,
228 p: [0., 1., 0., 1., 0., 1.].into(),
229 thetas,
230 };
231 let (result, report) = lm.minimize(problem);
232
233 if !report.termination.was_successful() {
234 return None;
235 }
236
237 let cp1_n = result.p[0];
238 let sp1_n = result.p[1];
239 let cp2_n = result.p[2];
240 let sp2_n = result.p[3];
241 let cp3_n = result.p[4];
242 let sp3_n = result.p[5];
243
244 let mut phi1 = sp1_n.atan2(cp1_n);
245 let mut phi2 = sp2_n.atan2(cp2_n);
246 let mut phi3 = sp3_n.atan2(cp3_n);
247
248 if !self.passiv_arms_direct {
249 phi1 += std::f64::consts::PI;
250 phi2 += std::f64::consts::PI;
251 phi3 += std::f64::consts::PI;
252 }
253
254 Some(SVector::from_row_slice(&[
255 sp1_n, cp1_n, sp2_n, cp2_n, sp3_n, cp3_n, phi1, phi2, phi3,
256 ]))
257 }
258}
259
260fn align_vectors(a: Matrix3<f64>, b: Matrix3<f64>) -> Rotation3<f64> {
261 let na = a.ref_ndarray2().into_shape((3, 3)).unwrap();
263 let nb = b.ref_ndarray2().into_shape((3, 3)).unwrap();
264
265 let mat_b = einsum("ji,jk->ik", &[&na, &nb])
266 .unwrap()
267 .into_shape((3, 3))
268 .unwrap();
269
270 let matrix_b = mat_b.view().into_nalgebra();
271
272 let mat_svd = matrix_b.svd(true, true);
273 let mut u = mat_svd.u.unwrap();
274 let vh = mat_svd.v_t.unwrap();
275
276 let uv = u.clone() * vh.clone();
277
278 if uv.determinant() < 0.0 {
279 u.set_column(
280 2,
281 &Vector3::from_row_slice(&[-u.column(2)[0], -u.column(2)[1], -u.column(2)[2]]),
282 );
283 }
284
285 let mat_c = u * vh;
286
287 let m = Matrix3::from_row_slice(&[
288 mat_c.row(0)[0],
289 mat_c.row(0)[1],
290 mat_c.row(0)[2],
291 mat_c.row(1)[0],
292 mat_c.row(1)[1],
293 mat_c.row(1)[2],
294 mat_c.row(2)[0],
295 mat_c.row(2)[1],
296 mat_c.row(2)[2],
297 ]);
298
299 Rotation3::from_matrix_unchecked(m)
300}
301
302struct Orbita3dForwardProblem {
303 kin: Orbita3dKinematicsModel,
304 thetas: Vector3<f64>,
305 p: SVector<f64, 6>,
306}
307
308impl Orbita3dForwardProblem {
309 fn phis_system_equations(&self) -> SVector<f64, 12> {
310 let cp1 = self.p[0];
312 let sp1 = self.p[1];
313 let cp2 = self.p[2];
314 let sp2 = self.p[3];
315 let cp3 = self.p[4];
316 let sp3 = self.p[5];
317 let ca2 = self.kin.beta.cos();
318
319 let st1 = self.thetas[0].sin();
320 let st2 = self.thetas[1].sin();
321 let st3 = self.thetas[2].sin();
322 let ct1 = self.thetas[0].cos();
323 let ct2 = self.thetas[1].cos();
324 let ct3 = self.thetas[2].cos();
325
326 let v1n = self.kin.v_i(cp1, sp1, ct1, st1);
327 let v2n = self.kin.v_i(cp2, sp2, ct2, st2);
328 let v3n = self.kin.v_i(cp3, sp3, ct3, st3);
329
330 let w1n = self.kin.w_i(ct1, st1);
331 let w2n = self.kin.w_i(ct2, st2);
332 let w3n = self.kin.w_i(ct3, st3);
333
334 let eq1 = v1n.dot(&v2n) + 0.5;
335 let eq2 = v2n.dot(&v3n) + 0.5;
336 let eq3 = v3n.dot(&v1n) + 0.5;
337
338 let eq4 = w1n.dot(&v1n) - ca2;
339 let eq5 = w2n.dot(&v2n) - ca2;
340 let eq6 = w3n.dot(&v3n) - ca2;
341
342 let eq7 = v1n.dot(&v1n) - 1.;
343 let eq8 = v2n.dot(&v2n) - 1.;
344 let eq9 = v3n.dot(&v3n) - 1.;
345
346 let eq10 = v1n[0] + v2n[0] + v3n[0];
347 let eq11 = v1n[1] + v2n[1] + v3n[1];
348 let eq12 = v1n[2] + v2n[2] + v3n[2];
349
350 SVector::from_row_slice(&[
351 eq1, eq2, eq3, eq4, eq5, eq6, eq7, eq8, eq9, eq10, eq11, eq12,
352 ])
353 }
354}
355
356impl LeastSquaresProblem<f64, U12, U6> for Orbita3dForwardProblem {
358 type ParameterStorage = Owned<f64, U6>;
359 type ResidualStorage = Owned<f64, U12>;
360 type JacobianStorage = Owned<f64, U12, U6>;
361
362 fn set_params(&mut self, p: &SVector<f64, 6>) {
363 self.p = *p;
364 }
366
367 fn params(&self) -> SVector<f64, 6> {
368 self.p
369 }
370
371 fn residuals(&self) -> Option<SVector<f64, 12>> {
372 Some(self.phis_system_equations())
373 }
374
375 fn jacobian(&self) -> Option<SMatrix<f64, 12, 6>> {
376 let sa1 = self.kin.alpha.sin();
380 let sa2 = self.kin.beta.sin();
381
382 let ca1 = self.kin.alpha.cos();
383 let ca2 = self.kin.beta.cos();
384
385 let cp1 = self.p[0];
386 let sp1 = self.p[1];
387
388 let cp2 = self.p[2];
389 let sp2 = self.p[3];
390
391 let cp3 = self.p[4];
392 let sp3 = self.p[5];
393
394 let st1 = self.thetas[0].sin();
395 let ct1 = self.thetas[0].cos();
396
397 let st2 = self.thetas[1].sin();
398 let ct2 = self.thetas[1].cos();
399
400 let st3 = self.thetas[2].sin();
401 let ct3 = self.thetas[2].cos();
402
403 let col1 = SVector::from_row_slice(&[
405 ca1.powi(2) * cp2 * ct1 * ct2 * sa2.powi(2)
406 + ca1.powi(2) * cp2 * sa2.powi(2) * st1 * st2
407 - ca1 * ca2 * ct1 * ct2 * sa1 * sa2
408 - ca1 * ca2 * sa1 * sa2 * st1 * st2
409 + ca1 * ca2 * sa1 * sa2
410 + ca1 * ct1 * sa2.powi(2) * sp2 * st2
411 - ca1 * ct2 * sa2.powi(2) * sp2 * st1
412 + cp2 * sa1.powi(2) * sa2.powi(2),
413 0.0,
414 ca1.powi(2) * cp3 * ct1 * ct3 * sa2.powi(2)
415 + ca1.powi(2) * cp3 * sa2.powi(2) * st1 * st3
416 - ca1 * ca2 * ct1 * ct3 * sa1 * sa2
417 - ca1 * ca2 * sa1 * sa2 * st1 * st3
418 + ca1 * ca2 * sa1 * sa2
419 + ca1 * ct1 * sa2.powi(2) * sp3 * st3
420 - ca1 * ct3 * sa2.powi(2) * sp3 * st1
421 + cp3 * sa1.powi(2) * sa2.powi(2),
422 -ca1 * ct1.powi(2) * sa1 * sa2 - ca1 * sa1 * sa2 * st1.powi(2) + ca1 * sa1 * sa2,
423 0.0,
424 0.0,
425 2.0 * ca1 * ct1 * sa2 * (ca1 * cp1 * ct1 * sa2 - ca2 * ct1 * sa1 + sa2 * sp1 * st1)
426 - 2.0
427 * ca1
428 * sa2
429 * st1
430 * (-ca1 * cp1 * sa2 * st1 + ca2 * sa1 * st1 + ct1 * sa2 * sp1)
431 - 2.0 * sa1 * sa2 * (-ca1 * ca2 - cp1 * sa1 * sa2),
432 0.0,
433 0.0,
434 -ca1 * sa2 * st1,
435 ca1 * ct1 * sa2,
436 -sa1 * sa2,
437 ]);
438
439 let col2 = SVector::from_row_slice(&[
441 -ca1 * cp2 * ct1 * sa2.powi(2) * st2
442 + ca1 * cp2 * ct2 * sa2.powi(2) * st1
443 + ca2 * ct1 * sa1 * sa2 * st2
444 - ca2 * ct2 * sa1 * sa2 * st1
445 + ct1 * ct2 * sa2.powi(2) * sp2
446 + sa2.powi(2) * sp2 * st1 * st2,
447 0.0,
448 -ca1 * cp3 * ct1 * sa2.powi(2) * st3
449 + ca1 * cp3 * ct3 * sa2.powi(2) * st1
450 + ca2 * ct1 * sa1 * sa2 * st3
451 - ca2 * ct3 * sa1 * sa2 * st1
452 + ct1 * ct3 * sa2.powi(2) * sp3
453 + sa2.powi(2) * sp3 * st1 * st3,
454 0.0,
455 0.0,
456 0.0,
457 2.0 * ct1 * sa2 * (-ca1 * cp1 * sa2 * st1 + ca2 * sa1 * st1 + ct1 * sa2 * sp1)
458 + 2.0 * sa2 * st1 * (ca1 * cp1 * ct1 * sa2 - ca2 * ct1 * sa1 + sa2 * sp1 * st1),
459 0.0,
460 0.0,
461 ct1 * sa2,
462 sa2 * st1,
463 0.0,
464 ]);
465
466 let col3 = SVector::from_row_slice(&[
468 ca1.powi(2) * cp1 * ct1 * ct2 * sa2.powi(2)
469 + ca1.powi(2) * cp1 * sa2.powi(2) * st1 * st2
470 - ca1 * ca2 * ct1 * ct2 * sa1 * sa2
471 - ca1 * ca2 * sa1 * sa2 * st1 * st2
472 + ca1 * ca2 * sa1 * sa2
473 - ca1 * ct1 * sa2.powi(2) * sp1 * st2
474 + ca1 * ct2 * sa2.powi(2) * sp1 * st1
475 + cp1 * sa1.powi(2) * sa2.powi(2),
476 ca1.powi(2) * cp3 * ct2 * ct3 * sa2.powi(2)
477 + ca1.powi(2) * cp3 * sa2.powi(2) * st2 * st3
478 - ca1 * ca2 * ct2 * ct3 * sa1 * sa2
479 - ca1 * ca2 * sa1 * sa2 * st2 * st3
480 + ca1 * ca2 * sa1 * sa2
481 + ca1 * ct2 * sa2.powi(2) * sp3 * st3
482 - ca1 * ct3 * sa2.powi(2) * sp3 * st2
483 + cp3 * sa1.powi(2) * sa2.powi(2),
484 0.0,
485 0.0,
486 -ca1 * ct2.powi(2) * sa1 * sa2 - ca1 * sa1 * sa2 * st2.powi(2) + ca1 * sa1 * sa2,
487 0.0,
488 0.0,
489 2.0 * ca1 * ct2 * sa2 * (ca1 * cp2 * ct2 * sa2 - ca2 * ct2 * sa1 + sa2 * sp2 * st2)
490 - 2.0
491 * ca1
492 * sa2
493 * st2
494 * (-ca1 * cp2 * sa2 * st2 + ca2 * sa1 * st2 + ct2 * sa2 * sp2)
495 - 2.0 * sa1 * sa2 * (-ca1 * ca2 - cp2 * sa1 * sa2),
496 0.0,
497 -ca1 * sa2 * st2,
498 ca1 * ct2 * sa2,
499 -sa1 * sa2,
500 ]);
501
502 let col4 = SVector::from_row_slice(&[
504 ca1 * cp1 * ct1 * sa2.powi(2) * st2
505 - ca1 * cp1 * ct2 * sa2.powi(2) * st1
506 - ca2 * ct1 * sa1 * sa2 * st2
507 + ca2 * ct2 * sa1 * sa2 * st1
508 + ct1 * ct2 * sa2.powi(2) * sp1
509 + sa2.powi(2) * sp1 * st1 * st2,
510 -ca1 * cp3 * ct2 * sa2.powi(2) * st3
511 + ca1 * cp3 * ct3 * sa2.powi(2) * st2
512 + ca2 * ct2 * sa1 * sa2 * st3
513 - ca2 * ct3 * sa1 * sa2 * st2
514 + ct2 * ct3 * sa2.powi(2) * sp3
515 + sa2.powi(2) * sp3 * st2 * st3,
516 0.0,
517 0.0,
518 0.0,
519 0.0,
520 0.0,
521 2.0 * ct2 * sa2 * (-ca1 * cp2 * sa2 * st2 + ca2 * sa1 * st2 + ct2 * sa2 * sp2)
522 + 2.0 * sa2 * st2 * (ca1 * cp2 * ct2 * sa2 - ca2 * ct2 * sa1 + sa2 * sp2 * st2),
523 0.0,
524 ct2 * sa2,
525 sa2 * st2,
526 0.0,
527 ]);
528
529 let col5 = SVector::from_row_slice(&[
531 0.0,
532 ca1.powi(2) * cp2 * ct2 * ct3 * sa2.powi(2)
533 + ca1.powi(2) * cp2 * sa2.powi(2) * st2 * st3
534 - ca1 * ca2 * ct2 * ct3 * sa1 * sa2
535 - ca1 * ca2 * sa1 * sa2 * st2 * st3
536 + ca1 * ca2 * sa1 * sa2
537 - ca1 * ct2 * sa2.powi(2) * sp2 * st3
538 + ca1 * ct3 * sa2.powi(2) * sp2 * st2
539 + cp2 * sa1.powi(2) * sa2.powi(2),
540 ca1.powi(2) * cp1 * ct1 * ct3 * sa2.powi(2)
541 + ca1.powi(2) * cp1 * sa2.powi(2) * st1 * st3
542 - ca1 * ca2 * ct1 * ct3 * sa1 * sa2
543 - ca1 * ca2 * sa1 * sa2 * st1 * st3
544 + ca1 * ca2 * sa1 * sa2
545 - ca1 * ct1 * sa2.powi(2) * sp1 * st3
546 + ca1 * ct3 * sa2.powi(2) * sp1 * st1
547 + cp1 * sa1.powi(2) * sa2.powi(2),
548 0.0,
549 0.0,
550 -ca1 * ct3.powi(2) * sa1 * sa2 - ca1 * sa1 * sa2 * st3.powi(2) + ca1 * sa1 * sa2,
551 0.0,
552 0.0,
553 2.0 * ca1 * ct3 * sa2 * (ca1 * cp3 * ct3 * sa2 - ca2 * ct3 * sa1 + sa2 * sp3 * st3)
554 - 2.0
555 * ca1
556 * sa2
557 * st3
558 * (-ca1 * cp3 * sa2 * st3 + ca2 * sa1 * st3 + ct3 * sa2 * sp3)
559 - 2.0 * sa1 * sa2 * (-ca1 * ca2 - cp3 * sa1 * sa2),
560 -ca1 * sa2 * st3,
561 ca1 * ct3 * sa2,
562 -sa1 * sa2,
563 ]);
564
565 let col6 = SVector::from_row_slice(&[
567 0.0,
568 ca1 * cp2 * ct2 * sa2.powi(2) * st3
569 - ca1 * cp2 * ct3 * sa2.powi(2) * st2
570 - ca2 * ct2 * sa1 * sa2 * st3
571 + ca2 * ct3 * sa1 * sa2 * st2
572 + ct2 * ct3 * sa2.powi(2) * sp2
573 + sa2.powi(2) * sp2 * st2 * st3,
574 ca1 * cp1 * ct1 * sa2.powi(2) * st3
575 - ca1 * cp1 * ct3 * sa2.powi(2) * st1
576 - ca2 * ct1 * sa1 * sa2 * st3
577 + ca2 * ct3 * sa1 * sa2 * st1
578 + ct1 * ct3 * sa2.powi(2) * sp1
579 + sa2.powi(2) * sp1 * st1 * st3,
580 0.0,
581 0.0,
582 0.0,
583 0.0,
584 0.0,
585 2.0 * ct3 * sa2 * (-ca1 * cp3 * sa2 * st3 + ca2 * sa1 * st3 + ct3 * sa2 * sp3)
586 + 2.0 * sa2 * st3 * (ca1 * cp3 * ct3 * sa2 - ca2 * ct3 * sa1 + sa2 * sp3 * st3),
587 ct3 * sa2,
588 sa2 * st3,
589 0.0,
590 ]);
591
592 let j = SMatrix::from_columns(&[col1, col2, col3, col4, col5, col6]);
593
594 Some(j)
595 }
596}