Inverse Kinematics

The moro.inverse_kinematics module provides numerical methods for position inverse kinematics and Cartesian position trajectories.

Numython R&D, (c) 2026 Moro is a Python library for kinematic and dynamic modeling of serial robots. This library has been designed, mainly, for academic and research purposes, using SymPy as base library.

class moro.inverse_kinematics.IKSolution(q: list, converged: bool, iterations: int, error: float, method: str = 'lm', residual: list | None = None, message: str = '')[source]

Bases: object

Represents the result of a position inverse kinematics solve.

Attributes:

qlist: Joint variables for the final solver state.
convergedbool: Whether the solver reached the requested tolerance.
iterationsint: Number of completed global iterations.
errorfloat: Final position error norm. It equals norm(residual) when residual is finite; otherwise it is np.inf.
methodstr: Solver method used (“newton”, “lm”, or “ccd”).
residuallist, optional: Final position residual target_position - current_position. It has three elements when available, or None for numerical failures where no finite residual can be computed.
messagestr: Human-readable solver outcome message.

converged: bool

error: float

iterations: int

message: str = ''

method: str = 'lm'

q: list

residual: list | None = None

class moro.inverse_kinematics.IKTrajectorySolution(solutions: list, converged: bool, failed_index: int | None = None, message: str = '')[source]

Bases: object

Represents the global result of solving a sequence of position IK targets.

Attributes:

solutionslist of IKSolution: Per-target IK results in processing order. The list includes the failing solution when convergence stops at an intermediate target. It always contains at least one result.
convergedbool: True only when all processed targets converged; a converged trajectory requires all individual solutions to be converged.
failed_indexint, optional: Index of the first non-converged target. It is required for a failed trajectory and is None when all targets converged. The referenced solution is non-converged, is the final processed solution, and all previous solutions are converged.
messagestr: Short global outcome message.

converged: bool

property errors: Return per-target final error norms in processing order.

failed_index: int | None = None

property iterations: Return per-target iteration counts in processing order.

message: str = ''

property qs: Return per-target joint vectors in processing order.

solutions: list

moro.inverse_kinematics.solve_position_ik(robot, target_position, q0=None, joint_limits=None, tol=1e-06, max_iter=None, method='lm', damping=1.0, damping_scale=0.5, *, parameters=None, random_state=None, step_tol=1e-12, error_change_tol=1e-12, stagnation_iterations=5)[source]

Solve the position inverse kinematics problem using Newton-Raphson, Levenberg-Marquardt, or Cyclic Coordinate Descent (CCD).

Parameters:

robotRobot: A Robot instance with forward kinematics and Jacobian defined.
target_positionlist, tuple or numpy.ndarray: Desired end-effector position \([x, y, z]\).
q0list or numpy.ndarray, optional: Initial guess for joint variables. If None, a random guess within joint limits is generated.
joint_limitslist of tuples, optional: Joint limits as [(q1_min, q1_max), (q2_min, q2_max), ...]. If None, robot.joint_limits is used.
tolfloat, optional: Convergence tolerance on the position error norm. Default is 1e-6.
max_iterint, optional: Maximum number of iterations. Default is 100 for Newton and LM, 500 for CCD.
methodstr, optional: Solver method: "newton", "lm" (Levenberg-Marquardt), or "ccd" (Cyclic Coordinate Descent). Default is "lm".
dampingfloat, optional: Initial damping parameter \(\lambda\) for Levenberg-Marquardt. Only used when method="lm". Default is 1.0.
damping_scalefloat, optional: Scaling factor for the damping parameter in Levenberg-Marquardt. Only used when method="lm". Default is 0.5.
parametersdict-like, optional: Symbol substitutions for geometric constants and other non-joint symbols used by IK expressions. This mapping is applied locally using SymPy subs and does not modify robot or its cached expressions.
random_stateNone, int, or numpy.random.Generator, optional: Random state used only when q0 is None. When None, a local generator from np.random.default_rng() is used. Integer seeds provide reproducible random initial guesses.
step_tolfloat, optional: Stagnation threshold for effective joint movement. The effective step is measured after applying joint limits: norm(q_trial - q_current) for Newton/LM and norm(q_after_sweep - q_before_sweep) for CCD. Must satisfy step_tol >= 0.
error_change_tolfloat, optional: Stagnation threshold for error improvement. If previous_error - current_error <= error_change_tol consecutively, the solver can terminate due to stagnation. Must satisfy error_change_tol >= 0.
stagnation_iterationsint, optional: Number of consecutive stalled iterations (or CCD sweeps) required before returning a stagnation result. Must be an integer >= 1.

Returns:

IKSolution: An object containing the final joint variables, convergence status, iteration count, final error norm, method, residual, and message.

Raises:

ValueError: If inputs are invalid, if unresolved non-joint symbols remain in IK expressions, or if finite numeric functions cannot be built.

Notes

Newton-Raphson (method="newton"):

\[\mathbf{q}_{k+1} = \mathbf{q}_k + \mathbf{J}_p^\dagger(\mathbf{q}_k) \, (\mathbf{p}_d - \mathbf{f}(\mathbf{q}_k))\]

Levenberg-Marquardt (method="lm", default):

\[\mathbf{q}_{k+1} = \mathbf{q}_k + (\mathbf{J}_p^T \mathbf{J}_p + \lambda^2 \mathbf{I})^{-1} \mathbf{J}_p^T \, (\mathbf{p}_d - \mathbf{f}(\mathbf{q}_k))\]

CCD (method="ccd") adjusts one joint at a time from the end-effector toward the base. For each revolute joint it computes the angle that rotates the end-effector toward the target in the plane perpendicular to the joint axis. For prismatic joints, it slides along the axis to reduce the error. CCD does not use a Jacobian matrix and is robust near singularities, but converges linearly.

tol is expressed in the same linear units as the robot DH parameters and target_position.

Joint limits are validated and used to clip every joint update. If q0 is provided, it is validated and clipped to joint limits before iterations.

residual is the 3D vector target_position - current_position at termination whenever a finite value is available. error is the norm of that residual when available; otherwise, error is np.inf and residual is None (for example, after numerical failures).

message is a stable short description of the solver outcome (converged, maximum iterations, stagnation reason, or numerical failure).

Stagnation can stop the solver early when the effective step becomes too small or the position error stops improving for stagnation_iterations consecutive iterations/sweeps.

If q0 is None, random initialization is sampled with a local NumPy Generator, so integer random_state values make initialization reproducible without changing NumPy’s global random state.

iterations counts completed global algorithm steps: Newton/LM count one per attempted update; CCD counts one per full sweep from joint n to 1. Therefore, if the initial guess already satisfies the tolerance, iterations is 0. If convergence is not reached, iterations equals the number of completed attempts/sweeps at termination (or max_iter when the iteration limit is reached).

Examples

>>> import moro as mr
>>> from moro.abc import l1, l2, q1, q2
>>> from moro.inverse_kinematics import solve_position_ik
>>> 
>>> # 2R planar robot
>>> rr = mr.Robot((l1, 0, 0, q1, "r"), (l2, 0, 0, q2, "r"))
>>> 
>>> # Levenberg-Marquardt (default)
>>> sol = solve_position_ik(
...     rr,
...     [1.5, 0.5, 0.0],
...     q0=[0.1, 0.1],
...     parameters={l1: 1.0, l2: 1.0},
... )
>>>
>>> # Reproducible random initialization (used only when q0 is None)
>>> sol = solve_position_ik(
...     rr,
...     [1.5, 0.5, 0.0],
...     parameters={l1: 1.0, l2: 1.0},
...     random_state=42,
... )
>>> 
>>> # Newton-Raphson
>>> sol = solve_position_ik(rr, [1.5, 0.5, 0.0], q0=[0.1, 0.1],
...                         method="newton", parameters={l1: 1.0, l2: 1.0})
>>> 
>>> # CCD
>>> sol = solve_position_ik(rr, [1.5, 0.5, 0.0], q0=[0.1, 0.1],
...                         method="ccd", parameters={l1: 1.0, l2: 1.0})

moro.inverse_kinematics.solve_position_trajectory(robot, target_positions, q0, *, parameters=None, method='lm', joint_limits=None, tol=1e-06, max_iter=None, damping=1.0, damping_scale=0.5, random_state=None, step_tol=1e-12, error_change_tol=1e-12, stagnation_iterations=5)[source]

Solve position IK for a sequence of Cartesian position targets.

This function is a thin orchestration layer over solve_position_ik. It solves each target sequentially and reuses each converged solution as the initial guess for the next target.

Parameters:

robotRobot: Robot model with forward kinematics and Jacobian support.
target_positionsiterable or numpy.ndarray: Sequence of position targets with shape (m, 3). A single vector with shape (3,) is rejected to avoid ambiguity with solve_position_ik.
q0list or numpy.ndarray: Initial joint seed for the first target. This argument is required.
parametersdict-like, optional: Symbol substitutions passed directly to solve_position_ik.
methodstr, optional: Solver method passed to solve_position_ik.
joint_limitslist of tuples, optional: Joint limits passed to solve_position_ik.
tolfloat, optional: Position error tolerance passed to solve_position_ik.
max_iterint, optional: Maximum iterations passed to solve_position_ik.
dampingfloat, optional: LM damping parameter passed to solve_position_ik.
damping_scalefloat, optional: LM damping scale passed to solve_position_ik.
random_stateNone, int, or numpy.random.Generator, optional: Forwarded for API consistency with solve_position_ik. With mandatory q0 and sequential seeding, it typically has no effect.
step_tolfloat, optional: Stagnation step tolerance passed to solve_position_ik.
error_change_tolfloat, optional: Stagnation error-change tolerance passed to solve_position_ik.
stagnation_iterationsint, optional: Stagnation iteration count passed to solve_position_ik.

Returns:

IKTrajectorySolution: Trajectory-level result. Processing stops at the first non-converged target, and the failing solution is included in solutions.

Notes

This utility does not perform interpolation, timing, smoothing, full-pose orientation IK, branch optimization, or global trajectory planning. Reusing the previous solution can improve local continuity but does not guarantee global branch continuity.

The returned trajectory.qs can be used directly as a sequence of joint configurations for animation pipelines.

Examples

>>> import moro as mr
>>> from moro.abc import l1, l2, q1, q2
>>> from moro.inverse_kinematics import solve_position_trajectory
>>>
>>> robot = mr.Robot((l1, 0, 0, q1, "r"), (l2, 0, 0, q2, "r"))
>>> targets = [
...     [1.5, 0.2, 0.0],
...     [1.4, 0.4, 0.0],
...     [1.2, 0.6, 0.0],
... ]
>>> trajectory = solve_position_trajectory(
...     robot,
...     targets,
...     q0=[0.1, 0.1],
...     parameters={l1: 1.0, l2: 1.0},
... )
>>> trajectory.qs