-
Notifications
You must be signed in to change notification settings - Fork 2.6k
Changes Pink IK solver and null space computation to reduce runtime speed #3904
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Greptile Overview
Greptile Summary
This PR optimizes the Pink IK controller to achieve a 2x performance improvement (1.23ms → 0.52ms) through two key changes:
Key Changes:
- Replaced
np.linalg.pinv()with optimized Cholesky-based pseudoinverse using direct LAPACK/BLAS calls in null space computation - Changed QP solver from
osqptodaqpfor better performance - Added
daqp==0.7.2as a new dependency for Linux platforms
Issues Found:
- Critical:
scipyis imported innull_space_posture_task.pybut not listed insetup.pydependencies - Silent fallback behavior when Cholesky factorization fails may mask numerical problems
Documentation:
- Added helpful tips to emphasize testing policies from multiple checkpoint epochs rather than just the final one
Confidence Score: 3/5
- This PR has a critical dependency issue that will cause runtime failures
- Score reflects that while the optimization approach is mathematically sound and performance improvements are significant, there is a critical missing dependency (
scipy) that must be added before merging. The silent fallback behavior also needs attention to prevent masking of numerical issues. - Pay close attention to
source/isaaclab/setup.py(missing scipy dependency) andsource/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py(silent error fallback)
Important Files Changed
File Analysis
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py | 4/5 | Optimized pseudoinverse using Cholesky factorization with LAPACK/BLAS, but scipy is not listed in dependencies |
| source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py | 5/5 | Changed QP solver from osqp to daqp for performance improvement |
| source/isaaclab/setup.py | 4/5 | Added daqp==0.7.2 dependency for Linux platforms, but missing scipy dependency |
Sequence Diagram
sequenceDiagram
participant User as PinkIKController
participant Config as Configuration
participant NSP as NullSpacePostureTask
participant LAPACK as LAPACK/BLAS
participant Solver as DAQP Solver
User->>NSP: compute_jacobian(config)
NSP->>Config: get_frame_jacobian(frame_name)
Config-->>NSP: J_frame (for each frame)
NSP->>NSP: np.concatenate(J_frames) → J_combined
Note over NSP,LAPACK: Optimized Pseudoinverse Computation
NSP->>LAPACK: blas.dgemm(J, J^T) → JJT
NSP->>NSP: add damping to diagonal
NSP->>LAPACK: lapack.dpotrf(JJT) → L (Cholesky)
alt Cholesky Success (info == 0)
NSP->>LAPACK: lapack.dpotrs(L, J) → X
NSP->>NSP: N = I - J^T @ X
NSP-->>User: N (null space projector)
else Cholesky Failed (info != 0)
NSP-->>User: I (identity matrix)
end
User->>Solver: solve_ik(config, tasks, dt, solver="daqp")
Solver-->>User: velocity (joint velocities)
6 files reviewed, 2 comments
source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py
Outdated
Show resolved
Hide resolved
adf63de to
9eac095
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Greptile Overview
Greptile Summary
This PR achieves a 2x performance improvement (1.23ms → 0.52ms) for the Pink IK controller through two key optimizations:
Key Changes:
- Optimized pseudoinverse computation: Replaced
np.linalg.pinv()with direct LAPACK/BLAS calls (dpotrf+dpotrs) using Cholesky factorization in the null space projector calculation - QP solver upgrade: Changed from
osqptodaqpfor faster quadratic programming - New dependency: Added
daqp==0.7.2tosetup.pyfor Linux platforms
Issues Found:
- Previous comments already identified: scipy imports not in dependencies (though scipy is already used elsewhere in the codebase), and silent fallback to identity matrix when Cholesky factorization fails
Documentation Updates:
- Added helpful tips throughout teleop documentation emphasizing the importance of evaluating policies from multiple training epochs, not just final checkpoints
Confidence Score: 4/5
- This PR is safe to merge with minor concerns about dependency management and error handling
- The optimizations are mathematically sound and well-implemented. The Cholesky-based pseudoinverse is a standard numerical optimization for wide matrices. The daqp solver change is minimal risk. Two issues noted in previous comments (scipy dependency, silent fallback) should be addressed but don't block merging
- Pay close attention to
source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py- verify scipy is available and consider logging warnings on Cholesky failures
Important Files Changed
File Analysis
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py | 3/5 | Optimized pseudoinverse computation using LAPACK/BLAS for 2x speedup. Added scipy imports and damping constant. Two issues: scipy not in dependencies, silent fallback to identity on Cholesky failure. |
| source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py | 5/5 | Changed QP solver from osqp to daqp for performance improvement. Simple one-line change. |
| source/isaaclab/setup.py | 5/5 | Added daqp==0.7.2 dependency for Linux platforms to support new QP solver. |
Sequence Diagram
sequenceDiagram
participant Client as IK Controller
participant Pink as Pink IK Solver
participant NSP as Null Space Posture Task
participant LAPACK as LAPACK/BLAS
participant DAQP as DAQP Solver
Client->>Pink: compute(curr_joint_pos, dt)
Pink->>Pink: Update configuration with joint positions
Note over Pink,NSP: For each task, compute Jacobian
Pink->>NSP: compute_jacobian(configuration)
NSP->>NSP: Get frame Jacobians for controlled frames
NSP->>NSP: Concatenate: J_combined
Note over NSP,LAPACK: Optimized Pseudoinverse Computation
NSP->>LAPACK: dgemm(J, J^T) - Matrix multiply
NSP->>NSP: Add damping: J*J^T + λ²I
NSP->>LAPACK: dpotrf(J*J^T + λ²I) - Cholesky factorization
alt Cholesky succeeds
NSP->>LAPACK: dpotrs(L, J) - Triangular solve
NSP->>NSP: Compute N = I - J^T * X
else Cholesky fails
NSP->>NSP: Fallback to identity matrix
end
NSP-->>Pink: Return null space projector N
Note over Pink,DAQP: Solve IK using DAQP
Pink->>DAQP: solve_ik(config, tasks, dt, solver="daqp")
DAQP-->>Pink: Joint velocities
Pink->>Pink: joint_changes = velocity * dt
Pink->>Pink: Reorder to IsaacLab conventions
Pink-->>Client: Target joint positions
6 files reviewed, no comments
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Greptile Overview
Greptile Summary
This PR delivers a 2x performance improvement (1.23ms → 0.52ms) to the Pink IK controller by optimizing pseudoinverse computation and switching QP solvers.
Key optimizations:
- Replaced
np.linalg.pinv()with custom LAPACK/BLAS implementation using Cholesky factorization (dpotrf) and triangular solvers (dpotrs) - Changed QP solver from
osqptodaqpfor faster convergence - Added graceful fallback to numpy's pseudoinverse if Cholesky fails
Issues identified:
scipyis imported but not listed in dependencies (existing comment addresses this)- Silent fallback could mask numerical issues (existing comment addresses this)
- Damping factor 1e-9 is squared to 1e-18, which may be too small for meaningful regularization
The implementation is mathematically sound and well-tested with comprehensive unit tests covering null space properties, multiple frame tasks, and edge cases.
Confidence Score: 4/5
- Safe to merge with minor improvements needed
- The optimization is mathematically sound with proven 2x speedup and comprehensive test coverage. Two issues prevent a score of 5: (1) scipy dependency not declared in setup.py, and (2) the damping factor value could be adjusted for better numerical stability. Both are non-critical but should be addressed.
null_space_posture_task.pyneeds scipy added to dependencies and damping factor review
Important Files Changed
File Analysis
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py | 4/5 | Optimizes pseudoinverse with LAPACK/BLAS calls for 2x speedup; scipy import missing from dependencies, very small damping factor may need adjustment |
| source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py | 5/5 | Simple solver change from osqp to daqp for performance improvement |
| source/isaaclab/setup.py | 5/5 | Adds daqp==0.7.2 dependency for new QP solver |
Sequence Diagram
sequenceDiagram
participant PinkIK as Pink IK Controller
participant Solver as QP Solver (daqp)
participant NSTask as NullSpacePostureTask
participant BLAS as BLAS/LAPACK
PinkIK->>Solver: solve_ik(config, tasks, dt, solver="daqp")
Note over PinkIK,Solver: Changed from "osqp" to "daqp"
Solver->>NSTask: compute_jacobian(configuration)
NSTask->>NSTask: Get frame Jacobians for controlled frames
NSTask->>NSTask: Concatenate J_combined
Note over NSTask: Fast pseudoinverse computation
NSTask->>BLAS: dgemm(J_combined, J_combined.T)
BLAS-->>NSTask: JJT matrix
NSTask->>NSTask: Add damping: JJT + λ²I
NSTask->>BLAS: dpotrf(JJT) [Cholesky]
alt Cholesky succeeds
BLAS-->>NSTask: L (lower triangular)
NSTask->>BLAS: dpotrs(L, J_combined)
BLAS-->>NSTask: X = (JJT + λ²I)^-1 @ J
NSTask->>NSTask: N = I - J^T @ X
else Cholesky fails (info != 0)
NSTask->>NSTask: Fallback to np.linalg.pinv
NSTask->>NSTask: N = I - J^+ @ J
end
NSTask-->>Solver: Null space projector N
Solver-->>PinkIK: Joint velocities
1 file reviewed, 1 comment
| """ | ||
|
|
||
| # Regularization factor for pseudoinverse computation to ensure numerical stability | ||
| PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-9 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
style: The damping factor 1e-9 is squared (line 254), resulting in 1e-18 regularization. This is far below machine epsilon (~2.2e-16) and may not provide meaningful numerical stability. Typical damping values for robotics are 1e-4 to 1e-6.
| PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-9 | |
| PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-4 |
ooctipus
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nice optimization!
| ] | ||
|
|
||
| # Append Linux x86_64 and ARM64 deps via PEP 508 markers | ||
| SUPPORTED_ARCHS_ARM = "platform_machine in 'x86_64,AMD64,aarch64,arm64'" |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
what is happening here? how come it seems like all system is now supported maybe we should just remove this :))
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Perf numbers added to the PR notes
|
Can you please also provide some perf number here, so the impact of this pr can be referenceable in future |
|
I can't speak to the code, but I verified in headset for XR Teleop and found this PR helped with perf. In our tests, I saw frame times go down from 26-29ms down to 23-25ms. I did not see any functional regressions. |
Added new isaaclab dependency on daqp library. On Teleop mimic documentation, adding a recommendation to further emphasize to users to evaluate policies from multiple epochs
426c8fe to
cc19eb2
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Greptile Overview
Greptile Summary
This PR achieves a 2x performance improvement in the Pink IK controller by optimizing the null space posture task pseudoinverse computation and changing the QP solver.
Key Changes:
- Replaced
np.linalg.pinv()with optimized LAPACK/BLAS implementation using Cholesky factorization (dpotrf) and triangular solvers (dpotrs) innull_space_posture_task.py:245-271 - Changed QP solver from
osqptodaqpinpink_ik.py:224 - Added
daqp==0.7.2dependency for Linux platforms - Added comprehensive documentation tips about evaluating policies from multiple checkpoint epochs
Issues Found:
- Critical:
scipyis imported but not listed insetup.pydependencies - will cause import failures - The damping factor
1e-9is squared (line 254), resulting in1e-18regularization which is below machine epsilon and may not provide meaningful numerical stability - Silent fallback to identity matrix when Cholesky fails could mask numerical issues in the primary task Jacobian
Confidence Score: 3/5
- This PR has a critical dependency issue and numerical stability concerns that should be addressed before merging
- The performance optimization is well-implemented and achieves documented 2x speedup, but missing scipy dependency will cause runtime failures. The extremely small damping factor (1e-18 after squaring) and silent error fallback could lead to subtle numerical issues in production.
- Pay close attention to
setup.py(missing scipy dependency) andnull_space_posture_task.py(numerical stability parameters)
Important Files Changed
File Analysis
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py | 3/5 | Optimizes pseudoinverse using LAPACK/BLAS calls for 2x speedup. Has scipy import missing from setup.py dependencies, potentially problematic damping factor (1e-9 squared = 1e-18), and silent fallback that could mask issues. |
| source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py | 5/5 | Changed QP solver from osqp to daqp for improved performance. Straightforward one-line change with no issues found. |
| source/isaaclab/setup.py | 4/5 | Added daqp==0.7.2 dependency for Linux platforms. Missing scipy dependency which is now required by the pseudoinverse optimization. |
Sequence Diagram
sequenceDiagram
participant Client as PinkIKController
participant Task as NullSpacePostureTask
participant BLAS as scipy.linalg.blas
participant LAPACK as scipy.linalg.lapack
participant Solver as daqp solver
Client->>Task: compute_jacobian(configuration)
Note over Task: Get frame Jacobians for primary tasks
Task->>Task: concatenate frame Jacobians → J_combined
Note over Task: Optimized pseudoinverse computation
Task->>BLAS: dgemm(J_combined, J_combined.T)
BLAS-->>Task: JJT matrix
Task->>Task: Add damping: JJT + λ²I (λ=1e-9)
Task->>LAPACK: dpotrf(JJT) - Cholesky factorization
alt Cholesky succeeds (info == 0)
LAPACK-->>Task: L (lower triangular)
Task->>LAPACK: dpotrs(L, J_combined) - solve system
LAPACK-->>Task: X solution matrix
Task->>Task: Compute N = I - J_combined.T @ X
Task-->>Client: Null space projector N
else Cholesky fails (info != 0)
Note over Task: Fallback to numpy
Task->>Task: np.linalg.pinv(J_combined)
Task->>Task: N = I - J_pinv @ J_combined
Task-->>Client: Null space projector N
end
Client->>Solver: solve_ik(..., solver="daqp")
Note over Solver: Changed from osqp for 2x speedup
Solver-->>Client: Joint velocities
6 files reviewed, 1 comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Greptile Overview
Greptile Summary
This PR achieves a 2x performance improvement (1.23ms → 0.52ms) in the Pink IK controller by making two key optimizations:
Key Changes
- QP Solver Upgrade: Switched from
osqptodaqpfor faster quadratic programming (pink_ik.py:224) - Optimized Pseudoinverse: Replaced
np.linalg.pinv()with direct LAPACK/BLAS calls using Cholesky factorization in null space computation (null_space_posture_task.py:244-271)
Mathematical Approach
The optimization cleverly exploits that robotics Jacobians are typically "wide" (more DOFs than constraints). Instead of computing J^T @ inv(J^T @ J + λ²I) (inverting an n×n matrix), it computes J^T @ inv(J @ J^T + λ²I) @ J (inverting a smaller m×m matrix), then uses Cholesky factorization and triangular solvers to avoid explicit matrix inversion.
Critical Issue
- Missing dependency:
scipyis imported but not added tosetup.py. This will cause import failures if scipy isn't already installed transitively. The fix is already documented in previous comments.
Minor Observations
- The damping factor is squared (1e-9² = 1e-18), which is below machine epsilon. Previous comments suggest 1e-4, though the current value may be intentional for this specific use case.
- Fallback behavior when Cholesky fails is mathematically correct (returns
I - J^+ @ J, not identity as claimed in previous comments) - Documentation changes appropriately emphasize testing multiple checkpoint epochs
Confidence Score: 3/5
- This PR is mostly safe but has a critical dependency issue that must be resolved before merging
- The core optimization logic is mathematically sound and achieves the stated 2x performance improvement. However, the missing scipy dependency will cause runtime import failures. The QP solver change is straightforward. Once the scipy dependency is added to setup.py, this PR would be safe to merge. The damping factor choice may warrant discussion but isn't a blocker.
- setup.py requires scipy dependency addition. null_space_posture_task.py has the optimization logic that should be tested thoroughly.
Important Files Changed
File Analysis
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py | 3/5 | Optimizes pseudoinverse computation using LAPACK/BLAS for 2x speedup. Missing scipy dependency. Fallback path is mathematically correct but lacks error logging. |
| source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py | 5/5 | Simple QP solver change from osqp to daqp - straightforward and safe. |
| source/isaaclab/setup.py | 2/5 | Adds daqp dependency but missing scipy dependency required by null_space_posture_task.py. |
Sequence Diagram
sequenceDiagram
participant User
participant PinkIK as PinkIKController
participant Solver as QP Solver (daqp)
participant NSTask as NullSpacePostureTask
participant LAPACK as LAPACK/BLAS
User->>PinkIK: compute(q_current, dt)
PinkIK->>Solver: solve_ik() with daqp
Note over Solver: Changed from osqp to daqp<br/>for faster QP solving
Solver->>NSTask: compute_error()
Solver->>NSTask: compute_jacobian()
NSTask->>NSTask: Get Jacobians for frame tasks
NSTask->>LAPACK: dgemm(J, J^T)
Note over LAPACK: Optimized: J@J^T instead of J^T@J<br/>(faster for wide matrices)
LAPACK-->>NSTask: JJ^T matrix
NSTask->>NSTask: Add damping: JJ^T + λ²I
NSTask->>LAPACK: dpotrf(JJ^T) - Cholesky
alt Cholesky succeeds
LAPACK-->>NSTask: Lower triangular L
NSTask->>LAPACK: dpotrs(L, J) - Solve
LAPACK-->>NSTask: X = (JJ^T)^-1 @ J
NSTask->>NSTask: N = I - J^T @ X
else Cholesky fails (info != 0)
NSTask->>NSTask: Fallback: pinv(J)
NSTask->>NSTask: N = I - pinv(J) @ J
end
NSTask-->>Solver: Null space projector N
Solver-->>PinkIK: Joint velocities
PinkIK-->>User: Joint angle changes
6 files reviewed, no comments
Description
This PR optimizes the Pink IK controller solver by changing the qpsolver to use daqp, and also optimizing the matrix pseudo inverse in Null Space Posture Task. This achieves a 2x performance improvement by reducing runtime from 1.23 ms to 0.52 ms.
Perf experiments documented in third page here:
https://docs.google.com/document/d/1E9UiYUU-oCOIetUkqAIva8oK0NvdNkMeqP2gxmeqxNA/edit?tab=t.0#heading=h.snu74q2v857w
Key Changes
Optimized Pseudoinverse Computation: Replaced
np.linalg.pinv()with a custom implementation using direct LAPACK/BLAS calls in the null space projector calculation. The new approach uses Cholesky factorization (dpotrf) and triangular solvers (dpotrs) for faster computation.QP Solver Update: Changed the Pink IK solver from
osqptodaqpfor improved performance.New Dependency: Added
daqp==0.7.2tosetup.pyfor Linux platforms.Type of change
Checklist
pre-commitchecks with./isaaclab.sh --formatconfig/extension.tomlfileCONTRIBUTORS.mdor my name already exists there