BB.IK.DLS.Motion (bb_ik_dls v0.6.0)

Copy Markdown View Source

Convenience functions for DLS-based motion.

This module wraps BB.Motion with the DLS solver pre-configured, providing a simpler API for common inverse kinematics motion tasks.

Single Target

# Move end-effector to target position
case BB.IK.DLS.Motion.move_to(MyRobot, :gripper, {0.3, 0.2, 0.1},
       source_link: :base_link
     ) do
  {:ok, meta} -> IO.puts("Reached in #{meta.iterations} iterations")
  {:error, error} -> IO.puts("Failed: #{Exception.message(error)}")
end

# Just solve without moving (for validation)
case BB.IK.DLS.Motion.solve(MyRobot, :gripper, {0.3, 0.2, 0.1},
       source_link: :base_link
     ) do
  {:ok, positions, meta} -> IO.inspect(positions)
  {:error, error} -> IO.puts("Unreachable: #{Exception.message(error)}")
end

Multiple Targets (for gait, coordinated motion)

targets = %{left_foot: {0.1, 0.0, 0.0}, right_foot: {-0.1, 0.0, 0.0}}

case BB.IK.DLS.Motion.move_to_multi(MyRobot, targets, source_link: :base_link) do
  {:ok, results} -> IO.puts("All targets reached")
  {:error, error} -> IO.puts("Failed: #{Exception.message(error)}")
end

In Custom Commands

use BB.Command

@impl BB.Command
def handle_command(%{target: target}, context, state) do
  case BB.IK.DLS.Motion.move_to(context, :gripper, target, source_link: :base_link) do
    {:ok, meta} ->
      {:stop, :normal, %{state | result: %{residual: meta.residual}}}

    {:error, error} ->
      {:stop, :normal, %{state | result: {:error, error}}}
  end
end

@impl BB.Command
def result(%{result: {:error, _} = error}), do: error
def result(%{result: result}), do: {:ok, result}

Summary

Types

meta()
motion_result()
multi_motion_result()
multi_solve_result()
positions()
robot_or_context()
solve_result()
target()
targets()

Functions

move_to(robot_or_context, target_link, target, opts \\ [])

Move an end-effector to a target position using DLS.

move_to_multi(robot_or_context, targets, opts \\ [])

Move multiple end-effectors to target positions simultaneously using DLS.

solve(robot_or_context, target_link, target, opts \\ [])

Solve DLS without moving the robot.

solve_multi(robot_or_context, targets, opts \\ [])

Solve DLS for multiple targets without moving the robot.

Types

meta()

@type meta() :: BB.IK.Solver.meta()

motion_result()

@type motion_result() :: BB.Motion.motion_result()

multi_motion_result()

@type multi_motion_result() :: BB.Motion.multi_motion_result()

multi_solve_result()

@type multi_solve_result() :: BB.Motion.multi_solve_result()

positions()

@type positions() :: BB.IK.Solver.positions()

robot_or_context()

@type robot_or_context() :: module() | BB.Command.Context.t()

solve_result()

@type solve_result() :: BB.Motion.solve_result()

target()

@type target() :: BB.IK.Solver.target()

targets()

@type targets() :: %{required(atom()) => target()}

Functions

move_to(robot_or_context, target_link, target, opts \\ [])

@spec move_to(robot_or_context(), atom(), target(), keyword()) :: motion_result()

Move an end-effector to a target position using DLS.

This is a convenience wrapper around BB.Motion.move_to/4 with the DLS solver pre-configured.

Options

DLS-specific:

  • :max_iterations - Maximum DLS iterations (default: 100)
  • :tolerance - Convergence tolerance in metres (default: 1.0e-4)
  • :orientation_tolerance - Orientation tolerance in radians (default: 0.01)
  • :lambda - Damping factor (default: 0.5)
  • :adaptive_damping - Adapt lambda based on error (default: true)
  • :step_size - Max joint update per iteration (default: 0.1)
  • :respect_limits - Whether to clamp to joint limits (default: true)

Motion:

  • :source_link - Link the chain starts at (required, no default). Pass BB.Robot.root_link(robot) if you mean the whole tree
  • :delivery - How to send actuator commands. :pubsub (default) publishes each command and waits for the actuator to accept it, reporting the first refusal; :direct casts to each actuator and waits for nothing, so a refusal is never reported
  • :timeout - How long to wait for each actuator to accept its command, in milliseconds (default 5000). Unused under :direct. A timeout exits the caller, as GenServer.call/3 does

