From 813885b8085b95fb3353904a5259d7447788ece7 Mon Sep 17 00:00:00 2001 From: Kyle Cesare Date: Mon, 5 Feb 2024 20:50:09 -0700 Subject: [PATCH] Remove old nalgebra bug workaround. --- crates/optik-py/src/lib.rs | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/crates/optik-py/src/lib.rs b/crates/optik-py/src/lib.rs index 6b3764b..c88105a 100644 --- a/crates/optik-py/src/lib.rs +++ b/crates/optik-py/src/lib.rs @@ -1,6 +1,6 @@ use optik::{Robot, SolverConfig}; -use nalgebra::{Isometry3, Matrix4, Rotation3, Translation3, UnitQuaternion}; +use nalgebra::{Isometry3, Matrix4, Translation3, UnitQuaternion}; use pyo3::prelude::*; #[pyclass] @@ -96,16 +96,9 @@ impl PyRobot { let m = Matrix4::from_iterator(v.into_iter().flatten()).transpose(); let t = Translation3::from(m.fixed_view::<3, 1>(0, 3).into_owned()); + let r = UnitQuaternion::from_matrix(&m.fixed_view::<3, 3>(0, 0).into_owned()); - // Some rotational gymnastics here to work around an nalgebra bug - // (fixed in 0.31) which fails on direct conversion from Matrix3 -> - // UnitQuaternion for inputs that are already a rotation matrix. - // https://github.com/dimforge/nalgebra/pull/1101 - let g = m.fixed_view::<3, 3>(0, 0).into_owned(); - let r = Rotation3::from_matrix_unchecked(g); - let q = UnitQuaternion::from_rotation_matrix(&r); - - Isometry3::from_parts(t, q) + Isometry3::from_parts(t, r) } let target = parse_pose(target);