diff --git a/Cargo.lock b/Cargo.lock index c11c0ae..a2d5256 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -17,6 +17,15 @@ dependencies = [ "memchr", ] +[[package]] +name = "amd" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a679e001575697a3bd195813feb57a4718ecc08dc194944015cbc5f6213c2b96" +dependencies = [ + "num-traits", +] + [[package]] name = "anes" version = "0.1.6" @@ -135,6 +144,26 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b94f61472cee1439c0b966b47e3aca9ae07e45d070759512cd390ea2bebc6675" +[[package]] +name = "clarabel" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d950c870fad4feed732bc3f7da489d223ba3911eac0522e6512eed7e659cfafe" +dependencies = [ + "amd", + "cfg-if", + "derive_builder", + "enum_dispatch", + "itertools 0.11.0", + "lazy_static", + "num-traits", + "serde", + "serde-big-array", + "serde_json", + "thiserror", + "web-time", +] + [[package]] name = "cmake" version = "0.1.54" @@ -208,12 +237,90 @@ version = "0.2.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "460fbee9c2c2f33933d720630a6a0bac33ba7053db5344fac858d4b8952d77d5" +[[package]] +name = "darling" +version = "0.14.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b750cb3417fd1b327431a470f388520309479ab0bf5e323505daf0290cd3850" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.14.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "109c1ca6e6b7f82cc233a97004ea8ed7ca123a9af07a8230878fcfda9b158bf0" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 1.0.109", +] + +[[package]] +name = "darling_macro" +version = "0.14.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4aab4dbc9f7611d8b55048a3a16d2d010c2c8334e46304b40ac1cc14bf3b48e" +dependencies = [ + "darling_core", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "derive_builder" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d07adf7be193b71cc36b193d0f5fe60b918a3a9db4dad0449f57bcfd519704a3" +dependencies = [ + "derive_builder_macro", +] + +[[package]] +name = "derive_builder_core" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f91d4cfa921f1c05904dc3c57b4a32c38aed3340cce209f3a6fd1478babafc4" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "derive_builder_macro" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f0314b72bed045f3a68671b3c86328386762c93f82d98c65c3cb5e5f573dd68" +dependencies = [ + "derive_builder_core", + "syn 1.0.109", +] + [[package]] name = "either" version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" +[[package]] +name = "enum_dispatch" +version = "0.3.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa18ce2bc66555b3218614519ac839ddb759a7d6720732f979ef8d13be147ecd" +dependencies = [ + "once_cell", + "proc-macro2", + "quote", + "syn 2.0.104", +] + [[package]] name = "equivalent" version = "1.0.2" @@ -226,6 +333,12 @@ version = "0.5.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1d674e81391d1e1ab681a28d99df07927c6d4aa5b027d7da16ba32d1d21ecd99" +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + [[package]] name = "getrandom" version = "0.3.3" @@ -260,6 +373,12 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + [[package]] name = "indexmap" version = "2.10.0" @@ -285,6 +404,15 @@ dependencies = [ "either", ] +[[package]] +name = "itertools" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57" +dependencies = [ + "either", +] + [[package]] name = "itertools" version = "0.13.0" @@ -310,6 +438,12 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + [[package]] name = "libc" version = "0.2.174" @@ -378,7 +512,7 @@ source = "git+https://github.com/dimforge/nalgebra?branch=main#fa7afd5e47e009742 dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.104", ] [[package]] @@ -456,6 +590,7 @@ name = "optik" version = "0.5.0-beta.5" dependencies = [ "approx", + "clarabel", "criterion", "nalgebra", "nlopt", @@ -611,7 +746,7 @@ dependencies = [ "proc-macro2", "pyo3-macros-backend", "quote", - "syn", + "syn 2.0.104", ] [[package]] @@ -624,7 +759,7 @@ dependencies = [ "proc-macro2", "pyo3-build-config", "quote", - "syn", + "syn 2.0.104", ] [[package]] @@ -785,6 +920,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde-xml-rs" version = "0.6.0" @@ -805,7 +949,7 @@ checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.104", ] [[package]] @@ -839,6 +983,23 @@ dependencies = [ "wide", ] +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + [[package]] name = "syn" version = "2.0.104" @@ -873,7 +1034,7 @@ checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.104", ] [[package]] @@ -959,7 +1120,7 @@ dependencies = [ "log", "proc-macro2", "quote", - "syn", + "syn 2.0.104", "wasm-bindgen-shared", ] @@ -981,7 +1142,7 @@ checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.104", "wasm-bindgen-backend", "wasm-bindgen-shared", ] @@ -1005,6 +1166,16 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "web-time" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa30049b1c872b72c89866d458eae9f20380ab280ffd1b1e18df2d3e2d98cfe0" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + [[package]] name = "wide" version = "0.7.33" @@ -1129,5 +1300,5 @@ checksum = "9ecf5b4cc5364572d7f4c329661bcc82724222973f2cab6f050a4e5c22f75181" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.104", ] diff --git a/Cargo.toml b/Cargo.toml index 5823134..91dcaea 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,6 +17,7 @@ rust-version = "1.85" optik = { path = "crates/optik" } # Solver +clarabel = "0.11" nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "main", features = [ "rand", "serde-serialize" ] } nlopt = { git = "https://github.com/kylc/rust-nlopt.git", branch = "optik-patches" } ordered-float = "4.2" diff --git a/crates/optik-cpp/Cargo.toml b/crates/optik-cpp/Cargo.toml index 4d8751c..d2a1ecc 100644 --- a/crates/optik-cpp/Cargo.toml +++ b/crates/optik-cpp/Cargo.toml @@ -10,6 +10,9 @@ rust-version.workspace = true name = "optikcpp" crate-type = ["staticlib"] +[lints] +workspace = true + [dependencies] nalgebra.workspace = true optik.workspace = true diff --git a/crates/optik-cpp/include/optik.hpp b/crates/optik-cpp/include/optik.hpp index dd61929..0077fb8 100644 --- a/crates/optik-cpp/include/optik.hpp +++ b/crates/optik-cpp/include/optik.hpp @@ -84,6 +84,15 @@ class Robot final { bool DoIk(const SolverConfig& config, const Eigen::Isometry3d& target, const Eigen::VectorXd& x0, Eigen::VectorXd* q) const; + //! Attempts to solve for the velocity `v` which satisfies `v * J(q) = V_WE`. + //! + //! If `true` is returned, then out-parameter `v` will contain the solution + //! joint velocity. Otherwise, the solve has not converged and `v` will be + //! left untouched. + bool DoDiffIk(const Eigen::VectorXd& x0, + const Eigen::Matrix& V_WE, + const Eigen::VectorXd& v_max, Eigen::VectorXd* v) const; + //! Returns the size of the generalized position vector of this robot. unsigned int num_positions() const noexcept; diff --git a/crates/optik-cpp/src/lib.cpp b/crates/optik-cpp/src/lib.cpp index 66cb529..dc00b4a 100644 --- a/crates/optik-cpp/src/lib.cpp +++ b/crates/optik-cpp/src/lib.cpp @@ -25,6 +25,9 @@ extern double* optik_robot_fk(const optik::detail::robot* robot, extern double* optik_robot_ik(const optik::detail::robot* robot, const optik::SolverConfig* config, const double* target, const double* x0); +extern double* optik_robot_diff_ik(const optik::detail::robot* robot, + const double* x0, const double* V_WE, + const double* v_max); } namespace optik { @@ -116,6 +119,20 @@ bool Robot::DoIk(const SolverConfig& config, const Eigen::Isometry3d& target, } } +bool Robot::DoDiffIk(const Eigen::VectorXd& x0, + const Eigen::Matrix& V_WE, + const Eigen::VectorXd& v_max, Eigen::VectorXd* v) const { + double* v_data = + optik_robot_diff_ik(inner_, x0.data(), V_WE.data(), v_max.data()); + if (v_data != nullptr) { + *v = Eigen::Map(v_data, num_positions()); + free(v_data); + return true; + } else { + return false; + } +} + unsigned int Robot::num_positions() const noexcept { return optik_robot_num_positions(inner_); } diff --git a/crates/optik-cpp/src/lib.rs b/crates/optik-cpp/src/lib.rs index 2cd5fc2..9f999fe 100644 --- a/crates/optik-cpp/src/lib.rs +++ b/crates/optik-cpp/src/lib.rs @@ -3,7 +3,7 @@ use std::{ mem, }; -use nalgebra::{DMatrix, Isometry3, Translation3, UnitQuaternion}; +use nalgebra::{DMatrix, Isometry3, Translation3, UnitQuaternion, Vector6}; use optik::{Robot, SolutionMode, SolverConfig}; @@ -160,3 +160,24 @@ extern "C" fn optik_robot_ik( } } } + +#[unsafe(no_mangle)] +extern "C" fn optik_robot_diff_ik( + robot: *const Robot, + x0: *const c_double, + V_WE: *const c_double, + v_max: *const c_double, +) -> *const c_double { + unsafe { + let robot = robot.as_ref().unwrap(); + let x0 = std::slice::from_raw_parts(x0, robot.num_positions()).to_vec(); + let V_WE = Vector6::from_row_slice(std::slice::from_raw_parts(V_WE, 6)); + let v_max = std::slice::from_raw_parts(v_max, robot.num_positions()); + + if let Some((_alpha, v_n)) = robot.diff_ik(x0, &V_WE, v_max, &Isometry3::identity()) { + v_n.leak().as_ptr() + } else { + std::ptr::null() + } + } +} diff --git a/crates/optik-py/Cargo.toml b/crates/optik-py/Cargo.toml index 5f9761d..02908b6 100644 --- a/crates/optik-py/Cargo.toml +++ b/crates/optik-py/Cargo.toml @@ -10,6 +10,9 @@ rust-version.workspace = true name = "optik" crate-type = ["cdylib"] +[lints] +workspace = true + [build-dependencies] pyo3-build-config = "0.25" diff --git a/crates/optik-py/src/lib.rs b/crates/optik-py/src/lib.rs index d65040d..52dc642 100644 --- a/crates/optik-py/src/lib.rs +++ b/crates/optik-py/src/lib.rs @@ -2,7 +2,7 @@ use optik::{Robot, SolverConfig}; -use nalgebra::{Isometry3, Matrix4}; +use nalgebra::{Isometry3, Matrix4, Vector6}; use pyo3::prelude::*; fn parse_pose(v: Option>>) -> Isometry3 { @@ -130,6 +130,28 @@ impl PyRobot { let ee_offset = parse_pose(ee_offset); robot.ik(&config.0, &target, x0, &ee_offset) } + + #[pyo3(signature=(x0, V_WE, v_max, ee_offset=None))] + fn diff_ik( + &self, + x0: Vec, + V_WE: Vec, + v_max: Vec, + ee_offset: Option>>, + ) -> Option<(f64, Vec)> { + let robot = &self.0; + + assert_eq!(x0.len(), self.num_positions(), "len(x0) != num_positions"); + assert_eq!( + v_max.len(), + self.num_positions(), + "len(v_max) != num_positions" + ); + + let V_WE = Vector6::from_row_slice(&V_WE); + let ee_offset = parse_pose(ee_offset); + robot.diff_ik(x0, &V_WE, &v_max, &ee_offset) + } } #[pymodule()] diff --git a/crates/optik/Cargo.toml b/crates/optik/Cargo.toml index b47a853..0e9d683 100644 --- a/crates/optik/Cargo.toml +++ b/crates/optik/Cargo.toml @@ -10,7 +10,11 @@ rust-version.workspace = true name = "optik" crate-type = ["rlib"] +[lints] +workspace = true + [dependencies] +clarabel.workspace = true nalgebra.workspace = true nlopt.workspace = true ordered-float.workspace = true diff --git a/crates/optik/benches/bench.rs b/crates/optik/benches/bench.rs index 379038f..fc8ee0e 100644 --- a/crates/optik/benches/bench.rs +++ b/crates/optik/benches/bench.rs @@ -71,6 +71,22 @@ fn bench_joint_jacobian(c: &mut Criterion) { c.bench_function("joint_jacobian", |b| b.iter(|| robot.joint_jacobian(&fk))); } +fn bench_diff_ik(c: &mut Criterion) { + let robot = load_benchmark_model(); + + let x0 = vec![0.1, 0.2, 0.0, 0.3, -0.2, -1.1]; + let v_max = vec![1.0; 6]; + let v = vector![0.1, 0.2, 0.3, -0.1, -0.2, -0.3]; + + c.bench_function("diff_ik", |b| { + b.iter(|| { + let res = robot.diff_ik(x0.clone(), black_box(&v), &v_max, &Isometry3::identity()); + assert_ne!(res, None); + res + }) + }); +} + fn bench_ik(c: &mut Criterion) { let robot = load_benchmark_model(); let config = SolverConfig::default(); @@ -97,6 +113,7 @@ criterion_group!( bench_gradient, bench_objective, bench_fk, + bench_diff_ik, bench_joint_jacobian, bench_ik ); diff --git a/crates/optik/src/lib.rs b/crates/optik/src/lib.rs index 2b09dee..857d6b2 100644 --- a/crates/optik/src/lib.rs +++ b/crates/optik/src/lib.rs @@ -7,7 +7,14 @@ use std::{ time::Instant, }; -use nalgebra::{DVectorView, Isometry3, Matrix6xX, Vector3}; +use clarabel::{ + algebra::{BlockConcatenate, CscMatrix}, + solver::{ + DefaultSettingsBuilder, DefaultSolver, IPSolver, SolverStatus, + SupportedConeT::{self, NonnegativeConeT, ZeroConeT}, + }, +}; +use nalgebra::{DVectorView, Isometry3, Matrix6xX, Vector3, Vector6, stack}; use nlopt::{Algorithm, Nlopt, SuccessState, Target}; use ordered_float::OrderedFloat; use rand::SeedableRng; @@ -91,6 +98,146 @@ impl Robot { self.chain.forward_kinematics(q, ee_offset) } + /// Given a desired end-effector frame velocity Vᴱ, attempt to solve for the + /// corresponding joint velocity v = [Jᴱ(q)]⁻¹Vᴱ. However, because the robot + /// may be joint velocity limited, an exact solution may not be possible. + /// + /// We therefore formulate the following optimization problem (as in [1]): + /// + /// max_{vₙ, α} α + /// s.t. + /// # A scaling factor for how much to move in the desired direction. + /// # We attempt to maximize this in the interval (0, 1), i.e. move as + /// # far as we can. + /// 0 ≤ α ≤ 1 + /// # Apply symmetric jointspace velocity limits. + /// -vmax ≤ vₙ ≤ vmax + /// # Movement is only allowed in the direction of the desired + /// # end-effector frame velocity. + /// Jᴱ(q)vₙ = αVᴱ + /// + /// If a solution is found, returns a tuple of the alpha scaling factor and + /// the resulting joint velocities. Otherwise, returns None. + /// + /// [1]: https://manipulation.csail.mit.edu/pick.html#section6 + pub fn diff_ik( + &self, + x0: Vec, + V_WE: &Vector6, + v_max: &[f64], + ee_offset: &Isometry3, + ) -> Option<(f64, Vec)> { + let n = self.num_positions(); + + // Appends the following constraint: + // 0 ≤ α ≤ 1. + let append_alpha_constraints = + |A: &mut CscMatrix, b: &mut Vec, K: &mut Vec>| { + let mut A_ext = CscMatrix::zeros((2, A.n)); + + // α ≤ 1.0 + // α + s = 1.0, s ≥ 0 + A_ext.set_entry((0, n), 1.0); + b.push(1.0); + K.push(NonnegativeConeT(1)); + + // α >= 0 + // -α + s = 0.0, s ≥ 0 + A_ext.set_entry((1, n), -1.0); + b.push(0.0); + K.push(NonnegativeConeT(1)); + + *A = CscMatrix::vcat(A, &A_ext).unwrap(); + }; + + // Appends the following constraint: + // -vₘₐₓ ≤ vₙ ≤ vₘₐₓ + let append_velocity_constraints = + |A: &mut CscMatrix, b: &mut Vec, K: &mut Vec>| { + let mut A_ext = CscMatrix::zeros((2 * n, A.n)); + + for i in 0..n { + // vᵢ ≤ vₘₐₓ + // vᵢ + s = vₘₐₓ, s ≥ 0 + A_ext.set_entry((i * 2, i), 1.0); + b.push(v_max[i]); + K.push(NonnegativeConeT(1)); + + // vᵢ ≥ -vₘₐₓ + // -vᵢ + s = vₘₐₓ, s ≥ 0 + A_ext.set_entry((i * 2 + 1, i), -1.0); + b.push(v_max[i]); + K.push(NonnegativeConeT(1)); + } + + *A = CscMatrix::vcat(A, &A_ext).unwrap(); + }; + + // Appends the following constraint: + // Jᴱ(q)vₙ = αVᴱ + let append_cartesian_constraints = + |A: &mut CscMatrix, b: &mut Vec, K: &mut Vec>| { + let fk = self.fk(&x0, ee_offset); + + // Rotate the local frame end-effector Jacobian into the world frame to + // match the input velocity frame. + let R_WE = fk.ee_tfm().rotation.to_rotation_matrix(); + let JEq = self.joint_jacobian(&fk); + let JEq_W = stack![ + R_WE * JEq.fixed_rows::<3>(0); + R_WE * JEq.fixed_rows::<3>(3) + ]; + + // Jᴱ(q)vₙ = αVᴱ + let A_ext = CscMatrix::from(stack![JEq_W, -V_WE].row_iter()); + b.extend(vec![0.0; n]); + K.push(ZeroConeT(n)); + + *A = CscMatrix::vcat(A, &A_ext).unwrap(); + }; + + // Solve + // min 0.5 xᵀPx + qᵀx + // s.t. + // Ax + s = b + // s ∈ K + let P = CscMatrix::zeros((n + 1, n + 1)); + let mut q = vec![0.0; n + 1]; + q[n] = -100.0; // Reward large alpha values. + + let mut A = CscMatrix::zeros((0, n + 1)); + let mut b = vec![]; + let mut K = vec![]; + + append_alpha_constraints(&mut A, &mut b, &mut K); + append_velocity_constraints(&mut A, &mut b, &mut K); + append_cartesian_constraints(&mut A, &mut b, &mut K); + + let mut solver = DefaultSolver::new( + &P, + &q, + &A, + &b, + &K, + DefaultSettingsBuilder::default() + .verbose(false) + .build() + .unwrap(), + ) + .expect("solver initialization failed"); + solver.solve(); + + let solution = solver.solution; + match solution.status { + SolverStatus::Solved => { + let x = solution.x[0..n].to_vec(); + let alpha = solution.x[n]; + Some((alpha, x)) + } + _ => None, + } + } + pub fn ik( &self, config: &SolverConfig, diff --git a/crates/optik/tests/test_ik.rs b/crates/optik/tests/test_ik.rs index 78d4285..de31a8d 100644 --- a/crates/optik/tests/test_ik.rs +++ b/crates/optik/tests/test_ik.rs @@ -180,3 +180,30 @@ fn test_solution_quality() { assert!(sol_quality.metric_distance(&x0) <= sol_speed.metric_distance(&x0)); } } + +#[test] +fn test_diff_ik() { + let robot = Robot::from_urdf_str(TEST_MODEL_STR, "ur_base_link", "ur_ee_link"); + + let mut rng = StdRng::seed_from_u64(42); + + for _ in 0..20 { + let x0 = robot.random_configuration(&mut rng); + let v_max = vec![1.0; 6]; + + let V_WE: Vector6 = rng.random(); + + let (alpha, v_n) = robot + .diff_ik(x0, &V_WE, &v_max, &Isometry3::identity()) + .unwrap(); + + let eps = 1e-6; + assert!((-eps..=1.0 + eps).contains(&alpha)); + for i in 0..robot.num_positions() { + assert!(-v_max[i] - eps <= v_n[i]); + assert!(v_n[i] <= v_max[i] + eps); + } + + // TODO: Assert the Cartesian velocity is tracked. + } +} diff --git a/examples/example_diff_ik.py b/examples/example_diff_ik.py new file mode 100644 index 0000000..2ee0e6f --- /dev/null +++ b/examples/example_diff_ik.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 + +""" +Demonstration of the differential IK feature of OptIK. Loads a model, +configures the solver, and steps of diff-IK with varying velocity limits. + +Usage: + Run this script by providing your own URDF and kinematic chain: + + $ python example_diff_ik.py +""" + +import sys + +import numpy as np +from optik import Robot + +np.set_printoptions(suppress=True, precision=2) + +urdf_path, base_name, ee_name = sys.argv[1:4] + +robot = Robot.from_urdf_file(urdf_path, base_name, ee_name) +n = robot.num_positions() + +rng = np.random.default_rng(seed=42) +x0 = rng.uniform(*robot.joint_limits()) + +for v_max in [0.1, 0.5, 1.0, 10.0]: + V_tgt = np.array([0.0, 0.0, 0.5, 0.0, 0.0, 1.0]) + + if (sol := robot.diff_ik(x0, V_tgt, [v_max] * n)) is not None: + alpha, v_star = sol + + J = np.array(robot.joint_jacobian(x0)) + R_W = np.array(robot.fk(x0))[:3, :3] + J_W = np.vstack( + ( + R_W @ J[:3, :], + R_W @ J[3:, :], + ) + ) + V_star = np.matmul(J_W, v_star) + + print("------") + print(" x0 =", np.array(x0)) + print(" v_max =", np.array(v_max)) + print(" V_tgt =", np.array(V_tgt)) + print(" alpha =", alpha) + print(" v* =", np.array(v_star)) + print(" V* =", np.array(V_star)) + + assert 0.0 <= alpha <= 1.0 + np.testing.assert_allclose(V_tgt, V_star / alpha, atol=1e-6) diff --git a/flake.lock b/flake.lock index b4553ef..2242c38 100644 --- a/flake.lock +++ b/flake.lock @@ -1,15 +1,37 @@ { "nodes": { + "fenix": { + "inputs": { + "nixpkgs": [ + "naersk", + "nixpkgs" + ], + "rust-analyzer-src": "rust-analyzer-src" + }, + "locked": { + "lastModified": 1752475459, + "narHash": "sha256-z6QEu4ZFuHiqdOPbYss4/Q8B0BFhacR8ts6jO/F/aOU=", + "owner": "nix-community", + "repo": "fenix", + "rev": "bf0d6f70f4c9a9cf8845f992105652173f4b617f", + "type": "github" + }, + "original": { + "owner": "nix-community", + "repo": "fenix", + "type": "github" + } + }, "flake-parts": { "inputs": { "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1736143030, - "narHash": "sha256-+hu54pAoLDEZT9pjHlqL9DNzWz0NbUn8NEAHP7PQPzU=", + "lastModified": 1756770412, + "narHash": "sha256-+uWLQZccFHwqpGqr2Yt5VsW/PbeJVTn9Dk6SHWhNRPw=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "b905f6fc23a9051a6e1b741e1438dbfc0634c6de", + "rev": "4524271976b625a4a605beefd893f270620fd751", "type": "github" }, "original": { @@ -20,14 +42,15 @@ }, "naersk": { "inputs": { + "fenix": "fenix", "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1733346208, - "narHash": "sha256-a4WZp1xQkrnA4BbnKrzJNr+dYoQr5Xneh2syJoddFyE=", + "lastModified": 1752689277, + "narHash": "sha256-uldUBFkZe/E7qbvxa3mH1ItrWZyT6w1dBKJQF/3ZSsc=", "owner": "nix-community", "repo": "naersk", - "rev": "378614f37a6bee5a3f2ef4f825a73d948d3ae921", + "rev": "0e72363d0938b0208d6c646d10649164c43f4d64", "type": "github" }, "original": { @@ -39,35 +62,42 @@ }, "nixpkgs": { "locked": { - "lastModified": 0, - "narHash": "sha256-DjkQPnkAfd7eB522PwnkGhOMuT9QVCZspDpJJYyOj60=", - "path": "/nix/store/g3jyakqb3ipnr6gz5rw10fb17ckr2z00-source", - "type": "path" + "lastModified": 1752077645, + "narHash": "sha256-HM791ZQtXV93xtCY+ZxG1REzhQenSQO020cu6rHtAPk=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "be9e214982e20b8310878ac2baa063a961c1bdf6", + "type": "github" }, "original": { - "id": "nixpkgs", - "type": "indirect" + "owner": "NixOS", + "ref": "nixpkgs-unstable", + "repo": "nixpkgs", + "type": "github" } }, "nixpkgs-lib": { "locked": { - "lastModified": 1735774519, - "narHash": "sha256-CewEm1o2eVAnoqb6Ml+Qi9Gg/EfNAxbRx1lANGVyoLI=", - "type": "tarball", - "url": "https://github.com/NixOS/nixpkgs/archive/e9b51731911566bbf7e4895475a87fe06961de0b.tar.gz" + "lastModified": 1754788789, + "narHash": "sha256-x2rJ+Ovzq0sCMpgfgGaaqgBSwY+LST+WbZ6TytnT9Rk=", + "owner": "nix-community", + "repo": "nixpkgs.lib", + "rev": "a73b9c743612e4244d865a2fdee11865283c04e6", + "type": "github" }, "original": { - "type": "tarball", - "url": "https://github.com/NixOS/nixpkgs/archive/e9b51731911566bbf7e4895475a87fe06961de0b.tar.gz" + "owner": "nix-community", + "repo": "nixpkgs.lib", + "type": "github" } }, "nixpkgs_2": { "locked": { - "lastModified": 1736010017, - "narHash": "sha256-pMttboU4LraZezi3tc7NiHODd4eKziFRb2a4s4WO8Ms=", + "lastModified": 1756911493, + "narHash": "sha256-6n/n1GZQ/vi+LhFXMSyoseKdNfc2QQaSBXJdgamrbkE=", "owner": "nixos", "repo": "nixpkgs", - "rev": "a1945f760a8fe019a4d753808de424dcd4e5b3cf", + "rev": "c6a788f552b7b7af703b1a29802a7233c0067908", "type": "github" }, "original": { @@ -79,11 +109,11 @@ }, "nixpkgs_3": { "locked": { - "lastModified": 1728538411, - "narHash": "sha256-f0SBJz1eZ2yOuKUr5CA9BHULGXVSn6miBuUWdTyhUhU=", + "lastModified": 1744536153, + "narHash": "sha256-awS2zRgF4uTwrOKwwiJcByDzDOdo3Q1rPZbiHQg/N38=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "b69de56fac8c2b6f8fd27f2eca01dcda8e0a4221", + "rev": "18dd725c29603f582cf1900e0d25f9f1063dbf11", "type": "github" }, "original": { @@ -102,16 +132,33 @@ "systems": "systems" } }, + "rust-analyzer-src": { + "flake": false, + "locked": { + "lastModified": 1752428706, + "narHash": "sha256-EJcdxw3aXfP8Ex1Nm3s0awyH9egQvB2Gu+QEnJn2Sfg=", + "owner": "rust-lang", + "repo": "rust-analyzer", + "rev": "591e3b7624be97e4443ea7b5542c191311aa141d", + "type": "github" + }, + "original": { + "owner": "rust-lang", + "ref": "nightly", + "repo": "rust-analyzer", + "type": "github" + } + }, "rust-overlay": { "inputs": { "nixpkgs": "nixpkgs_3" }, "locked": { - "lastModified": 1736130662, - "narHash": "sha256-z+WGez9oTR2OsiUWE5ZhIpETqM1ogrv6Xcd24WFi6KQ=", + "lastModified": 1757125853, + "narHash": "sha256-noKkYHKpT5lpvNSYrlH56d8cedthZfs010Uv6vTqLT4=", "owner": "oxalica", "repo": "rust-overlay", - "rev": "2f5d4d9cd31cc02c36e51cb2e21c4b25c4f78c52", + "rev": "8b70793a6be183536a5d562056dac10b7b36820d", "type": "github" }, "original": { diff --git a/optik.pyi b/optik.pyi index 258732b..218206c 100644 --- a/optik.pyi +++ b/optik.pyi @@ -40,3 +40,10 @@ class Robot: x0: VectorXd, ee_offset: MatrixXd | None = ..., ) -> tuple[list[float], float] | None: ... + def diff_ik( + self, + x0: VectorXd, + V_WE: VectorXd, + v_max: VectorXd, + ee_offset: MatrixXd | None = ..., + ) -> tuple[float, list[float]] | None: ...