Returns

  • {:ok, meta} - Successfully moved; meta contains solver info
  • {:error, error} - Either the target couldn't be solved, in which case the error is a BB.Error.Kinematics struct, or an actuator refused the command it was sent, in which case it is the actuator's own error

Examples

BB.IK.DLS.Motion.move_to(MyRobot, :gripper, {0.3, 0.2, 0.1},
  source_link: :base_link
)

BB.IK.DLS.Motion.move_to(context, :gripper, target,
  source_link: :base_link,
  delivery: :direct,
  max_iterations: 200,
  tolerance: 0.001
)

move_to_multi(robot_or_context, targets, opts \\ [])

@spec move_to_multi(robot_or_context(), targets(), keyword()) :: multi_motion_result()

Move multiple end-effectors to target positions simultaneously using DLS.

Useful for coordinated motion like walking gaits. Each target is solved independently using DLS and all actuator commands are sent together.

Options

Same as move_to/4.

Returns

  • {:ok, results} - All targets solved; results is a map of link → {:ok, positions, meta}
  • {:error, %BB.Error.Kinematics.MultiFailed{}} - A target failed to solve. The error names the link that failed, carries the underlying kinematics error, and keeps the results of the targets solved before it
  • {:error, error} - Every target solved, but an actuator refused the command it was sent, so the failure arrives as the actuator's own error rather than wrapped in MultiFailed

Examples

alias BB.Error.Kinematics.MultiFailed

targets = %{
  left_foot: {0.1, 0.0, 0.0},
  right_foot: {-0.1, 0.0, 0.0}
}

case BB.IK.DLS.Motion.move_to_multi(MyRobot, targets, source_link: :base_link) do
  {:ok, results} ->
    IO.puts("All limbs positioned")

  {:error, %MultiFailed{failed_link: link} = error} ->
    IO.puts("Failed to reach #{link}: #{Exception.message(error)}")

  {:error, error} ->
    IO.puts("An actuator refused: #{Exception.message(error)}")
end

solve(robot_or_context, target_link, target, opts \\ [])

@spec solve(robot_or_context(), atom(), target(), keyword()) :: solve_result()

Solve DLS without moving the robot.

Useful for validating targets are reachable before committing to motion, or for planning multi-step movements.

Options

  • :max_iterations - Maximum DLS iterations (default: 100)
  • :tolerance - Convergence tolerance in metres (default: 1.0e-4)
  • :orientation_tolerance - Orientation tolerance in radians (default: 0.01)
  • :lambda - Damping factor (default: 0.5)
  • :adaptive_damping - Adapt lambda based on error (default: true)
  • :step_size - Max joint update per iteration (default: 0.1)
  • :respect_limits - Whether to clamp to joint limits (default: true)
  • :source_link - Link the chain starts at (required, no default)

Returns

  • {:ok, positions, meta} - Successfully solved
  • {:error, error} - Failed to solve; a struct from BB.Error.Kinematics

Examples

case BB.IK.DLS.Motion.solve(MyRobot, :gripper, target, source_link: :base_link) do
  {:ok, positions, %{reached: true}} ->
    IO.puts("Target reachable")
    IO.inspect(positions)

  {:ok, _positions, %{reached: false, residual: residual}} ->
    IO.puts("Close but not exact, residual: #{residual}m")

  {:error, %BB.Error.Kinematics.NoSolution{}} ->
    IO.puts("Failed to converge")
end

solve_multi(robot_or_context, targets, opts \\ [])

@spec solve_multi(robot_or_context(), targets(), keyword()) :: multi_solve_result()

Solve DLS for multiple targets without moving the robot.

Useful for validating that all targets in a coordinated motion are reachable.

Options

Same as solve/4.

Returns

  • {:ok, results} - All targets solved
  • {:error, %BB.Error.Kinematics.MultiFailed{}} - A target failed. The error names the link that failed, carries the underlying kinematics error, and keeps the results of the targets solved before it

Examples

alias BB.Error.Kinematics.MultiFailed

targets = %{left_foot: {0.1, 0.0, 0.0}, right_foot: {-0.1, 0.0, 0.0}}

case BB.IK.DLS.Motion.solve_multi(MyRobot, targets, source_link: :base_link) do
  {:ok, results} ->
    Enum.each(results, fn {link, {:ok, _pos, meta}} ->
      IO.puts("#{link}: #{meta.residual}m residual")
    end)

  {:error, %MultiFailed{failed_link: link} = error} ->
    IO.puts("#{link} is unreachable: #{Exception.message(error)}")
end