BB.IK.FABRIK.Motion (bb_ik_fabrik v0.8.0)

Copy Markdown View Source

Convenience functions for FABRIK-based motion.

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

Single Target

# Move end-effector to target position
case BB.IK.FABRIK.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.FABRIK.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.FABRIK.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.FABRIK.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 FABRIK.

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

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

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

Solve FABRIK without moving the robot.

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

Solve FABRIK 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 FABRIK.

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

Options

FABRIK-specific:

  • :max_iterations - Maximum FABRIK iterations (default: 50)
  • :tolerance - Convergence tolerance in metres (default: 1.0e-4)
  • :respect_limits - Whether to clamp to joint limits (default: true)
  • :source_link - Link the chain starts at (required, no default). The chain must contain no :planar or :floating joint — FABRIK cannot solve those, so scope past them or use a Jacobian-based solver such as BB.IK.DLS

Motion:

  • :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.FABRIK.Motion.move_to(MyRobot, :gripper, {0.3, 0.2, 0.1}, source_link: :base_link)

BB.IK.FABRIK.Motion.move_to(context, :gripper, target,
  source_link: :base_link,
  delivery: :direct,
  max_iterations: 100,
  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 FABRIK.

Useful for coordinated motion like walking gaits. Each target is solved independently using FABRIK 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.FABRIK.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 FABRIK without moving the robot.

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

Options

  • :max_iterations - Maximum FABRIK iterations (default: 50)
  • :tolerance - Convergence tolerance in metres (default: 1.0e-4)
  • :respect_limits - Whether to clamp to joint limits (default: true)
  • :source_link - Link the chain starts at (required, no default). The chain must contain no :planar or :floating joint — FABRIK cannot solve those, so scope past them or use a Jacobian-based solver such as BB.IK.DLS

Returns

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

Examples

case BB.IK.FABRIK.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.Unreachable{}} ->
    IO.puts("Target is out of reach")
end

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

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

Solve FABRIK 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.FABRIK.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