improvement!: match bb's synchronous set_position changes

AGENTS.md

@@ -32,7 +32,7 @@ lib/bb/ik/
 | `BB.IK.DLS` | Main entry point. Implements `BB.IK.Solver` behaviour. Provides `solve/6` and `solve_and_update/6` |
 | `BB.IK.DLS.Algorithm` | Core iteration using BB's analytical Jacobians and an Nx damped pseudoinverse update |
 | `BB.IK.DLS.Motion` | Wraps `BB.Motion` with DLS pre-configured. Convenience API for `move_to`, `solve`, and multi-target operations |
-| `BB.IK.DLS.Tracker` | GenServer for continuous position tracking at configurable update rates |
+| `BB.IK.DLS.Tracker` | GenServer for continuous position tracking at configurable update rates. Needs position feedback on the tracked joints — each solve seeds from the robot's configuration, which only `JointState` messages write |
 
 ## Build and Test Commands
 
@@ -96,7 +96,8 @@ Tests use robot fixtures from `test/support/test_robots.ex` (compiled via `elixi
 2. Position-only solves use `BB.Robot.Kinematics.position_jacobian/4`; orientation-constrained solves use `BB.Robot.Kinematics.jacobian/4`
 3. Adaptive damping adjusts λ by ×0.9 on error reduction, ×1.5 on increase
 4. Lambda is clamped to [1.0e-6, 100.0]
-5. The `Tracker` GenServer uses `:direct` delivery by default for low latency
+5. The `Tracker` GenServer uses `:direct` delivery by default for low latency;
+   under `:pubsub` each command is a blocking call, so a timeout exits the tracker
 
 ## Licensing headers
 

README.md

@@ -81,14 +81,14 @@ target = {Vec3.new(0.3, 0.2, 0.1), {:axis, Vec3.unit_z()}}
 
 ```elixir
 # Move end-effector using BB.Motion integration
-case BB.IK.DLS.Motion.move_to(MyRobot, :gripper, {0.3, 0.2, 0.1}) do
+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, reason, _meta} -> IO.puts("Failed: #{reason}")
+  {:error, error} -> IO.puts("Failed: #{Exception.message(error)}")
 end
 
 # Coordinated multi-limb motion
 targets = %{left_foot: {0.1, 0.0, 0.0}, right_foot: {-0.1, 0.0, 0.0}}
-BB.IK.DLS.Motion.move_to_multi(MyRobot, targets)
+BB.IK.DLS.Motion.move_to_multi(MyRobot, targets, source_link: :base_link)
 ```
 
 ### Continuous Tracking
@@ -98,6 +98,7 @@ BB.IK.DLS.Motion.move_to_multi(MyRobot, targets)
 {:ok, tracker} = BB.IK.DLS.Tracker.start_link(
   robot: MyRobot,
   target_link: :gripper,
+  source_link: :base_link,
   initial_target: {0.3, 0.2, 0.1},
   update_rate: 30
 )

lib/bb/ik/dls.ex

@@ -178,6 +178,12 @@ defmodule BB.IK.DLS do
   Convenience function that calls `solve/6` and applies the result
   to the given `BB.Robot.State`.
 
+  Meant for a state of your own - one from `BB.Robot.State.new/1`, stepped
+  through a planned motion. A running robot's state belongs to its sensors,
+  which write it from `BB.Message.Sensor.JointState` messages, and writing a
+  solved configuration into it claims the joints have arrived somewhere they
+  have only been asked to go.
+
   ## Returns
 
   Same as `solve/6`, but on success the state's ETS table is updated.

lib/bb/ik/dls/motion.ex

@@ -12,26 +12,28 @@ defmodule BB.IK.DLS.Motion do
   ## Single Target
 
       # Move end-effector to target position
-      case BB.IK.DLS.Motion.move_to(MyRobot, :gripper, {0.3, 0.2, 0.1}) do
+      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, reason, _meta} -> IO.puts("Failed: \#{reason}")
+        {: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, reason, _meta} -> IO.puts("Unreachable: \#{reason}")
+        {: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) do
+      case BB.IK.DLS.Motion.move_to_multi(MyRobot, targets, source_link: :base_link) do
         {:ok, results} -> IO.puts("All targets reached")
-        {:error, failed, reason, _} -> IO.puts("Failed: \#{failed}: \#{reason}")
+        {:error, error} -> IO.puts("Failed: \#{Exception.message(error)}")
       end
 
   ## In Custom Commands
@@ -40,12 +42,12 @@ defmodule BB.IK.DLS.Motion do
 
       @impl BB.Command
       def handle_command(%{target: target}, context, state) do
-        case BB.IK.DLS.Motion.move_to(context, :gripper, target) 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, reason, _meta} ->
-            {:stop, :normal, %{state | result: {:error, reason}}}
+          {:error, error} ->
+            {:stop, :normal, %{state | result: {:error, error}}}
         end
       end
 
@@ -64,8 +66,8 @@ defmodule BB.IK.DLS.Motion do
   @type robot_or_context :: module() | Context.t()
   @type targets :: %{atom() => target()}
 
-  @type motion_result :: {:ok, meta()} | {:error, atom(), meta()}
-  @type solve_result :: {:ok, positions(), meta()} | {:error, atom(), meta()}
+  @type motion_result :: Motion.motion_result()
+  @type solve_result :: Motion.solve_result()
   @type multi_motion_result :: Motion.multi_motion_result()
   @type multi_solve_result :: Motion.multi_solve_result()
 
@@ -96,12 +98,20 @@ defmodule BB.IK.DLS.Motion do
   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), `:direct`, or `:sync`
+  - `: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, reason, meta}` - Failed to reach target
+  - `{: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
 
@@ -142,7 +152,7 @@ defmodule BB.IK.DLS.Motion do
   ## Returns
 
   - `{:ok, positions, meta}` - Successfully solved
-  - `{:error, reason, meta}` - Failed to solve
+  - `{:error, error}` - Failed to solve; a struct from `BB.Error.Kinematics`
 
   ## Examples
 
@@ -154,7 +164,7 @@ defmodule BB.IK.DLS.Motion do
         {:ok, _positions, %{reached: false, residual: residual}} ->
           IO.puts("Close but not exact, residual: \#{residual}m")
 
-        {:error, :no_solution, _meta} ->
+        {:error, %BB.Error.Kinematics.NoSolution{}} ->
           IO.puts("Failed to converge")
       end
   """
@@ -177,21 +187,31 @@ defmodule BB.IK.DLS.Motion do
   ## Returns
 
   - `{:ok, results}` - All targets solved; results is a map of link → `{:ok, positions, meta}`
-  - `{:error, failed_link, reason, results}` - A target failed
+  - `{: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) do
+      case BB.IK.DLS.Motion.move_to_multi(MyRobot, targets, source_link: :base_link) do
         {:ok, results} ->
           IO.puts("All limbs positioned")
 
-        {:error, failed_link, reason, _results} ->
-          IO.puts("Failed to reach \#{failed_link}: \#{reason}")
+        {: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
   """
   @spec move_to_multi(robot_or_context(), targets(), keyword()) :: multi_motion_result()
@@ -212,20 +232,24 @@ defmodule BB.IK.DLS.Motion do
   ## Returns
 
   - `{:ok, results}` - All targets solved
-  - `{:error, failed_link, reason, results}` - A target failed
+  - `{: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) do
+      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, failed_link, reason, _results} ->
-          IO.puts("\#{failed_link} unreachable: \#{reason}")
+        {:error, %MultiFailed{failed_link: link} = error} ->
+          IO.puts("\#{link} is unreachable: \#{Exception.message(error)}")
       end
   """
   @spec solve_multi(robot_or_context(), targets(), keyword()) :: multi_solve_result()
@@ -255,7 +279,7 @@ defmodule BB.IK.DLS.Motion do
       )
 
     opts
-    |> Keyword.take([:delivery])
+    |> Keyword.take([:delivery, :timeout])
     |> Keyword.merge(dls_opts)
     |> Keyword.put(:solver, DLS)
     |> Keyword.put(:source_link, source_link)

lib/bb/ik/dls/tracker.ex

@@ -37,7 +37,11 @@ defmodule BB.IK.DLS.Tracker do
   - `:source_link` - Link the chain starts at (required, no default)
   - `:initial_target` - Starting target position (required)
   - `:update_rate` - Solve frequency in Hz (default: 20)
-  - `:delivery` - Actuator command delivery: `:direct` (default), `:pubsub`, `:sync`
+  - `:delivery` - Actuator command delivery. `:direct` (default) casts each
+    command and waits for nothing; `:pubsub` publishes it and waits for the
+    actuator to accept it, which blocks the loop for as long as that takes
+  - `:timeout` - How long to wait for each actuator under `:pubsub`, in
+    milliseconds (default 5000). Ignored under `:direct`
   - `:max_iterations` - Maximum DLS iterations per update (default: 100)
   - `:tolerance` - Convergence tolerance in metres (default: 1.0e-4)
   - `:lambda` - Damping factor (default: 0.5)
@@ -46,28 +50,57 @@ defmodule BB.IK.DLS.Tracker do
   - `:respect_limits` - Whether to clamp to joint limits (default: true)
   - `:name` - Optional GenServer name for registration
 
+  ## Position feedback is a prerequisite
+
+  Every solve starts from the robot's current configuration, which is written
+  from `BB.Message.Sensor.JointState` messages and from nothing else - a
+  commanded position is not a measured one. A joint that nothing reports on
+  therefore stays at its initial configuration, and the tracker re-solves from
+  that same frozen pose on every tick.
+
+  That does not diverge: a solve is a function of its seed and its target, so a
+  frozen seed still yields an absolute joint configuration that reaches the
+  target. What it loses is the warm start, and with it the continuity between
+  ticks. Each solve pays the iteration count a distant seed needs rather than
+  the handful a nearby one does, and converges less reliably near singularities
+  - and because `:step_size` caps how far the configuration moves per iteration,
+  `:max_iterations` bounds how far the answer can travel from its seed at all.
+  The one that bites is that the answer stops depending on the path taken to
+  reach the target, which leaves the arm free to change solution branch from one
+  tick to the next: a redundant arm can be asked to swing between two equally
+  valid postures inside a single tick period.
+
+  So the tracked joints want something that reports where they are: an encoder,
+  a driver that declares `:position_feedback` through
+  `c:BB.Actuator.capabilities/1`, or `BB.Sensor.OpenLoopPositionEstimator`
+  interpolating from the actuator's own `BeginMotion` messages. `BB.Dsl` warns
+  at compile time about a driven joint with none of the three, and simulation
+  supplies an estimator itself.
+
   ## Notes
 
-  - Uses `:direct` delivery by default for low latency
+  - Uses `:direct` delivery by default for low latency. Under `:pubsub` a solve
+    that outlives `:timeout` exits the tracker, as `GenServer.call/3` does
   - Continues tracking even if individual solves fail (best-effort)
   - Call `stop/1` to cleanly terminate tracking
   """
 
   use GenServer
 
+  require Logger
+
   alias BB.IK.DLS
   alias BB.Motion
   alias BB.Robot.Runtime
-  alias BB.Robot.State, as: RobotState
 
   defstruct [
     :robot_module,
     :robot,
-    :robot_state,
     :target_link,
     :source_link,
     :target,
     :delivery,
+    :timeout,
     :solver_opts,
     :update_rate,
     :loop,
@@ -105,7 +138,7 @@ defmodule BB.IK.DLS.Tracker do
   end
 
   @doc """
-  Stop tracking and return final positions.
+  Stop tracking and return the configuration the last solve arrived at.
 
   ## Options
 
@@ -124,6 +157,7 @@ defmodule BB.IK.DLS.Tracker do
 
     update_rate = Keyword.get(opts, :update_rate, @default_update_rate)
     delivery = Keyword.get(opts, :delivery, @default_delivery)
+    timeout = Keyword.get(opts, :timeout)
 
     solver_opts =
       Keyword.take(opts, [
@@ -138,16 +172,15 @@ defmodule BB.IK.DLS.Tracker do
       |> Keyword.reject(fn {_k, v} -> is_nil(v) end)
 
     robot = Runtime.get_robot(robot_module)
-    robot_state = Runtime.get_robot_state(robot_module)
 
     state = %__MODULE__{
       robot_module: robot_module,
       robot: robot,
-      robot_state: robot_state,
       target_link: target_link,
       source_link: source_link,
       target: initial_target,
       delivery: delivery,
+      timeout: timeout,
       solver_opts: solver_opts,
       update_rate: update_rate,
       loop:
@@ -205,18 +238,22 @@ defmodule BB.IK.DLS.Tracker do
     {:noreply, %{state | loop: loop}}
   end
 
+  # Solving and sending separately rather than through `BB.Motion.move_to/4`,
+  # because the tracker has to report the configuration it solved for and
+  # `move_to/4` keeps that to itself.
   defp do_solve_and_send(state) do
-    motion_opts =
+    solve_opts =
       state.solver_opts
       |> Keyword.put(:solver, DLS)
       |> Keyword.put(:source_link, state.source_link)
-      |> Keyword.put(:delivery, state.delivery)
 
-    case Motion.move_to(state.robot_module, state.target_link, state.target, motion_opts) do
-      {:ok, meta} ->
+    case Motion.solve_only(state.robot_module, state.target_link, state.target, solve_opts) do
+      {:ok, positions, meta} ->
+        send_positions(state, positions)
+
         %{
           state
-          | last_positions: RobotState.get_all_configurations(state.robot_state),
+          | last_positions: positions,
             last_meta: meta,
             last_update: DateTime.utc_now()
         }
@@ -231,6 +268,23 @@ defmodule BB.IK.DLS.Tracker do
     end
   end
 
+  defp send_positions(state, positions) do
+    opts =
+      [delivery: state.delivery, timeout: state.timeout]
+      |> Keyword.reject(fn {_key, value} -> is_nil(value) end)
+
+    case Motion.send_positions(state.robot_module, positions, opts) do
+      :ok ->
+        :ok
+
+      {:error, error} ->
+        Logger.warning(
+          "Tracking #{inspect(state.target_link)} on #{inspect(state.robot_module)}: " <>
+            "actuator refused its position command: #{Exception.message(error)}"
+        )
+    end
+  end
+
   defp send_hold_commands(state) do
     Enum.each(state.robot.actuators, fn {name, _info} ->
       BB.Actuator.hold!(state.robot_module, name)