From ca4fa15d1531624e94c791f912e119a4a454c78c Mon Sep 17 00:00:00 2001 From: Sibo Wang-Chen Date: Sat, 25 Jul 2026 13:10:39 +0200 Subject: [PATCH 1/2] Support fixed-base kinematic trees for robot arms Adds KinematicTree::fixed_base (JSON key "fixed_base", default false): when set, the root has no free 6-DOF state at all, instead of a free-floating base solved for like an animal's root pose. Threads a new KinematicTree::n_root_dofs() through forward.rs/state.rs/solver.rs in place of the previously-hardcoded N_ROOT_DOFS, so a fixed-base tree's Jacobian, apply_delta, and convergence check all correctly have zero root columns instead of six. A semi-fixed base (sliding rail, turntable) needs no new mechanism -- just a zero-offset child joint under the fixed root carrying that one hinge/slide DOF, documented in usage.md alongside the new schema field. Co-Authored-By: Claude Sonnet 5 --- docs/usage.md | 6 +++- src/body_plan.rs | 57 +++++++++++++++++++++++++++++------ src/forward.rs | 52 ++++++++++++++++++-------------- src/solver.rs | 21 +++++++++---- src/state.rs | 17 ++++++----- tests/body_plan_test.rs | 27 ++++++++++++++++- tests/common/mod.rs | 67 +++++++++++++++++++++++++++++++++++++++++ tests/forward_test.rs | 38 +++++++++++++++++++++++ tests/solver_test.rs | 34 +++++++++++++++++++++ tests/state_test.rs | 19 ++++++++++++ 10 files changed, 291 insertions(+), 47 deletions(-) diff --git a/docs/usage.md b/docs/usage.md index e5ac2ef..21f35c2 100644 --- a/docs/usage.md +++ b/docs/usage.md @@ -6,9 +6,12 @@ A body plan describes the kinematic tree QuickIK solves against – a robot's jo One modeling consequence worth knowing: a joint's own DOF reorients its *children*, not itself – rotating a joint never moves its own keypoint, only the keypoints downstream of it. So every DOF needs at least one keypoint further down its own chain to be observable at all; a chain that ends exactly at its last DOF-bearing joint, with nothing past it, leaves that DOF's angle undetermined by any observation. This is why even a fixed, 0-DOF "tip" joint (a fingertip, a fly's claw, a robot's end effector) is usually worth keeping in the body plan even though it never actuates anything itself. +By default the root is a free-floating base with its own 6 DOFs (position and rotation), solved for like everything else – this fits a tracked animal or a robot free to move through the world. Setting the top-level `"fixed_base": true` instead anchors the root in place (e.g. a robot arm bolted to a table), removing those 6 DOFs from the state entirely. A *semi*-fixed base – one that only slides along a rail or spins on a turntable – isn't a separate setting: keep `fixed_base` set and give the root a zero-offset child joint carrying that one hinge/slide DOF, then attach the rest of the body to that joint instead of directly to the root. Because a joint's own DOF only moves its descendants (see above), this joint acts as exactly that one-DOF base – and unlike the root's own DOFs, it gets `limits` and `weight_scaler` like any other DOF. + ??? note "Body plan JSON schema" ```json { + "fixed_base": false, "joints": [ { "name": "root", @@ -46,6 +49,7 @@ One modeling consequence worth knowing: a joint's own DOF reorients its *childre } ``` + - `fixed_base`: whether the root is fixed in the world rather than a free-floating base. Optional, defaults to `false`. - `parent`: joint name, or `null` for the root. - `offset_pos`/`offset_quat`: this joint's offset from its parent. - `weight_scaler`: multiplied together with each frame's `KeypointObservation`'s `weight` for this joint's keypoint. Optional, defaults to `1.0`. @@ -60,7 +64,7 @@ One modeling consequence worth knowing: a joint's own DOF reorients its *childre ## Inverse kinematics on a single frame -QuickIK solves *whole-tree* IK: one `Solver::solve` call takes one `KeypointObservation` per keypoint – `Missing`, `Position3D`, or `Position2D` (see [below](#keypoint-positions-observed-in-2d)) – in `kinematic_tree.joints` order, and jointly fits the free-floating root pose and every joint angle at once against all of them. This is what makes it different from solving each limb as its own small IK problem: a keypoint on one limb can still help constrain the root pose (and therefore every other limb) even if that other limb's own keypoints are all `Missing` this frame. +QuickIK solves *whole-tree* IK: one `Solver::solve` call takes one `KeypointObservation` per keypoint – `Missing`, `Position3D`, or `Position2D` (see [below](#keypoint-positions-observed-in-2d)) – in `kinematic_tree.joints` order, and jointly fits every joint angle at once against all of them – plus the root pose too, unless the body plan's root is [fixed](#body-plan). This is what makes it different from solving each limb as its own small IK problem: a keypoint on one limb can still help constrain the root pose (and therefore every other limb) even if that other limb's own keypoints are all `Missing` this frame. === "Rust" diff --git a/src/body_plan.rs b/src/body_plan.rs index 85300fd..af22d1e 100644 --- a/src/body_plan.rs +++ b/src/body_plan.rs @@ -11,7 +11,9 @@ use crate::utils::quat_from_wxyz; // Data structures for the actual algorithm // ============================================================================= -pub const N_ROOT_DOFS: usize = 6; // 3 for root position, 3 for root rotation +/// DOFs of a free-floating root: 3 for position, 3 for rotation. See +/// [`KinematicTree::n_root_dofs`], which is `0` instead for a fixed-base tree. +pub const N_ROOT_DOFS: usize = 6; /// Whether a [`Dof`] is a hinge (rotational) or slide (translational) DOF. #[derive(Clone, Copy, Debug, PartialEq, Eq, Deserialize)] @@ -61,8 +63,9 @@ pub struct Joint { /// consistent with MoCap data. pub dofs: Vec, /// Index of the parent joint in the body plan. Should be `None` for the - /// root joint, which is attached to an imaginary floating base - /// (i.e. connected to the world with a free joint). + /// root joint, which is attached to the world -- either via an imaginary + /// free joint (floating base), or fixed in place if the tree's + /// [`fixed_base`](KinematicTree::fixed_base) is set. pub parent: Option, /// Indices of this joint's direct children. Populated by // `KinematicTree::new`. Redundant with `parent` but useful as a cache. @@ -85,18 +88,44 @@ pub struct KinematicTree { pub joints: Vec, /// Index of the root joint in `joints`. Should usually be 0. pub root_idx: usize, + /// Whether the root is fixed in the world (e.g. a robot arm bolted to a + /// table) rather than a free-floating base with its own `N_ROOT_DOFS`. + /// + /// A semi-fixed base -- one that only slides along a rail or spins on a + /// turntable -- isn't modeled by this flag. Instead, keep `fixed_base` + /// set and give the root a zero-offset child joint carrying the one + /// hinge/slide DOF the base actually has, then attach the rest of the + /// body to that joint: since a joint's own DOF only moves its + /// descendants (never its own keypoint), this joint acts as exactly that + /// one-DOF base, and -- unlike the root's own DOFs -- it gets `limits` + /// and `weight_scaler` like any other DOF. + pub fixed_base: bool, } impl KinematicTree { - /// Construct a tree directly from parsed `joints` and a `root_idx`, - /// populating each joint's `children` from `parent`. - pub fn new(mut joints: Vec, root_idx: usize) -> Self { + /// Construct a free-floating-base tree directly from parsed `joints` and + /// a `root_idx`, populating each joint's `children` from `parent`. + pub fn new(joints: Vec, root_idx: usize) -> Self { + Self::new_impl(joints, root_idx, false) + } + + /// Same as [`new`](Self::new), but the root is fixed in the world -- + /// see [`fixed_base`](Self::fixed_base). + pub fn new_fixed_base(joints: Vec, root_idx: usize) -> Self { + Self::new_impl(joints, root_idx, true) + } + + fn new_impl(mut joints: Vec, root_idx: usize, fixed_base: bool) -> Self { for i in 0..joints.len() { if let Some(parent_idx) = joints[i].parent { joints[parent_idx].children.push(i); } } - Self { joints, root_idx } + Self { + joints, + root_idx, + fixed_base, + } } pub fn n_joints(&self) -> usize { @@ -107,9 +136,14 @@ impl KinematicTree { self.joints.iter().map(|joint| joint.dofs.len()).sum() } + /// Number of free root DOFs: `N_ROOT_DOFS` for a free-floating base, or + /// `0` if [`fixed_base`](Self::fixed_base) is set. + pub fn n_root_dofs(&self) -> usize { + if self.fixed_base { 0 } else { N_ROOT_DOFS } + } + pub fn state_dim(&self) -> usize { - // 3 for root position, 3 for root rotation, plus DOFs on the body - N_ROOT_DOFS + self.n_dofs() + self.n_root_dofs() + self.n_dofs() } /// Return indices of the direct children of the joint at the given index @@ -149,7 +183,7 @@ impl KinematicTree { joints.push(joint); curr_dof_offset += n_dofs; } - Self::new(joints, root_idx) + Self::new_impl(joints, root_idx, body.fixed_base) } pub fn from_json_str(json_str: &str) -> Self { @@ -179,6 +213,9 @@ impl KinematicTree { /// The "metadata" field in the JSON is ignored (it's for JSON self-documentation only). #[derive(Deserialize)] struct BodyPlanSpec { + /// See [`KinematicTree::fixed_base`]. Optional, defaults to `false`. + #[serde(default)] + fixed_base: bool, joints: Vec, } diff --git a/src/forward.rs b/src/forward.rs index 7686bc2..c214411 100644 --- a/src/forward.rs +++ b/src/forward.rs @@ -3,7 +3,7 @@ use nalgebra::{DMatrix, Unit, UnitQuaternion, Vector3}; -use crate::body_plan::{DofType, Joint, KinematicTree, N_ROOT_DOFS}; +use crate::body_plan::{DofType, Joint, KinematicTree}; use crate::state::State; #[derive(Clone, Copy, Debug)] @@ -14,7 +14,8 @@ struct Frame { /// A record of a single DOF's current configuration. struct DofRecord { - /// DOF's flat index in the state vector, starting from 6 + /// DOF's flat index in the state vector, starting from the tree's + /// [`n_root_dofs`](crate::body_plan::KinematicTree::n_root_dofs) state_idx: usize, /// Hinge or slide. dof_type: DofType, @@ -106,9 +107,11 @@ fn traverse_dfs( &workspace.dof_records[..n_records_before], ); - // Record which DOFs can affect this keypoint. Root pos/rot affect all. + // Record which DOFs can affect this keypoint. Root pos/rot (if any -- + // empty for a fixed-base tree) affect all. workspace.relevant_dof_idxs_by_joint[curr_joint_idx].clear(); - workspace.relevant_dof_idxs_by_joint[curr_joint_idx].extend(0..N_ROOT_DOFS); + workspace.relevant_dof_idxs_by_joint[curr_joint_idx] + .extend(0..state.kinematic_tree.n_root_dofs()); for i in 0..n_records_before { let state_idx = workspace.dof_records[i].state_idx; workspace.relevant_dof_idxs_by_joint[curr_joint_idx].push(state_idx); @@ -138,13 +141,14 @@ fn evaluate_frame_at_joint( let own_origin = parent_frame.origin + parent_frame.rotation * joint.offset_pos; let mut rotation = parent_frame.rotation * joint.offset_quat; let mut origin_for_children = own_origin; + let n_root_dofs = state.kinematic_tree.n_root_dofs(); // ... then apply the joint's own DOFs for (i, dof) in joint.dofs.iter().enumerate() { let axis_local = dof.axis; let axis_world = rotation * axis_local; let record = DofRecord { - state_idx: N_ROOT_DOFS + joint.dof_offset + i, + state_idx: n_root_dofs + joint.dof_offset + i, dof_type: dof.dof_type, axis_world, origin_world: origin_for_children, @@ -184,23 +188,27 @@ fn write_keypoint_jacobian( let row1: usize = row0 + 1; let row2: usize = row0 + 2; - // Root translation (state cols 0..3): - // Moving the root moves every keypoint by the same amount - jacobian[(row0, 0)] = 1.0; - jacobian[(row1, 1)] = 1.0; - jacobian[(row2, 2)] = 1.0; - - // Root rotation (state cols 3..6): - // Rotate about the root's current position - let radius = pos - state.root_pos; - for (i, axis) in [Vector3::x(), Vector3::y(), Vector3::z()] - .iter() - .enumerate() - { - let d = axis.cross(&radius); - jacobian[(row0, 3 + i)] = d.x; - jacobian[(row1, 3 + i)] = d.y; - jacobian[(row2, 3 + i)] = d.z; + // A fixed-base tree's root isn't a state variable at all, so it + // contributes no Jacobian columns. + if state.kinematic_tree.n_root_dofs() > 0 { + // Root translation (state cols 0..3): + // Moving the root moves every keypoint by the same amount + jacobian[(row0, 0)] = 1.0; + jacobian[(row1, 1)] = 1.0; + jacobian[(row2, 2)] = 1.0; + + // Root rotation (state cols 3..6): + // Rotate about the root's current position + let radius = pos - state.root_pos; + for (i, axis) in [Vector3::x(), Vector3::y(), Vector3::z()] + .iter() + .enumerate() + { + let d = axis.cross(&radius); + jacobian[(row0, 3 + i)] = d.x; + jacobian[(row1, 3 + i)] = d.y; + jacobian[(row2, 3 + i)] = d.z; + } } // Upstream joint dofs. Note that each keypoint is only affected by a few diff --git a/src/solver.rs b/src/solver.rs index 9cca2b9..7686c4c 100644 --- a/src/solver.rs +++ b/src/solver.rs @@ -2,7 +2,7 @@ use nalgebra::{DMatrix, DVector, Vector3}; -use crate::body_plan::{KinematicTree, N_ROOT_DOFS}; +use crate::body_plan::KinematicTree; use crate::forward::{ForwardKinematicsWorkspace, evaluate_fwdkin}; use crate::observation::{KeypointObservation, Mapper3Dto2D, NoMapper}; use crate::state::State; @@ -65,6 +65,10 @@ impl Default for SolverConfig { /// [`Position2D`]: crate::observation::KeypointObservation::Position2D pub struct Solver { workspace: ForwardKinematicsWorkspace, + /// Cached from the kinematic tree at construction time: `0` for a + /// fixed-base tree, [`N_ROOT_DOFS`](crate::body_plan::N_ROOT_DOFS) + /// otherwise. + n_root_dofs: usize, neutral_joint_angles: Vec, /// Per-DOF [`Dof::weight_scaler`](crate::body_plan::Dof::weight_scaler), /// same indexing as `neutral_joint_angles`. @@ -103,6 +107,7 @@ impl Solver { let state_dim = kinematic_tree.state_dim(); Self { workspace: ForwardKinematicsWorkspace::new(kinematic_tree), + n_root_dofs: kinematic_tree.n_root_dofs(), neutral_joint_angles, dof_weight_scalers, joint_weight_scalers, @@ -217,14 +222,17 @@ impl Solver { } fn has_converged(&self, delta: &DVector) -> bool { - // Positions: delta[0..3] is root position + // Positions: delta[0..n_root_position_dofs] is root position -- empty + // for a fixed-base tree (n_root_dofs == 0), since it has no root + // position state at all. + let n_root_position_dofs = self.n_root_dofs.min(3); let max_abs_position_delta = delta - .rows(0, 3) + .rows(0, n_root_position_dofs) .iter() .fold(0.0f32, |acc, &x| acc.max(x.abs())); - // Angles: delta[3..6] is root rotation, the rest are DOF angles + // Angles: the rest -- root rotation (if any) plus every DOF angle. let max_abs_angle_delta = delta - .rows(3, delta.len() - 3) + .rows(n_root_position_dofs, delta.len() - n_root_position_dofs) .iter() .fold(0.0f32, |acc, &x| acc.max(x.abs())); max_abs_position_delta <= self.config.position_tolerance @@ -299,6 +307,7 @@ fn accumulate_neutral_pose_prior( if weight == 0.0 { return; } + let n_root_dofs = state.kinematic_tree.n_root_dofs(); for (i, ((&curr_angle, &neutral_angle), &dof_weight_scaler)) in (state.dof_angles) .iter() .zip(neutral_joint_angles) @@ -306,7 +315,7 @@ fn accumulate_neutral_pose_prior( .enumerate() { let weight = weight * dof_weight_scaler; - let state_idx = N_ROOT_DOFS + i; + let state_idx = n_root_dofs + i; jtj[(state_idx, state_idx)] += weight; // only contributor is self jtr[state_idx] += weight * (neutral_angle - curr_angle); } diff --git a/src/state.rs b/src/state.rs index ea94fe0..aee56a9 100644 --- a/src/state.rs +++ b/src/state.rs @@ -4,7 +4,7 @@ use std::sync::Arc; use nalgebra::{DVector, UnitQuaternion, Vector3}; -use crate::body_plan::{KinematicTree, N_ROOT_DOFS}; +use crate::body_plan::KinematicTree; use crate::utils::unit_quat_from_axis_angle_vec; /// The pose being solved for. @@ -46,16 +46,19 @@ impl State { pub fn apply_delta(&mut self, delta: &DVector) { debug_assert_eq!(delta.len(), self.state_dim()); - // Root state - let d_root_pos = Vector3::new(delta[0], delta[1], delta[2]); - self.root_pos += d_root_pos; - let d_root_rot = Vector3::new(delta[3], delta[4], delta[5]); - self.root_rot = unit_quat_from_axis_angle_vec(d_root_rot) * self.root_rot; + // Root state -- absent (0 columns) for a fixed-base tree. + let n_root_dofs = self.kinematic_tree.n_root_dofs(); + if n_root_dofs > 0 { + let d_root_pos = Vector3::new(delta[0], delta[1], delta[2]); + self.root_pos += d_root_pos; + let d_root_rot = Vector3::new(delta[3], delta[4], delta[5]); + self.root_rot = unit_quat_from_axis_angle_vec(d_root_rot) * self.root_rot; + } // Body DOF state, clamped to each DOF's angle limits (if any) let dofs = self.kinematic_tree.joints.iter().flat_map(|j| &j.dofs); for (i, (angle, dof)) in self.dof_angles.iter_mut().zip(dofs).enumerate() { - *angle += delta[N_ROOT_DOFS + i]; + *angle += delta[n_root_dofs + i]; if let Some([min, max]) = dof.limits { *angle = angle.clamp(min, max); } diff --git a/tests/body_plan_test.rs b/tests/body_plan_test.rs index 06d23bd..d719838 100644 --- a/tests/body_plan_test.rs +++ b/tests/body_plan_test.rs @@ -1,4 +1,4 @@ -use quickik::body_plan::{DofType, KinematicTree}; +use quickik::body_plan::{DofType, KinematicTree, N_ROOT_DOFS}; fn valid_body_json() -> &'static str { r#"{ @@ -142,6 +142,31 @@ fn parses_slide_dofs() { assert!((dof.axis - nalgebra::Vector3::new(0.0, 0.0, 1.0)).norm() < 1e-6); } +#[test] +fn fixed_base_defaults_to_false_and_keeps_all_root_dofs() { + let tree = KinematicTree::from_json_str(valid_body_json()); + assert!(!tree.fixed_base); + assert_eq!(tree.n_root_dofs(), N_ROOT_DOFS); + assert_eq!(tree.state_dim(), N_ROOT_DOFS + tree.n_dofs()); +} + +#[test] +fn fixed_base_true_excludes_root_dofs_from_state_dim() { + let json = r#"{ + "fixed_base": true, + "joints": [ + {"name": "root", "parent": null, "offset_pos": [0,0,0], "offset_quat": [1,0,0,0], "dofs": []}, + {"name": "a", "parent": "root", "offset_pos": [1,0,0], "offset_quat": [1,0,0,0], + "dofs": [{"axis": [0,0,1], "type": "hinge", "neutral": 0.0, "limits": null}]} + ] + }"#; + let tree = KinematicTree::from_json_str(json); + + assert!(tree.fixed_base); + assert_eq!(tree.n_root_dofs(), 0); + assert_eq!(tree.state_dim(), tree.n_dofs()); +} + #[test] #[should_panic(expected = "invalid type")] fn rejects_explicit_null_weight_scaler() { diff --git a/tests/common/mod.rs b/tests/common/mod.rs index b19ef22..248cd43 100644 --- a/tests/common/mod.rs +++ b/tests/common/mod.rs @@ -279,6 +279,73 @@ pub fn hinge_then_slide_chain() -> Arc { )) } +/// Same joints as [`two_joint_chain`], but with a fixed (not free-floating) +/// root -- used to test [`KinematicTree::fixed_base`]. +/// +/// root (fixed, no local DOFs) +/// -> joint1: offset (1,0,0), 1 DOF about local Z, unbounded +/// -> joint2: offset (1,0,0), 1 DOF about local Z, limited to [-0.5, 0.5] +/// -> tip: offset (1,0,0), no DOFs +#[allow(dead_code)] // only used by tests exercising fixed_base, not every binary sharing this module +pub fn fixed_base_two_joint_chain() -> Arc { + let root = Joint { + name: "root".to_string(), + offset_pos: Vector3::zeros(), + offset_quat: UnitQuaternion::identity(), + dofs: vec![], + parent: None, + children: Vec::new(), + dof_offset: 0, + weight_scaler: 1.0, + }; + let joint1 = Joint { + name: "joint1".to_string(), + offset_pos: Vector3::new(1.0, 0.0, 0.0), + offset_quat: UnitQuaternion::identity(), + dofs: vec![Dof { + axis: Vector3::z(), + dof_type: DofType::Hinge, + neutral: 0.0, + limits: None, + weight_scaler: 1.0, + }], + parent: Some(0), + children: Vec::new(), + dof_offset: 0, + weight_scaler: 1.0, + }; + let joint2 = Joint { + name: "joint2".to_string(), + offset_pos: Vector3::new(1.0, 0.0, 0.0), + offset_quat: UnitQuaternion::identity(), + dofs: vec![Dof { + axis: Vector3::z(), + dof_type: DofType::Hinge, + neutral: 0.0, + limits: Some([-0.5, 0.5]), + weight_scaler: 1.0, + }], + parent: Some(1), + children: Vec::new(), + dof_offset: 1, + weight_scaler: 1.0, + }; + let tip = Joint { + name: "tip".to_string(), + offset_pos: Vector3::new(1.0, 0.0, 0.0), + offset_quat: UnitQuaternion::identity(), + dofs: vec![], + parent: Some(2), + children: Vec::new(), + dof_offset: 2, + weight_scaler: 1.0, + }; + Arc::new(KinematicTree::new_fixed_base( + vec![root, joint1, joint2, tip], + 0, + )) +} + /// A single joint carrying both a hinge DOF (applied first) and a slide DOF, /// so the slide's own translation is expressed along an axis that the same /// joint's own hinge has already rotated -- the tightest version of the diff --git a/tests/forward_test.rs b/tests/forward_test.rs index 6c146fb..7c1e9d3 100644 --- a/tests/forward_test.rs +++ b/tests/forward_test.rs @@ -187,6 +187,44 @@ fn hinge_then_slide_positions() { assert!((workspace.kpt_positions[3] - Vector3::new(1.0, 2.3, 0.0)).norm() < 1e-5); } +/// On a fixed-base tree, the root contributes no state at all, so a +/// keypoint's active indices are just its ancestors' own DOFs (never a +/// leading `0..N_ROOT_DOFS` block, unlike the free-floating case in +/// `active_indices_track_ancestor_dofs_not_own_dof` above). +#[test] +fn fixed_base_tree_has_no_root_dofs_in_active_indices() { + let tree = common::fixed_base_two_joint_chain(); + let state = State::neutral_pose(tree.clone()); + let mut workspace = ForwardKinematicsWorkspace::new(&tree); + evaluate_fwdkin(&mut workspace, &state); + + assert_eq!(workspace.relevant_dof_idxs_by_joint[0], Vec::::new()); + assert_eq!(workspace.relevant_dof_idxs_by_joint[1], Vec::::new()); + assert_eq!(workspace.relevant_dof_idxs_by_joint[2], vec![0]); + assert_eq!(workspace.relevant_dof_idxs_by_joint[3], vec![0, 1]); +} + +/// A fixed-base tree's keypoints move exactly like its free-floating +/// counterpart's when the root state is left at `neutral_pose`'s default +/// (zero position, identity rotation) -- `fixed_base` only removes the +/// root's own state variables, it doesn't change where the root sits. +#[test] +fn fixed_base_tree_neutral_pose_matches_free_floating_counterpart() { + let tree = common::fixed_base_two_joint_chain(); + let state = State::neutral_pose(tree.clone()); + let mut workspace = ForwardKinematicsWorkspace::new(&tree); + evaluate_fwdkin(&mut workspace, &state); + + assert!((workspace.kpt_positions[0] - Vector3::new(0.0, 0.0, 0.0)).norm() < 1e-6); + assert!((workspace.kpt_positions[1] - Vector3::new(1.0, 0.0, 0.0)).norm() < 1e-6); + assert!((workspace.kpt_positions[2] - Vector3::new(2.0, 0.0, 0.0)).norm() < 1e-6); +} + +#[test] +fn jacobian_matches_finite_differences_fixed_base() { + assert_jacobian_matches_finite_differences(&common::fixed_base_two_joint_chain(), &[0.3, -0.2]); +} + /// Hand-derived expected position and Jacobian for a single joint carrying a /// hinge DOF then a slide DOF, at theta = pi/2, d = 0.5: /// tip = (1 + (d+1) cos(theta), (d+1) sin(theta), 0) = (1, 1.5, 0); diff --git a/tests/solver_test.rs b/tests/solver_test.rs index 319206a..e33a753 100644 --- a/tests/solver_test.rs +++ b/tests/solver_test.rs @@ -46,6 +46,38 @@ fn recovers_pose_from_3d_observations() { assert!((state.dof_angles[1] - 0.3).abs() < 1e-3); } +/// A fixed-base tree's root has no state to fit, so the solver should recover +/// the same DOF angles as the free-floating case above while leaving +/// `root_pos`/`root_rot` untouched at their `neutral_pose` default. +#[test] +fn recovers_pose_on_fixed_base_tree_without_moving_root() { + let tree = common::fixed_base_two_joint_chain(); + let target_positions = keypoints_at(&tree, &[0.4, 0.3]); + + let observations: Vec = target_positions + .iter() + .map(|&obs_pos| KeypointObservation::Position3D { + obs_pos, + weight: 1.0, + }) + .collect(); + + let mut state = State::neutral_pose(tree.clone()); + let mut solver: Solver = Solver::new( + &tree, + SolverConfig { + weight: 0.0, + ..SolverConfig::default() + }, + ); + solver.solve(&mut state, &observations); + + assert!((state.dof_angles[0] - 0.4).abs() < 1e-3); + assert!((state.dof_angles[1] - 0.3).abs() < 1e-3); + assert_eq!(state.root_pos, Vector3::zeros()); + assert_eq!(state.root_rot, nalgebra::UnitQuaternion::identity()); +} + #[test] fn recovers_pose_with_slide_dof_from_3d_observations() { let tree = common::hinge_then_slide_chain(); @@ -278,6 +310,7 @@ fn joint_weight_scaler_zero_matches_missing_observation() { let zero_weight_tree = std::sync::Arc::new(quickik::body_plan::KinematicTree { joints: zero_weight_joints, root_idx: tree.root_idx, + fixed_base: tree.fixed_base, }); let tip_target = keypoints_at(&tree, &[0.4, 0.3])[3]; @@ -323,6 +356,7 @@ fn dof_weight_scaler_zero_recovers_exact_target_despite_nonzero_global_neutral_w let zero_weight_tree = std::sync::Arc::new(quickik::body_plan::KinematicTree { joints, root_idx: tree.root_idx, + fixed_base: tree.fixed_base, }); let target_positions = keypoints_at(&tree, &[0.4, 0.3]); diff --git a/tests/state_test.rs b/tests/state_test.rs index 60928f8..5f3540f 100644 --- a/tests/state_test.rs +++ b/tests/state_test.rs @@ -38,3 +38,22 @@ fn apply_delta_updates_root_position_and_rotation() { assert!((state.root_pos - nalgebra::Vector3::new(1.0, 2.0, 3.0)).norm() < 1e-6); } + +/// On a fixed-base tree, `delta` has no root columns at all -- index 0 is +/// already the first DOF -- and `apply_delta` must never touch `root_pos`/ +/// `root_rot`, which stay at `neutral_pose`'s default. +#[test] +fn apply_delta_on_fixed_base_tree_never_touches_root() { + let tree = common::fixed_base_two_joint_chain(); + let mut state = State::neutral_pose(tree.clone()); + + let mut delta = DVector::zeros(state.state_dim()); + delta[0] = 10.0; // joint1 (unbounded) + delta[1] = 10.0; // joint2 (limited to [-0.5, 0.5]) + state.apply_delta(&delta); + + assert!((state.dof_angles[0] - 10.0).abs() < 1e-6); + assert!((state.dof_angles[1] - 0.5).abs() < 1e-6); + assert_eq!(state.root_pos, nalgebra::Vector3::zeros()); + assert_eq!(state.root_rot, nalgebra::UnitQuaternion::identity()); +} From 26c914e918dce88a3594c122af50d1148cb4e277 Mon Sep 17 00:00:00 2001 From: Sibo Wang-Chen Date: Sat, 25 Jul 2026 13:10:47 +0200 Subject: [PATCH 2/2] Declare numpy as a Python dependency of the quickik bindings python/tests/test_bindings.py imports numpy, but python/pyproject.toml never declared it, so CI's python-test job (which only installs maturin+pytest before running maturin develop) failed to import it. numpy is a real runtime dependency now anyway, since the array-based solve_sequence_segmented_parallel entry points take numpy arrays. Co-Authored-By: Claude Sonnet 5 --- python/pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/python/pyproject.toml b/python/pyproject.toml index e8bc1b1..115422a 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -7,6 +7,7 @@ name = "quickik" version = "0.1.0" description = "Python bindings for the QuickIK inverse kinematics library" requires-python = ">=3.8" +dependencies = ["numpy"] [tool.maturin] module-name = "quickik"