From 38eaaf774f2f61d638842e13a86058957dfc2c8f Mon Sep 17 00:00:00 2001 From: Daniel Lemus Perez Date: Sat, 22 Mar 2025 00:24:08 +0100 Subject: [PATCH 1/3] fix: fix ik bug with ignored joints and available_dofs = required_dofs --- Cargo.toml | 1 + src/ik.rs | 20 ++++--- tests/test_ik.rs | 150 ++++++++++++++++++++++++++++++++++++++++++++++- 3 files changed, 159 insertions(+), 12 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ea525ff..d21906f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -26,6 +26,7 @@ tracing = "0.1" urdf-rs = "0.9" serde = { version = "1.0", features = ["derive"], optional = true } +approx = "0.5.1" [target.'cfg(not(target_family = "wasm"))'.dev-dependencies] clap = { version = "4", features = ["derive"] } diff --git a/src/ik.rs b/src/ik.rs index f4620dd..7381602 100644 --- a/src/ik.rs +++ b/src/ik.rs @@ -244,8 +244,8 @@ where jacobi = jacobi.remove_column(*joint_index - i); } + const EPS: f64 = 0.0001; let positions_vec = if available_dof > required_dof { - const EPS: f64 = 0.0001; // redundant: pseudo inverse match self.nullspace_function { Some(ref f) => { @@ -277,14 +277,16 @@ where } } else { // normal inverse matrix - self.add_positions_with_multiplier( - &orig_positions, - jacobi - .lu() - .solve(&err) - .ok_or(Error::InverseMatrixError)? - .as_slice(), - ) + let mut dq = jacobi + .svd(true, true) + .solve(&err, na::convert(EPS)) + .unwrap() + .as_slice() + .to_vec(); + for (i, joint_index) in ignored_joint_indices.iter().enumerate() { + dq.insert(*joint_index - i, T::zero()); + } + self.add_positions_with_multiplier(&orig_positions, dq.as_slice()) }; arm.set_joint_positions_clamped(&positions_vec); Ok(calc_pose_diff_with_constraints( diff --git a/tests/test_ik.rs b/tests/test_ik.rs index 0904eea..a6a6d84 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 approx::assert_abs_diff_eq; +use k::{connect, joint::Range, prelude::*}; +use na::{Translation3, UnitQuaternion, Vector3}; use nalgebra as na; #[cfg(target_family = "wasm")] use wasm_bindgen_test::wasm_bindgen_test as test; @@ -176,3 +176,147 @@ fn ik_fk7_with_constraints() { assert!((angles[6] - end_angles[6]).abs() < f32::EPSILON); } } + +#[test] +fn test_roc_x_chain() { + let base: k::Node = k::NodeBuilder::new() + .name("base") + .joint_type(k::JointType::Fixed) + .rotation(UnitQuaternion::from_euler_angles( + 0.0, + 0.0, + 90.0_f64.to_radians(), + )) + .finalize() + .into(); + + let linear_z: k::Node = k::NodeBuilder::new() + .name("linear_1") + .joint_type(k::JointType::Linear { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.0, 0.0, 1.2)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .finalize() + .into(); + + let shoulder: k::Node = k::NodeBuilder::new() + .name("shoulder") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.2, 0.0, 0.1)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .limits(Some(Range::new( + -90.0_f64.to_radians(), + 90.0_f64.to_radians(), + ))) + .finalize() + .into(); + + let elbow: k::Node = k::NodeBuilder::new() + .name("elbow") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.5, 0.0, -0.1)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 180.0)) + .limits(Some(Range::new( + -90.0_f64.to_radians(), + 90.0_f64.to_radians(), + ))) + .finalize() + .into(); + + let yaw: k::Node = k::NodeBuilder::new() + .name("yaw") + .joint_type(k::JointType::Rotational { + axis: Vector3::z_axis(), + }) + .translation(Translation3::new(0.5, 0.0, 0.02)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .limits(Some(Range::new( + -90.0_f64.to_radians(), + 90.0_f64.to_radians(), + ))) + .finalize() + .into(); + + let pitch: k::Node = k::NodeBuilder::new() + .name("pitch") + .joint_type(k::JointType::Rotational { + axis: Vector3::y_axis(), + }) + .translation(Translation3::new(0.2, 0.0, -0.3)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .limits(Some(Range::new( + -45.0_f64.to_radians(), + 45.0_f64.to_radians(), + ))) + .finalize() + .into(); + + let linear_x: k::Node = k::NodeBuilder::new() + .name("linear_x") + .joint_type(k::JointType::Linear { + axis: Vector3::x_axis(), + }) + .translation(Translation3::new(0.1, 0.0, -0.1)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .finalize() + .into(); + + let roll: k::Node = k::NodeBuilder::new() + .name("roll") + .joint_type(k::JointType::Rotational { + axis: Vector3::x_axis(), + }) + .translation(Translation3::new(0.1, 0.0, 0.1)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .limits(Some(Range::new( + -30.0_f64.to_radians(), + 30.0_f64.to_radians(), + ))) + .finalize() + .into(); + + let tool: k::Node = k::NodeBuilder::new() + .name("tool") + .joint_type(k::JointType::Fixed) + .translation(Translation3::new(0.1, 0.0, 0.0)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .finalize() + .into(); + + connect![base => linear_z => shoulder => elbow => yaw => pitch => linear_x => roll => tool]; + + let chain = k::SerialChain::from_end(&roll); + + let _result = chain.set_joint_positions(&[ + 0.5, // linear_z + 60.0_f64.to_radians(), // shoulder + 30.0_f64.to_radians(), // wrist + 5.0_f64.to_radians(), // yaw + 5.0_f64.to_radians(), // pitch + 0.0, // linear_x + 5.0_f64.to_radians(), // roll + ]); + + let tool_pose = chain.end_transform(); + + let solver = k::JacobianIkSolver::new(1e-6, 1e-6, 0.5, 1000); + let constraints = k::Constraints { + ignored_joint_names: vec!["linear_x".to_string()], + ..Default::default() + }; + + chain.set_joint_positions(&[0.0; 7]).unwrap(); + + solver + .solve_with_constraints(&chain, &tool_pose, &constraints) + .unwrap(); + + let ik_tool_pose = chain.end_transform(); + + assert_abs_diff_eq!(tool_pose, ik_tool_pose, epsilon = 1e-6); +} From 69d928a6c0ee1febb622593c26772f94ba8afb28 Mon Sep 17 00:00:00 2001 From: Daniel Lemus Perez Date: Tue, 25 Mar 2025 11:02:57 +0100 Subject: [PATCH 2/3] fix: fix typo in adding ignored joints --- src/ik.rs | 4 ++-- tests/test_ik.rs | 17 ++++++++++++++--- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/ik.rs b/src/ik.rs index 7381602..654b8fe 100644 --- a/src/ik.rs +++ b/src/ik.rs @@ -283,8 +283,8 @@ where .unwrap() .as_slice() .to_vec(); - for (i, joint_index) in ignored_joint_indices.iter().enumerate() { - dq.insert(*joint_index - i, T::zero()); + for joint_index in ignored_joint_indices.iter() { + dq.insert(*joint_index, T::zero()); } self.add_positions_with_multiplier(&orig_positions, dq.as_slice()) }; diff --git a/tests/test_ik.rs b/tests/test_ik.rs index a6a6d84..1fae5fc 100644 --- a/tests/test_ik.rs +++ b/tests/test_ik.rs @@ -242,6 +242,16 @@ fn test_roc_x_chain() { .finalize() .into(); + let linear_y: k::Node = k::NodeBuilder::new() + .name("linear_y") + .joint_type(k::JointType::Linear { + axis: Vector3::x_axis(), + }) + .translation(Translation3::new(0.1, 0.0, -0.1)) + .rotation(UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)) + .finalize() + .into(); + let pitch: k::Node = k::NodeBuilder::new() .name("pitch") .joint_type(k::JointType::Rotational { @@ -288,7 +298,7 @@ fn test_roc_x_chain() { .finalize() .into(); - connect![base => linear_z => shoulder => elbow => yaw => pitch => linear_x => roll => tool]; + connect![base => linear_z => shoulder => elbow => yaw => linear_y => pitch => linear_x => roll => tool]; let chain = k::SerialChain::from_end(&roll); @@ -297,6 +307,7 @@ fn test_roc_x_chain() { 60.0_f64.to_radians(), // shoulder 30.0_f64.to_radians(), // wrist 5.0_f64.to_radians(), // yaw + 0.0, // linear_y 5.0_f64.to_radians(), // pitch 0.0, // linear_x 5.0_f64.to_radians(), // roll @@ -306,11 +317,11 @@ fn test_roc_x_chain() { let solver = k::JacobianIkSolver::new(1e-6, 1e-6, 0.5, 1000); let constraints = k::Constraints { - ignored_joint_names: vec!["linear_x".to_string()], + ignored_joint_names: vec!["linear_x".to_string(), "linear_y".to_string()], ..Default::default() }; - chain.set_joint_positions(&[0.0; 7]).unwrap(); + chain.set_joint_positions(&[0.0; 8]).unwrap(); solver .solve_with_constraints(&chain, &tool_pose, &constraints) From 8d7f82fad8c8ed94756a74cdf6cd12a269983ae4 Mon Sep 17 00:00:00 2001 From: Daniel Lemus Perez Date: Tue, 25 Mar 2025 11:04:48 +0100 Subject: [PATCH 3/3] chore: rename test --- tests/test_ik.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_ik.rs b/tests/test_ik.rs index 1fae5fc..4b7f60c 100644 --- a/tests/test_ik.rs +++ b/tests/test_ik.rs @@ -178,7 +178,7 @@ fn ik_fk7_with_constraints() { } #[test] -fn test_roc_x_chain() { +fn test_ik_with_ignored_joints() { let base: k::Node = k::NodeBuilder::new() .name("base") .joint_type(k::JointType::Fixed)