Inverse Kinematics

moro provides numerical solvers for position inverse kinematics.

Given a desired Cartesian position for the end-effector,

\[\begin{split} p_d = \begin{bmatrix} x_d \\ y_d \\ z_d \end{bmatrix}, \end{split}\]

the inverse-kinematics solver searches for a joint configuration \(q\) such that the end-effector position is sufficiently close to the target.

The current interface focuses on position only. Full-pose inverse kinematics, including end-effector orientation constraints, is not currently supported.

This section focuses on practical use of the inverse-kinematics API. For the mathematical background of the numerical methods, see Theory → Inverse Kinematics.

Solving a position target

The main function is:

from moro.inverse_kinematics import solve_position_ik

Consider a planar 2R robot:

from moro import Robot
from moro.abc import q1, q2

robot = Robot(
    (1.0, 0, 0, q1, "r"),
    (1.0, 0, 0, q2, "r"),
)

A Cartesian target can be defined as:

target = [1.5, 0.5, 0.0]

Then solve the inverse-kinematics problem:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
)

The function returns an IKSolution object rather than only the final joint vector.

A basic convergence check is:

if solution.converged:
    print(solution.q)
else:
    print(solution.message)

The target must contain exactly three finite numeric values:

[x, y, z]

and the convergence tolerance is expressed in the same linear units used by the robot geometry and target position.

Providing numerical model parameters

A robot can contain symbolic geometric parameters.

For example:

from moro import Robot
from moro.abc import q1, q2, l1, l2

robot = Robot(
    (l1, 0, 0, q1, "r"),
    (l2, 0, 0, q2, "r"),
)

Because inverse kinematics is solved numerically, all non-joint symbolic quantities involved in the forward kinematics must have numerical values.

Use the parameters argument:

solution = solve_position_ik(
    robot,
    [1.5, 0.5, 0.0],
    q0=[0.1, 0.1],
    parameters={
        l1: 1.0,
        l2: 1.0,
    },
)

These substitutions are applied only to the expressions used by the solver.

The original symbolic robot model is not modified:

robot.T

remains symbolic after the IK computation.

If unresolved non-joint symbols remain after applying parameters, moro raises an error instead of attempting to evaluate an incomplete numerical model.

This allows the same symbolic Robot instance to be reused with different geometric parameter values.

Choosing a solver

Three numerical methods are currently available:

"lm"
"newton"
"ccd"

The solver is selected with:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    method="lm",
)

Levenberg-Marquardt

Levenberg-Marquardt is the default method:

method="lm"

It uses the linear part of the geometric Jacobian together with a damping term.

In simplified form, the joint update is:

\[ \Delta q = \left( J^T J + \lambda^2 I \right)^{-1} J^T e, \]

where:

\[ e = p_d-p(q). \]

The damping parameter is adjusted during the solution process according to whether a proposed step improves the position error.

The initial damping value can be changed with:

damping=1.0

and the scaling factor with:

damping_scale=0.5

For example:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    method="lm",
    damping=0.5,
    damping_scale=0.5,
)

Levenberg-Marquardt is generally a useful first choice for numerical position IK because damping improves behavior near poorly conditioned configurations.

Newton-Raphson

The Newton method can be selected with:

method="newton"

For example:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    method="newton",
)

The method uses the end-effector position Jacobian and computes an update based on its inverse or pseudoinverse.

Conceptually:

\[ \Delta q = J^\dagger e. \]

Newton-type methods can converge rapidly when the initial guess is appropriate, but their behavior may be more sensitive to singularities, poor initial guesses, or joint constraints.

Cyclic Coordinate Descent

CCD can be selected with:

method="ccd"

For example:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    method="ccd",
)

CCD does not construct or invert a Jacobian matrix.

Instead, it updates one joint at a time, starting from the joint closest to the end-effector and moving toward the base.

For revolute joints, the algorithm rotates the corresponding joint to move the end-effector toward the target in the plane perpendicular to the joint axis.

For prismatic joints, the update moves the joint along its translation axis.

A single CCD iteration corresponds to one complete sweep through all robot joints from joint n back to joint 1.

CCD can be useful when a Jacobian-free iterative approach is preferred, although its convergence is generally linear.

Initial guess and joint limits

Inverse kinematics is generally not unique, and numerical methods depend on their starting configuration.

The initial guess can be supplied with:

q0=[...]

For example:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.2, -0.1],
)

The number of values in q0 must match:

robot.dof

Random initialization

If q0=None, moro generates a random initial configuration inside the active joint limits:

solution = solve_position_ik(
    robot,
    target,
    q0=None,
)

A reproducible random initial guess can be obtained with:

solution = solve_position_ik(
    robot,
    target,
    q0=None,
    random_state=42,
)

Using the same integer seed produces the same initial random configuration without changing NumPy’s global random state.

For repeatable examples and tests, providing an explicit q0 is usually preferable.

Joint limits

By default, the solver uses:

robot.joint_limits

For example:

robot.joint_limits = [
    (-1.5, 1.5),
    (-2.0, 2.0),
]

These limits are then used automatically:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
)

Limits can also be overridden for a specific solve:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    joint_limits=[
        (-1.0, 1.0),
        (-1.5, 1.5),
    ],
)

The temporary limits do not modify the Robot object.

Every joint update is clipped to the active limits.

If an explicitly provided q0 lies outside them, the initial configuration is clipped before the iterative process begins.

Because the default limits stored by Robot are convenience values rather than physical constraints, actual robot limits should be defined before solving constrained IK problems.

Convergence and stagnation

The solver is considered converged when:

\[ \left\| p_d-p(q) \right\| < \mathrm{tol}. \]

The default tolerance is:

tol=1e-6

For example:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    tol=1e-8,
)

Maximum iterations

The maximum number of iterations can be controlled with:

max_iter=...

If it is not specified, the defaults are:

Newton-Raphson       100
Levenberg-Marquardt  100
CCD                   500

For example:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
    method="ccd",
    max_iter=1000,
)

Stagnation detection

A numerical method can stop making useful progress before reaching the maximum number of iterations.

moro detects two forms of stagnation.

The first uses:

step_tol

to detect joint updates that have become too small.

The second uses:

error_change_tol

to detect cases where the position error stops improving.

The number of consecutive stalled iterations required before termination is controlled by:

stagnation_iterations

The defaults are:

step_tol=1e-12
error_change_tol=1e-12
stagnation_iterations=5

These options normally do not need to be changed for basic use, but they can be useful when diagnosing difficult IK problems.

Inspecting an IK solution

solve_position_ik() returns an IKSolution.

The main fields are:

solution.q
solution.converged
solution.iterations
solution.error
solution.method
solution.residual
solution.message

Joint configuration

The final solver state is:

solution.q

For a two-joint robot this may look like:

[0.98, -1.32]

Convergence status

Always inspect:

solution.converged

before treating the returned joint vector as a valid IK solution.

For example:

if solution.converged:
    q_solution = solution.q

Final error

The final Cartesian error norm is:

solution.error

and corresponds to:

\[ \left\| p_d-p(q) \right\|. \]

Residual

The complete Cartesian residual is available through:

solution.residual

and is defined as:

\[ r = p_d-p(q). \]

When available, it contains three values:

[rx, ry, rz]

and:

\[ \text{solution.error} = \left\|\text{solution.residual}\right\|. \]

If a numerical failure prevents a finite residual from being evaluated, residual can be None and error is reported as infinity.

Iterations

The number of completed global solver steps is:

solution.iterations

For Newton and LM, one iteration corresponds to one attempted global joint update.

For CCD, one iteration corresponds to one complete sweep through all joints.

If the initial guess already satisfies the requested tolerance:

solution.iterations == 0

Outcome message

A short description of the solver termination condition is stored in:

solution.message

Possible outcomes include successful convergence, maximum iterations, stagnation, or a numerical failure.

