diff --git a/Cargo.lock b/Cargo.lock index 62ccd43..0bae550 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -201,6 +201,12 @@ version = "1.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a26ae43d7bcc3b814de94796a5e736d4029efb0ee900c12e2d54c993ad1a1e07" +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + [[package]] name = "errno" version = "0.3.5" @@ -211,6 +217,12 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "fixedbitset" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80" + [[package]] name = "getrandom" version = "0.2.10" @@ -228,6 +240,12 @@ version = "1.8.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "eabb4a44450da02c90444cf74558da904edde8fb4e9035a9a6a4e15445af0bd7" +[[package]] +name = "hashbrown" +version = "0.14.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" + [[package]] name = "heck" version = "0.4.1" @@ -240,6 +258,16 @@ version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d77f7ec81a6d05a3abb01ab6eb7590f6083d08449fe5a1c8b1e620283546ccb7" +[[package]] +name = "indexmap" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d530e1a18b1cb4c484e6e34556a0d948706958449fca0cab753d649f2bce3d1f" +dependencies = [ + "equivalent", + "hashbrown", +] + [[package]] name = "indoc" version = "1.0.9" @@ -281,20 +309,6 @@ dependencies = [ "wasm-bindgen", ] -[[package]] -name = "k" -version = "0.31.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1c22704d2d3c33ee3b32d08fda58336630dda6df248709362f7f363e64e76d59" -dependencies = [ - "nalgebra", - "parking_lot", - "simba", - "thiserror", - "tracing", - "urdf-rs", -] - [[package]] name = "libc" version = "0.2.149" @@ -356,9 +370,9 @@ dependencies = [ [[package]] name = "nalgebra" -version = "0.30.1" +version = "0.32.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fb2d0de08694bed883320212c18ee3008576bfe8c306f4c3c4a58b4876998be" +checksum = "307ed9b18cc2423f29e83f84fd23a8e73628727990181f18641a8b5dc2ab1caa" dependencies = [ "approx", "matrixmultiply", @@ -375,9 +389,9 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.1.0" +version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01fcc0b8149b4632adc89ac3b7b31a12fb6099a0317a4eb2ebff574ef7de7218" +checksum = "91761aed67d03ad966ef783ae962ef9bbaca728d2dd7ceb7939ec110fffad998" dependencies = [ "proc-macro2", "quote", @@ -443,9 +457,9 @@ version = "0.3.0" dependencies = [ "approx", "criterion", - "k", "nalgebra", "ordered-float", + "petgraph", "rand", "rand_chacha", "rayon", @@ -513,10 +527,14 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" [[package]] -name = "pin-project-lite" -version = "0.2.13" +name = "petgraph" +version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8afb450f006bf6385ca15ef45d71d2288452bc3683ce2e2cacc0d18e4be60b58" +checksum = "e1d3afd2628e69da2be385eb6f2fd57c8ac7977ceeff6dc166ff1657b0e386a9" +dependencies = [ + "fixedbitset", + "indexmap", +] [[package]] name = "plotters" @@ -810,9 +828,9 @@ dependencies = [ [[package]] name = "simba" -version = "0.7.3" +version = "0.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2f3fd720c48c53cace224ae62bef1bbff363a70c68c4802a78b5cc6159618176" +checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae" dependencies = [ "approx", "num-complex", @@ -889,37 +907,6 @@ dependencies = [ "serde_json", ] -[[package]] -name = "tracing" -version = "0.1.40" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c3523ab5a71916ccf420eebdf5521fcef02141234bbc0b8a49f2fdc4544364ef" -dependencies = [ - "pin-project-lite", - "tracing-attributes", - "tracing-core", -] - -[[package]] -name = "tracing-attributes" -version = "0.1.27" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.38", -] - -[[package]] -name = "tracing-core" -version = "0.1.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c06d3da6113f116aaee68e4d601191614c9053067f9ab7f6edbcb161237daa54" -dependencies = [ - "once_cell", -] - [[package]] name = "typenum" version = "1.17.0" diff --git a/crates/optik-cpp/Cargo.toml b/crates/optik-cpp/Cargo.toml index d6aa5b6..d336894 100644 --- a/crates/optik-cpp/Cargo.toml +++ b/crates/optik-cpp/Cargo.toml @@ -11,6 +11,6 @@ name = "optikcpp" crate-type = ["staticlib"] [dependencies] -nalgebra = "0.30" +nalgebra = "0.32" optik = { path = "../optik" } rand = "0.8" diff --git a/crates/optik-py/Cargo.toml b/crates/optik-py/Cargo.toml index 343af88..f6d8fed 100644 --- a/crates/optik-py/Cargo.toml +++ b/crates/optik-py/Cargo.toml @@ -14,6 +14,6 @@ crate-type = ["cdylib"] pyo3-build-config = "0.19" [dependencies] -nalgebra = "0.30" +nalgebra = "0.32" optik = { path = "../optik" } pyo3 = { version = "0.19", features = ["extension-module", "abi3-py38"] } diff --git a/crates/optik/Cargo.toml b/crates/optik/Cargo.toml index 7d82432..1b57bfd 100644 --- a/crates/optik/Cargo.toml +++ b/crates/optik/Cargo.toml @@ -11,9 +11,9 @@ name = "optik" crate-type = ["rlib"] [dependencies] -k = "0.31" -nalgebra = "0.30" +nalgebra = "0.32" ordered-float = "4.2" +petgraph = "0.6.4" rand = "0.8" rand_chacha = "0.3" rayon = "1.8" @@ -27,7 +27,7 @@ path = "../../examples/example.rs" [dev-dependencies] approx = "0.5" criterion = "0.5" -nalgebra = { version = "0.30", features = [ "rand", "serde-serialize" ] } +nalgebra = { version = "0.32", features = [ "rand", "serde-serialize" ] } serde = "1.0" serde_json = "1.0" diff --git a/crates/optik/src/kinematics.rs b/crates/optik/src/kinematics.rs index ff5609a..fb1799a 100644 --- a/crates/optik/src/kinematics.rs +++ b/crates/optik/src/kinematics.rs @@ -1,132 +1,293 @@ -use nalgebra::{Isometry3, Matrix6xX, RealField, Translation3, Unit, UnitQuaternion, Vector3}; +use nalgebra::{Isometry3, Matrix6xX, Translation, Translation3, Unit, UnitQuaternion, Vector3}; +use petgraph::{ + algo::is_cyclic_directed, + prelude::{DiGraph, EdgeIndex}, +}; -#[derive(Debug, Clone)] -pub struct Joint { - offset: Isometry3, - typ: JointType, +#[derive(Clone)] +pub struct KinematicChain { + pub joints: Vec, } -#[derive(Debug, Clone)] -enum JointType { - Revolute { axis: Unit> }, - Prismatic { axis: Unit> }, -} +impl KinematicChain { + /// Parse a kinematic chain from the given URDF representation spanning from + /// the given base link to the EE link. + /// + /// Panics if one of the links does not exist or there is no path between + /// them. + pub fn from_urdf(robot: &urdf_rs::Robot, base_link: &str, ee_link: &str) -> Self { + let graph = parse_urdf(robot); -impl Joint { - pub fn revolute(offset: Isometry3, axis: Unit>) -> Joint { - Joint { - offset, - typ: JointType::Revolute { axis }, - } + assert!(!is_cyclic_directed(&graph), "robot model contains loops"); + + // Identify the links along the chain from base to EE. + let sorted_links = { + let base_link_ix = graph + .node_indices() + .find(|&ix| graph[ix].name == base_link) + .unwrap_or_else(|| panic!("base link '{}' does not exist", base_link)); + + let ee_link_ix = graph + .node_indices() + .find(|&ix| graph[ix].name == ee_link) + .unwrap_or_else(|| panic!("EE link '{}' does not exist", base_link)); + + petgraph::algo::astar( + &graph, + base_link_ix, + |ix| ix == ee_link_ix, + |_| 1.0, + |_| 0.0, + ) + .expect("no path from base to EE link") + .1 + }; + + // Identify the serial chain of joints that connects the links of + // interest. + let joints = sorted_links.windows(2).map(|links| { + let joint = graph.find_edge(links[0], links[1]).unwrap(); + graph[joint].clone() + }); + + // OPTIMIZATION: Fold fixed joints to avoid recomputing constant offsets + // on every forward kinematic computation. + let joints = joints + .scan(Isometry3::identity(), |tfm, joint| match joint.typ { + JointType::Fixed => { + // Accumulate the transforms from successive fixed joints. + *tfm = joint.origin * *tfm; + Some(None) + } + _ => { + // When we encounter a non-fixed joint, apply the + // accumulated fixed joint transforms (if any) and then + // reset the accumulation. + let new_joint = Joint { + origin: joint.origin * *tfm, + ..joint + }; + *tfm = Isometry3::identity(); + + Some(Some(new_joint)) + } + }) + .flatten(); + + let joints: Vec = joints.collect(); + + // We don't care to support empty chains. + assert!(!joints.is_empty(), "kinematic chain is empty"); + + Self { joints } } - pub fn prismatic(offset: Isometry3, axis: Unit>) -> Joint { - Joint { - offset, - typ: JointType::Prismatic { axis }, - } + pub fn nq(&self) -> usize { + self.joints.iter().map(|j| j.typ.nq()).sum() } - fn local_transform(&self, q: &[T]) -> Isometry3 { - match self.typ { - JointType::Revolute { axis } => { - // let s = q[0].sin(); - // let c = q[0].cos(); - - // let v = Vector3::x(); - // let k = &axis; - // let v_rot = - // v * c + k.cross(&v) * s + k.scale(k.dot(&v) * (nalgebra::one::() - c)); - self.offset * UnitQuaternion::from_axis_angle(&axis, q[0]) - } - JointType::Prismatic { axis } => self.offset * Translation3::from(axis.scale(q[0])), + pub fn forward_kinematics(&self, q: &[f64]) -> ForwardKinematics { + struct State { + qidx: usize, + tfm: Isometry3, } + + let joint_tfms: Vec> = self + .joints + .iter() + .scan( + State { + tfm: Isometry3::identity(), + qidx: 0, + }, + |state, joint| { + let qrange = state.qidx..(state.qidx + joint.typ.nq()); + let joint_q = &q[qrange]; + + state.tfm *= joint.origin * joint.typ.local_transform(joint_q); + state.qidx += joint.typ.nq(); + + Some(state.tfm) + }, + ) + .collect(); + + let ee_tfm = *joint_tfms.last().unwrap(); + ForwardKinematics { joint_tfms, ee_tfm } } - pub fn nq(&self) -> usize { - match self.typ { - JointType::Revolute { .. } => 1, - JointType::Prismatic { .. } => 1, + pub fn joint_jacobian(&self, fk: &ForwardKinematics) -> Matrix6xX { + let dof = self.nq(); + let tfm_w_ee = fk.ee_tfm(); + + let mut m = Matrix6xX::zeros(dof); + let mut col = 0; + for (i, joint) in self.joints.iter().enumerate() { + let tfm_w_i = fk.joint_tfms[i]; + + match joint.typ { + JointType::Revolute(axis) => { + let a_i = tfm_w_i.rotation * axis; + let dp_i = + a_i.cross(&(tfm_w_ee.translation.vector - tfm_w_i.translation.vector)); + + // in local frame + let a_i = tfm_w_ee.inverse_transform_vector(&a_i); + let dp_i = tfm_w_ee.inverse_transform_vector(&dp_i); + + m.fixed_view_mut::<3, 1>(0, col).copy_from(&dp_i); + m.fixed_view_mut::<3, 1>(3, col).copy_from(&a_i); + } + JointType::Prismatic(_) => todo!(), + JointType::Fixed => {} + }; + + col += joint.typ.nq(); } + + m } } -pub fn joint_kinematics<'a>( - chain: &'a [Joint], - q: &'a [f64], -) -> impl Iterator> + 'a { - struct State { - qidx: usize, - tfm: Isometry3, +#[derive(Default, Clone)] +pub struct ForwardKinematics { + joint_tfms: Vec>, + ee_tfm: Isometry3, +} + +impl ForwardKinematics { + pub fn joint_tfm(&self, joint_ix: EdgeIndex) -> &Isometry3 { + &self.joint_tfms[joint_ix.index()] + } + + pub fn ee_tfm(&self) -> &Isometry3 { + &self.ee_tfm } +} - chain.iter().scan( - State { - tfm: Isometry3::identity(), - qidx: 0, - }, - |state, joint| { - state.tfm *= joint.local_transform(&q[state.qidx..(state.qidx + joint.nq())]); - state.qidx += joint.nq(); - - Some(state.tfm) - }, - ) +#[derive(Clone)] +pub struct Joint { + pub name: String, + pub typ: JointType, + pub limits: (f64, f64), + pub origin: Isometry3, } -pub fn joint_jacobian( - arm: &[Joint], - ee_offset: &Isometry3, - tfms: &[Isometry3], -) -> Matrix6xX { - let dof = arm.len(); - let tfm_w_ee = tfms.last().unwrap() * ee_offset; - - let mut m = Matrix6xX::zeros(dof); - for (col, joint) in arm.iter().enumerate() { - let tfm_w_i = tfms[col]; - - match joint.typ { - JointType::Revolute { axis, .. } => { - let a_i = tfm_w_i.rotation * axis; - let dp_i = a_i.cross(&(tfm_w_ee.translation.vector - tfm_w_i.translation.vector)); - - // in local frame - let a_i = tfm_w_ee.inverse_transform_vector(&a_i); - let dp_i = tfm_w_ee.inverse_transform_vector(&dp_i); - - m.fixed_slice_mut::<3, 1>(0, col).copy_from(&dp_i); - m.fixed_slice_mut::<3, 1>(3, col).copy_from(&a_i); +#[derive(Clone, Debug, PartialEq)] +pub enum JointType { + Revolute(Unit>), + Prismatic(Unit>), + Fixed, +} + +impl JointType { + pub fn nq(&self) -> usize { + match self { + JointType::Revolute(_) => 1, + JointType::Prismatic(_) => 1, + JointType::Fixed => 0, + } + } + + pub fn local_transform(&self, q: &[f64]) -> Isometry3 { + match self { + JointType::Revolute(axis) => { + let rotation = UnitQuaternion::from_axis_angle(axis, q[0]); + Isometry3::from_parts(Translation3::identity(), rotation) } - JointType::Prismatic { .. } => todo!(), + JointType::Prismatic(axis) => { + let translation = axis.scale(q[0]); + Isometry3::from_parts(Translation3::from(translation), UnitQuaternion::identity()) + } + JointType::Fixed => Isometry3::identity(), + } + } +} + +#[derive(Clone)] +pub struct Link { + pub name: String, +} + +fn urdf_to_tfm(pose: &urdf_rs::Pose) -> Isometry3 { + let xyz = Vector3::from_row_slice(&pose.xyz.0); + let rot = UnitQuaternion::from_euler_angles(pose.rpy.0[0], pose.rpy.0[1], pose.rpy.0[2]); + Isometry3::from_parts(Translation::from(xyz), rot) +} + +fn parse_urdf(urdf: &urdf_rs::Robot) -> DiGraph { + let mut graph = DiGraph::::new(); + + for link in &urdf.links { + graph.add_node(Link { + name: link.name.clone(), + }); + } + + for joint in &urdf.joints { + let parent_ix = graph + .node_indices() + .find(|&l| graph[l].name == joint.parent.link) + .unwrap_or_else(|| panic!("joint parent link '{}' does not exist", joint.parent.link)); + let child_ix = graph + .node_indices() + .find(|&l| graph[l].name == joint.child.link) + .unwrap_or_else(|| panic!("joint child link '{}' does not exist", joint.parent.link)); + + let joint_type = match &joint.joint_type { + urdf_rs::JointType::Revolute => JointType::Revolute(Unit::new_normalize( + Vector3::from_row_slice(&joint.axis.xyz.0), + )), + urdf_rs::JointType::Prismatic => JointType::Prismatic(Unit::new_normalize( + Vector3::from_row_slice(&joint.axis.xyz.0), + )), + urdf_rs::JointType::Fixed => JointType::Fixed, + x => panic!("joint type not supported: {:?}", x), }; + + let limits = if joint.limit.upper - joint.limit.lower > 0.0 { + (joint.limit.lower, joint.limit.upper) + } else { + (f64::NEG_INFINITY, f64::INFINITY) + }; + let origin = urdf_to_tfm(&joint.origin); + + graph.add_edge( + parent_ix, + child_ix, + Joint { + name: joint.name.clone(), + typ: joint_type, + limits, + origin, + }, + ); } - m + graph } /// A cache structure which stores the expensive forward kinematics computation /// for its respective generalized joint configuration. -#[derive(Clone, Default)] +#[derive(Default, Clone)] pub struct KinematicsCache { q: Vec, - frames: Vec>, + kinematics: ForwardKinematics, } impl KinematicsCache { /// If a cache entry exists for the given joint configuration vector `q`, /// return that. Otherwise, compute the forward kinematics, replace the the /// cache with the results, and return them. - pub fn get_or_update(&mut self, joints: &[Joint], q: &[f64]) -> &[Isometry3] { + pub fn get_or_update(&mut self, chain: &KinematicChain, q: &[f64]) -> &ForwardKinematics { if self.q != q { // Clear and extend the cached vectors to prevent any allocations. self.q.clear(); self.q.extend_from_slice(q); - self.frames.clear(); - self.frames.extend(joint_kinematics(joints, q)); + self.kinematics = chain.forward_kinematics(q) } - &self.frames + &self.kinematics } } diff --git a/crates/optik/src/lib.rs b/crates/optik/src/lib.rs index 1357565..1073cc6 100644 --- a/crates/optik/src/lib.rs +++ b/crates/optik/src/lib.rs @@ -8,8 +8,7 @@ use std::{ time::Instant, }; -use k::{joint::Range, Chain, JointType, SerialChain}; -use nalgebra::{DVector, DVectorSlice, Isometry3, Matrix6xX}; +use nalgebra::{DVector, DVectorView, Isometry3, Matrix6xX}; use ordered_float::OrderedFloat; use rand::SeedableRng; use rand_chacha::ChaCha8Rng; @@ -17,7 +16,6 @@ use rayon::{ prelude::{IntoParallelIterator, ParallelIterator}, ThreadPoolBuilder, }; -use slsqp_sys::*; mod config; pub mod kinematics; @@ -27,6 +25,7 @@ pub mod objective; pub use config::*; use kinematics::*; use objective::*; +use slsqp_sys::{IterationResult, SlsqpSolver}; pub fn set_parallelism(n: usize) { ThreadPoolBuilder::new() @@ -37,37 +36,12 @@ pub fn set_parallelism(n: usize) { #[derive(Clone)] pub struct Robot { - chain: SerialChain, - joints: Vec>, - ee_offset: Isometry3, + chain: KinematicChain, } impl Robot { - pub fn new(chain: SerialChain) -> Self { - let mut joints = vec![]; - let mut fixed_offset = Isometry3::identity(); - for node in chain.iter() { - let joint = node.joint(); - match joint.joint_type { - JointType::Rotational { axis } => { - joints.push(Joint::revolute(joint.origin() * fixed_offset, axis)); - fixed_offset = Isometry3::identity(); - } - JointType::Linear { axis } => { - joints.push(Joint::prismatic(joint.origin() * fixed_offset, axis)); - fixed_offset = Isometry3::identity(); - } - JointType::Fixed {} => { - fixed_offset *= joint.origin(); - } - } - } - - Self { - chain, - joints, - ee_offset: fixed_offset, - } + pub fn new(chain: KinematicChain) -> Self { + Self { chain } } pub fn from_urdf_file(path: impl AsRef, base_link: &str, ee_link: &str) -> Self { @@ -81,32 +55,18 @@ impl Robot { } pub fn from_urdf(urdf: &urdf_rs::Robot, base_link: &str, ee_link: &str) -> Self { - let chain = Chain::::from(urdf); - - let base_link = chain - .find_link(base_link) - .unwrap_or_else(|| panic!("link '{}' does not exist!", base_link)); - let ee_link = chain - .find_link(ee_link) - .unwrap_or_else(|| panic!("link '{}' does not exist!", ee_link)); - - let serial = SerialChain::from_end_to_root(ee_link, base_link); - Robot::new(serial) + let chain = KinematicChain::from_urdf(urdf, base_link, ee_link); + Robot::new(chain) } +} +impl Robot { pub fn num_positions(&self) -> usize { - self.joints.len() + self.chain.nq() } pub fn joint_limits(&self) -> (DVector, DVector) { - let unlimited = Range::new(f64::NEG_INFINITY, f64::INFINITY); - - let (lb, ub) = self - .chain - .iter_joints() - .map(|j| j.limits.unwrap_or(unlimited)) - .map(|l| (l.min, l.max)) - .unzip(); + let (lb, ub) = self.chain.joints.iter().map(|j| j.limits).unzip(); (DVector::from_vec(lb), DVector::from_vec(ub)) } @@ -118,15 +78,12 @@ impl Robot { .collect() } - pub fn joint_jacobian(&self, frames: &[Isometry3]) -> Matrix6xX { - joint_jacobian(&self.joints, &self.ee_offset, frames) + pub fn joint_jacobian(&self, fk: &ForwardKinematics) -> Matrix6xX { + self.chain.joint_jacobian(fk) } pub fn fk(&self, q: &[f64]) -> Isometry3 { - joint_kinematics(&self.joints, q) - .last() - .unwrap_or_else(Isometry3::identity) - * self.ee_offset + *self.chain.forward_kinematics(q).ee_tfm() } pub fn ik( @@ -141,9 +98,6 @@ impl Robot { x0[i] = x0[i].clamp(lb[i], ub[i]); } - // Fix a global RNG seed, which is used to compute sub-seeds for each thread. - const RNG_SEED: u64 = 42; - // Compute the time at which the user-specified timeout will expire. We // will ensure that no solve threads continue iterating beyond this // time. If no max time is specified then run until the retry count is @@ -169,98 +123,97 @@ impl Robot { // Build a parallel stream of solutions from which we can choose how to // draw a final result. - let solution_stream = (0..max_restarts) - .into_par_iter() - .map(|i| { - let mut rng = ChaCha8Rng::seed_from_u64(RNG_SEED); - rng.set_stream(i); - - let cache = RefCell::new(KinematicsCache::default()); - let args = ObjectiveArgs { - robot: self, - config, - tfm_target, - }; - - // The first attempt gets the initial seed provided by the caller. - // All other attempts start at some random point. - let mut x = if i == 0 { - x0.clone() - } else { - self.random_configuration(&mut rng) - }; - - let mut solver = SlsqpSolver::new(x.len()); - solver.set_tol_f(config.tol_f); - solver.set_tol_df(config.tol_df); - solver.set_tol_dx(config.tol_dx); - solver.set_lb(lb.as_slice()); - solver.set_ub(ub.as_slice()); - - // Bookkeeping for stopping criteria. - let mut x_prev = x.clone(); - let mut f_prev = objective(&x, &args, &mut cache.borrow_mut()); - - // Iterate the solver until any of: - // - The solver converges within the active convergence criteria - // - Another thread arrives at a solution (in speed mode) - // - The specified timeout expires - while !should_exit.load(Ordering::Relaxed) && !is_timed_out() { - match solver.iterate( - &mut x, - |x| objective(x, &args, &mut cache.borrow_mut()), - |x, g| objective_grad(x, g, &args, &mut cache.borrow_mut()), - ) { - IterationResult::Continue => { - x_prev.copy_from_slice(&x); - f_prev = solver.cost(); - - continue; - } - IterationResult::Converged => { - // The SLSQP solver can report convergence - // regardless of whether our tol_f, tol_df, or - // tol_dx conditions are met. If we verify that this - // solution does meet the criteria then it can be - // returned. + let solution_stream = (0..max_restarts).into_par_iter().flat_map(|i| { + // Fix a global RNG seed, which is used to compute sub-seeds for each thread. + const RNG_SEED: u64 = 42; + + let mut rng = ChaCha8Rng::seed_from_u64(RNG_SEED); + rng.set_stream(i); + + let cache = RefCell::new(KinematicsCache::default()); + let args = ObjectiveArgs { + robot: self, + config, + tfm_target, + }; + + // The first attempt gets the initial seed provided by the caller. + // All other attempts start at some random point. + let mut x = if i == 0 { + x0.clone() + } else { + self.random_configuration(&mut rng) + }; + + let mut solver = SlsqpSolver::new(x.len()); + solver.set_tol_f(config.tol_f); + solver.set_tol_df(config.tol_df); + solver.set_tol_dx(config.tol_dx); + solver.set_lb(lb.as_slice()); + solver.set_ub(ub.as_slice()); + + // Bookkeeping for stopping criteria. + let mut x_prev = x.clone(); + let mut f_prev = objective(&x, &args, &mut cache.borrow_mut()); + + // Iterate the solver until any of: + // - The solver converges within the active convergence criteria + // - Another thread arrives at a solution (in speed mode) + // - The specified timeout expires + while !should_exit.load(Ordering::Relaxed) && !is_timed_out() { + match solver.iterate( + &mut x, + |x| objective(x, &args, &mut cache.borrow_mut()), + |x, g| objective_grad(x, g, &args, &mut cache.borrow_mut()), + ) { + IterationResult::Continue => { + x_prev.copy_from_slice(&x); + f_prev = solver.cost(); + + continue; + } + IterationResult::Converged => { + // The SLSQP solver can report convergence + // regardless of whether our tol_f, tol_df, or + // tol_dx conditions are met. If we verify that this + // solution does meet the criteria then it can be + // returned. + // + // This is due to an internal `accuracy` parameter, + // which reports convergence if the change in + // objective function falls below it. + let df = solver.cost() - f_prev; + let dx = DVectorView::from_slice(&x, x.len()) + - DVectorView::from_slice(&x_prev, x_prev.len()); + + // Report convergence if _any_ of the active + // convergence criteria are met. + if solver.cost().abs() < config.tol_f + || (config.tol_df > 0.0 && df.abs() < config.tol_df) + || (config.tol_dx > 0.0 && dx.norm().abs() < config.tol_dx) + { + // Short-circuit any other threads for a modest + // performance increase. // - // This is due to an internal `accuracy` parameter, - // which reports convergence if the change in - // objective function falls below it. - let df = solver.cost() - f_prev; - let dx = DVectorSlice::from_slice(&x, x.len()) - - DVectorSlice::from_slice(&x_prev, x_prev.len()); - - // Report convergence if _any_ of the active - // convergence criteria are met. - if solver.cost().abs() < config.tol_f - || (config.tol_df > 0.0 && df.abs() < config.tol_df) - || (config.tol_dx > 0.0 && dx.norm().abs() < config.tol_dx) - { - // Short-circuit any other threads for a modest - // performance increase. - // - // In `Speed` mode, if the current thread has - // converged on a satisfactory solution then we - // don't care what the others are going to - // produce. - if config.solution_mode == SolutionMode::Speed { - should_exit.store(true, Ordering::Relaxed); - } - - return Some((x, solver.cost())); + // In `Speed` mode, if the current thread has + // converged on a satisfactory solution then we + // don't care what the others are going to + // produce. + if config.solution_mode == SolutionMode::Speed { + should_exit.store(true, Ordering::Relaxed); } - return None; + return Some((x, solver.cost())); } - IterationResult::Error => return None, + + return None; } + IterationResult::Error => return None, } + } - None - }) - .take_any_while(|_| !is_timed_out()) - .flatten(); + None + }); match config.solution_mode { SolutionMode::Quality => { @@ -268,8 +221,8 @@ impl Robot { // best of all solutions. In this case, the cost of a given // solution is computed as its distance from the seed. solution_stream.min_by_key(|(x, _)| { - let x = DVectorSlice::from_slice(x, x.len()); - let x0 = DVectorSlice::from_slice(&x0, x0.len()); + let x = DVectorView::from_slice(x, x.len()); + let x0 = DVectorView::from_slice(&x0, x0.len()); OrderedFloat(x.metric_distance(&x0)) }) diff --git a/crates/optik/src/math.rs b/crates/optik/src/math.rs index ab48c92..4abf32a 100644 --- a/crates/optik/src/math.rs +++ b/crates/optik/src/math.rs @@ -196,9 +196,9 @@ pub mod se3 { let q = se3::right_jacobian_q_matrix(v, &w); let mut u = Matrix6::zeros(); - u.fixed_slice_mut::<3, 3>(0, 0).copy_from(&j); - u.fixed_slice_mut::<3, 3>(0, 3).copy_from(&q); - u.fixed_slice_mut::<3, 3>(3, 3).copy_from(&j); + u.fixed_view_mut::<3, 3>(0, 0).copy_from(&j); + u.fixed_view_mut::<3, 3>(0, 3).copy_from(&q); + u.fixed_view_mut::<3, 3>(3, 3).copy_from(&j); u } } diff --git a/crates/optik/src/objective.rs b/crates/optik/src/objective.rs index d57f6e7..262f643 100644 --- a/crates/optik/src/objective.rs +++ b/crates/optik/src/objective.rs @@ -13,8 +13,8 @@ pub fn objective(x: &[f64], args: &ObjectiveArgs, cache: &mut KinematicsCache) - // Compute the pose error w.r.t. the actual pose: // // X_AT = X_WA^1 * X_WT - let frames = cache.get_or_update(&args.robot.joints, x); - let tfm_actual = frames.last().unwrap() * args.robot.ee_offset; + let frames = cache.get_or_update(&args.robot.chain, x); + let tfm_actual = frames.ee_tfm(); let tfm_target = args.tfm_target; let tfm_error = tfm_target.inv_mul(&tfm_actual); @@ -29,10 +29,9 @@ pub fn objective_grad(q: &[f64], g: &mut [f64], args: &ObjectiveArgs, cache: &mu match args.config.gradient_mode { GradientMode::Analytical => { - let frames = cache.get_or_update(&args.robot.joints, q); - // Pose error is computed as in the objective function. - let tfm_actual = frames.last().unwrap() * robot.ee_offset; + let frames = cache.get_or_update(&args.robot.chain, q); + let tfm_actual = frames.ee_tfm(); let tfm_target = &args.tfm_target; let tfm_error = tfm_target.inv_mul(&tfm_actual); @@ -43,6 +42,7 @@ pub fn objective_grad(q: &[f64], g: &mut [f64], args: &ObjectiveArgs, cache: &mu // - Jtask: the Jacobian of our pose tracking task // // Jtask(q) = Jlog6(X) * J(q) + // let j_qdot = robot.joint_jacobian(frames); let j_qdot = robot.joint_jacobian(frames); let j_log6 = se3::right_jacobian(&tfm_error); let j_task = j_log6 * j_qdot; diff --git a/examples/example.py b/examples/example.py index 698d451..ab38f8c 100644 --- a/examples/example.py +++ b/examples/example.py @@ -17,7 +17,7 @@ from optik import Robot, SolverConfig robot = Robot.from_urdf_file(*sys.argv[1:4]) -config = SolverConfig() +config = SolverConfig(tol_f=1e-30) N = 10000 @@ -36,6 +36,14 @@ end = time.time() if q_opt is not None: + target_ee_pose = np.reshape(target_ee_pose, (4, 4)) + actual_ee_pose = np.reshape(robot.fk(q_opt), (4, 4)) + print("xyz:", np.linalg.norm(target_ee_pose[0:3, 3] - actual_ee_pose[0:3, 3])) + print("rot:", np.linalg.norm(target_ee_pose[0:3, 0:3] - actual_ee_pose[0:3, 0:3])) + # print("---") + # print(np.reshape(target_ee_pose, (4, 4))) + # print(np.reshape(robot.fk(q_opt), (4, 4))) + total_time += end - start print("Total time: {}us (to {:.1e})".format(int(1e6 * (end - start)), c)) diff --git a/examples/example.rs b/examples/example.rs index 82aa7ba..23cffbe 100644 --- a/examples/example.rs +++ b/examples/example.rs @@ -8,12 +8,12 @@ fn main() { let base_name = std::env::args().nth(2).unwrap(); let ee_name = std::env::args().nth(3).unwrap(); - let chain = k::Chain::::from_urdf_file(urdf_path).unwrap(); - let serial = k::SerialChain::from_end_to_root( - chain.find_link(&ee_name).unwrap(), - chain.find_link(&base_name).unwrap(), - ); - let robot = Robot::new(serial); + // let chain = k::Chain::::from_urdf_file(urdf_path).unwrap(); + // let serial = k::SerialChain::from_end_to_root( + // chain.find_link(&ee_name).unwrap(), + // chain.find_link(&base_name).unwrap(), + // ); + let robot = Robot::from_urdf_file(urdf_path, &base_name, &ee_name); let config = SolverConfig::default();