Skip to content

novaphy.ik.solve_ik_pose

Pose IK (position + orientation) with Levenberg-Marquardt adaptive damping. Uses the geometric (angular-velocity) Jacobian for orientation rows so convergence stays robust through large rotation errors, including near-180° cases.

def solve_ik_pose(
    art,
    q0,
    target_position,
    target_orientation,
    joint_limits=None,
    ee_link_index=-1,
    local_offset=None,
    tol_pos=1e-4,
    tol_orient=1e-2,
    max_iter=150,
    lambda_damp=0.01,
    step_size=1.0,
    weight_orientation=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_position Target end-effector world position (3,).
target_orientation Target rotation matrix (3, 3).
joint_limits Optional (q_min, q_max) arrays for box clipping.
ee_link_index End-effector link index (-1 = last link).
local_offset Optional 3D offset in the end-effector's link frame.
tol_pos Position-error tolerance for termination.
tol_orient Orientation-error tolerance (axis-angle norm) for termination.
max_iter Maximum LM iterations.
lambda_damp Initial damping factor λ.
step_size Step scale applied to the LM update.
weight_orientation Scalar weight on the orientation rows in the stacked Jacobian and residual.

Returns

(q_sol, converged) — solution q (np.float32) and a convergence flag.

Algorithm

Forms the stacked 6 × nv system

J_w = vstack(J_pos, w · J_ω)
e   = concat(target_pos − p, w · axis_angle(R, target_R))

and solves (J_w^T J_w + λ I) Δq = J_w^T e (n × n, smaller and better conditioned than the m × m DLS form when m > n). Damping is updated adaptively based on accepted vs rejected steps.

Example

import numpy as np
from novaphy.ik import ik_solver

target_R = np.eye(3, dtype=np.float64)
q_sol, converged = ik_solver.solve_ik_pose(
    bundle,
    q0=q_init,
    target_position=np.array([0.4, 0.1, 0.3]),
    target_orientation=target_R,
    weight_orientation=0.5,
    max_iter=300,
)

See Also