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

Issue:
Rust Subproblem 2



Discussion / hints:

Fix patch:
diff --git a/rust/src/subproblems.rs b/rust/src/subproblems.rs
index a9fd923..8764fe1 100644
--- a/rust/src/subproblems.rs
+++ b/rust/src/subproblems.rs
@@ -1,7 +1,8 @@
-use nalgebra::{ Vector3, Matrix3x2 };
+use nalgebra::{ Vector3, Matrix3x2, Matrix4x3, Matrix4x1, DVector };
 
 /// Solves for theta where rot(k, theta) * p1 = p2 if possible.
 /// 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 a = Matrix3x2::from_columns(&[kxp, -k.cross(&kxp)]);
@@ -12,3 +13,64 @@ pub fn subproblem1(p1: Vector3<f64>, p2: Vector3<f64>, k: Vector3<f64>) -> (f64,
 
     (theta, is_ls)
 }
+
+/// Solves for theta1 and theta2 where rot(k1, theta1)*p1 = rot(k2, theta2)*p2 if possible
+/// If not, minimizes || rot(k1, theta1)*p1 - rot(k2, theta2)*p2 ||
+/// Also returns a boolean of whether or not { theta1, theta2 } is a least-squares solution
+/// There may be 1 or 2 solutions for theta1 and theta2 so they are returned as 1 or 2 sized dynamic vectors
+pub fn subproblem2(p1: Vector3<f64>, p2: Vector3<f64>, k1: Vector3<f64>, k2: Vector3<f64>) -> (DVector<f64>, DVector<f64>, bool) {
+    let is_ls = (p1.norm() - p2.norm()).abs() >= 1e-8;
+
+    let p1 = p1.normalize();
+    let p2 = p2.normalize();
+
+    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)]);
+
+    let radius_1_sq = kxp1.dot(&kxp1);
+    let radius_2_sq = kxp2.dot(&kxp2);
+
+    let k1_d_p1 = k1.dot(&p1);
+    let k2_d_p2 = k2.dot(&p2);
+    let k1_d_k2 = k1.dot(&k2);
+    let ls_frac = 1.0 / (1.0 - k1_d_k2 * k1_d_k2);
+    let alpha_1 = ls_frac * (k1_d_p1 - k1_d_k2 * k2_d_p2);
+    let alpha_2 = ls_frac * (k2_d_p2 - k1_d_k2 * k1_d_p1);
+    let x_ls_1 = alpha_2 * a1.transpose() * k2 / radius_1_sq;
+    let x_ls_2 = alpha_1 * a2.transpose() * k1 / radius_2_sq;
+    let x_ls = Matrix4x1::new(x_ls_1[0], x_ls_1[1], x_ls_2[0], x_ls_2[1]);
+
+    let n_sym = k1.cross(&k2);
+    let pinv_a1 = a1.transpose() / radius_1_sq;
+    let pinv_a2 = a2.transpose() / radius_2_sq;
+
+    let a_perp_tilde = Matrix4x3::from_rows(&[pinv_a1.row(0), pinv_a1.row(1), pinv_a2.row(0), pinv_a2.row(1)]) * n_sym;
+
+    let x_ls_2_norm = x_ls.fixed_rows::<2>(0).norm();
+
+    if x_ls_2_norm < 1.0 {
+        let xi = (1.0 - x_ls_2_norm * x_ls_2_norm).sqrt() / a_perp_tilde.fixed_slice::<2, 1>(0, 0).norm();
+        let sc_1 = x_ls + xi * a_perp_tilde;
+        let sc_2 = x_ls - xi * a_perp_tilde;
+
+        let theta1 = DVector::from_vec(vec![
+            sc_1[0].atan2(sc_1[1]),
+            sc_2[0].atan2(sc_2[1]),
+        ]);
+
+        let theta2 = DVector::from_vec(vec![
+            sc_1[2].atan2(sc_1[3]),
+            sc_2[2].atan2(sc_2[3]),
+        ]);
+
+        (theta1, theta2, is_ls)
+    }
+    else {
+        let theta1 = x_ls[0].atan2(x_ls[1]);
+        let theta2 = x_ls[2].atan2(x_ls[3]);
+
+        (DVector::from_vec(vec![theta1]), DVector::from_vec(vec![theta2]), true)
+    }
+}


Test patch:
diff --git a/rust/src/tests/correctness.rs b/rust/src/tests/correctness.rs
index 70192cd..629289a 100644
--- a/rust/src/tests/correctness.rs
+++ b/rust/src/tests/correctness.rs
@@ -1,4 +1,4 @@
-use super::setups::{ Setup, Subproblem1Setup };
+use super::setups::{ Setup, Subproblem1Setup, Subproblem2Setup };
 
 const TEST_ITERATIONS: usize = 1000;
 
@@ -8,6 +8,7 @@ fn run_tests() {
 
     let setups: Vec<Box<dyn Setup>> = vec![
         Box::new(Subproblem1Setup::new()),
+        Box::new(Subproblem2Setup::new()),
     ];
 
     for mut setup in setups {
@@ -30,6 +31,7 @@ fn run_tests() {
 fn run_tests_ls() {
     let setups: Vec<Box<dyn Setup>> = vec![
         Box::new(Subproblem1Setup::new()),
+        Box::new(Subproblem2Setup::new()),
     ];
 
     for mut setup in setups {
diff --git a/rust/src/tests/setups.rs b/rust/src/tests/setups.rs
index 1c15b40..2106c4c 100644
--- a/rust/src/tests/setups.rs
+++ b/rust/src/tests/setups.rs
@@ -7,10 +7,10 @@ use {
             random_norm_vector3
         },
 
-        subproblems::subproblem1
+        subproblems::{ subproblem1, subproblem2 }
     },
 
-    nalgebra::Vector3,
+    nalgebra::{ Vector3, DVector },
 };
 
 /// An interface for setting up subproblem testing. Resposible for generating parameters, running
@@ -37,6 +37,17 @@ pub struct Subproblem1Setup {
     theta: f64,
 }
 
+#[derive(Clone)]
+pub struct Subproblem2Setup {
+    p1: Vector3<f64>,
+    p2: Vector3<f64>,
+    k1: Vector3<f64>,
+    k2: Vector3<f64>,
+
+    theta1: DVector<f64>,
+    theta2: DVector<f64>,
+}
+
 impl Subproblem1Setup {
     pub fn new() -> Self {
         Self {
@@ -53,6 +64,29 @@ impl Subproblem1Setup {
     }
 }
 
+impl Subproblem2Setup {
+    pub fn new() -> Self {
+        Self {
+            p1: Vector3::zeros(),
+            p2: Vector3::zeros(),
+            k1: Vector3::zeros(),
+            k2: Vector3::zeros(),
+
+            theta1: DVector::zeros(0),
+            theta2: DVector::zeros(0),
+        }
+    }
+
+    fn calculate_error(&self, theta1: &DVector<f64>, theta2: &DVector<f64>) -> f64 {
+        theta1.iter()
+            .zip(theta2.iter())
+            .map(|(&t1, &t2)| {
+                (rot(self.k2, t2) * self.p2 - rot(self.k1,t1) * self.p1).norm()
+            })
+            .sum()
+    }
+}
+
 impl Setup for Subproblem1Setup {
     fn setup(&mut self) {
         self.p1 = random_vector3();
@@ -79,8 +113,8 @@ impl Setup for Subproblem1Setup {
         let error = self.error();
         let error_check = error - DELTA;
 
-        self.calculate_error(error + DELTA) >= error_check &&
-        self.calculate_error(error - DELTA) >= error_check
+        self.calculate_error(self.theta + DELTA) >= error_check &&
+        self.calculate_error(self.theta - DELTA) >= error_check
     }
 
     fn run(&mut self) {
@@ -91,3 +125,63 @@ impl Setup for Subproblem1Setup {
         "Subproblem 1"
     }
 }
+
+impl Setup for Subproblem2Setup {
+    fn setup(&mut self) {
+        self.p1 = random_vector3();
+        self.k1 = random_norm_vector3();
+        self.k2 = random_norm_vector3();
+        self.theta1 = DVector::from_vec(vec![random_angle()]);
+        self.theta2 = DVector::from_vec(vec![random_angle()]);
+
+        self.p2 = rot(self.k2, -self.theta2[0]) * rot(self.k1,self.theta1[0]) * self.p1;
+    }
+
+    fn setup_ls(&mut self) {
+        self.p1 = random_vector3();
+        self.p2 = random_vector3();
+        self.k1 = random_norm_vector3();
+        self.k2 = random_norm_vector3();
+        self.k2 = random_norm_vector3();
+        self.theta1 = DVector::from_vec(vec![random_angle()]);
+        self.theta2 = DVector::from_vec(vec![random_angle()]);
+    }
+
+    fn run(&mut self) {
+        (self.theta1, self.theta2, _) = subproblem2(self.p1, self.p2, self.k1, self.k2);
+    }
+
+    fn error(&self) -> f64 {
+        self.calculate_error(&self.theta1, &self.theta2)
+    }
+
+    fn is_at_local_min(&self) -> bool {
+        const DELTA: f64 = 1e-12;
+
+        let error = self.error();
+        let error_check = error - DELTA;
+
+        let mut solutions = [self.theta1.clone(), self.theta2.clone()];
+
+        for i in 0..solutions.len() {
+            for j in 0..self.theta1.len() {
+                for sign in [-1.0, 1.0] {
+                    let solution_prev = solutions[i][j];
+                    solutions[i][j] += sign;
+
+                    if self.calculate_error(&solutions[0], &solutions[1]) < error_check {
+                        return false;
+                    }
+
+                    solutions[i][j] = solution_prev;
+                }
+            }
+        }
+
+        true
+    }
+
+    fn name(&self) -> &'static str {
+        "Subproblem 2"
+    }
+}