Skip to content
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

[RSDK-732] orientation vector and rotation matrices #20

Merged
merged 5 commits into from
Dec 14, 2022
Merged

[RSDK-732] orientation vector and rotation matrices #20

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
78aaba1
[RSDK-732] orientation vector and rotation matrices
gvaradarajan Dec 5, 2022
2779fe4
eof new lines
gvaradarajan Dec 5, 2022
ec9064f
add FFI for rotating vector
gvaradarajan Dec 6, 2022
435ba25
use From trait implementation for quaternion conversion
gvaradarajan Dec 6, 2022
24db956
address some review comments
gvaradarajan Dec 12, 2022
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
14 changes: 2 additions & 12 deletions src/ffi/spatialmath/axis_angle.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
use ffi_helpers::null_pointer_check;
use nalgebra::{Quaternion, UnitQuaternion};
use nalgebra::{Quaternion};

use crate::spatialmath::utils::AxisAngle;

Expand Down Expand Up @@ -52,16 +52,6 @@ pub unsafe extern "C" fn axis_angle_from_quaternion(
quat: *const Quaternion<f64>
) -> *mut AxisAngle {
null_pointer_check!(quat);
let unit_quat = UnitQuaternion::from_quaternion(*quat);
let axis_opt = unit_quat.axis();
let angle = unit_quat.angle();
let axis_angle = match axis_opt {
Some(value) => {
AxisAngle::new(value[0], value[1], value[2], angle)
},
None => {
AxisAngle::new(0.0, 0.0, 0.0, 0.0)
},
};
let axis_angle = (*quat).into();
to_raw_pointer(&axis_angle)
}
2 changes: 1 addition & 1 deletion src/ffi/spatialmath/euler_angles.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,6 @@ pub extern "C" fn new_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut Eule
#[no_mangle]
pub unsafe extern "C" fn euler_angles_from_quaternion(quat_ptr: *const Quaternion<f64>) -> *mut EulerAngles {
null_pointer_check!(quat_ptr);
let euler_angles = EulerAngles::from_quaternion(&*quat_ptr);
let euler_angles: EulerAngles = (*quat_ptr).into();
to_raw_pointer(&euler_angles)
}
2 changes: 1 addition & 1 deletion src/ffi/spatialmath/orientation_vector.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,6 @@ pub unsafe extern "C" fn orientation_vector_from_quaternion(
quat_ptr: *const Quaternion<f64>
) -> *mut OrientationVector {
null_pointer_check!(quat_ptr);
let o_vec = OrientationVector::from_quaternion(&*quat_ptr);
let o_vec: OrientationVector = (*quat_ptr).into();
to_raw_pointer(&o_vec)
}
94 changes: 71 additions & 23 deletions src/spatialmath/utils.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,36 @@ impl EulerAngles {
}
}

impl From<Quaternion<f64>> for EulerAngles {
fn from(quat: Quaternion<f64>) -> Self {
// get a normalized version of the quaternion
let norm_quat = quat.normalize();

// calculate yaw
let yaw_sin_pitch_cos: f64 = 2.0 * ((norm_quat.w * norm_quat.k) + (norm_quat.i * norm_quat.j));
let yaw_cos_pitch_cos: f64 = 1.0 - 2.0 * ((norm_quat.j * norm_quat.j) + (norm_quat.k * norm_quat.k));
let yaw = yaw_sin_pitch_cos.atan2(yaw_cos_pitch_cos);

// calculate pitch and roll
let pitch_sin: f64 = 2.0 * ((norm_quat.w * norm_quat.j) - (norm_quat.k * norm_quat.i));
let pitch: f64;
let roll: f64;
// for a pitch that is π / 2, we experience gimbal lock
// and must calculate roll based on the real rotation and yaw
if pitch_sin.abs() >= 1.0 {
pitch = (std::f64::consts::PI / 2.0).copysign(pitch_sin);
roll = (2.0 * norm_quat.i.atan2(norm_quat.w)) + yaw.copysign(pitch_sin);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note: We should make this change in rdk as well.

} else {
pitch = pitch_sin.asin();
let roll_sin_pitch_cos = 2.0 * ((norm_quat.w * norm_quat.i) + (norm_quat.j * norm_quat.k));
let roll_cos_pitch_cos = 1.0 - 2.0 * ((norm_quat.i * norm_quat.i) + (norm_quat.j * norm_quat.j));
roll = roll_sin_pitch_cos.atan2(roll_cos_pitch_cos);
}

Self { roll, pitch, yaw }
}
}

