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..654b8fe 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 joint_index in ignored_joint_indices.iter() { + dq.insert(*joint_index, 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..4b7f60c 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,158 @@ fn ik_fk7_with_constraints() { assert!((angles[6] - end_angles[6]).abs() < f32::EPSILON); } } + +#[test] +fn test_ik_with_ignored_joints() { + 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 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 { + 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 => linear_y => 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 + 0.0, // linear_y + 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(), "linear_y".to_string()], + ..Default::default() + }; + + chain.set_joint_positions(&[0.0; 8]).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); +}