Skip to content

feat: differential inverse kinematics for even more speed#165

Open
Ipuch wants to merge 9 commits into
mainfrom
dik
Open

feat: differential inverse kinematics for even more speed#165
Ipuch wants to merge 9 commits into
mainfrom
dik

Conversation

@Ipuch

@Ipuch Ipuch commented May 12, 2026

Copy link
Copy Markdown
Owner

On examples/inverse_kinematics.py
Time to solve 200 frames with sqpmethod: 0.7363345623016357
Time to solve 200 frames with ipopt: 1.4329211711883545
Time to solve 200 frames with dik: 0.43631577491760254

This PR adds a new method "dik" to InverseKinematics.solve(), as a QP-based alternative to the nonlinear solvers (ipopt, sqpmethod).

Method

Instead of solving the full nonlinear MKO at each frame, dik iteratively solves a linearized Gauss–Newton QP:

min_{ΔQ}  ½ ‖Φ_m(Q) + K_m ΔQ‖²
s.t.       K_h ΔQ = −Φ_h(Q)

where Φ_m are marker defects (with constant Jacobian K_m in natural coordinates) and Φ_h are holonomic constraints — rigid body + joint — with Jacobian K_h. The QP is solved with [proxsuite/proxQP](https://github.com/Simple-Robotics/proxsuite) and iterates until marker objective stagnation and constraint satisfaction (configurable tolerances).

Why add this

  • Speed. K_m and the Hessian K_mᵀ K_m + λI are constant and precomputed once outside the frame loop. Each iteration is a warm-started dense QP — much lighter than an IPOPT call. Particularly attractive for batch processing long c3d files.
  • Deterministic behavior. Gauss–Newton + equality-constrained QP has predictable per-iteration convergence, easier to tune than a generic NLP.
  • Light dependency. Only requires proxsuite (lazy import, clear error if missing).
  • Optional step bounds. max_delta_q adds −Δ ≤ ΔQ ≤ Δ box inequalities to stabilize the linearization far from feasible Q.

Usage

ik = InverseKinematics(model, experimental_markers=markers)
Qopt = ik.solve(method="dik")

Defaults: max_iter=100, eps=1e-6, constraint_eps, step_eps, objective_eps=1e-12, regularization=1e-8 (Tikhonov on the Hessian), max_delta_q=inf, proxqp_eps_abs=1e-8, proxqp_max_iter=1000, use_casadi_dik_evaluators=True (holonomic constraints compiled via CasADi Function.expand() — faster than the NumPy path), verbose=False.

Scope / limitations

_validate_dik_problem() restricts dik to the standard marker-based problem. It rejects:

  • heatmap data,
  • active_direct_frame_constraints=True (segment-determinant inequalities not yet wired in),
  • extra objectives added via add_objective (only the default marker least-squares is supported).

Implementation

New private helpers: _validate_dik_problem, _solve_frame_per_frame_dik, _check_proxsuite_available, _setup_dik_evaluator, _dik_marker_defects, _dik_holonomic_constraints, _setup_dik_qp, _solve_dik_qp. The existing _solve_frame_per_frame path is untouched; routing happens in solve(). success_optim is populated per-frame as before.


This change is Reviewable

@Ipuch

Ipuch commented May 12, 2026

Copy link
Copy Markdown
Owner Author

@ANaaim do you want to get a look at it ? :)

@codecov

codecov Bot commented May 12, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 84.90566% with 16 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.56%. Comparing base (198adb8) to head (5f69122).
⚠️ Report is 4 commits behind head on main.

Files with missing lines Patch % Lines
bionc/bionc_numpy/inverse_kinematics.py 84.61% 16 Missing ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##             main     #165      +/-   ##
==========================================
+ Coverage   85.07%   85.56%   +0.49%     
==========================================
  Files         112      112              
  Lines        6478     6581     +103     
==========================================
+ Hits         5511     5631     +120     
+ Misses        967      950      -17     
Flag Coverage Δ
unittests 85.56% <84.90%> (+0.49%) ⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant