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

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}) do
  {:ok, meta} -> IO.puts("Reached in #{meta.iterations} iterations")
  {:error, reason, _meta} -> IO.puts("Failed: #{reason}")
end

# Just solve without moving (for validation)
case BB.IK.FABRIK.Motion.solve(MyRobot, :gripper, {0.3, 0.2, 0.1}) do
  {:ok, positions, meta} -> IO.inspect(positions)
  {:error, reason, _meta} -> IO.puts("Unreachable: #{reason}")
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) do
  {:ok, results} -> IO.puts("All targets reached")
  {:error, failed, reason, _} -> IO.puts("Failed: #{failed}: #{reason}")
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) do
    {:ok, meta} ->
      {:stop, :normal, %{state | result: %{residual: meta.residual}}}

    {:error, reason, _meta} ->
      {:stop, :normal, %{state | result: {:error, reason}}}
  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() :: {:ok, meta()} | {:error, atom(), meta()}

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() :: {:ok, positions(), meta()} | {:error, atom(), meta()}

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)

Motion:

  • :delivery - How to send actuator commands: :pubsub (default), :direct, or :sync

Returns

  • {:ok, meta} - Successfully moved; meta contains solver info
  • {:error, reason, meta} - Failed to reach target

Examples

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

BB.IK.FABRIK.Motion.move_to(context, :gripper, target,
  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, failed_link, reason, results} - A target failed

Examples

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) do
  {:ok, results} ->
    IO.puts("All limbs positioned")

  {:error, failed_link, reason, _results} ->
    IO.puts("Failed to reach #{failed_link}: #{reason}")
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)

Returns

  • {:ok, positions, meta} - Successfully solved
  • {:error, reason, meta} - Failed to solve

Examples

case BB.IK.FABRIK.Motion.solve(MyRobot, :gripper, target) 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, :unreachable, _meta} ->
    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, failed_link, reason, results} - A target failed

Examples

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) do
  {:ok, results} ->
    Enum.each(results, fn {link, {:ok, _pos, meta}} ->
      IO.puts("#{link}: #{meta.residual}m residual")
    end)

  {:error, failed_link, reason, _results} ->
    IO.puts("#{failed_link} unreachable: #{reason}")
end