orbita3d_kinematics/
lib.rs1use nalgebra::{Matrix3, Rotation3, Vector3};
12
13pub mod conversion;
14mod jacobian;
15mod position;
16mod torque;
17mod velocity;
18
19pub use position::InverseSolutionErrorKind;
20use serde::{Deserialize, Serialize};
21
22#[derive(Debug, Copy, Clone, Deserialize, Serialize)]
23pub struct Orbita3dKinematicsModel {
25 pub alpha: f64,
26 pub gamma_min: f64,
27 pub offset: f64,
28 pub beta: f64,
29 pub gamma_max: f64,
30 pub passiv_arms_direct: bool,
31}
32
33impl Default for Orbita3dKinematicsModel {
34 fn default() -> Self {
36 Orbita3dKinematicsModel {
37 alpha: 54.0_f64.to_radians(),
38 gamma_min: 0.0_f64.to_radians(),
39 offset: 0.0_f64.to_radians(),
40 beta: 90.0_f64.to_radians(),
41 gamma_max: 180.0_f64.to_radians(),
42 passiv_arms_direct: true,
43 }
44 }
45}
46
47impl Orbita3dKinematicsModel {
48 fn platform_unit_vectors_from_mat(&self, rot: Rotation3<f64>) -> Matrix3<f64> {
49 let delta_v = 120.0_f64.to_radians();
51 let mut beta = self.beta;
52
53 if !self.passiv_arms_direct {
54 beta *= -1.0;
55 }
56 let v_initial1 = Vector3::from_row_slice(&[beta.cos(), beta.sin(), 0.]);
57 let v_initial2 =
58 Vector3::from_row_slice(&[(beta + delta_v).cos(), (beta + delta_v).sin(), 0.]);
59 let v_initial3 =
60 Vector3::from_row_slice(&[(beta - delta_v).cos(), (beta - delta_v).sin(), 0.]);
61
62 let roffset = Rotation3::from_euler_angles(0., 0., self.offset);
63
64 let v_initial1 = roffset * v_initial1;
65 let v_initial2 = roffset * v_initial2;
66 let v_initial3 = roffset * v_initial3;
67
68 let v_rotation = Matrix3::from_columns(&[v_initial1, v_initial2, v_initial3]);
69
70 rot * v_rotation
71 }
72}