Skip to content

Commit

Permalink
Remove old nalgebra bug workaround.
Browse files Browse the repository at this point in the history
  • Loading branch information
kylc committed Feb 6, 2024
1 parent fd36496 commit 813885b
Showing 1 changed file with 3 additions and 10 deletions.
13 changes: 3 additions & 10 deletions crates/optik-py/src/lib.rs
Original file line number Diff line number Diff line change
@@ -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]
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 813885b

Please sign in to comment.