TRAC-IK Solver Module#

TRAC-IK Style Solver - ManipulaPy

A high-performance IK solver inspired by TRAC-IK that runs multiple algorithms in parallel and returns the first successful result.

Key Features: - Dual solver: DLS (Damped Least-Squares) + SQP (Sequential Quadratic Programming) - Levenberg-Marquardt style adaptive damping with trust region - SVD-robust Jacobian solve (primary path, not fallback) - Stagnation detection with perturbation recovery - Backtracking line search for step acceptance - Parallel execution using threading - Timeout-based termination (not iteration count) - Diverse initial guesses with random restarts

Algorithm:

┌─────────────────────────────────────────────┐ │ TRAC-IK │ │ ┌─────────────┐ ┌─────────────┐ │ │ │ DLS │ │ SQP │ │ │ │ (Levenberg- │ │ (Constrained│ │ │ │ Marquardt) │ │ Optimizer) │ │ │ └──────┬──────┘ └──────┬──────┘ │ │ │ │ │ │ └───── First ───────┘ │ │ Success │ └─────────────────────────────────────────────┘

Copyright (c) 2025 Mohamed Aboelnasr Licensed under the GNU Affero General Public License v3.0 or later (AGPL-3.0-or-later)

class ManipulaPy.trac_ik.TracIKSolver(fk_func, jacobian_func, joint_limits, n_joints, error_func=None)[source]#

Bases: object

TRAC-IK style solver combining DLS and SQP algorithms.

Runs multiple IK algorithms in parallel and returns the first successful solution. Achieves high success rates through diverse initial guesses, SVD-robust Jacobian solving, and perturbation-based stagnation recovery.

Example

>>> from ManipulaPy.trac_ik import TracIKSolver
>>>
>>> # Create solver
>>> trac_ik = TracIKSolver(
...     fk_func=robot.forward_kinematics,
...     jacobian_func=robot.jacobian,
...     joint_limits=robot.joint_limits,
...     n_joints=6
... )
>>>
>>> # Solve IK
>>> theta, success, solve_time = trac_ik.solve(T_desired)
>>> if success:
...     print(f"Solution found in {solve_time*1000:.1f}ms")
Parameters:
solve(T_desired, theta0=None, timeout=0.2, eomg=0.0001, ev=0.0001, num_restarts=5, use_parallel=False)[source]#

Solve IK using TRAC-IK algorithm.

Uses a DLS-first strategy with SQP fallback. Supports both sequential (default) and parallel execution modes.

Sequential mode (default, recommended): Tries DLS on each initial guess with time budgeting, then falls back to SQP if needed. Avoids Python GIL contention that limits threaded performance for CPU-bound NumPy work.

Parallel mode: Runs DLS + SQP simultaneously on each guess using 2 worker threads. Useful when one solver may find a solution the other cannot, but subject to GIL overhead.

Parameters:

  • T_desired (ndarray[tuple[Any, ...], dtype[float64]]) – Target 4x4 transformation matrix

  • theta0 (ndarray[tuple[Any, ...], dtype[float64]] | None) – Initial guess (optional, uses heuristic if None)

  • timeout (float) – Maximum total solve time in seconds (default: 200ms)

  • eomg (float) – Orientation tolerance in radians

  • ev (float) – Position tolerance in meters

  • num_restarts (int) – Number of initial guesses (default: 5)

  • use_parallel (bool) – Run DLS+SQP in parallel per guess (default: False)

Returns:

Tuple of (theta, success, solve_time) - theta: Joint configuration (best found if not successful) - success: True if solution within tolerances - solve_time: Actual solve time in seconds

Return type:

Tuple[ndarray[tuple[Any, …], dtype[float64]], bool, float]

ManipulaPy.trac_ik.trac_ik_solve(robot, T_desired, theta0=None, timeout=0.2, eomg=0.0001, ev=0.0001, num_restarts=5, use_parallel=False)[source]#

Convenience function to solve IK using TRAC-IK for a SerialManipulator.

Parameters:

  • robot (Any) – SerialManipulator instance

  • T_desired (ndarray[tuple[Any, ...], dtype[float64]]) – Target 4x4 transformation matrix

  • theta0 (ndarray[tuple[Any, ...], dtype[float64]] | None) – Initial guess (optional)

  • timeout (float) – Maximum total solve time in seconds (default: 200ms)

  • eomg (float) – Orientation tolerance

  • ev (float) – Position tolerance

  • num_restarts (int) – Number of initial guesses (default: 5)

  • use_parallel (bool) – Run DLS+SQP in parallel per guess (default: False)

Returns:

Tuple of (theta, success, solve_time)

Return type:

Tuple[ndarray[tuple[Any, …], dtype[float64]], bool, float]

Example

>>> from ManipulaPy.trac_ik import trac_ik_solve
>>> theta, success, time = trac_ik_solve(robot, T_target)