Conversation
Owner
Author
|
@ANaaim do you want to get a look at it ? :) |
Codecov Report❌ Patch coverage is
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
Flags with carried forward coverage won't be shown. Click here to find out more. ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
import numpy spce
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
On
examples/inverse_kinematics.pyTime 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"toInverseKinematics.solve(), as a QP-based alternative to the nonlinear solvers (ipopt,sqpmethod).Method
Instead of solving the full nonlinear MKO at each frame,
dikiteratively solves a linearized Gauss–Newton QP:where
Φ_mare marker defects (with constant JacobianK_min natural coordinates) andΦ_hare holonomic constraints — rigid body + joint — with JacobianK_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
K_mand the HessianK_mᵀ K_m + λIare 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.proxsuite(lazy import, clear error if missing).max_delta_qadds−Δ ≤ ΔQ ≤ Δbox inequalities to stabilize the linearization far from feasible Q.Usage
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 CasADiFunction.expand()— faster than the NumPy path),verbose=False.Scope / limitations
_validate_dik_problem()restrictsdikto the standard marker-based problem. It rejects:active_direct_frame_constraints=True(segment-determinant inequalities not yet wired in),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_framepath is untouched; routing happens insolve().success_optimis populated per-frame as before.This change is