Skip to content

novaphy.ik.solve_ik

Position-only inverse kinematics with Levenberg-Marquardt adaptive damping. Solves for joint coordinates that drive the end-effector to a target world position.

def solve_ik(
    art,
    q0,
    target_ee,
    joint_limits=None,
    ee_link_index=-1,
    local_offset=None,
    tol=1e-4,
    max_iter=100,
    lambda_damp=0.01,
    step_size=1.0,
) -> tuple[np.ndarray, bool]

Parameters

Parameter Description
art JointBodyBundle, an object with joints / bodies attributes, or a plain joints list.
q0 Initial generalized positions, length art.total_q().
target_ee Desired end-effector world position (3,).
joint_limits Optional (q_min, q_max) arrays for box clipping each step.
ee_link_index End-effector link index. -1 selects the last link.
local_offset Optional 3D offset of the end-effector point in the link frame.
tol Position-error tolerance for early termination.
max_iter Maximum LM iterations.
lambda_damp Initial damping factor λ.
step_size Step scale applied to the LM update.

Returns

(q_sol, converged) — solution q (np.float32) and a flag indicating whether the position tolerance was met within max_iter.

Algorithm

Iteratively solves the normal-equation form (J^T J + λ I) Δq = J^T e with J the position Jacobian (compute_jacobian_position, shape 3 × nv) and e = target − p. The damping λ halves on accepted steps and triples on rejected steps; the update is clipped against joint_limits if provided.

Note: nq must equal nv (typical for fixed-base serial arms with single-DOF joints). Models with Free / Ball joints (nq > nv) need the pose formulation in solve_ik_pose() instead.

Example

import numpy as np
from novaphy.ik import ik_solver

q_sol, converged = ik_solver.solve_ik(
    bundle,
    q0=np.zeros(6, dtype=np.float32),
    target_ee=np.array([0.5, 0.2, 0.4]),
    tol=1e-4,
    max_iter=200,
)

See Also