From 4fefbced8375e4111d804403c362373ee26a0260 Mon Sep 17 00:00:00 2001 From: James Harton Date: Tue, 12 May 2026 20:49:22 +1200 Subject: [PATCH] improvement: faithful, faster `demo_circle` MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The `demo_circle` command previously fired waypoint commands on a fixed 150 ms cadence regardless of whether the arm had reached the previous target, so the EE perpetually lagged one waypoint behind the planned trajectory and the traced path bore little resemblance to a circle. The start position `(0.15, 0.0, 0.20)` was also genuinely unreachable within the joint limits — three joints had to clamp to their limit, so the start move never converged. Changes to `command/demo_circle.ex`: - Move the start to `(0.25, 0.0, 0.30)`, which is well inside the joint-limited reachable workspace and lets the entire 30 mm-radius circle stay reachable. - Replace the fixed `Process.sleep/1` between waypoints with `wait_for_arrival/4`, which polls the position estimator's FK until the EE is within `settle_tolerance_m` of the commanded target or `settle_timeout_ms` elapses. - Solve IK directly via `DLS.solve` with the **previous IK solution** as the warm-start seed instead of relying on `Motion.move_to` (whose warm-start gets clobbered by the position estimator during arm motion). Adjacent waypoints differ by ~3° in joint space, so this brings IK convergence from ~17 iterations to 1-3 per waypoint. - Send the resulting joint positions via `BBMotion.send_positions/3` (bypassing IK on the second call). - Tune the IK solver for the warm-start case: `tolerance: 2e-3, step_size: 0.3, lambda: 0.1, max_iterations: 15`. The tight default tolerance (0.1 mm) just wasted iterations when the settle tolerance is 5 mm anyway. - New goal params: `settle_tolerance_m` (default 5 mm) and `settle_timeout_ms` (default 1500 ms). Drops the old `delay` param, which existed only because nothing else governed the cadence. Changes to `robot.ex`: - Bump every joint's `acceleration` limit from `720 °/s²` to `2160 °/s²`. This is consumed only by `BB.Sim.Actuator` when building its trapezoidal motion profile, so it speeds up the visual sim without affecting real-hardware behaviour (the Feetech STS3215 uses its own internal velocity profile). Halves the per-waypoint motion time. End-to-end the sim demo now completes in ~8 s (previously ~25 s with `wait_for_arrival` at the original limits, or producing a non-circular trace at the original 150 ms cadence) and the EE actually visits every commanded point. --- lib/bb/example/so101/command/demo_circle.ex | 125 +++++++++++++++----- lib/bb/example/so101/robot.ex | 12 +- 2 files changed, 104 insertions(+), 33 deletions(-) diff --git a/lib/bb/example/so101/command/demo_circle.ex b/lib/bb/example/so101/command/demo_circle.ex index f6dd1ec..a1204a4 100644 --- a/lib/bb/example/so101/command/demo_circle.ex +++ b/lib/bb/example/so101/command/demo_circle.ex @@ -9,12 +9,17 @@ defmodule BB.Example.SO101.Command.DemoCircle do First moves to a safe starting position, then traces a small circle in the XZ plane (vertical plane in front of the robot). + Each waypoint waits for the end-effector to actually arrive (within + `settle_tolerance_m`) before commanding the next one, so the traced path + stays faithful to the planned circle. + ## Goal Parameters Optional: - `radius` - Circle radius in metres (default: `0.03`) - `points` - Number of points around the circle (default: `16`) - - `delay` - Delay between points in milliseconds (default: `150`) + - `settle_tolerance_m` - EE distance from target to consider arrived (default: `5.0e-3`) + - `settle_timeout_ms` - Max wait per waypoint before continuing (default: `1500`) ## Usage @@ -23,47 +28,64 @@ defmodule BB.Example.SO101.Command.DemoCircle do """ use BB.Command + alias BB.IK.DLS alias BB.IK.DLS.Motion alias BB.Math.Vec3 - - # Safe starting position - comfortably within SO101 workspace - # X forward, Y=0 (centred), Z at reasonable height - # SO101 has ~350mm max reach, so 150mm forward is safe - @start_x 0.15 + alias BB.Motion, as: BBMotion + alias BB.Robot.Kinematics + alias BB.Robot.State, as: RobotState + + # Safe starting position - comfortably within SO101 workspace. + # At all-zero joints the EE rests at (0.407, 0, 0.116); this point is + # well within the joint-limited reachable workspace. + @start_x 0.25 @start_y 0.0 - @start_z 0.20 + @start_z 0.30 @default_radius 0.03 @default_points 16 - @default_delay 150 + @default_settle_tolerance_m 5.0e-3 + @default_settle_timeout_ms 1500 + + # Loose IK tolerance/large step/low damping/few iterations let the warm-started + # solver converge in 1-3 iterations per waypoint. Tighter values just waste FK + # calls when the settle tolerance is 5mm anyway. + @ik_tolerance 2.0e-3 + @ik_step_size 0.3 + @ik_lambda 0.1 + @ik_max_iterations 15 # Motion.move_to/4 can return errors at runtime but dialyzer can't see # through :telemetry.span/3 and thinks it always returns {:ok, meta} - @dialyzer {:no_match, [handle_command: 3, execute_path: 4]} + @dialyzer {:no_match, [handle_command: 3, execute_path: 5]} @impl BB.Command def handle_command(goal, context, state) do radius = Map.get(goal, :radius, @default_radius) points = Map.get(goal, :points, @default_points) - delay = Map.get(goal, :delay, @default_delay) + tolerance = Map.get(goal, :settle_tolerance_m, @default_settle_tolerance_m) + timeout = Map.get(goal, :settle_timeout_ms, @default_settle_timeout_ms) start_position = Vec3.new(@start_x, @start_y, @start_z) - # Exclude gripper from IK - it's an end-effector, not a positioning joint - ik_opts = [delivery: :direct, exclude_joints: [:gripper]] + ik_opts = [ + delivery: :direct, + exclude_joints: [:gripper], + tolerance: @ik_tolerance, + step_size: @ik_step_size, + lambda: @ik_lambda, + max_iterations: @ik_max_iterations + ] - # First move to the starting position case Motion.move_to(context, :ee_link, start_position, ik_opts) do {:ok, _meta} -> - Process.sleep(500) - - # Generate circle points in XZ plane (vertical, forward-back motion) + wait_for_arrival(context, start_position, tolerance, timeout) targets = generate_circle_points(@start_x, @start_y, @start_z, radius, points) - case execute_path(context, targets, delay, ik_opts) do + case execute_path(context, targets, tolerance, timeout, ik_opts) do :ok -> - # Return to start position Motion.move_to(context, :ee_link, start_position, ik_opts) + wait_for_arrival(context, start_position, tolerance, timeout) {:stop, :normal, %{state | result: :complete}} {:error, reason} -> @@ -80,7 +102,6 @@ defmodule BB.Example.SO101.Command.DemoCircle do def result(%{result: result}), do: {:ok, result} defp generate_circle_points(cx, cy, cz, radius, num_points) do - # Circle in XZ plane - Y stays constant for i <- 0..num_points do angle = 2 * :math.pi() * i / num_points x = cx + radius * :math.cos(angle) @@ -89,16 +110,66 @@ defmodule BB.Example.SO101.Command.DemoCircle do end end - defp execute_path(context, targets, delay, ik_opts) do - Enum.reduce_while(targets, :ok, fn target, :ok -> - case Motion.move_to(context, :ee_link, target, ik_opts) do - {:ok, _meta} -> - Process.sleep(delay) - {:cont, :ok} - - error -> + # Solves each waypoint warm-starting from the previous IK solution rather + # than from robot_state, which the position estimator overwrites during arm + # motion. With consecutive waypoints ~3° apart in joint space, IK converges + # in 1-3 iterations instead of the ~15-25 it takes from a cold start. + defp execute_path(context, targets, tolerance, timeout, ik_opts) do + seed_positions = RobotState.get_all_positions(context.robot_state) + solver_opts = Keyword.delete(ik_opts, :delivery) + + targets + |> Enum.reduce_while({:ok, seed_positions}, fn target, {:ok, positions} -> + case DLS.solve(context.robot, positions, :ee_link, target, solver_opts) do + {:ok, new_positions, _meta} -> + BBMotion.send_positions(context, new_positions, delivery: :direct) + wait_for_arrival(context, target, tolerance, timeout) + {:cont, {:ok, new_positions}} + + {:error, _} = error -> {:halt, {:error, {:ik_failed, target, error}}} end end) + |> case do + {:ok, _last_positions} -> :ok + {:error, _} = error -> error + end + end + + # Poll the position estimator until the EE is within `tolerance` of `target` + # or `timeout_ms` elapses. Motion.move_to/send_positions writes its target + # into robot_state immediately, so we sleep one estimator tick (~20 ms) first + # to let the actual interpolated position arrive. + defp wait_for_arrival(context, target, tolerance, timeout_ms) do + Process.sleep(25) + deadline = System.monotonic_time(:millisecond) + timeout_ms + do_wait_for_arrival(context, target, tolerance, deadline) + end + + defp do_wait_for_arrival(context, target, tolerance, deadline) do + distance = current_ee_distance(context, target) + + cond do + distance <= tolerance -> + :ok + + System.monotonic_time(:millisecond) >= deadline -> + :timeout + + true -> + Process.sleep(20) + do_wait_for_arrival(context, target, tolerance, deadline) + end + end + + defp current_ee_distance(context, target) do + positions = RobotState.get_all_positions(context.robot_state) + {x, y, z} = Kinematics.link_position(context.robot, positions, :ee_link) + + :math.sqrt( + :math.pow(x - Vec3.x(target), 2) + + :math.pow(y - Vec3.y(target), 2) + + :math.pow(z - Vec3.z(target), 2) + ) end end diff --git a/lib/bb/example/so101/robot.ex b/lib/bb/example/so101/robot.ex index 9c465f7..ddbf187 100644 --- a/lib/bb/example/so101/robot.ex +++ b/lib/bb/example/so101/robot.ex @@ -132,7 +132,7 @@ defmodule BB.Example.SO101.Robot do upper(~u(110 degree)) effort(~u(2.5 newton_meter)) velocity(~u(360 degree_per_second)) - acceleration(~u(720 degree_per_square_second)) + acceleration(~u(2160 degree_per_square_second)) end actuator( @@ -181,7 +181,7 @@ defmodule BB.Example.SO101.Robot do upper(~u(190 degree)) effort(~u(2.5 newton_meter)) velocity(~u(360 degree_per_second)) - acceleration(~u(720 degree_per_square_second)) + acceleration(~u(2160 degree_per_square_second)) end actuator( @@ -230,7 +230,7 @@ defmodule BB.Example.SO101.Robot do upper(~u(7 degree)) effort(~u(2.5 newton_meter)) velocity(~u(360 degree_per_second)) - acceleration(~u(720 degree_per_square_second)) + acceleration(~u(2160 degree_per_square_second)) end actuator( @@ -279,7 +279,7 @@ defmodule BB.Example.SO101.Robot do upper(~u(95 degree)) effort(~u(2.5 newton_meter)) velocity(~u(360 degree_per_second)) - acceleration(~u(720 degree_per_square_second)) + acceleration(~u(2160 degree_per_square_second)) end actuator( @@ -328,7 +328,7 @@ defmodule BB.Example.SO101.Robot do upper(~u(160 degree)) effort(~u(2.5 newton_meter)) velocity(~u(360 degree_per_second)) - acceleration(~u(720 degree_per_square_second)) + acceleration(~u(2160 degree_per_square_second)) end actuator( @@ -377,7 +377,7 @@ defmodule BB.Example.SO101.Robot do upper(~u(100 degree)) effort(~u(2.5 newton_meter)) velocity(~u(360 degree_per_second)) - acceleration(~u(720 degree_per_square_second)) + acceleration(~u(2160 degree_per_square_second)) end actuator(