diff --git a/.github/.cspell/project-dictionary.txt b/.github/.cspell/project-dictionary.txt index 79d4519..58be33c 100644 --- a/.github/.cspell/project-dictionary.txt +++ b/.github/.cspell/project-dictionary.txt @@ -1,13 +1,25 @@ +cholesky deque +dls doctest funcs kajita +levenberg libglu +liegeois +manipulability +marquardt nullspace +pseudoinverse rctree rgba +rng +rngs rustdocflags rustflags ryzen +seedable +wellconditioned xacro xorg +yoshikawa diff --git a/Cargo.toml b/Cargo.toml index d2f5981..bbedc6e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,7 +4,7 @@ name = "k" # - Create "v0.x.y" git tag # - Push the above tag (run `git push origin --tags`) # Then, CI will publish to crates.io and create a GitHub release. -version = "0.32.0" +version = "0.33.0" authors = ["Takashi Ogura "] edition = "2021" description = "k is for kinematics" @@ -27,6 +27,11 @@ urdf-rs = "0.9" serde = { version = "1.0", features = ["derive"], optional = true } +# `urdf-rs` is also a normal dependency above; it is repeated here so integration tests and the +# wasm test build can parse the `urdf/test_arm7.urdf` fixture via `urdf_rs::read_from_string`. +[dev-dependencies] +urdf-rs = "0.9" + [target.'cfg(not(target_family = "wasm"))'.dev-dependencies] clap = { version = "4", features = ["derive"] } criterion = "0.8" diff --git a/benches/ik.rs b/benches/ik.rs index f9ec2b4..d6b4a80 100644 --- a/benches/ik.rs +++ b/benches/ik.rs @@ -1,28 +1,60 @@ -use criterion::{criterion_group, criterion_main, Bencher, Criterion}; +use criterion::{criterion_group, criterion_main, Criterion}; use k::prelude::*; -fn bench_tree_ik(robot: &k::Chain, target_link: &str, b: &mut Bencher<'_>) { - let target_node = robot.find(target_link).unwrap(); - let arm = k::SerialChain::from_end(target_node); - // set joint angles - let angles = vec![0.5, 0.2, 0.0, -0.5, 0.0, -0.3]; - arm.set_joint_positions(&angles).unwrap(); - arm.update_transforms(); - let mut target = target_node.world_transform().unwrap(); +// Nominal, well-conditioned pose for the 6-DOF `l_wrist_pitch` arm of `sample.urdf`. +// The bent elbow keeps the Jacobian away from singularities, so the solve converges and +// the timing is a clean speed-regression guard (and stays apples-to-apples across solvers). +const NOMINAL_ANGLES: [f64; 6] = [0.5, 0.2, 0.0, -0.5, 0.0, -0.3]; + +fn sample_arm() -> k::SerialChain { + let robot = k::Chain::::from_urdf_file("urdf/sample.urdf").unwrap(); + let target_node = robot.find("l_wrist_pitch").unwrap(); + k::SerialChain::from_end(target_node) +} + +// Target is the forward-kinematics pose at `NOMINAL_ANGLES` nudged by a small reachable +// translation, so the chosen inputs converge on the pre-DLS solver too. +fn nominal_target(arm: &k::SerialChain) -> k::Isometry3 { + arm.set_joint_positions(&NOMINAL_ANGLES).unwrap(); + let mut target = arm.end_transform(); target.translation.vector[0] += 0.02; + target +} + +// Full 6-DOF constraint: square Jacobian (`m = n = 6`), the original speed-regression guard. +fn bench_wellconditioned(c: &mut Criterion) { + let arm = sample_arm(); + let target = nominal_target(&arm); + arm.set_joint_positions(&NOMINAL_ANGLES).unwrap(); let solver = k::JacobianIkSolver::new(0.001, 0.01, 0.8, 10); - b.iter(|| { - solver.solve(&arm, &target).unwrap(); - arm.set_joint_positions(&angles).unwrap(); + c.bench_function("bench_wellconditioned", |b| { + b.iter(|| { + solver.solve(&arm, &target).unwrap(); + arm.set_joint_positions(&NOMINAL_ANGLES).unwrap(); + }); }); } -fn bench_rctree_ik(c: &mut Criterion) { - let robot = k::Chain::::from_urdf_file("urdf/sample.urdf").unwrap(); - c.bench_function("bench_rctree_ik", |b| { - bench_tree_ik(&robot, "l_wrist_pitch", b); +// Relaxing `rotation_x` drops the operational space to `m = 5` while `n = 6`, so the +// Jacobian is wide and the solve takes the redundant null-space path. +fn bench_redundant_constrained(c: &mut Criterion) { + let arm = sample_arm(); + let target = nominal_target(&arm); + arm.set_joint_positions(&NOMINAL_ANGLES).unwrap(); + let constraints = k::Constraints { + rotation_x: false, + ..Default::default() + }; + let solver = k::JacobianIkSolver::new(0.001, 0.01, 0.8, 10); + c.bench_function("bench_redundant_constrained", |b| { + b.iter(|| { + solver + .solve_with_constraints(&arm, &target, &constraints) + .unwrap(); + arm.set_joint_positions(&NOMINAL_ANGLES).unwrap(); + }); }); } -criterion_group!(benches, bench_rctree_ik); +criterion_group!(benches, bench_wellconditioned, bench_redundant_constrained); criterion_main!(benches); diff --git a/examples/ik_benchmark.rs b/examples/ik_benchmark.rs new file mode 100644 index 0000000..74c970b --- /dev/null +++ b/examples/ik_benchmark.rs @@ -0,0 +1,233 @@ +//! Robustness / accuracy benchmark harness for `JacobianIkSolver`. +//! +//! Sweeps a fixed, seeded set of IK problems in three regimes — well-conditioned, +//! near-singular, and redundant-with-limits — and prints a markdown table of success +//! rate, panic / non-finite counts, and final residual. It calls only the preserved +//! public API and `urdf/sample.urdf`, so the same source can be run against the solver +//! before and after the Damped-Least-Squares upgrade to compare robustness. +//! +//! Run with `cargo run --release --example ik_benchmark`. + +#[cfg(not(target_family = "wasm"))] +fn main() { + imp::run(); +} + +// `rand` is only a dev-dependency off-wasm and CI does not build examples for wasm, so the +// harness compiles to an empty entry point there. +#[cfg(target_family = "wasm")] +fn main() {} + +#[cfg(not(target_family = "wasm"))] +mod imp { + use k::prelude::*; + use k::{Chain, Constraints, Isometry3, JacobianIkSolver, SerialChain}; + use rand::rngs::StdRng; + use rand::{RngExt, SeedableRng}; + use std::panic::{catch_unwind, AssertUnwindSafe}; + + // Fixed seed => identical target sweep on every run, so `before.md`/`after.md` diff cleanly. + const SEED: u64 = 20_240_614; + // Number of sampled problems per regime. + const N_PER_SET: usize = 200; + // Well-conditioned reference pose for the 6-DOF arm (bent elbow, away from singularities). + const NOMINAL: [f64; 6] = [0.3, 0.2, 0.0, -0.8, 0.2, 0.1]; + + struct Case { + start: Vec, + target: Isometry3, + constraints: Constraints, + } + + #[derive(Default)] + struct SetResult { + successes: usize, + panics: usize, + non_finite: usize, + sum_pos: f64, + max_pos: f64, + sum_rot: f64, + max_rot: f64, + } + + impl SetResult { + fn record_success(&mut self, pos: f64, rot: f64) { + self.successes += 1; + self.sum_pos += pos; + self.sum_rot += rot; + self.max_pos = self.max_pos.max(pos); + self.max_rot = self.max_rot.max(rot); + } + } + + fn arm() -> SerialChain { + let robot = Chain::::from_urdf_file("urdf/sample.urdf").unwrap(); + let end = robot.find("l_wrist_pitch").unwrap(); + SerialChain::from_end(end) + } + + // Forward kinematics: end pose of `a` at `angles`. + fn fk(a: &SerialChain, angles: &[f64]) -> Isometry3 { + a.set_joint_positions(angles).unwrap(); + a.end_transform() + } + + // Well-conditioned: random reachable targets a small perturbation away from `NOMINAL`. + fn wellconditioned_cases(a: &SerialChain, rng: &mut StdRng) -> Vec { + (0..N_PER_SET) + .map(|_| { + let mut q = NOMINAL; + for v in &mut q { + *v += rng.random_range(-0.2..0.2); + } + Case { + start: NOMINAL.to_vec(), + target: fk(a, &q), + constraints: Constraints::default(), + } + }) + .collect() + } + + // Near-singular: start almost straight (elbow/wrist aligned) and push the target along the + // shoulder->end reach line to/just past the workspace boundary, where the undamped Jacobian + // loses rank. `extra < 0` is reachable, `extra > 0` is just beyond reach. + fn near_singular_cases(a: &SerialChain, rng: &mut StdRng) -> Vec { + (0..N_PER_SET) + .map(|_| { + let mut start = [0.0f64; 6]; + for v in &mut start { + *v += rng.random_range(-0.03..0.03); + } + a.set_joint_positions(&start).unwrap(); + a.update_transforms(); + let base = a.iter().next().unwrap().world_transform().unwrap(); + let end = a.end_transform(); + let reach = (end.translation.vector - base.translation.vector).normalize(); + let extra = rng.random_range(-0.03..0.18); + let mut target = end; + target.translation.vector += reach * extra; + Case { + start: start.to_vec(), + target, + constraints: Constraints::default(), + } + }) + .collect() + } + + // Redundant + limits: relax `rotation_x` (m=5 < n=6) so the null-space path runs, with larger + // perturbations so the reachable targets drive joints toward the URDF limits. + fn redundant_cases(a: &SerialChain, rng: &mut StdRng) -> Vec { + let constraints = Constraints { + rotation_x: false, + ..Default::default() + }; + (0..N_PER_SET) + .map(|_| { + let mut q = NOMINAL; + for v in &mut q { + *v += rng.random_range(-0.6..0.6); + } + Case { + start: NOMINAL.to_vec(), + target: fk(a, &q), + constraints: constraints.clone(), + } + }) + .collect() + } + + // Each solve runs on an independent clone of the arm so a panicking solve cannot poison later + // cases: chain nodes are `Arc>`, and a panic mid-solve would poison shared mutexes. + fn run_set( + base: &SerialChain, + solver: &JacobianIkSolver, + cases: &[Case], + ) -> SetResult { + let mut res = SetResult::default(); + for case in cases { + let solved = base.clone(); + let start = case.start.clone(); + let target = case.target; + let constraints = case.constraints.clone(); + let outcome = catch_unwind(AssertUnwindSafe(move || { + solved.set_joint_positions(&start).unwrap(); + let ok = solver + .solve_with_constraints(&solved, &target, &constraints) + .is_ok(); + (ok, solved.joint_positions(), solved.end_transform()) + })); + match outcome { + // Solver panicked (e.g. an undamped `.unwrap()` on a singular factorization). + Err(_) => res.panics += 1, + Ok((ok, positions, end)) => { + if !positions.iter().all(|p| p.is_finite()) { + res.non_finite += 1; + } else if ok { + let pos = (case.target.translation.vector - end.translation.vector).norm(); + let rot = case.target.rotation.angle_to(&end.rotation); + if pos.is_finite() && rot.is_finite() { + res.record_success(pos, rot); + } else { + res.non_finite += 1; + } + } + } + } + } + res + } + + fn print_row(name: &str, r: &SetResult) { + let succ = r.successes; + let panics = r.panics; + let nf = r.non_finite; + let rate = 100.0 * succ as f64 / N_PER_SET as f64; + let (mean_pos, max_pos, mean_rot, max_rot) = if succ > 0 { + ( + format!("{:.2e}", r.sum_pos / succ as f64), + format!("{:.2e}", r.max_pos), + format!("{:.2e}", r.sum_rot / succ as f64), + format!("{:.2e}", r.max_rot), + ) + } else { + ( + "—".to_owned(), + "—".to_owned(), + "—".to_owned(), + "—".to_owned(), + ) + }; + println!( + "| {name} | {N_PER_SET} | {succ} | {rate:.1}% | {panics} | {nf} | {mean_pos} | {max_pos} | {mean_rot} | {max_rot} |" + ); + } + + pub(crate) fn run() { + let base = arm(); + let solver = JacobianIkSolver::default(); + let mut rng = StdRng::seed_from_u64(SEED); + + // Build every set before solving so the RNG draw order (hence the sweep) is independent + // of solver behavior and bit-identical across runs. + let well = wellconditioned_cases(&base, &mut rng); + let near = near_singular_cases(&base, &mut rng); + let redundant = redundant_cases(&base, &mut rng); + + let well_res = run_set(&base, &solver, &well); + let near_res = run_set(&base, &solver, &near); + let redundant_res = run_set(&base, &solver, &redundant); + + println!("# IK solver robustness / accuracy benchmark\n"); + println!("- Arm: `urdf/sample.urdf` `l_wrist_pitch` (6 DOF)"); + println!("- Samples per set: {N_PER_SET}"); + println!("- Seed: `{SEED}`"); + println!("- Residuals (meters / radians) are over successful solves only (`end_transform` vs target).\n"); + println!("| Target set | Samples | Success | Rate | Panics | Non-finite | Pos mean | Pos max | Rot mean | Rot max |"); + println!("|---|---|---|---|---|---|---|---|---|---|"); + print_row("Well-conditioned", &well_res); + print_row("Near-singular", &near_res); + print_row("Redundant + limits", &redundant_res); + } +} diff --git a/src/ik.rs b/src/ik.rs index f4620dd..2e07790 100644 --- a/src/ik.rs +++ b/src/ik.rs @@ -145,7 +145,30 @@ where ) -> Result<(), Error>; } -/// Inverse Kinematics Solver using Jacobian matrix +/// Default manipulability threshold `w0`: dynamic damping turns on once the Yoshikawa +/// manipulability measure `w = sqrt(det(J J^T))` drops below it. Kept small so that +/// well-conditioned configurations get zero damping and the step is the exact minimum-norm +/// pseudoinverse. Scale-dependent — see `set_manipulability_threshold`. +const DEFAULT_MANIPULABILITY_THRESHOLD: f64 = 1e-3; +/// Default maximum squared damping factor `lambda^2_max`, approached as `w -> 0`. +const DEFAULT_MAX_DAMPING_SQUARED: f64 = 1e-3; +/// Default joint-limit-avoidance gain `k0`. Zero disables the opt-in null-space avoidance, so the +/// solver is numerically identical to plain damped least squares unless a caller raises it. +const DEFAULT_JOINT_LIMIT_AVOIDANCE_GAIN: f64 = 0.0; + +/// Inverse Kinematics Solver using the Jacobian matrix with Damped Least Squares (DLS). +/// +/// Each iteration takes a damped least squares (Levenberg-Marquardt) step +/// `J^T (J J^T + lambda^2 I)^-1 e`. The damping `lambda^2` is dynamic: it is recomputed every +/// iteration from the Yoshikawa manipulability measure `w = sqrt(det(J J^T))`, so it is zero for +/// well-conditioned configurations — where the step reduces exactly to the minimum-norm +/// pseudoinverse — and grows smoothly toward `max_damping_squared` as the arm approaches a +/// singularity, keeping the step bounded and the solve panic-free. +/// +/// `manipulability_threshold` (`w0`) is scale-dependent: `w` mixes length-unit translation rows +/// with dimensionless rotation rows and varies with the active constraints, so a good value depends +/// on the robot's size and the task. The default is deliberately small; if it is too small for the +/// robot scale a near-singular step fails cleanly with `InverseMatrixError` rather than blowing up. pub struct JacobianIkSolver { /// If the distance is smaller than this value, it is reached. pub allowable_target_distance: T, @@ -155,18 +178,87 @@ pub struct JacobianIkSolver { pub jacobian_multiplier: T, /// How many times the joints are tried to be moved pub num_max_try: usize, + /// Manipulability threshold `w0`: damping engages once `w = sqrt(det(J J^T))` drops below it. + /// Scale-dependent; see the type-level docs and `set_manipulability_threshold`. + pub manipulability_threshold: T, + /// Maximum squared damping factor `lambda^2_max`, approached as the manipulability `w -> 0`. + pub max_damping_squared: T, + /// Joint-limit-avoidance gain `k0` for the opt-in null-space secondary task (Liegeois 1977). + /// Defaults to 0 (avoidance off). Only acts on redundant systems (`n > m`); see + /// `set_joint_limit_avoidance_gain`. + pub joint_limit_avoidance_gain: T, /// Nullspace function for a redundant system #[allow(clippy::type_complexity)] nullspace_function: Option Vec + Send + Sync>>, } +/// Dynamic damping factor `lambda^2` for damped least squares, from the Yoshikawa manipulability +/// measure `w` and the threshold `w0`: +/// +/// ```text +/// lambda^2 = 0 if w >= w0 (or w0 <= 0) +/// lambda^2 = lambda_squared_max * (1 - w/w0)^2 if 0 <= w < w0 +/// ``` +/// +/// Zero for well-conditioned configurations (`w >= w0`), so the damped step reduces exactly to the +/// undamped minimum-norm pseudoinverse; it rises to `lambda_squared_max` as `w -> 0`. The quadratic +/// is continuous at `w0` (both branches give 0 there). Standalone for unit testing. +fn damping_squared(w: T, w0: T, lambda_squared_max: T) -> T { + // A non-positive threshold means "never damp"; `w >= w0` is the well-conditioned region. + if w0 <= T::zero() || w >= w0 { + return T::zero(); + } + // 0 <= w < w0 => 1 - w/w0 in (0, 1]; lambda^2 = lambda_squared_max * (1 - w/w0)^2. + let factor = T::one() - w / w0; + lambda_squared_max * factor.clone() * factor +} + +/// Liegeois (1977) joint-limit-avoidance gradient, used as the secondary null-space task: each +/// component pushes a joint toward the center of its range, weighted so a joint near a bound moves +/// more strongly. +/// +/// `subtask_i = gain * (mid_i - theta_i) / (max_i - min_i)^2`, with `mid_i = (min_i + max_i) / 2`. +/// The sign descends the cost `H = sum_i ((theta_i - mid_i) / (max_i - min_i))^2`, i.e. it moves +/// toward `mid` (avoidance). A joint with no limit (`None`) or a degenerate range (`max <= min`, e.g. +/// a locked joint) contributes `0` — this is the divide-by-zero guard. The result drops the +/// `ignored_joint_indices` components so it lines up with the reduced `n`-column Jacobian. +fn joint_limit_avoidance_gradient( + gain: T, + positions: &[T], + limits: &[Option<(T, T)>], + ignored_joint_indices: &[usize], +) -> DVector { + let mut subtask: Vec = positions + .iter() + .zip(limits) + .map(|(theta, limit)| match limit { + Some((min, max)) => { + let range = max.clone() - min.clone(); + if range <= T::zero() { + T::zero() + } else { + let mid = (min.clone() + max.clone()) / (T::one() + T::one()); + gain.clone() * (mid - theta.clone()) / (range.clone() * range) + } + } + None => T::zero(), + }) + .collect(); + // Indices are sorted ascending; removing in order keeps the remaining components aligned. + for (i, joint_index) in ignored_joint_indices.iter().enumerate() { + subtask.remove(*joint_index - i); + } + DVector::from_vec(subtask) +} + impl JacobianIkSolver where T: RealField + SubsetOf, { /// Create instance of `JacobianIkSolver`. /// - /// `JacobianIkSolverBuilder` is available instead of calling this `new` method. + /// Dynamic damping starts from the default manipulability threshold and maximum damping; adjust + /// them afterwards with `set_manipulability_threshold` and `set_max_damping`. /// /// # Examples /// @@ -184,6 +276,9 @@ where allowable_target_angle, jacobian_multiplier, num_max_try, + manipulability_threshold: na::convert(DEFAULT_MANIPULABILITY_THRESHOLD), + max_damping_squared: na::convert(DEFAULT_MAX_DAMPING_SQUARED), + joint_limit_avoidance_gain: na::convert(DEFAULT_JOINT_LIMIT_AVOIDANCE_GAIN), nullspace_function: None, } } @@ -210,6 +305,54 @@ where self.nullspace_function = None; } + /// Set the manipulability threshold `w0` below which dynamic damping engages. + /// + /// `w0` is compared against the Yoshikawa manipulability `w = sqrt(det(J J^T))`. It is + /// scale-dependent (the Jacobian mixes translation and rotation rows, and the row count changes + /// with the active constraints), so tune it for the robot: too large over-damps and slows + /// convergence, too small lets near-singular steps grow until the factorization fails with + /// `InverseMatrixError`. A non-positive value disables damping entirely. + /// + /// # Examples + /// + /// ``` + /// let mut solver = k::JacobianIkSolver::new(0.01, 0.01, 0.5, 100); + /// solver.set_manipulability_threshold(1e-4); + /// ``` + pub fn set_manipulability_threshold(&mut self, manipulability_threshold: T) { + self.manipulability_threshold = manipulability_threshold; + } + + /// Set the maximum squared damping factor `lambda^2_max` approached as the manipulability + /// `w -> 0`. Larger values give more stability near singularities at the cost of a larger + /// first-order perturbation of the primary task. + /// + /// # Examples + /// + /// ``` + /// let mut solver = k::JacobianIkSolver::new(0.01, 0.01, 0.5, 100); + /// solver.set_max_damping(1e-2); + /// ``` + pub fn set_max_damping(&mut self, max_damping_squared: T) { + self.max_damping_squared = max_damping_squared; + } + + /// Set the joint-limit-avoidance gain `k0`, enabling the opt-in null-space secondary task that + /// pushes redundant joints toward the center of their range (Liegeois 1977). `0` (the default) + /// disables it, leaving the solve numerically identical to plain damped least squares. The + /// avoidance only acts on redundant systems (`n > m`) and perturbs the primary task to first + /// order, so keep `k0` small; a joint may need a larger `num_max_try` to be pulled off a bound. + /// + /// # Examples + /// + /// ``` + /// let mut solver = k::JacobianIkSolver::new(0.01, 0.01, 0.5, 100); + /// solver.set_joint_limit_avoidance_gain(0.1); + /// ``` + pub fn set_joint_limit_avoidance_gain(&mut self, joint_limit_avoidance_gain: T) { + self.joint_limit_avoidance_gain = joint_limit_avoidance_gain; + } + fn add_positions_with_multiplier(&self, input: &[T], add_values: &[T]) -> Vec { input .iter() @@ -229,8 +372,17 @@ where let orig_positions = arm.joint_positions(); let available_dof = arm.dof() - ignored_joint_indices.len(); - let t_n = arm.end_transform(); - let err = calc_pose_diff_with_constraints(target_pose, &t_n, *operational_space); + // required_dof == 0 means no operational constraints: nothing to solve, no empty determinant. + if required_dof == 0 { + return Ok(DVector::zeros(0)); + } + + // Task error e (length m = required_dof), sign target - end. + let err = + calc_pose_diff_with_constraints(target_pose, &arm.end_transform(), *operational_space); + + // Geometric Jacobian reduced to the kept operational rows and the movable columns: + // J is m x n with m = required_dof, n = available_dof, and n >= m (enforced before the loop). let mut jacobi = jacobian(arm); let mut num_removed_rows = 0; for (i, use_i) in operational_space.iter().enumerate() { @@ -239,53 +391,88 @@ where num_removed_rows += 1; } } - for (i, joint_index) in ignored_joint_indices.iter().enumerate() { jacobi = jacobi.remove_column(*joint_index - i); } - - let positions_vec = if available_dof > required_dof { - const EPS: f64 = 0.0001; - // redundant: pseudo inverse - match self.nullspace_function { - Some(ref f) => { - let jacobi_inv = jacobi.clone().pseudo_inverse(na::convert(EPS)).unwrap(); - - let mut subtask = na::DVector::from_vec(f(&orig_positions)); - for (i, joint_index) in ignored_joint_indices.iter().enumerate() { - subtask = subtask.remove_row(*joint_index - i); - } - let mut d_q = jacobi_inv.clone() * err - + (na::DMatrix::identity(available_dof, available_dof) - - jacobi_inv * jacobi) - * subtask; - for joint_index in ignored_joint_indices { - d_q = d_q.insert_row(*joint_index, T::zero()); - } - self.add_positions_with_multiplier(&orig_positions, d_q.as_slice()) - } - None => { - let mut d_q = jacobi - .svd(true, true) - .solve(&err, na::convert(EPS)) - .unwrap(); - for joint_index in ignored_joint_indices { - d_q = d_q.insert_row(*joint_index, T::zero()); - } - self.add_positions_with_multiplier(&orig_positions, d_q.as_slice()) + let j_t = jacobi.transpose(); // n x m + + // Damped least squares: invert the m x m Gram matrix J J^T (not the n x n J^T J, which is + // singular when n > m), keeping the factorization at most 6 by 6 and reusing det(J J^T). + let mut gram = &jacobi * &j_t; // J J^T (m x m) + + // Yoshikawa manipulability w = sqrt(det(J J^T)); clamp the determinant to >= 0 first + // (round-off can make a rank-deficient Gram matrix slightly negative before the sqrt). + let w = gram.determinant().max(T::zero()).sqrt(); + let lambda_squared = damping_squared( + w, + self.manipulability_threshold.clone(), + self.max_damping_squared.clone(), + ); + + // A = J J^T + lambda^2 I. Symmetric positive definite when lambda^2 > 0 or J has full row + // rank; Cholesky factors it once and returns None on a true rank loss (lambda^2 = 0, singular + // J), which becomes a graceful InverseMatrixError so the outer loop restores the positions. + for i in 0..required_dof { + gram[(i, i)] = gram[(i, i)].clone() + lambda_squared.clone(); + } + let cholesky = gram.cholesky().ok_or(Error::InverseMatrixError)?; + + // Primary task step d_q = J^T (J J^T + lambda^2 I)^-1 e. + let mut d_q = &j_t * cholesky.solve(&err); + + // Optional secondary task projected into the null space of the primary task, without forming + // the n x n projector: d_q += (I - J^T A^-1 J) g. Only meaningful when redundant (n > m). + // g sums the opt-in joint-limit-avoidance gradient (when the gain is non-zero) and the user + // nullspace gradient, both reduced over the ignored joints. With gain 0 and no nullspace + // function nothing is added, so the step is identical to plain damped least squares. + if available_dof > required_dof { + let mut g: Option> = None; + if self.joint_limit_avoidance_gain != T::zero() { + // Read the limits into owned data; the JointRefGuards are dropped here, before any + // matrix math or set_* call (holding a guard across set_joint_positions_* deadlocks). + let limits: Vec> = arm + .iter_joints() + .map(|joint| { + joint + .limits + .as_ref() + .map(|range| (range.min.clone(), range.max.clone())) + }) + .collect(); + g = Some(joint_limit_avoidance_gradient( + self.joint_limit_avoidance_gain.clone(), + &orig_positions, + &limits, + ignored_joint_indices, + )); + } + if let Some(ref f) = self.nullspace_function { + let mut user = DVector::from_vec(f(&orig_positions)); + for (i, joint_index) in ignored_joint_indices.iter().enumerate() { + user = user.remove_row(*joint_index - i); } + g = Some(match g { + Some(limit) => limit + user, + None => user, + }); } - } else { - // normal inverse matrix - self.add_positions_with_multiplier( - &orig_positions, - jacobi - .lu() - .solve(&err) - .ok_or(Error::InverseMatrixError)? - .as_slice(), - ) - }; + if let Some(g) = g { + let z = cholesky.solve(&(&jacobi * &g)); + d_q += g - &j_t * z; + } + } + + // Re-insert zero steps for the ignored joints so d_q matches the full joint vector. + for joint_index in ignored_joint_indices { + d_q = d_q.insert_row(*joint_index, T::zero()); + } + + // Reject a non-finite step instead of corrupting the chain state. + if d_q.iter().any(|v| !v.is_finite()) { + return Err(Error::InverseMatrixError); + } + + let positions_vec = self.add_positions_with_multiplier(&orig_positions, d_q.as_slice()); arm.set_joint_positions_clamped(&positions_vec); Ok(calc_pose_diff_with_constraints( target_pose, @@ -361,6 +548,12 @@ impl fmt::Debug for JacobianIkSolver { .field("allowable_target_angle", &self.allowable_target_angle) .field("jacobian_multiplier", &self.jacobian_multiplier) .field("num_max_try", &self.num_max_try) + .field("manipulability_threshold", &self.manipulability_threshold) + .field("max_damping_squared", &self.max_damping_squared) + .field( + "joint_limit_avoidance_gain", + &self.joint_limit_avoidance_gain, + ) .field("has_nullspace_function", &self.nullspace_function.is_some()) .finish() } @@ -525,4 +718,54 @@ mod tests { assert!((values[0] - 0.25f64).abs() < f64::EPSILON); assert!((values[1] - (-0.05f64)).abs() < f64::EPSILON); } + + #[test] + fn test_damping_squared() { + let w0 = 1e-3_f64; + let max = 1e-3_f64; + // No damping in the well-conditioned region w >= w0 (continuous: w == w0 also gives 0). + assert_eq!(damping_squared(2e-3, w0, max), 0.0); + assert_eq!(damping_squared(w0, w0, max), 0.0); + // Full damping as w -> 0. + assert!((damping_squared(0.0, w0, max) - max).abs() < 1e-15); + // Quadratic midpoint: lambda_squared_max * (1 - 1/2)^2 = lambda_squared_max / 4. + assert!((damping_squared(w0 / 2.0, w0, max) - max / 4.0).abs() < 1e-15); + // Monotonically decreasing in w on (0, w0). + let a = damping_squared(0.2e-3, w0, max); + let b = damping_squared(0.5e-3, w0, max); + let c = damping_squared(0.8e-3, w0, max); + assert!(a > b && b > c); + // A non-positive threshold disables damping for any w. + assert_eq!(damping_squared(0.5, 0.0, max), 0.0); + assert_eq!(damping_squared(0.5, -1.0, max), 0.0); + } + + #[test] + fn test_joint_limit_avoidance_gradient() { + let gain = 1.0_f64; + // Joints: symmetric [-1,1] (mid 0); locked [2,2]; unlimited; [0,2] (mid 1). + let limits = vec![Some((-1.0, 1.0)), Some((2.0, 2.0)), None, Some((0.0, 2.0))]; + let positions = vec![0.0, 2.0, 5.0, 1.9]; + let g = joint_limit_avoidance_gradient(gain, &positions, &limits, &[]); + assert_eq!(g.len(), 4); + // At mid -> 0. + assert!((g[0] - 0.0).abs() < 1e-12); + // Locked (max == min) -> 0 and finite (divide-by-zero guard). + assert_eq!(g[1], 0.0); + assert!(g[1].is_finite()); + // No limit -> 0. + assert_eq!(g[2], 0.0); + // Near max -> negative: (1 - 1.9) / 2^2 = -0.225. + assert!(g[3] < 0.0); + assert!((g[3] - (-0.225)).abs() < 1e-12); + // Near min -> positive. + let near_min = joint_limit_avoidance_gradient(gain, &[0.1], &[Some((0.0, 2.0))], &[]); + assert!(near_min[0] > 0.0); + // Reduction drops the ignored joint (index 1), keeping the rest aligned. + let reduced = joint_limit_avoidance_gradient(gain, &positions, &limits, &[1]); + assert_eq!(reduced.len(), 3); + assert_eq!(reduced[0], g[0]); + assert_eq!(reduced[1], g[2]); + assert_eq!(reduced[2], g[3]); + } } diff --git a/tests/test_ik.rs b/tests/test_ik.rs index 0904eea..7cd3e93 100644 --- a/tests/test_ik.rs +++ b/tests/test_ik.rs @@ -1,6 +1,6 @@ use k::connect; use k::prelude::*; -use na::{Translation3, Vector3}; +use na::{ComplexField, Translation3, Vector3}; use nalgebra as na; #[cfg(target_family = "wasm")] use wasm_bindgen_test::wasm_bindgen_test as test; @@ -8,6 +8,17 @@ use wasm_bindgen_test::wasm_bindgen_test as test; #[cfg(target_family = "wasm")] wasm_bindgen_test::wasm_bindgen_test_configure!(run_in_browser); +/// Load the redundant 7-DOF fixture from an embedded URDF string (wasm-safe: no file I/O). +fn arm7() -> k::SerialChain +where + T: na::RealField + k::SubsetOf, +{ + let robot = urdf_rs::read_from_string(include_str!("../urdf/test_arm7.urdf")).unwrap(); + let chain = k::Chain::::from(&robot); + let end = chain.find("joint7").unwrap(); + k::SerialChain::from_end(end) +} + fn create_joint_with_link_array6() -> k::SerialChain { let l0: k::Node = k::NodeBuilder::new() .name("shoulder_pitch") @@ -176,3 +187,316 @@ fn ik_fk7_with_constraints() { assert!((angles[6] - end_angles[6]).abs() < f32::EPSILON); } } + +// The damped solver is deterministic: the same problem solved twice gives identical positions. +#[test] +fn ik_determinism() { + let arm = arm7::(); + arm.set_joint_positions(&[0.3, 0.5, 0.3, 0.4, 0.3, 0.5, 0.3]) + .unwrap(); + let target = arm.end_transform(); + let start = vec![0.1, 0.2, 0.1, 0.2, 0.1, 0.2, 0.1]; + let solver = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 50); + + arm.set_joint_positions(&start).unwrap(); + let r1 = solver.solve(&arm, &target); + let p1 = arm.joint_positions(); + arm.set_joint_positions(&start).unwrap(); + let r2 = solver.solve(&arm, &target); + let p2 = arm.joint_positions(); + + assert_eq!(r1.is_ok(), r2.is_ok()); + assert_eq!(p1, p2); +} + +// Provable singularity: a planar two-link arm fully extended along x. Both z-axis joint columns +// point along y, so the 2-by-2 position Jacobian is rank 1; a target further along x is in the +// unreachable/singular direction. The damped solver must stay finite and fail gracefully. +#[test] +fn ik_planar_two_link_singular() { + let j0: k::Node = k::NodeBuilder::new() + .name("j0") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .finalize() + .into(); + let j1: k::Node = k::NodeBuilder::new() + .name("j1") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.5, 0.0, 0.0)) + .finalize() + .into(); + let tip: k::Node = k::NodeBuilder::new() + .name("tip") + .translation(Translation3::new(0.5, 0.0, 0.0)) + .finalize() + .into(); + connect![j0 => j1 => tip]; + let arm = k::SerialChain::from_end(&tip); + arm.set_joint_positions(&[0.0, 0.0]).unwrap(); + + // Push the target further along the straight line (+x), into the singular direction. + let mut target = arm.end_transform(); + target.translation.vector.x += 0.3; + // Position x,y only => m = 2 = n (square, rank-deficient at this pose). + let constraints = k::Constraints { + position_z: false, + rotation_x: false, + rotation_y: false, + rotation_z: false, + ..Default::default() + }; + let solver = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 10); + let result = solver.solve_with_constraints(&arm, &target, &constraints); + + assert!(arm.joint_positions().iter().all(|x| x.is_finite())); + assert!( + result.is_ok() || matches!(result, Err(k::Error::NotConvergedError { .. })), + "unexpected result: {result:?}" + ); +} + +// Near-singular redundant arm: the straight (all-zero) pose with a target pushed beyond reach. +// The undamped solver would blow up; the damped one must stay finite and either converge or +// return NotConvergedError (never panic, no non-finite values). Checked for both f32 and f64. +fn near_singular_arm7() +where + T: na::RealField + k::SubsetOf, +{ + let arm = arm7::(); + arm.set_joint_positions(&vec![na::zero::(); 7]).unwrap(); + let mut target = arm.end_transform(); + target.translation.vector.z -= na::convert(0.3); + let solver = k::JacobianIkSolver::::default(); // small num_max_try (10) + let result = solver.solve(&arm, &target); + + assert!(arm.joint_positions().iter().all(|x| x.is_finite())); + assert!(result.is_ok() || matches!(result, Err(k::Error::NotConvergedError { .. }))); +} + +#[test] +fn ik_arm7_near_singular_f64() { + near_singular_arm7::(); +} + +#[test] +fn ik_arm7_near_singular_f32() { + near_singular_arm7::(); +} + +// Opt-in joint-limit avoidance. joint4 has the tight range [0, 2.6] (mid 1.3). The target is the +// current pose (task already satisfied) with joint4 started near its lower bound, so gain=0 leaves +// it there while gain>0 drifts it toward mid within the null space, away from the bound. +#[test] +fn ik_joint_limit_avoidance() { + const JOINT4: usize = 3; // tight limit [0.0, 2.6], range center 1.3 + let target_angles = vec![0.5, 0.7, 0.4, 0.2, 0.4, 0.6, 0.3]; + let start = vec![0.2, 0.3, 0.2, 0.2, 0.2, 0.3, 0.2]; + + // Relax two orientation axes so the redundant null space (here 3-DOF) actually contains joint4 + // motion. Under full 6-DOF constraints this arm's 1-DOF null space barely involves joint4, so the + // avoidance — which can only act inside the null space — has nothing to push along there. + let constraints = k::Constraints { + rotation_x: false, + rotation_z: false, + ..Default::default() + }; + + let arm = arm7::(); + arm.set_joint_positions(&target_angles).unwrap(); + let target = arm.end_transform(); + + // Baseline: gain = 0. + let baseline = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 100); + arm.set_joint_positions(&start).unwrap(); + baseline + .solve_with_constraints(&arm, &target, &constraints) + .unwrap(); + let joint4_baseline = arm.joint_positions()[JOINT4]; + + // Avoidance: gain > 0 pushes joint4 toward the range center 1.3, away from the lower bound 0. + let mut avoid = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 100); + avoid.set_joint_limit_avoidance_gain(0.5); + arm.set_joint_positions(&start).unwrap(); + avoid + .solve_with_constraints(&arm, &target, &constraints) + .unwrap(); + let positions = arm.joint_positions(); + let joint4_avoid = positions[JOINT4]; + + // (a) every joint within its URDF limits. + for (i, p) in positions.iter().enumerate() { + let (lo, hi) = if i == JOINT4 { (0.0, 2.6) } else { (-2.6, 2.6) }; + assert!( + (lo..=hi).contains(p), + "joint{} = {p} out of [{lo}, {hi}]", + i + 1 + ); + } + // (b) joint4 ends measurably farther from its lower bound than the gain=0 baseline (sign check). + assert!( + joint4_avoid > joint4_baseline + 0.02, + "avoid {joint4_avoid} not farther from lower bound than baseline {joint4_baseline}" + ); + // (c) the constrained target is still reached. + let pos_err = (target.translation.vector - arm.end_transform().translation.vector).norm(); + assert!(pos_err < 0.01, "position error too large: {pos_err}"); +} + +// Redundant constrained solve with an ignored joint on the limited fixture: it converges, every +// joint stays within its URDF limits, and the ignored joint is untouched. +#[test] +fn ik_constraints_ignored_joint_limits() { + let arm = arm7::(); + // joint7 (ignored) is held at the same value used to build the target, so it stays reachable. + arm.set_joint_positions(&[0.3, 0.5, 0.3, 0.4, 0.3, 0.5, 0.7]) + .unwrap(); + let target = arm.end_transform(); + + arm.set_joint_positions(&[0.2, 0.4, 0.2, 0.3, 0.2, 0.4, 0.7]) + .unwrap(); + let joint7_initial = arm.joint_positions()[6]; + let constraints = k::Constraints { + rotation_x: false, + ignored_joint_names: vec!["joint7".to_string()], + ..Default::default() + }; + let solver = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 100); + solver + .solve_with_constraints(&arm, &target, &constraints) + .unwrap(); + + let positions = arm.joint_positions(); + assert!( + (positions[6] - joint7_initial).abs() < 1e-9, + "ignored joint moved" + ); + for (i, p) in positions.iter().enumerate() { + let (lo, hi) = if i == 3 { (0.0, 2.6) } else { (-2.6, 2.6) }; + assert!( + (lo..=hi).contains(p), + "joint{} = {p} out of [{lo}, {hi}]", + i + 1 + ); + } +} + +// A manually locked joint (Range::new(x, x)) must not break the avoidance gradient (its range is 0, +// the divide-by-zero guard returns 0) nor produce non-finite values. Planar three-joint arm, +// position-only (m=2, n=3 redundant). +#[test] +fn ik_locked_joint_finite() { + let j0: k::Node = k::NodeBuilder::new() + .name("j0") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .limits(Some(k::joint::Range::new(-2.6, 2.6))) + .finalize() + .into(); + let j1: k::Node = k::NodeBuilder::new() + .name("j1") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.4, 0.0, 0.0)) + .limits(Some(k::joint::Range::new(-2.6, 2.6))) + .finalize() + .into(); + // Locked: min == max. + let j2: k::Node = k::NodeBuilder::new() + .name("j2") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.4, 0.0, 0.0)) + .limits(Some(k::joint::Range::new(0.3, 0.3))) + .finalize() + .into(); + let tip: k::Node = k::NodeBuilder::new() + .name("tip") + .translation(Translation3::new(0.4, 0.0, 0.0)) + .finalize() + .into(); + connect![j0 => j1 => j2 => tip]; + let arm = k::SerialChain::from_end(&tip); + + arm.set_joint_positions(&[0.2, 0.3, 0.3]).unwrap(); + let target = arm.end_transform(); + arm.set_joint_positions(&[0.1, 0.5, 0.3]).unwrap(); + + let mut solver = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 50); + solver.set_joint_limit_avoidance_gain(0.5); + let constraints = k::Constraints { + position_z: false, + rotation_x: false, + rotation_y: false, + rotation_z: false, + ..Default::default() + }; + let result = solver.solve_with_constraints(&arm, &target, &constraints); + + let positions = arm.joint_positions(); + assert!(positions.iter().all(|x| x.is_finite())); + // The locked joint stays exactly at its single allowed value. + assert!((positions[2] - 0.3).abs() < 1e-9); + assert!(result.is_ok() || matches!(result, Err(k::Error::NotConvergedError { .. }))); +} + +// Zero task error: target equals the current pose, so the solve converges immediately and leaves +// the pose (and joints) unchanged within tolerance. +#[test] +fn ik_zero_error_converges() { + let arm = arm7::(); + let angles = vec![0.3, 0.5, 0.3, 0.4, 0.3, 0.5, 0.3]; + arm.set_joint_positions(&angles).unwrap(); + let target = arm.end_transform(); + let solver = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 10); + solver.solve(&arm, &target).unwrap(); + + let end_err = (target.translation.vector - arm.end_transform().translation.vector).norm(); + assert!(end_err < 1e-6); + for (a, b) in angles.iter().zip(arm.joint_positions().iter()) { + assert!((a - b).abs() < 1e-6); + } +} + +// Seeded fuzz: many random reachable targets must never panic or produce non-finite values; each solve +// converges or fails gracefully. (rand is a non-wasm dev-dependency.) +#[cfg(not(target_family = "wasm"))] +#[test] +fn ik_fuzz_arm7_graceful() { + use rand::rngs::StdRng; + use rand::{RngExt, SeedableRng}; + + fn random_config(rng: &mut StdRng) -> Vec { + (0..7) + .map(|i| { + let (lo, hi) = if i == 3 { (0.0, 2.6) } else { (-2.6, 2.6) }; + rng.random_range(lo..hi) + }) + .collect() + } + + let arm = arm7::(); + let mut rng = StdRng::seed_from_u64(20_240_615); + let solver = k::JacobianIkSolver::new(0.001, 0.005, 0.5, 50); + for _ in 0..200 { + arm.set_joint_positions(&random_config(&mut rng)).unwrap(); + let target = arm.end_transform(); + arm.set_joint_positions(&random_config(&mut rng)).unwrap(); + let result = solver.solve(&arm, &target); + assert!(arm.joint_positions().iter().all(|x| x.is_finite())); + assert!( + result.is_ok() + || matches!( + result, + Err(k::Error::NotConvergedError { .. }) | Err(k::Error::InverseMatrixError) + ), + "unexpected result: {result:?}" + ); + } +} diff --git a/urdf/test_arm7.urdf b/urdf/test_arm7.urdf new file mode 100644 index 0000000..08286ad --- /dev/null +++ b/urdf/test_arm7.urdf @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +