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

Issue:
Rust Subproblem 1



Discussion / hints:

Fix patch:
diff --git a/rust/.gitignore b/rust/.gitignore
new file mode 100644
index 0000000..079a798
--- /dev/null
+++ b/rust/.gitignore
@@ -0,0 +1,5 @@
+# Compilation directory
+target
+
+# Dependancy info
+Cargo.lock
diff --git a/rust/Cargo.toml b/rust/Cargo.toml
new file mode 100644
index 0000000..facc291
--- /dev/null
+++ b/rust/Cargo.toml
@@ -0,0 +1,11 @@
+[package]
+name = "linear-subproblem-solutions-rust"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+nalgebra = "0.31.1"
+fastrand = "1.8.0"
+
+[lib]
+doctest = false
diff --git a/rust/README.md b/rust/README.md
index e69de29..3f303e3 100644
--- a/rust/README.md
+++ b/rust/README.md
@@ -0,0 +1,25 @@
+# Linear Subproblem Solutions (Rust)
+
+## Usage
+
+1. Clone the rust directory
+
+2. Add the following to your project's `Cargo.toml`
+
+    ```
+    [dependencies.linear-subproblem-solutions-rust]
+        path = ".../linear-subproblem-solutions/rust"
+    ```
+
+3. Add to your source files where needed (Optional)
+
+    ```
+    use linear_subproblem_solutions_rust as subproblems;
+    use subproblems::nalgebra as na;
+    ```
+
+## Testing
+
+1. Clone the rust directory
+2. Run `cargo test`\
+Run `cargo test -- --nocapture` for diagnostic info
diff --git a/rust/src/lib.rs b/rust/src/lib.rs
new file mode 100644
index 0000000..6416dcf
--- /dev/null
+++ b/rust/src/lib.rs
@@ -0,0 +1,6 @@
+pub use nalgebra;
+
+pub mod subproblems;
+
+#[cfg(test)]
+mod tests;
diff --git a/rust/src/subproblems.rs b/rust/src/subproblems.rs
new file mode 100644
index 0000000..a9fd923
--- /dev/null
+++ b/rust/src/subproblems.rs
@@ -0,0 +1,14 @@
+use nalgebra::{ Vector3, Matrix3x2 };
+
+/// Solves for theta where rot(k, theta) * p1 = p2 if possible.
+/// If not, minimizes || rot(k, theta) * p1 - p2 ||
+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)]);
+    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;
+
+    (theta, is_ls)
+}


