Skip to content

Commit

Permalink
Updated 1D search, now using MLSL
Browse files Browse the repository at this point in the history
  • Loading branch information
kevindlewis23 committed Jun 19, 2024
1 parent deca1e5 commit 7b4327b
Show file tree
Hide file tree
Showing 6 changed files with 112 additions and 74 deletions.
25 changes: 25 additions & 0 deletions ik_python/Cargo.lock

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

5 changes: 3 additions & 2 deletions ik_python/test/test.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,15 +50,16 @@

zeroresult = [0.0] * 6

epsilon = 1e-2
epsilon = 1e-3



np.random.seed(0)


# Test the general robots
class TestGeneralRobots(unittest.TestCase):
def check_general_robot(self, bot):
np.random.seed(0)
for configNum, testCase in enumerate(bot.testcases):
hMatrix = np.reshape(testCase.hVals, (6, 3))
pMatrix = np.reshape(testCase.pVals, (7, 3))
Expand Down
25 changes: 25 additions & 0 deletions rust/Cargo.lock

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

1 change: 1 addition & 0 deletions rust/Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ nalgebra = "0.32.2"
fastrand = "1.8.0"
argmin = "0.8.1"
argmin-math = { version = "0.3.0", features = [ "nalgebra_latest-serde" ] }
nlopt = "0.7.0"

[dev-dependencies.criterion]
version = "0.3"
Expand Down
126 changes: 56 additions & 70 deletions rust/src/inverse_kinematics/auxiliary.rs
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ use {
consts::{PI, TAU},
INFINITY, NAN,
},
nlopt::{Algorithm, Nlopt},
};

pub type Matrix3x7<T> = Matrix<T, U3, U7, ArrayStorage<T, 3, 7>>;
Expand Down Expand Up @@ -85,85 +86,69 @@ pub fn wrap_to_pi(theta: f64) -> f64 {
(theta + PI).rem_euclid(TAU) - PI
}

fn find_zero<const N: usize, F: Fn(f64) -> Vector<f64, N>>(
pub fn search_1d<const N: usize, F: Fn(f64) -> Vector<f64, N> > (
f: F,
left: f64,
right: f64,
i: usize,
) -> Option<f64> {
const ITERATIONS: usize = 100;
const EPSILON: f64 = 1e-5;

let mut x_left = left;
let mut x_right = right;

let mut y_left = f(x_left)[i];
let mut y_right = f(x_right)[i];

for _ in 0..ITERATIONS {
let delta = y_right - y_left;

if delta.abs() < EPSILON {
break;
}

let x_0 = x_left - y_left * (x_right - x_left) / delta;
let y_0 = f(x_0)[i];

if !y_0.is_finite() {
return None;
}

if (y_left < 0.0) != (y_0 < 0.0) {
x_left = x_0;
y_left = y_0;
} else {
x_right = x_0;
y_right = y_0;
}
}

if left <= x_left && x_left <= right {
Some(x_left)
} else {
None
}
}

pub fn search_1d<const N: usize, F: Fn(f64) -> Vector<f64, N>>(
f: F,
left: f64,
right: f64,
initial_samples: usize,
right: f64
) -> Vec<(f64, usize)> {
const CROSS_THRESHOLD: f64 = 0.1;

let delta = (right - left) / initial_samples as f64;

let mut last_v = f(left);
let mut x = left + delta;

let mut zeros = Vec::with_capacity(8);

for _ in 0..initial_samples {
let v = f(x);
let mut results : Vec<(f64,usize)> = Vec::new();
let max_evals = [50, 200, 1000, 2000];
let populations = [10, 30, 200, 400];
let epsilon = 1e-8;

// Implement send with f
// f.clone();
// let f = f.clone();

// Do some iterative deepening
for (max_eval, population) in core::iter::zip(max_evals, populations) {
for i in 0..N {
// TODO: parallelize this, there are 4 branches that don't depend on each other
// Problems occur trying to multithread this due to the closure not being thread-safe.

let cost_function = |x: &[f64], _gradient: Option<&mut [f64]>, _params: &mut ()| {
let err = f(x[0])[i];
err * err
};


let mut optimizer = Nlopt::new(
Algorithm::GnMlslLds,
1,
cost_function,
nlopt::Target::Minimize, ());


let mut x = [(left + right) / 2.0];
optimizer.set_upper_bounds(&[right]).unwrap();
optimizer.set_lower_bounds(&[left]).unwrap();
optimizer.set_xtol_abs(&[epsilon]).unwrap();
optimizer.set_stopval(epsilon).unwrap();
optimizer.set_maxeval(max_eval).unwrap();
optimizer.set_population(population).unwrap();

let result = optimizer.optimize(&mut x);
// Check for error
if let Err(e) = result {
// Didn't optimize, no zero on this branch
println!("Error: {:?}", e);
continue;
}
// Get the error
let error = result.unwrap().1;

for (i, (&y, &last_y)) in v.iter().zip(last_v.into_iter()).enumerate() {
if (y < 0.0) != (last_y < 0.0)
&& y.abs() < CROSS_THRESHOLD
&& last_y.abs() < CROSS_THRESHOLD
{
if let Some(z) = find_zero(&f, x - delta, x, i) {
zeros.push((z, i));
}
if error < epsilon {
results.push((x[0], i));
}

}
if results.len() > 0 {
break;
}

last_v = v;
x += delta;
}
results

zeros
}

struct ProblemParams<const N: usize, F: Fn(f64, f64) -> Vector<f64, N>> {
Expand All @@ -186,6 +171,7 @@ pub fn search_2d<const N: usize, F: Fn(f64, f64) -> Vector<f64, N>>(
max: (f64, f64),
n: usize,
) -> Vec<(f64, f64, usize)> {
// TODO: replace this with MLSL algorithm
fn minimum<const N: usize>(
mesh: &Vec<Vector<f64, N>>,
n: usize,
Expand Down
4 changes: 2 additions & 2 deletions rust/src/inverse_kinematics/mod.rs
Original file line number Diff line number Diff line change
Expand Up @@ -456,7 +456,7 @@ pub fn two_parallel(
error
};

let q1_vec = search_1d(error_given_q1, -PI, PI, 200);
let q1_vec = search_1d(error_given_q1, -PI, PI);

for (q1, i) in q1_vec {
let t64 = t64_given_q1(r_06, kin, q1, &p_16).get_all();
Expand Down Expand Up @@ -558,7 +558,7 @@ pub fn two_intersecting(
error
};

let q4_vec = search_1d(alignment_error_given_q4, -PI, PI, 200);
let q4_vec = search_1d(alignment_error_given_q4, -PI, PI);

for (q4, i) in q4_vec {
let q_partial: Vector4<f64> = q_partial_given_q4(q4, kin, &p_16).column(i).into();
Expand Down

0 comments on commit 7b4327b

Please sign in to comment.