orbita3d_kinematics/position/
forward.rs

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    /// Compute the position forward kinematics
10    ///
11    /// Compute the output 3d orientation given the 3 angles of the motors (in radians)
12    ///
13    /// # Arguments
14    /// * thetas - the angles of the motors (in radians) without the 120° offsets
15    /// # Returns
16    /// * the 3d orientation of the platform
17    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        // Forward of Orbita, takes the disk position (in radians) and return a rotation matrix
25        // self.p = Vector6f64::from_row_slice(&[0., 1., 0., 1., 0., 1.]); //reset p?
26        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        // from the phis and the thetas we can compute the v_i vectors
45        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]]); //Why the f*ck can't I use slice here?
89                                                                                      // let vel = self
90                                                                                      //     .kinematics
91                                                                                      //     .compute_output_velocity(thetas, [fb[3], fb[4], fb[5]]);
92                                                                                      // let torque = self
93                                                                                      //     .kinematics
94                                                                                      //     .compute_output_torque(thetas, [fb[6], fb[7], fb[8]]);
95                                                                                      // let rpy_test = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
96
97        // When do we know that the |yaw|>=180°? is min(disks)>=180°? check if forward/inverse is the same?
98        let rpy = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
99        let ik = self.compute_inverse_kinematics_rpy_multiturn(rpy);
100
101        // let ik = self.compute_inverse_kinematics(rot);
102        // let mut ik_disks: [f64; 3] = [0.0, 0.0, 0.0];
103        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                    //Extract the "yaw" component of the disks
112                    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 rot_yawonly =
117                    //     conversion::intrinsic_roll_pitch_yaw_to_matrix(0.0, 0.0, rpy[2]);
118                    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 disk_yaw_comp =
128                            //     self.kinematics.compute_inverse_kinematics(rot_yawonly)?;
129
130                            // What is the sign of the disk angles? if the yaw >180° the sum is positive, if yaw<-180° the sum is negative
131                            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                            // From the average yaw of the disks, compute the real rpy
153                            // it can be 180<|yaw|<360 or |yaw|>360
154                            let nb_turns = (disk_yaw_avg / std::f64::consts::TAU).trunc(); //number of full turn
155                                                                                           // let nb_turns: f64 =
156                                                                                           //     (disk_yaw_avg / std::f64::consts::TAU).round();
157
158                            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                                // We are in 180<|yaw|<360
164                                if nb_turns.abs() > 0.0 || nb_turns == -1.0
165                                // && (disk_yaw_avg - rpy[2]).abs() > 0.1
166                                {
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                                // We are in |yaw|>360 => how many turns?
177
178                                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                // ik_disks = disks; //??
189                log::debug!("No extra yaw THETAS: {:?} thetas: {:?}", thetas, disks);
190                // let rpy = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
191                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        // Compute the phi angles with a least square minimization of the system of equations
224        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    // Find the rotation matrix to align two sets of vectors (based on scipy implementation)
262    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        // compute the system of equations
311        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
356// We implement a trait for every problem we want to solve
357impl 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        // do common calculations for residuals and the Jacobian here
365    }
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        // Compute the Jacobian of the optimization problem ie.
377        // the matrix for which each rows are the derivatives of each equation by a parameter
378
379        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        // derivative of all the eq by cp1
404        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        // derivation by sp1
440        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        // derivation by cp2
467        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        // derivative by sp2
503        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        // derivation by cp3
530        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        // derivative by sp3
566        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}