This makes the following pattern useful:

if solution.converged:
    print("Solution:", solution.q)
else:
    print("IK failed:", solution.message)

Solving a position trajectory

moro can also solve a sequence of Cartesian position targets.

Use:

from moro.inverse_kinematics import solve_position_trajectory

Consider:

targets = [
    [1.5, 0.2, 0.0],
    [1.4, 0.4, 0.0],
    [1.2, 0.6, 0.0],
]

A trajectory can be solved with:

trajectory = solve_position_trajectory(
    robot,
    targets,
    q0=[0.1, 0.1],
)

Unlike solve_position_ik(), the trajectory function requires an explicit q0.

The target sequence must contain one 3D position per row:

(m, 3)

A single three-element vector is not accepted by this function; use solve_position_ik() for one target.

Sequential initialization

Targets are processed in order.

The initial configuration is used for the first target:

target 0
    ↑
   q0

If that target converges, its solution is reused as the initial guess for the next target:

target 0 → q0
              ↓
target 1 → q solution from target 0
              ↓
target 2 → q solution from target 1
              ↓
...

This usually improves local continuity and convergence when neighboring Cartesian targets are close to each other.

For symbolic models, parameters are passed in the same way as for a single IK problem:

trajectory = solve_position_trajectory(
    robot,
    targets,
    q0=[0.1, 0.1],
    parameters={
        l1: 1.0,
        l2: 1.0,
    },
)

The same solver options can also be used:

trajectory = solve_position_trajectory(
    robot,
    targets,
    q0=[0.1, 0.1],
    method="lm",
    tol=1e-8,
)

Inspecting a trajectory solution

solve_position_trajectory() returns an IKTrajectorySolution.

The main fields are:

trajectory.solutions
trajectory.converged
trajectory.failed_index
trajectory.message

It also provides convenience properties:

trajectory.qs
trajectory.errors
trajectory.iterations

Individual solutions

Each processed target produces an IKSolution stored in:

trajectory.solutions

For example:

for solution in trajectory.solutions:
    print(solution.q, solution.error)

Joint configurations

All processed joint configurations are available directly through:

trajectory.qs

For example:

[
    [q11, q12],
    [q21, q22],
    [q31, q32],
]

These configurations can be used directly by workflows such as robot animation.

Errors and iteration counts

The per-target final errors are:

trajectory.errors

and the iteration counts are:

trajectory.iterations

Failed target

If every target converges:

trajectory.converged
# True

trajectory.failed_index
# None

If a target fails, the trajectory stops at that point.

The index of the failed target is:

trajectory.failed_index

and the corresponding failed IKSolution is included as the last element in:

trajectory.solutions

For example:

if not trajectory.converged:
    i = trajectory.failed_index
    print("Failed target:", i)
    print(trajectory.solutions[i].message)

Handling unsuccessful solutions

Numerical inverse kinematics is not guaranteed to converge.

A failure can occur because of:

an unreachable target;
restrictive joint limits;
an unsuitable initial guess;
a singular or poorly conditioned configuration;
solver stagnation;
unresolved symbolic parameters;
invalid numerical inputs;
numerical failures during forward-kinematics or Jacobian evaluation.

A returned IKSolution with:

solution.converged == False

should therefore not be interpreted as a successful solution simply because solution.q contains finite joint values.

The recommended pattern is:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
)

if solution.converged:
    print("q =", solution.q)
else:
    print(solution.message)
    print("Final error:", solution.error)

For difficult problems, useful actions include:

trying a different initial guess;
checking whether the target is inside the workspace;
checking physical joint limits;
increasing max_iter;
trying another solver;
verifying that all geometric parameters have numerical values.

Changing tolerances should be done with care. A looser tolerance may report convergence farther from the requested target.

Validating a solution with forward kinematics

An inverse-kinematics solution can be checked independently using forward kinematics.

Suppose:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.1, 0.1],
)

If the solution converged, build a substitution dictionary:

values = dict(zip(robot.qs, solution.q))

