orbita3d_c_api/
kinematics.rs

1//! C API for the kinematics module of the Orbita3D library.
2
3use orbita3d_kinematics::conversion;
4
5#[repr(C)]
6#[derive(Debug, Copy, Clone)]
7/// Kinematics model for Orbita3d
8pub struct Orbita3dKinematicsModel {
9    pub alpha: f64,
10    pub gamma_min: f64,
11    pub offset: f64,
12    pub beta: f64,
13    pub gamma_max: f64,
14    pub passiv_arms_direct: bool,
15}
16
17impl Orbita3dKinematicsModel {
18    #[no_mangle]
19    /// Create a new Orbita3dKinematicsModel.
20    pub extern "C" fn create_orbita3d_kinematics_model(
21        alpha: f64,
22        gamma_min: f64,
23        offset: f64,
24        beta: f64,
25        gamma_max: f64,
26        passiv_arms_direct: bool,
27    ) -> Self {
28        Orbita3dKinematicsModel {
29            alpha,
30            gamma_min,
31            offset,
32            beta,
33            gamma_max,
34            passiv_arms_direct,
35        }
36    }
37
38    #[no_mangle]
39    /// Compute the forward position kinematics.
40    ///
41    /// # Arguments
42    /// * thetas - The motor angles as a 3-element array.
43    /// * quat - Holder for the platform orientation as a quaternion result.
44    /// # Returns
45    /// * 0 if success, 1 if error.
46    pub extern "C" fn orbita3d_kinematics_forward_position(
47        &self,
48        thetas: &[f64; 3],
49        quat: &mut [f64; 4],
50    ) -> i32 {
51        *quat = conversion::rotation_matrix_to_quaternion(
52            self.inner().compute_forward_kinematics(*thetas),
53        );
54        0
55    }
56
57    #[no_mangle]
58    /// Compute the forward velocity.
59    ///
60    /// # Arguments
61    /// * thetas - The motor angles as a 3-element array.
62    /// * thetas_velocity - The motor velocities as a 3-element array.
63    /// * output_velocity - Holder for the platform orientation velocity as a velocity pseudo vector.
64    /// # Returns
65    /// * 0 if success, 1 if error.
66    pub extern "C" fn orbita3d_kinematics_forward_velocity(
67        &self,
68        thetas: &[f64; 3],
69        thetas_velocity: &[f64; 3],
70        output_velocity: &mut [f64; 3],
71    ) -> i32 {
72        let vel = self
73            .inner()
74            .compute_output_velocity(*thetas, *thetas_velocity);
75        *output_velocity = vel.into();
76
77        0
78    }
79
80    #[no_mangle]
81    /// Compute the forward torque.
82    ///
83    /// # Arguments
84    /// * thetas - The motor angles as a 3-element array.
85    /// * thetas_torque - The motor torques as a 3-element array.
86    /// * output_torque - Holder for the platform orientation torque as a pseudo vector result.
87    /// # Returns
88    /// * 0 if success, 1 if error.
89    pub extern "C" fn orbita3d_kinematics_forward_torque(
90        &self,
91        thetas: &[f64; 3],
92        thetas_torque: &[f64; 3],
93        output_torque: &mut [f64; 3],
94    ) -> i32 {
95        let rot = self.inner().compute_output_torque(*thetas, *thetas_torque);
96        *output_torque = rot.into();
97
98        0
99    }
100
101    #[no_mangle]
102    /// Compute the inverse position kinematics.
103    ///
104    /// # Arguments
105    /// * quat - The platform orientation as a quaternion.
106    /// * thetas - Holder for the motor angles as a 3-element array result.
107    /// # Returns
108    /// * 0 if success, 1 if no solution, 2 if invalid solution.
109    pub extern "C" fn orbita3d_kinematics_inverse_position(
110        &self,
111        quat: &[f64; 4],
112        thetas: &mut [f64; 3],
113    ) -> i32 {
114        match self
115            .inner()
116            .compute_inverse_kinematics(conversion::quaternion_to_rotation_matrix(
117                quat[0], quat[1], quat[2], quat[3],
118            )) {
119            Ok(sol) => {
120                *thetas = sol;
121                0
122            }
123            Err(err) => match err {
124                orbita3d_kinematics::InverseSolutionErrorKind::NoSolution(_) => 1,
125                orbita3d_kinematics::InverseSolutionErrorKind::InvalidSolution(_, _) => 2,
126            },
127        }
128    }
129
130    #[no_mangle]
131    /// Compute the inverse velocity.
132    ///
133    /// # Arguments
134    /// * thetas - The motor angles as a 3-element array.
135    /// * output_velocity - The platform orientation velocity as a velocity pseudo vector.
136    /// * thetas_velocity - Holder for the motor velocities as a 3-element array result.
137    /// # Returns
138    /// * 0 if success, 1 if error.
139    pub extern "C" fn orbita3d_kinematics_inverse_velocity(
140        &self,
141        thetas: &[f64; 3],
142        output_velocity: &[f64; 3],
143        thetas_velocity: &mut [f64; 3],
144    ) -> i32 {
145        *thetas_velocity = self
146            .inner()
147            .compute_input_velocity(*thetas, conversion::array_to_vector3(*output_velocity));
148
149        0
150    }
151
152    #[no_mangle]
153    /// Compute the inverse torque.
154    ///
155    /// # Arguments
156    /// * thetas - The motor angles as a 3-element array.
157    /// * output_torque - The platform orientation torque as a pseudo vector.
158    /// * thetas_torque - Holder for the motor torques as a 3-element array result.
159    /// # Returns
160    /// * 0 if success, 1 if error.
161    pub extern "C" fn orbita3d_kinematics_inverse_torque(
162        &self,
163        thetas: &[f64; 3],
164        output_torque: &[f64; 3],
165        thetas_torque: &mut [f64; 3],
166    ) -> i32 {
167        *thetas_torque = self
168            .inner()
169            .compute_input_torque(*thetas, conversion::array_to_vector3(*output_torque));
170
171        0
172    }
173
174    fn inner(&self) -> orbita3d_kinematics::Orbita3dKinematicsModel {
175        orbita3d_kinematics::Orbita3dKinematicsModel {
176            alpha: self.alpha,
177            gamma_min: self.gamma_min,
178            offset: self.offset,
179            beta: self.beta,
180            gamma_max: self.gamma_max,
181            passiv_arms_direct: self.passiv_arms_direct,
182        }
183    }
184}
185
186#[no_mangle]
187pub extern "C" fn quaternion_to_intrinsic_roll_pitch_yaw(
188    quat: &[f64; 4],
189    rpy: &mut [f64; 3],
190) -> i32 {
191    let rot = conversion::quaternion_to_rotation_matrix(quat[0], quat[1], quat[2], quat[3]);
192    *rpy = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
193
194    0
195}
196
197#[no_mangle]
198pub extern "C" fn intrinsic_roll_pitch_yaw_to_quaternion(
199    rpy: &[f64; 3],
200    quat: &mut [f64; 4],
201) -> i32 {
202    let rot = conversion::intrinsic_roll_pitch_yaw_to_matrix(rpy[0], rpy[1], rpy[2]);
203    *quat = conversion::rotation_matrix_to_quaternion(rot);
204
205    0
206}
orbita3d_c_api/kinematics.rs

orbita3d_c_api/
kinematics.rs
