Skip to content

Conversation

@michaellin6
Copy link
Contributor

@michaellin6 michaellin6 commented Nov 1, 2025

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

  1. 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.

  2. QP Solver Update: Changed the Pink IK solver from osqp to daqp for improved performance.

  3. New Dependency: Added daqp==0.7.2 to setup.py for Linux platforms.

Type of change

  • New feature (non-breaking change which adds functionality)

Checklist

  • I have read and understood the contribution guidelines
  • I have run the pre-commit checks with ./isaaclab.sh --format
  • I have made corresponding changes to the documentation
  • My changes generate no new warnings
  • I have added tests that prove my fix is effective or that my feature works
  • I have updated the changelog and the corresponding version in the extension's config/extension.toml file
  • I have added my name to the CONTRIBUTORS.md or my name already exists there

@github-actions github-actions bot added documentation Improvements or additions to documentation isaac-lab Related to Isaac Lab team labels Nov 1, 2025
Copy link
Contributor

@greptile-apps greptile-apps bot left a 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 osqp to daqp for better performance
  • Added daqp==0.7.2 as a new dependency for Linux platforms

Issues Found:

  • Critical: scipy is imported in null_space_posture_task.py but not listed in setup.py dependencies
  • 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) and source/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)
Loading

6 files reviewed, 2 comments

Edit Code Review Agent Settings | Greptile

@michaellin6 michaellin6 force-pushed the mlin/optimize-pink-ik branch from adf63de to 9eac095 Compare November 1, 2025 00:12
Copy link
Contributor

@greptile-apps greptile-apps bot left a 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 osqp to daqp for faster quadratic programming
  • New dependency: Added daqp==0.7.2 to setup.py for 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
Loading

6 files reviewed, no comments

Edit Code Review Agent Settings | Greptile

Copy link
Contributor

@greptile-apps greptile-apps bot left a 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 osqp to daqp for faster convergence
  • Added graceful fallback to numpy's pseudoinverse if Cholesky fails

Issues identified:

  • scipy is 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.py needs 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
Loading

1 file reviewed, 1 comment

Edit Code Review Agent Settings | Greptile

"""

# Regularization factor for pseudoinverse computation to ensure numerical stability
PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-9
Copy link
Contributor

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.

Suggested change
PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-9
PSEUDOINVERSE_DAMPING_FACTOR: float = 1e-4

Copy link
Collaborator

@ooctipus ooctipus left a 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'"
Copy link
Collaborator

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 :))

Copy link
Contributor Author

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

@ooctipus
Copy link
Collaborator

ooctipus commented Nov 3, 2025

Can you please also provide some perf number here, so the impact of this pr can be referenceable in future

@hougantc-nvda
Copy link
Contributor

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.

https://nvidia.slack.com/archives/C06HLQ6CB41/p1761834736666699?thread_ts=1758125706.083719&cid=C06HLQ6CB41

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
@michaellin6 michaellin6 force-pushed the mlin/optimize-pink-ik branch from 426c8fe to cc19eb2 Compare November 3, 2025 16:21
Copy link
Contributor

@greptile-apps greptile-apps bot left a 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) in null_space_posture_task.py:245-271
  • Changed QP solver from osqp to daqp in pink_ik.py:224
  • Added daqp==0.7.2 dependency for Linux platforms
  • Added comprehensive documentation tips about evaluating policies from multiple checkpoint epochs

Issues Found:

  • Critical: scipy is imported but not listed in setup.py dependencies - will cause import failures
  • The damping factor 1e-9 is squared (line 254), resulting in 1e-18 regularization 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) and null_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
Loading

6 files reviewed, 1 comment

Edit Code Review Agent Settings | Greptile

@kellyguo11 kellyguo11 merged commit 64a97f2 into isaac-sim:main Nov 4, 2025
6 of 7 checks passed
Copy link
Contributor

@greptile-apps greptile-apps bot left a 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

  1. QP Solver Upgrade: Switched from osqp to daqp for faster quadratic programming (pink_ik.py:224)
  2. 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: scipy is imported but not added to setup.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
Loading

6 files reviewed, no comments

Edit Code Review Agent Settings | Greptile

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

Labels

documentation Improvements or additions to documentation isaac-lab Related to Isaac Lab team

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants