Skip to content

Add no_std support to orbita3d_kinematics crate #93

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 10 commits into
base: develop
Choose a base branch
from
Open

Add no_std support to orbita3d_kinematics crate #93

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
f35414a
update dependencies
omelia-iliffe Mar 13, 2025
ba88d54
add no_std, change std to core
omelia-iliffe Mar 13, 2025
b7c8d7c
add GammasOutOfRange error
omelia-iliffe Mar 13, 2025
564a147
update compute_valid_solution method, remove use of Vec
omelia-iliffe Mar 13, 2025
286dc41
add feature `forward_kinematics` + `std` to avoid einsum dep in no_std
omelia-iliffe Apr 6, 2025
b1c7997
replace std float traits with nalgabra/num_traits
omelia-iliffe Apr 6, 2025
b24798d
change `align_vectors` to be no_std compatible
omelia-iliffe Apr 14, 2025
e8d456f
remove forward_kinematics feature & ndarray, ndarry_einsum_beta, nsha…
omelia-iliffe Apr 14, 2025
459533a
remove num_traits dependency
omelia-iliffe Apr 14, 2025
1243d5d
improve sol1/sol2 in `complete_valid_solution`
omelia-iliffe Apr 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 6 additions & 7 deletions orbita3d_kinematics/Cargo.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
5 changes: 3 additions & 2 deletions orbita3d_kinematics/src/conversion.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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<f64> {
Rotation3::from(UnitQuaternion::from_quaternion(Quaternion::new(
Expand Down Expand Up @@ -61,7 +62,7 @@ pub fn array_to_vector3(a: [f64; 3]) -> Vector3<f64> {
#[cfg(test)]
mod tests {
use rand::Rng;
use std::f64::consts::PI;
use core::f64::consts::PI;

use super::*;

Expand Down
2 changes: 2 additions & 0 deletions orbita3d_kinematics/src/jacobian.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
#[cfg(not(any(feature = "std", test)))]
use nalgebra::ComplexField;
use nalgebra::{Matrix3, Rotation3, RowVector3};

use crate::Orbita3dKinematicsModel;
Expand Down
4 changes: 4 additions & 0 deletions orbita3d_kinematics/src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
60 changes: 19 additions & 41 deletions orbita3d_kinematics/src/position/forward.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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};

Expand Down Expand Up @@ -141,41 +141,41 @@ 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
// && (disk_yaw_avg - rpy[2]).abs() > 0.1
{
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);
Expand Down Expand Up @@ -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(&[
Expand All @@ -258,22 +258,12 @@ impl Orbita3dKinematicsModel {
}

fn align_vectors(a: Matrix3<f64>, b: Matrix3<f64>) -> Rotation3<f64> {
// 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(
Expand All @@ -282,19 +272,7 @@ fn align_vectors(a: Matrix3<f64>, b: Matrix3<f64>) -> Rotation3<f64> {
);
}

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)
}
Expand Down Expand Up @@ -355,9 +333,9 @@ impl Orbita3dForwardProblem {

// We implement a trait for every problem we want to solve
impl LeastSquaresProblem<f64, U12, U6> for Orbita3dForwardProblem {
type ParameterStorage = Owned<f64, U6>;
type ResidualStorage = Owned<f64, U12>;
type JacobianStorage = Owned<f64, U12, U6>;
type ParameterStorage = Owned<f64, U6>;

fn set_params(&mut self, p: &SVector<f64, 6>) {
self.p = *p;
Expand Down
Loading