Then evaluate the end-effector position:

p = robot.T[:3, 3].subs(values)

For a symbolic robot, also include its geometric parameters:

values.update({
    l1: 1.0,
    l2: 1.0,
})

p = robot.T[:3, 3].subs(values).evalf()

The resulting position should be close to the requested target.

This is also a useful way to verify the meaning of:

solution.residual

which represents:

\[ p_d-p(q). \]

A worked example

Consider a symbolic planar 2R robot:

from moro import Robot
from moro.abc import q1, q2, l1, l2
from moro.inverse_kinematics import solve_position_ik

robot = Robot(
    (l1, 0, 0, q1, "r"),
    (l2, 0, 0, q2, "r"),
)

Set physical joint limits:

from sympy import pi

robot.joint_limits = [
    (-pi, pi),
    (-pi, pi),
]

Define a target:

target = [1.2, 0.8, 0.0]

and numerical geometric parameters:

parameters = {
    l1: 1.0,
    l2: 1.0,
}

Now solve the problem using Levenberg-Marquardt:

solution = solve_position_ik(
    robot,
    target,
    q0=[0.2, 0.2],
    parameters=parameters,
    method="lm",
    tol=1e-8,
)

Inspect the result:

print(solution.converged)
print(solution.q)
print(solution.error)
print(solution.message)

If convergence was successful, validate the joint configuration:

values = dict(zip(robot.qs, solution.q))
values.update(parameters)

p_solution = robot.T[:3, 3].subs(values).evalf()
p_solution

The evaluated position should agree with:

target

within the requested tolerance.

The same robot can now be used to solve several nearby targets:

from moro.inverse_kinematics import solve_position_trajectory

targets = [
    [1.2, 0.8, 0.0],
    [1.1, 0.9, 0.0],
    [1.0, 1.0, 0.0],
    [0.9, 1.1, 0.0],
]

trajectory = solve_position_trajectory(
    robot,
    targets,
    q0=solution.q,
    parameters=parameters,
    method="lm",
    tol=1e-8,
)

Check the global result:

trajectory.converged

and inspect the joint sequence:

trajectory.qs

These configurations can then be passed to the visualization tools to animate the resulting robot motion.

Notes and limitations

The inverse-kinematics capabilities in the current version of moro are intentionally focused on numerical position IK for serial manipulators.

Keep the following points in mind:

only Cartesian position targets [x, y, z] are currently supported;
orientation constraints are not part of the IK objective;
the solver may return different valid configurations for the same target;
the result can depend strongly on the initial guess;
joint limits are enforced during the numerical updates;
symbolic non-joint parameters must be assigned through parameters;
Levenberg-Marquardt is the default solver;
Newton and LM use the linear part of the geometric Jacobian;
CCD is Jacobian-free and supports both revolute and prismatic joints;
a non-converged result should always be inspected through converged, error, and message;
solve_position_trajectory() solves an existing sequence of Cartesian targets but does not generate that trajectory.

In particular, trajectory IK does not currently perform:

Cartesian interpolation;
timing or velocity assignment;
trajectory smoothing;
collision avoidance;
motion planning;
optimization over IK branches;
guaranteed global branch continuity.

Reusing each converged configuration as the next initial guess often produces locally continuous results, but this is not a formal guarantee of globally continuous joint motion.

Inverse Kinematics

Solving a position target

Providing numerical model parameters

Choosing a solver

Levenberg-Marquardt

Newton-Raphson

Cyclic Coordinate Descent

Initial guess and joint limits

Random initialization

Joint limits

Convergence and stagnation

Maximum iterations

Stagnation detection

Inspecting an IK solution

Joint configuration

Convergence status

Final error

Residual

Iterations

Outcome message

Solving a position trajectory

Sequential initialization

Inspecting a trajectory solution

Individual solutions

Joint configurations

Errors and iteration counts

Failed target

Handling unsuccessful solutions

Validating a solution with forward kinematics

A worked example

Notes and limitations

See also