diff --git a/orbita3d_kinematics/Cargo.toml b/orbita3d_kinematics/Cargo.toml index 3e22989..a231f4d 100644 --- a/orbita3d_kinematics/Cargo.toml +++ b/orbita3d_kinematics/Cargo.toml @@ -7,13 +7,12 @@ edition = "2021" [dependencies] log = "0.4.20" -env_logger = "0.9.0" -levenberg-marquardt = "0.12.0" -nalgebra = "0.30.1" -ndarray = "0.15.6" -ndarray_einsum_beta = "0.7.0" -nshare = "0.9.0" -serde = { version = "1.0.183", features = ["derive"] } +levenberg-marquardt = "0.14.0" +nalgebra = { version = "0.33.2", default-features = false, features = ["libm"]} +serde = { version = "1.0.183", default-features = false, features = ["derive"] } + +[features] +std = [] [dev-dependencies] rand = "0.8.5" diff --git a/orbita3d_kinematics/src/conversion.rs b/orbita3d_kinematics/src/conversion.rs index cab6abc..ecc308c 100644 --- a/orbita3d_kinematics/src/conversion.rs +++ b/orbita3d_kinematics/src/conversion.rs @@ -1,5 +1,6 @@ use nalgebra::{Quaternion, Rotation3, UnitQuaternion, Vector3}; - +#[cfg(not(any(feature = "std", test)))] +use nalgebra::{ComplexField, RealField}; /// Convert a quaternion to a rotation matrix. pub fn quaternion_to_rotation_matrix(qx: f64, qy: f64, qz: f64, qw: f64) -> Rotation3 { Rotation3::from(UnitQuaternion::from_quaternion(Quaternion::new( @@ -61,7 +62,7 @@ pub fn array_to_vector3(a: [f64; 3]) -> Vector3 { #[cfg(test)] mod tests { use rand::Rng; - use std::f64::consts::PI; + use core::f64::consts::PI; use super::*; diff --git a/orbita3d_kinematics/src/jacobian.rs b/orbita3d_kinematics/src/jacobian.rs index c152c5e..cde36df 100644 --- a/orbita3d_kinematics/src/jacobian.rs +++ b/orbita3d_kinematics/src/jacobian.rs @@ -1,3 +1,5 @@ +#[cfg(not(any(feature = "std", test)))] +use nalgebra::ComplexField; use nalgebra::{Matrix3, Rotation3, RowVector3}; use crate::Orbita3dKinematicsModel; diff --git a/orbita3d_kinematics/src/lib.rs b/orbita3d_kinematics/src/lib.rs index 7603ad4..4ae3dc4 100644 --- a/orbita3d_kinematics/src/lib.rs +++ b/orbita3d_kinematics/src/lib.rs @@ -8,6 +8,10 @@ //! //! See the [README.md](./README.md) for more information. +#![cfg_attr(not(feature = "std"), no_std)] + +#[cfg(not(any(feature = "std", test)))] +use nalgebra::ComplexField; use nalgebra::{Matrix3, Rotation3, Vector3}; pub mod conversion; diff --git a/orbita3d_kinematics/src/position/forward.rs b/orbita3d_kinematics/src/position/forward.rs index 2c49075..123b6e6 100644 --- a/orbita3d_kinematics/src/position/forward.rs +++ b/orbita3d_kinematics/src/position/forward.rs @@ -1,7 +1,7 @@ use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt}; +#[cfg(not(any(feature = "std", test)))] +use nalgebra::{ComplexField, RealField}; use nalgebra::{Matrix3, Owned, Rotation3, SMatrix, SVector, Vector3, U12, U6}; -use ndarray_einsum_beta::einsum; -use nshare::{RefNdarray2, ToNalgebra}; use crate::{conversion, InverseSolutionErrorKind, Orbita3dKinematicsModel}; @@ -141,24 +141,24 @@ impl Orbita3dKinematicsModel { log::debug!("bad yaw sign"); if rpy[2] < 0.0 { log::debug!("\t+TAU"); - rpy[2] += std::f64::consts::TAU; + rpy[2] += core::f64::consts::TAU; } else { log::debug!("\t-TAU"); - rpy[2] -= std::f64::consts::TAU; + rpy[2] -= core::f64::consts::TAU; } } log::debug!("=> RPY with yaw sign {:?}", rpy); // From the average yaw of the disks, compute the real rpy // it can be 180<|yaw|<360 or |yaw|>360 - let nb_turns = (disk_yaw_avg / std::f64::consts::TAU).trunc(); //number of full turn - // let nb_turns: f64 = - // (disk_yaw_avg / std::f64::consts::TAU).round(); + let nb_turns = (disk_yaw_avg / core::f64::consts::TAU).trunc(); //number of full turn + // let nb_turns: f64 = + // (disk_yaw_avg / std::f64::consts::TAU).round(); log::debug!("=> nb_turns {:?}", nb_turns); - if (disk_yaw_avg.abs() >= std::f64::consts::PI) - && (disk_yaw_avg.abs() < std::f64::consts::TAU) + if (disk_yaw_avg.abs() >= core::f64::consts::PI) + && (disk_yaw_avg.abs() < core::f64::consts::TAU) { // We are in 180<|yaw|<360 if nb_turns.abs() > 0.0 || nb_turns == -1.0 @@ -166,16 +166,16 @@ impl Orbita3dKinematicsModel { { log::debug!( "Adding offset {}", - disk_yaw_avg.signum() * std::f64::consts::TAU + disk_yaw_avg.signum() * core::f64::consts::TAU ); - rpy[2] += disk_yaw_avg.signum() * std::f64::consts::TAU; + rpy[2] += disk_yaw_avg.signum() * core::f64::consts::TAU; } log::debug!("180<|yaw|<360: {}", rpy[2]); } else { // We are in |yaw|>360 => how many turns? - rpy[2] += nb_turns * std::f64::consts::TAU; + rpy[2] += nb_turns * core::f64::consts::TAU; log::debug!("|yaw|>360: nb_turns {nb_turns} {}", rpy[2]); } log::debug!("=> Out RPY {:?}", rpy); @@ -246,9 +246,9 @@ impl Orbita3dKinematicsModel { let mut phi3 = sp3_n.atan2(cp3_n); if !self.passiv_arms_direct { - phi1 += std::f64::consts::PI; - phi2 += std::f64::consts::PI; - phi3 += std::f64::consts::PI; + phi1 += core::f64::consts::PI; + phi2 += core::f64::consts::PI; + phi3 += core::f64::consts::PI; } Some(SVector::from_row_slice(&[ @@ -258,22 +258,12 @@ impl Orbita3dKinematicsModel { } fn align_vectors(a: Matrix3, b: Matrix3) -> Rotation3 { - // Find the rotation matrix to align two sets of vectors (based on scipy implementation) - let na = a.ref_ndarray2().into_shape((3, 3)).unwrap(); - let nb = b.ref_ndarray2().into_shape((3, 3)).unwrap(); - - let mat_b = einsum("ji,jk->ik", &[&na, &nb]) - .unwrap() - .into_shape((3, 3)) - .unwrap(); - - let matrix_b = mat_b.view().into_nalgebra(); - + let matrix_b = a.transpose() * b; let mat_svd = matrix_b.svd(true, true); let mut u = mat_svd.u.unwrap(); let vh = mat_svd.v_t.unwrap(); - let uv = u.clone() * vh.clone(); + let uv = u * vh; if uv.determinant() < 0.0 { u.set_column( @@ -282,19 +272,7 @@ fn align_vectors(a: Matrix3, b: Matrix3) -> Rotation3 { ); } - let mat_c = u * vh; - - let m = Matrix3::from_row_slice(&[ - mat_c.row(0)[0], - mat_c.row(0)[1], - mat_c.row(0)[2], - mat_c.row(1)[0], - mat_c.row(1)[1], - mat_c.row(1)[2], - mat_c.row(2)[0], - mat_c.row(2)[1], - mat_c.row(2)[2], - ]); + let m = u * vh; Rotation3::from_matrix_unchecked(m) } @@ -355,9 +333,9 @@ impl Orbita3dForwardProblem { // We implement a trait for every problem we want to solve impl LeastSquaresProblem for Orbita3dForwardProblem { - type ParameterStorage = Owned; type ResidualStorage = Owned; type JacobianStorage = Owned; + type ParameterStorage = Owned; fn set_params(&mut self, p: &SVector) { self.p = *p; diff --git a/orbita3d_kinematics/src/position/inverse.rs b/orbita3d_kinematics/src/position/inverse.rs index c06d2de..0eff91f 100644 --- a/orbita3d_kinematics/src/position/inverse.rs +++ b/orbita3d_kinematics/src/position/inverse.rs @@ -1,6 +1,21 @@ use nalgebra::{Matrix2, Matrix3, Rotation3, Vector2, Vector3}; -use std::f64::consts::PI; - +#[cfg(not(any(feature = "std", test)))] +use nalgebra::{ComplexField, RealField}; + +#[cfg(not(any(feature = "std", test)))] +#[inline] +fn rem_euclid(lhs: &f64, rhs: &f64) -> f64 { + let r = lhs % rhs; + if r < 0.0 { + r + rhs.abs() + } else { + r + } +} +#[cfg(any(feature = "std", test))] +fn rem_euclid(lhs: &f64, rhs: &f64) -> f64 { + f64::rem_euclid(*lhs, *rhs) +} const TOLERANCE_ZERO_YAW: f64 = 1e-6; // Define a small tolerance for near-zero values use crate::{conversion, Orbita3dKinematicsModel}; @@ -13,8 +28,25 @@ pub enum InverseSolutionErrorKind { /// Invalid solution found. InvalidSolution(Rotation3, Vector3), } -impl std::fmt::Display for InverseSolutionErrorKind { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + +#[derive(Debug)] +pub struct GammasOutOfRange { + min: f64, + max: f64, + gammas: Vector3, + thetas: Vector3, +} + +impl core::fmt::Display for GammasOutOfRange { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!(f, "Gammas out of range: ! {} < {:?} < {} (thetas {})", self.min, self.max, self.gammas, self.thetas) + } +} + +impl core::error::Error for GammasOutOfRange {} + +impl core::fmt::Display for InverseSolutionErrorKind { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { match self { InverseSolutionErrorKind::NoSolution(rot) => { write!(f, "No solution found for rotation matrix: {}", rot) @@ -27,7 +59,7 @@ impl std::fmt::Display for InverseSolutionErrorKind { } } } -impl std::error::Error for InverseSolutionErrorKind {} +impl core::error::Error for InverseSolutionErrorKind {} impl Orbita3dKinematicsModel { /// Compute the inverse kinematics of the Orbita3d platform. @@ -81,24 +113,24 @@ impl Orbita3dKinematicsModel { log::debug!("valid Thetas {:?}", thetas); // if yaw is more than Pi, we may have to deal with some edge cases let true_yaw = target_rpy[2] + self.offset; //FIXME???? - if true_yaw.abs() >= std::f64::consts::PI { + if true_yaw.abs() >= core::f64::consts::PI { // Compute the k*2*Pi offset if the yaw target is more than 1 full rotation // let nb_turns = (target_rpy[2] / std::f64::consts::TAU).trunc(); //number of full turn - let nb_turns = (true_yaw / std::f64::consts::TAU).trunc(); //number of full turn + let nb_turns = (true_yaw / core::f64::consts::TAU).trunc(); //number of full turn if nb_turns.abs() >= 1.0 { - multiturn_offset = std::f64::consts::TAU * (nb_turns); + multiturn_offset = core::f64::consts::TAU * (nb_turns); } // also, if yaw.abs().rem_euclid(2.0 * PI) > pi, we might want to consider the 2pi complement // if target_rpy[2].abs().rem_euclid(std::f64::consts::TAU) >= std::f64::consts::PI - if true_yaw.abs().rem_euclid(std::f64::consts::TAU) >= std::f64::consts::PI + if rem_euclid(&true_yaw.abs(), &core::f64::consts::TAU) >= core::f64::consts::PI && !(thetas[0].signum() == thetas[1].signum() && thetas[1].signum() == thetas[2].signum()) { - multiturn_offset += target_rpy[2].signum() * std::f64::consts::TAU + multiturn_offset += target_rpy[2].signum() * core::f64::consts::TAU } - log::debug!("Yaw more than Pi, nb full turns: {nb_turns}, yaw%2pi: {:?} offset: {multiturn_offset} theta before: {:?}",true_yaw.abs().rem_euclid(std::f64::consts::TAU),thetas); + log::debug!("Yaw more than Pi, nb full turns: {nb_turns}, yaw%2pi: {:?} offset: {multiturn_offset} theta before: {:?}",rem_euclid(&true_yaw.abs(), &core::f64::consts::TAU),thetas); log::debug!("thetas {:?}", thetas); @@ -110,16 +142,17 @@ impl Orbita3dKinematicsModel { Ok(thetas) } - pub fn check_gammas(&self, thetas: Vector3) -> Result<(), Box> { + pub fn check_gammas(&self, thetas: Vector3) -> Result<(), GammasOutOfRange> { let gammas = compute_gammas(thetas); // println!("CHECK GAMMAS: {:?}", gammas); for g in gammas.iter() { if !((*g > self.gamma_min) && (*g < self.gamma_max)) { - let msg = format!( - "Gammas out of range: ! {:?} < {:?} < {:?} (thetas {:?})", - self.gamma_min, gammas, self.gamma_max, thetas - ); - return Err((msg).into()); + return Err(GammasOutOfRange { + min: self.gamma_min, + max: self.gamma_max, + gammas, + thetas, + }) } } Ok(()) @@ -128,7 +161,7 @@ impl Orbita3dKinematicsModel { pub fn compute_valid_solution( &self, target_rpy: [f64; 3], - mut thetas: [f64; 3], + thetas: [f64; 3], ) -> Result<[f64; 3], InverseSolutionErrorKind> { // Select the "real world" solution from the geometric one: => disks should not cross each over // For each theta, there is 2 solutions (only one valid): @@ -157,65 +190,67 @@ impl Orbita3dKinematicsModel { all_solutions[i as usize][j as usize] = thetas[j as usize]; } else { all_solutions[i as usize][j as usize] = - thetas[j as usize] - thetas[j as usize].signum() * std::f64::consts::TAU; + thetas[j as usize] - thetas[j as usize].signum() * core::f64::consts::TAU; } } } - let mut validvec = Vec::new(); - for sol in all_solutions { - match self.check_gammas(sol.into()) { - Ok(()) => validvec.push(sol), - Err(_) => continue, - } - } - log::debug!( - "all solutions: {:?}\nvalid solutions: {:?}", - all_solutions, - validvec - ); - // There is either one solution or 2 valid solutions - if validvec.len() == 1 { - thetas = validvec[0]; - } else { - if validvec.is_empty() { - log::debug!( - "NO VALID SOLUTION! target: {:?}\n thetas: {:?}\nall_solutions: {:?}", - target_rpy, - thetas, - all_solutions - ); - let rot = conversion::intrinsic_roll_pitch_yaw_to_matrix( - target_rpy[0], - target_rpy[1], - target_rpy[2], - ); - return Err(InverseSolutionErrorKind::InvalidSolution( - rot, - compute_gammas(thetas.into()), - )); - } - - // here we have the 2 solutions (both 2pi complement), we chose the one with the same yaw sign - let mut yaw_sign = (target_rpy[2] + self.offset).signum(); - let mut theta_sign = validvec[0][0].signum(); - // If the yaw or thetas are very close to zero, treat them as effectively zero - if (target_rpy[2] + self.offset).abs() < TOLERANCE_ZERO_YAW { - yaw_sign = 0.0; - } - if validvec[0][0].abs() < TOLERANCE_ZERO_YAW { - theta_sign = 0.0; - } - // Compare the yaw sign and theta sign - // but now accounting for near-zero values - if theta_sign == yaw_sign { - thetas = validvec[0]; - } else { - thetas = validvec[1]; + log::debug!("all solutions: {:?}", all_solutions); + let mut solution_iter = all_solutions + .iter() + .filter(|sol| self.check_gammas(Vector3::from(**sol)).is_ok()); + + let Some(sol1) = solution_iter.next() else { + log::debug!( + "NO VALID SOLUTION! target: {:?}\n thetas: {:?}\nall_solutions: {:?}", + target_rpy, + thetas, + all_solutions + ); + let rot = conversion::intrinsic_roll_pitch_yaw_to_matrix( + target_rpy[0], + target_rpy[1], + target_rpy[2], + ); + return Err(InverseSolutionErrorKind::InvalidSolution( + rot, + compute_gammas(thetas.into()), + )); + }; + + log::debug!("valid solution 1: {:?}", sol1); + + let sol2 = solution_iter.next(); + + let thetas = match sol2 { + None => sol1, + Some(sol2) => { + // here we have the 2 solutions (both 2pi complement), we chose the one with the same yaw sign + + // If the yaw or thetas are very close to zero, treat them as effectively zero + let yaw_sign = if (target_rpy[2] + self.offset).abs() < TOLERANCE_ZERO_YAW { + 0.0 + } else { + // otherwise get the sign + (target_rpy[2] + self.offset).signum() + }; + let theta_sign = if sol1[0].abs() < TOLERANCE_ZERO_YAW { + 0.0 + } else { + sol1[0].signum() + }; + // Compare the yaw sign and theta sign + // but now accounting for near-zero values + if theta_sign == yaw_sign { + sol1 + } else { + sol2 + } } - } - // log::debug!("valid Thetas {:?}", thetas); - Ok(thetas) + }; + + log::debug!("valid Thetas {:?}", thetas); + Ok(*thetas) } fn find_thetas_from_v(&self, v: Matrix3) -> Vector3 { @@ -232,7 +267,7 @@ impl Orbita3dKinematicsModel { // Unique solution if a_i.abs() <= 1.5 * f64::EPSILON { let unique_sol = -c_i / (2.0 * b_i); - solutions_theta = [unique_sol.atan() * 2.0, PI]; + solutions_theta = [unique_sol.atan() * 2.0, core::f64::consts::PI]; } // Polynome has 2 roots else { @@ -247,7 +282,8 @@ impl Orbita3dKinematicsModel { solutions_theta = dual_sol.map(|v| v.atan() * 2.0); } - solutions_theta = solutions_theta.map(|v| v.rem_euclid(2.0 * std::f64::consts::PI)); + solutions_theta = + solutions_theta.map(|v| rem_euclid(&v, &(2.0 * core::f64::consts::PI))); if solutions_theta[0].is_nan() && solutions_theta[1].is_nan() { thetas[i] = f64::NAN; @@ -261,7 +297,7 @@ impl Orbita3dKinematicsModel { let mut theta = 0.0; - let v_i = Vector2::from_iterator(v.row(i).columns(0, 2).transpose().iter().cloned()); + let v_i: Vector2<_> = v.fixed_rows::<1>(i).fixed_columns::<2>(0).transpose(); if self.passiv_arms_direct { for (j, &sol) in solutions_theta.iter().enumerate() { diff --git a/orbita3d_kinematics/src/position/mod.rs b/orbita3d_kinematics/src/position/mod.rs index 8d1029d..6ac8125 100644 --- a/orbita3d_kinematics/src/position/mod.rs +++ b/orbita3d_kinematics/src/position/mod.rs @@ -7,7 +7,7 @@ mod tests { use crate::{conversion::*, Orbita3dKinematicsModel}; use rand::Rng; - use std::f64::consts::PI; + use core::f64::consts::PI; const ROLL_RANGE: f64 = 30.0; const PITCH_RANGE: f64 = 30.0;