Support fixed-base kinematic trees; fix Python CI numpy dependency

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"
 

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"

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<Dof>,
     /// 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<usize>,
     /// 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<Joint>,
     /// 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<Joint>, 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<Joint>, 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<Joint>, root_idx: usize) -> Self {
+        Self::new_impl(joints, root_idx, true)
+    }
+
+    fn new_impl(mut joints: Vec<Joint>, 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<JointSpec>,
 }
 

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

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<M: Mapper3Dto2D> Default for SolverConfig<M> {
 /// [`Position2D`]: crate::observation::KeypointObservation::Position2D
 pub struct Solver<M: Mapper3Dto2D = NoMapper> {
     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<f32>,
     /// Per-DOF [`Dof::weight_scaler`](crate::body_plan::Dof::weight_scaler),
     /// same indexing as `neutral_joint_angles`.
@@ -103,6 +107,7 @@ impl<M: Mapper3Dto2D> Solver<M> {
         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<M: Mapper3Dto2D> Solver<M> {
     }
 
     fn has_converged(&self, delta: &DVector<f32>) -> 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,14 +307,15 @@ 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)
         .zip(dof_weight_scalers)
         .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);
     }

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<f32>) {
         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);
             }

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() {