Repository: rpiRobotics/ik-geo (language: python)

Issue:
Rust Subproblem 3



Discussion / hints:

Fix patch:
diff --git a/rust/src/subproblems.rs b/rust/src/subproblems.rs
index 1afb789..e730f1e 100644
--- a/rust/src/subproblems.rs
+++ b/rust/src/subproblems.rs
@@ -1,4 +1,4 @@
-use nalgebra::{ Vector3, Matrix3x2, Matrix4x3, Matrix4x1, Matrix3x4, Matrix3 };
+use nalgebra::{ Vector3, Matrix3x2, Matrix4x3, Matrix4x1, Matrix3x4, Matrix3, Vector2 };
 
 #[derive(Clone)]
 pub enum SolutionSet2<T> {
@@ -42,12 +42,12 @@ If not, minimizes `|| rot(k, theta) * p1 - p2 ||`.
 Also returns a boolean of whether or not `theta` is a least-squares solution.
 */
 pub fn subproblem1(p1: &Vector3<f64>, p2: &Vector3<f64>, k: &Vector3<f64>) -> (f64, bool) {
-    let kxp = k.cross(&p1);
+    let kxp = k.cross(p1);
     let a = Matrix3x2::from_columns(&[kxp, -k.cross(&kxp)]);
     let x = a.transpose() * p2;
 
     let theta = x[0].atan2(x[1]);
-    let is_ls = (p1.norm() - p2.norm()).abs() > 1e-8 || (k.dot(&p1) - k.dot(&p2)).abs() > 1e-8;
+    let is_ls = (p1.norm() - p2.norm()).abs() > 1e-8 || (k.dot(p1) - k.dot(p2)).abs() > 1e-8;
 
     (theta, is_ls)
 }
@@ -120,8 +120,8 @@ Solves for `theta1` and `theta2` where `p0 + rot(k1, theta1) * p1 = rot(k2, thet
 Assumes only one solution. If there could be two, `subproblem2` should be used.
 */
 pub fn subproblem2extended(p0: &Vector3<f64>, p1: &Vector3<f64>, p2: &Vector3<f64>, k1: &Vector3<f64>, k2: &Vector3<f64>) -> (f64, f64) {
-    let kxp1 = k1.cross(&p1);
-    let kxp2 = k2.cross(&p2);
+    let kxp1 = k1.cross(p1);
+    let kxp2 = k2.cross(p2);
 
     let a1  = Matrix3x2::from_columns(&[kxp1, -k1.cross(&kxp1)]);
     let a2  = Matrix3x2::from_columns(&[kxp2, -k2.cross(&kxp2)]);
@@ -129,7 +129,7 @@ pub fn subproblem2extended(p0: &Vector3<f64>, p1: &Vector3<f64>, p2: &Vector3<f6
 
     let a = Matrix3x4::from_columns(&[a1.column(0), a1.column(1), a2_neg.column(0), a2_neg.column(1)]);
 
-    let p = -k1 * k1.dot(&p1) + k2 * k2.dot(p2) - p0;
+    let p = -k1 * k1.dot(p1) + k2 * k2.dot(p2) - p0;
 
     let radius1_sq = kxp1.dot(&kxp1);
     let radius2_sq = kxp2.dot(&kxp2);
@@ -154,3 +154,37 @@ pub fn subproblem2extended(p0: &Vector3<f64>, p1: &Vector3<f64>, p2: &Vector3<f6
 
     (sc[0].atan2(sc[1]), sc[2].atan2(sc[3]))
 }
+
+/**
+Solves for theta where || rot(k, theta) * p1 - p2 || = d if possibble.
+If not, minimizes | || rot(k, theta)*p1 - p2 || - d |.
+Also returns a boolean of whether or not theta is a least-squares solution.
+*/
+pub fn subproblem3(p1: &Vector3<f64>, p2: &Vector3<f64>, k: &Vector3<f64>, d: f64) -> (SolutionSet2<f64>, bool) {
+    let kxp = k.cross(p1);
+    let a_1 = Matrix3x2::from_columns(&[kxp, -k.cross(&kxp)]);
+    let a = -2.0 * p2.transpose() * a_1;
+    let norm_a_sq = a.norm_squared();
+    let norm_a = a.norm();
+
+    let b = d * d - (p2 - k * k.transpose() * p1).norm_squared() - kxp.norm_squared();
+
+    let x_ls = a_1.transpose() * (-2.0 * p2 * b / norm_a_sq);
+
+    if x_ls.norm_squared() > 1.0 {
+        return (SolutionSet2::One(x_ls[0].atan2(x_ls[1])), true);
+    }
+
+    let xi = (1.0 - b * b / norm_a_sq).sqrt();
+
+    let a_perp_tilde = Vector2::new(a[1], -a[0]);
+    let a_perp = a_perp_tilde / norm_a;
+
+    let sc_1 = x_ls + xi * a_perp;
+    let sc_2 = x_ls - xi * a_perp;
+
+    (SolutionSet2::Two(
+        sc_1[0].atan2(sc_1[1]),
+        sc_2[0].atan2(sc_2[1]),
+    ), false)
+}


Test patch:
diff --git a/rust/src/tests/correctness.rs b/rust/src/tests/correctness.rs
index cdb5cc0..f2d3ee5 100644
--- a/rust/src/tests/correctness.rs
+++ b/rust/src/tests/correctness.rs
@@ -1,4 +1,4 @@
-use super::setups::{ Setup, Subproblem1Setup, Subproblem2Setup, Subproblem2ExtendedSetup };
+use super::setups::{ Setup, Subproblem1Setup, Subproblem2Setup, Subproblem2ExtendedSetup, Subproblem3Setup };
 
 const TEST_ITERATIONS: usize = 1000;
 
@@ -10,6 +10,7 @@ fn run_tests() {
         Box::new(Subproblem1Setup::new()),
         Box::new(Subproblem2Setup::new()),
         Box::new(Subproblem2ExtendedSetup::new()),
+        Box::new(Subproblem3Setup::new()),
     ];
 
     for mut setup in setups {
@@ -33,6 +34,7 @@ fn run_tests_ls() {
     let setups: Vec<Box<dyn Setup>> = vec![
         Box::new(Subproblem1Setup::new()),
         Box::new(Subproblem2Setup::new()),
+        Box::new(Subproblem3Setup::new()),
     ];
 
     for mut setup in setups {
diff --git a/rust/src/tests/setups.rs b/rust/src/tests/setups.rs
index 2838a4b..f5fcdb0 100644
--- a/rust/src/tests/setups.rs
+++ b/rust/src/tests/setups.rs
@@ -7,12 +7,14 @@ use {
             random_norm_vector3
         },
 
-        subproblems::{ SolutionSet2, subproblem1, subproblem2, subproblem2extended }
+        subproblems::{ SolutionSet2, subproblem1, subproblem2, subproblem2extended, subproblem3 }
     },
 
     nalgebra::Vector3,
 };
 
+const DELTA: f64 = 1e-12;
+
 /// An interface for setting up subproblem testing. Resposible for generating parameters, running
 /// the function, and calculating data such as the error.
 pub trait Setup {
@@ -59,6 +61,15 @@ pub struct Subproblem2ExtendedSetup {
     theta2: f64,
 }
 
+pub struct Subproblem3Setup {
+    p1: Vector3<f64>,
+    p2: Vector3<f64>,
+    k: Vector3<f64>,
+    d: f64,
+
+    theta: SolutionSet2<f64>,
+}
+
 impl Subproblem1Setup {
     pub fn new() -> Self {
         Self {
@@ -88,7 +99,7 @@ impl Subproblem2Setup {
         }
     }
 
-    fn calculate_error(&self, theta1: &Vec<f64>, theta2: &Vec<f64>) -> f64 {
+    fn calculate_error(&self, theta1: &[f64], theta2: &[f64]) -> f64 {
         theta1.iter()
             .zip(theta2.iter())
             .map(|(&t1, &t2)| {
@@ -113,6 +124,28 @@ impl Subproblem2ExtendedSetup {
     }
 }
 
+impl Subproblem3Setup {
+    pub fn new() -> Self {
+        Self {
+            p1: Vector3::zeros(),
+            p2: Vector3::zeros(),
+            k: Vector3::zeros(),
+            d: 0.0,
+
+            theta: SolutionSet2::One(0.0),
+        }
+    }
+
+    pub fn calculate_error(&self, theta: &[f64]) -> f64 {
+        theta
+            .iter()
+            .map(|&t| {
+                ((self.p2 - rot(self.k, t) * self.p1).norm() - self.d).abs()
+            })
+            .sum()
+    }
+}
+
 impl Setup for Subproblem1Setup {
     fn setup(&mut self) {
         self.p1 = random_vector3();
@@ -134,8 +167,6 @@ impl Setup for Subproblem1Setup {
     }
 
     fn is_at_local_min(&self) -> bool {
-        const DELTA: f64 = 1e-12;
-
         let error = self.error();
         let error_check = error - DELTA;
 
@@ -185,8 +216,6 @@ impl Setup for Subproblem2Setup {
     }
 
     fn is_at_local_min(&self) -> bool {
-        const DELTA: f64 = 1e-12;
-
         let error = self.error();
         let error_check = error - DELTA;
 
@@ -196,7 +225,7 @@ impl Setup for Subproblem2Setup {
             for j in 0..solutions[i].len() {
                 for sign in [-1.0, 1.0] {
                     let solution_prev = solutions[i][j];
-                    solutions[i][j] += sign;
+                    solutions[i][j] += sign * DELTA;
 
                     if self.calculate_error(&solutions[0], &solutions[1]) < error_check {
                         return false;
@@ -249,3 +278,58 @@ impl Setup for Subproblem2ExtendedSetup {
         "Subproblem 2 Ex"
     }
 }
+
+impl Setup for Subproblem3Setup {
+    fn setup(&mut self) {
+        let theta = random_angle();
+
+        self.p1 = random_vector3();
+        self.p2 = random_vector3();
+        self.k = random_norm_vector3();
+        self.theta = SolutionSet2::One(theta);
+
+        self.d = (self.p2 - rot(self.k, theta) * self.p1).norm();
+    }
+
+    fn setup_ls(&mut self) {
+        self.p1 = random_vector3();
+        self.p2 = random_vector3();
+        self.k = random_norm_vector3();
+        self.d = fastrand::f64();
+        self.theta = SolutionSet2::One(random_angle());
+    }
+
+    fn run(&mut self) {
+        (self.theta, _) = subproblem3(&self.p1, &self.p2, &self.k, self.d);
+    }
+
+    fn error(&self) -> f64 {
+        self.calculate_error(&self.theta.get_all())
+    }
+
+    fn is_at_local_min(&self) -> bool {
+        let error = self.error();
+        let error_check = error - DELTA;
+
+        let mut solution = self.theta.get_all();
+
+        for i in 0..solution.len() {
+            for sign in [-1.0, 1.0] {
+                let solution_prev = solution[i];
+                solution[i] += sign * DELTA;
+
+                if self.calculate_error(&solution) < error_check {
+                    return false;
+                }
+
+                solution[i] = solution_prev;
+            }
+        }
+
+        true
+    }
+
+    fn name(&self) -> &'static str {
+        "Subproblem 3"
+    }
+}