#[repr(C)]
#[derive(Clone, Copy, Debug)]
pub struct AxisAngle {
Expand All @@ -61,6 +91,22 @@ impl AxisAngle {
}
}

impl From<Quaternion<f64>> for AxisAngle {
fn from(quat: Quaternion<f64>) -> Self {
let unit_quat = UnitQuaternion::from_quaternion(quat);
let axis_opt = unit_quat.axis();
let angle = unit_quat.angle();
match axis_opt {
Some(value) => {
AxisAngle::new(value[0], value[1], value[2], angle)
},
None => {
AxisAngle::new(0.0, 0.0, 0.0, 0.0)
randhid marked this conversation as resolved.
Show resolved Hide resolved
},
}
}
}

fn orientation_vector_theta_from_rotated_axes(
new_x: Quaternion<f64>, new_z: Quaternion<f64>
) -> f64 {
Expand Down Expand Up @@ -116,19 +162,6 @@ impl OrientationVector {
OrientationVector{o_vector, theta}
}

pub fn from_quaternion(quat: &Quaternion<f64>) -> Self {
let x_quat = Quaternion::new(0.0, -1.0, 0.0, 0.0);
let z_quat = Quaternion::new(0.0, 0.0, 0.0, 1.0);

let conj = quat.conjugate();
let new_x = (quat * x_quat) * conj;
let new_z = (quat * z_quat) * conj;

let o_vector = UnitVector3::new_normalize(new_z.imag());
let theta = orientation_vector_theta_from_rotated_axes(new_x, new_z);
OrientationVector { o_vector, theta }
}

pub fn to_quaternion(&self) -> Quaternion<f64> {
let lat = self.o_vector.z.acos();
let lon = match self.o_vector.z {
Expand Down Expand Up @@ -169,6 +202,21 @@ impl ApproxEq for OrientationVector {
}
}

impl From<Quaternion<f64>> for OrientationVector {
fn from(quat: Quaternion<f64>) -> Self {
let x_quat = Quaternion::new(0.0, -1.0, 0.0, 0.0);
let z_quat = Quaternion::new(0.0, 0.0, 0.0, 1.0);

let conj = quat.conjugate();
let new_x = (quat * x_quat) * conj;
let new_z = (quat * z_quat) * conj;

let o_vector = UnitVector3::new_normalize(new_z.imag());
let theta = orientation_vector_theta_from_rotated_axes(new_x, new_z);
randhid marked this conversation as resolved.
Show resolved Hide resolved
Self { o_vector, theta }
}
}

pub fn rotate_vector_by_quaternion(
quat: &Quaternion<f64>, vector: &Vector3<f64>
) -> Vector3<f64> {
Expand Down Expand Up @@ -204,7 +252,7 @@ mod tests {
let expected_ov = OrientationVector::new(
0.0, -1.0, 0.0, 1.5707963267948966
);
let calc_ov = OrientationVector::from_quaternion(&quat);
let calc_ov: OrientationVector = quat.into();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[minor] round-trip test might be good if it makes sense in this PR, rather than your next ticket.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

don't these two tests essentially provide the equivalent of a round-trip test? (the expected quaternion in one test is one of the input quaternions in the other and for the same case the opposite is true)

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you show me where in person? Approving to unblock you, but I'm not sure I'm seeing that round trip logic.

assert_approx_eq!(OrientationVector, calc_ov, expected_ov);

let quat2 = Quaternion::new(
Expand All @@ -213,7 +261,7 @@ mod tests {
let expected_ov2 = OrientationVector::new(
0.0, 1.0, 0.0, -1.5707963267948966
);
let calc_ov2 = OrientationVector::from_quaternion(&quat2);
let calc_ov2: OrientationVector = quat2.into();
assert_approx_eq!(OrientationVector, calc_ov2, expected_ov2);

let quat3 = Quaternion::new(
Expand All @@ -222,7 +270,7 @@ mod tests {
let expected_ov3 = OrientationVector::new(
-0.5376, 0.0, 0.8432, -1.0 * std::f64::consts::PI
);
let calc_ov3 = OrientationVector::from_quaternion(&quat3);
let calc_ov3: OrientationVector = quat3.into();
assert_approx_eq!(OrientationVector, calc_ov3, expected_ov3);

let quat4 = Quaternion::new(
Expand All @@ -231,7 +279,7 @@ mod tests {
let expected_ov4 = OrientationVector::new(
0.0, 0.0, 1.0, -0.5675882184166557
);
let calc_ov4 = OrientationVector::from_quaternion(&quat4);
let calc_ov4: OrientationVector = quat4.into();
assert_approx_eq!(OrientationVector, calc_ov4, expected_ov4);

let quat5 = Quaternion::new(
Expand All @@ -240,7 +288,7 @@ mod tests {
let expected_ov5 = OrientationVector::new(
0.0, 0.5376, 0.8432, -1.5707963267948966
);
let calc_ov5 = OrientationVector::from_quaternion(&quat5);
let calc_ov5: OrientationVector = quat5.into();
assert_approx_eq!(OrientationVector, calc_ov5, expected_ov5);

let quat6 = Quaternion::new(
Expand All @@ -249,14 +297,14 @@ mod tests {
let expected_ov6 = OrientationVector::new(
0.0, -0.5376, 0.8432, 1.5707963267948966
);
let calc_ov6 = OrientationVector::from_quaternion(&quat6);
let calc_ov6: OrientationVector = quat6.into();
assert_approx_eq!(OrientationVector, calc_ov6, expected_ov6);

let quat7 = Quaternion::new(0.5, -0.5, -0.5, -0.5);
let expected_ov7 = OrientationVector::new(
0.0, 1.0, 0.0, -1.0 * std::f64::consts::PI
);
let calc_ov7 = OrientationVector::from_quaternion(&quat7);
let calc_ov7: OrientationVector = quat7.into();
assert_approx_eq!(OrientationVector, calc_ov7, expected_ov7);

let quat8 = Quaternion::new(
Expand All @@ -265,7 +313,7 @@ mod tests {
let expected_ov8 = OrientationVector::new(
0.5048437942940054, 0.5889844266763397, 0.631054742867507, 0.02
);
let calc_ov8 = OrientationVector::from_quaternion(&quat8);
let calc_ov8: OrientationVector = quat8.into();
assert_approx_eq!(OrientationVector, calc_ov8, expected_ov8, epsilon = 0.0001);

}
Expand Down Expand Up @@ -357,15 +405,15 @@ mod tests {
let quat = Quaternion::new(
0.2705980500730985, -0.6532814824381882, 0.27059805007309856, 0.6532814824381883
);
let euler_angles = EulerAngles::from_quaternion(&quat);
let euler_angles: EulerAngles = quat.into();
assert_approx_eq!(f64, euler_angles.pitch, std::f64::consts::PI / 2.0);
assert_approx_eq!(f64, euler_angles.yaw, std::f64::consts::PI);
assert_approx_eq!(f64, euler_angles.roll, std::f64::consts::PI / 4.0);

let quat2 = Quaternion::new(
0.4619397662556435, -0.19134171618254486, 0.4619397662556434, 0.7325378163287418
);
let euler_angles2 = EulerAngles::from_quaternion(&quat2);
let euler_angles2: EulerAngles = quat2.into();
assert_approx_eq!(f64, euler_angles2.pitch, std::f64::consts::PI / 4.0);
assert_approx_eq!(f64, euler_angles2.yaw, 3.0 * std::f64::consts::PI / 4.0);
assert_approx_eq!(f64, euler_angles2.roll, std::f64::consts::PI / 4.0);
Expand Down