BB.IK.DLS.Algorithm (bb_ik_dls v0.3.2)

Core Damped Least Squares iteration algorithm.

Implements the Levenberg-Marquardt style damped pseudoinverse:

Δθ = J^T (J J^T + λ²I)^(-1) e

where:

J is the Jacobian matrix
e is the pose error vector
λ is the damping factor
Δθ is the joint update

The damping factor prevents instability near kinematic singularities where J J^T becomes ill-conditioned.

Summary

Types

config()

iterate_result()

Functions

compute_update(jacobian, error, lambda)

Compute the damped pseudoinverse update.

iterate(robot, initial_positions, target_link, target_position, target_orientation, joint_names, config)

Run the DLS iteration loop until convergence or max iterations.

Types

config()

@type config() :: %{
  max_iterations: pos_integer(),
  tolerance: float(),
  orientation_tolerance: float(),
  lambda: float(),
  adaptive_damping: boolean(),
  step_size: float(),
  respect_limits: boolean()
}

iterate_result()

@type iterate_result() ::
  {:ok, map(), %{iterations: non_neg_integer(), converged: boolean()}}
  | {:error, :max_iterations,
     %{iterations: non_neg_integer(), positions: map()}}

Functions

compute_update(jacobian, error, lambda)

Compute the damped pseudoinverse update.

Uses the formula: Δθ = J^T (J J^T + λ²I)^(-1) e

This is a defn function for performance.

iterate(robot, initial_positions, target_link, target_position, target_orientation, joint_names, config)

@spec iterate(
  BB.Robot.t(),
  map(),
  atom(),
  BB.Math.Vec3.t(),
  BB.Math.Quaternion.t() | nil,
  [atom()],
  config()
) :: iterate_result()

Run the DLS iteration loop until convergence or max iterations.

Parameters

robot - The BB.Robot struct
initial_positions - Starting joint positions
target_link - End-effector link name
target_position - Target position as Vec3
target_orientation - Target orientation as Quaternion (or nil for position-only)
joint_names - Ordered list of joint names
config - Solver configuration map

Returns

{:ok, positions, meta} - Converged successfully
{:error, :max_iterations, meta} - Failed to converge