Test patch:
diff --git a/rust/src/tests/auxiliary.rs b/rust/src/tests/auxiliary.rs
new file mode 100644
index 0000000..339e2e4
--- /dev/null
+++ b/rust/src/tests/auxiliary.rs
@@ -0,0 +1,32 @@
+use nalgebra::{ Vector3, Matrix3 };
+
+/// Creates a 3x3 rotation matrix
+pub fn rot(k: Vector3<f64>, theta: f64) -> Matrix3<f64> {
+    let k = k.normalize();
+    Matrix3::identity() + hat(k) * theta.sin() + hat(k) * hat(k) * (1.0 - theta.cos())
+}
+
+pub fn random_vector3() -> Vector3<f64> {
+    Vector3::new(random(), random(), random())
+}
+
+pub fn random_norm_vector3() -> Vector3<f64> {
+    Vector3::new(random(), random(), random()).normalize()
+}
+
+pub fn random_angle() -> f64 {
+    random() * std::f64::consts::PI
+}
+
+fn random() -> f64 {
+    fastrand::f64() * 2.0 - 1.0
+}
+
+// Matrix cross-product for a 3 x 3 vector
+fn hat(k: Vector3<f64>) -> Matrix3<f64> {
+    Matrix3::new(
+        0.0, -k.z, k.y,
+        k.z, 0.0, -k.x,
+        -k.y, k.x, 0.0,
+    )
+}
diff --git a/rust/src/tests/correctness.rs b/rust/src/tests/correctness.rs
new file mode 100644
index 0000000..70192cd
--- /dev/null
+++ b/rust/src/tests/correctness.rs
@@ -0,0 +1,43 @@
+use super::setups::{ Setup, Subproblem1Setup };
+
+const TEST_ITERATIONS: usize = 1000;
+
+#[test]
+fn run_tests() {
+    const ERROR_THRESHOLD: f64 = 1e-12;
+
+    let setups: Vec<Box<dyn Setup>> = vec![
+        Box::new(Subproblem1Setup::new()),
+    ];
+
+    for mut setup in setups {
+        let mut total_error = 0.0;
+
+        for _ in 0..TEST_ITERATIONS {
+            setup.setup();
+            setup.run();
+
+            let error = setup.error();
+            assert!(error <= ERROR_THRESHOLD);
+            total_error += error;
+        }
+
+        println!("{} Avg Error:\t{}", setup.name(), total_error / TEST_ITERATIONS as f64);
+    }
+}
+
+#[test]
+fn run_tests_ls() {
+    let setups: Vec<Box<dyn Setup>> = vec![
+        Box::new(Subproblem1Setup::new()),
+    ];
+
+    for mut setup in setups {
+        for _ in 0..TEST_ITERATIONS {
+            setup.setup_ls();
+            setup.run();
+
+            assert!(setup.is_at_local_min());
+        }
+    }
+}
diff --git a/rust/src/tests/mod.rs b/rust/src/tests/mod.rs
new file mode 100644
index 0000000..4810479
--- /dev/null
+++ b/rust/src/tests/mod.rs
@@ -0,0 +1,3 @@
+mod correctness;
+mod setups;
+mod auxiliary;
diff --git a/rust/src/tests/setups.rs b/rust/src/tests/setups.rs
new file mode 100644
index 0000000..1c15b40
--- /dev/null
+++ b/rust/src/tests/setups.rs
@@ -0,0 +1,93 @@
+use {
+    crate::{
+        tests::auxiliary::{
+            random_vector3,
+            random_angle,
+            rot,
+            random_norm_vector3
+        },
+
+        subproblems::subproblem1
+    },
+
+    nalgebra::Vector3,
+};
+
+/// An interface for setting up subproblem testing. Resposible for generating parameters, running
+/// the function, and calculating data such as the error.
+pub trait Setup {
+    fn name(&self) -> &'static str;
+    fn is_at_local_min(&self) -> bool;
+    fn error(&self) -> f64;
+
+    /// Initialize parameters to test the case where theta is solved for exactly
+    fn setup(&mut self);
+
+    /// Initialize parameters to test the case where theta is minimized using least squares
+    fn setup_ls(&mut self);
+
+    fn run(&mut self);
+}
+
+pub struct Subproblem1Setup {
+    p1: Vector3<f64>,
+    p2: Vector3<f64>,
+    k: Vector3<f64>,
+
+    theta: f64,
+}
+
+impl Subproblem1Setup {
+    pub fn new() -> Self {
+        Self {
+            p1: Vector3::zeros(),
+            p2: Vector3::zeros(),
+            k: Vector3::zeros(),
+
+            theta: 0.0,
+        }
+    }
+
+    fn calculate_error(&self, theta: f64) -> f64 {
+        (self.p2 - rot(self.k, theta) * self.p1).norm()
+    }
+}
+
+impl Setup for Subproblem1Setup {
+    fn setup(&mut self) {
+        self.p1 = random_vector3();
+        self.k = random_norm_vector3();
+        self.theta = random_angle();
+
+        self.p2 = rot(self.k, self.theta) * self.p1;
+    }
+
+    fn error(&self) -> f64 {
+        self.calculate_error(self.theta)
+    }
+
+    fn setup_ls(&mut self) {
+        self.p1 = random_vector3();
+        self.p2 = random_vector3();
+        self.k = random_norm_vector3();
+        self.theta = random_angle();
+    }
+
+    fn is_at_local_min(&self) -> bool {
+        const DELTA: f64 = 1e-12;
+
+        let error = self.error();
+        let error_check = error - DELTA;
+
+        self.calculate_error(error + DELTA) >= error_check &&
+        self.calculate_error(error - DELTA) >= error_check
+    }
+
+    fn run(&mut self) {
+        (self.theta, _) = subproblem1(self.p1, self.p2, self.k);
+    }
+
+    fn name(&self) -> &'static str {
+        "Subproblem 1"
+    }
+}