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

Issue:
Rust Subproblem 4



Discussion / hints:

Fix patch:
diff --git a/rust/src/subproblems.rs b/rust/src/subproblems.rs
index e730f1e..f56a371 100644
--- a/rust/src/subproblems.rs
+++ b/rust/src/subproblems.rs
@@ -188,3 +188,39 @@ pub fn subproblem3(p1: &Vector3<f64>, p2: &Vector3<f64>, k: &Vector3<f64>, d: f6
         sc_2[0].atan2(sc_2[1]),
     ), false)
 }
+
+/**
+Solves for theta where h' * rot(k, theta) * p = d if possible.
+If not minimizes | h' * rot(k, theta) * p - d |.
+Also returns a boolean of whether or not theta is a least-squares solution.
+ */
+pub fn subproblem4(h: &Vector3<f64>, p: &Vector3<f64>, k: &Vector3<f64>, d: f64) -> (SolutionSet2<f64>, bool) {
+    let a_11 = k.cross(p);
+    let a_1 = Matrix3x2::from_columns(&[a_11, -k.cross(&a_11)]);
+    let a = h.transpose() * a_1;
+
+    let b = d - (h.transpose() * k * k.transpose() * p)[0];
+
+    let norm_a_2 = a.norm_squared();
+
+    let x_ls = a_1.transpose() * h * b;
+
+    if norm_a_2 > b * b {
+        let xi = (norm_a_2 - b * b).sqrt();
+        let a_perp_tilde = Vector2::new(a[1], -a[0]);
+
+        let sc_1 = x_ls + xi * a_perp_tilde;
+        let sc_2 = x_ls - xi * a_perp_tilde;
+
+        (
+            SolutionSet2::Two(sc_1[0].atan2(sc_1[1]), sc_2[0].atan2(sc_2[1])),
+            false,
+        )
+    }
+    else {
+        (
+            SolutionSet2::One(x_ls[0].atan2(x_ls[1])),
+            true,
+        )
+    }
+}


Test patch:
diff --git a/rust/src/tests/correctness.rs b/rust/src/tests/correctness.rs
index f2d3ee5..271e035 100644
--- a/rust/src/tests/correctness.rs
+++ b/rust/src/tests/correctness.rs
@@ -1,4 +1,11 @@
-use super::setups::{ Setup, Subproblem1Setup, Subproblem2Setup, Subproblem2ExtendedSetup, Subproblem3Setup };
+use super::setups::{
+    Setup,
+    Subproblem1Setup,
+    Subproblem2Setup,
+    Subproblem2ExtendedSetup,
+    Subproblem3Setup,
+    Subproblem4Setup,
+};
 
 const TEST_ITERATIONS: usize = 1000;
 
@@ -11,6 +18,7 @@ fn run_tests() {
         Box::new(Subproblem2Setup::new()),
         Box::new(Subproblem2ExtendedSetup::new()),
         Box::new(Subproblem3Setup::new()),
+        Box::new(Subproblem4Setup::new()),
     ];
 
     for mut setup in setups {
@@ -35,6 +43,7 @@ fn run_tests_ls() {
         Box::new(Subproblem1Setup::new()),
         Box::new(Subproblem2Setup::new()),
         Box::new(Subproblem3Setup::new()),
+        Box::new(Subproblem4Setup::new()),
     ];
 
     for mut setup in setups {
diff --git a/rust/src/tests/setups.rs b/rust/src/tests/setups.rs
index f5fcdb0..edb103e 100644
--- a/rust/src/tests/setups.rs
+++ b/rust/src/tests/setups.rs
@@ -1,3 +1,5 @@
+use crate::subproblems::subproblem4;
+
 use {
     crate::{
         tests::auxiliary::{
@@ -60,6 +62,23 @@ pub struct Subproblem2ExtendedSetup {
     theta1: f64,
     theta2: f64,
 }
+pub struct Subproblem3Setup {
+    p1: Vector3<f64>,
+    p2: Vector3<f64>,
+    k: Vector3<f64>,
+    d: f64,
+
+    theta: SolutionSet2<f64>,
+}
+
+pub struct Subproblem4Setup {
+    h: Vector3<f64>,
+    p: Vector3<f64>,
+    k: Vector3<f64>,
+    d: f64,
+
+    theta: SolutionSet2<f64>,
+}
 
 pub struct Subproblem3Setup {
     p1: Vector3<f64>,
@@ -146,6 +165,28 @@ impl Subproblem3Setup {
     }
 }
 
+impl Subproblem4Setup {
+    pub fn new() -> Self {
+        Self {
+            h: Vector3::zeros(),
+            p: 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.h.transpose() * rot(self.k, t) * self.p)[0] - self.d).abs()
+            })
+            .sum()
+    }
+}
+
 impl Setup for Subproblem1Setup {
     fn setup(&mut self) {
         self.p1 = random_vector3();
@@ -333,3 +374,58 @@ impl Setup for Subproblem3Setup {
         "Subproblem 3"
     }
 }
+
+impl Setup for Subproblem4Setup {
+    fn setup(&mut self) {
+        let theta = random_angle();
+
+        self.h = random_norm_vector3();
+        self.p = random_vector3();
+        self.k = random_norm_vector3();
+        self.theta = SolutionSet2::One(theta);
+
+        self.d = (self.h.transpose() * rot(self.k, theta) * self.p)[0];
+    }
+
+    fn setup_ls(&mut self) {
+        self.h = random_norm_vector3();
+        self.p = random_vector3();
+        self.k = random_norm_vector3();
+        self.d = fastrand::f64();
+        self.theta = SolutionSet2::One(random_angle());
+    }
+
+    fn run(&mut self) {
+        (self.theta, _) = subproblem4(&self.h, &self.p, &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 4"
+    }
+}