orbita3d_kinematics/
lib.rs

1//! Kinematics model for Orbita3d
2//!
3//! ### Model definition
4//! * _thetas_ disk angles
5//! * _rot_ platform orientation
6//!
7//! All values are expressed in radians.
8//!
9//! See the [README.md](./README.md) for more information.
10
11use 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)]
23/// Kinematics model for Orbita3d
24pub 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    /// Creates a new Orbita3dKinematicsModel with default values corresponding to Reachy's neck.
35    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        // Compute the unit vectors of the platform in the base frame, from an input rotation matrix
50        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}
orbita3d_kinematics/lib.rs

orbita3d_kinematics/
lib.rs
