From 02a7da38056c87364511a44d273028e7155f573c Mon Sep 17 00:00:00 2001 From: James Harton Date: Sun, 28 Dec 2025 20:51:12 +1300 Subject: [PATCH] feat: add simulation mode for running robots without hardware Add runtime simulation mode via `start_link(simulation: :kinematic)`. A single robot definition now works for both hardware and simulation. Changes: - Runtime option to start robots in kinematic simulation mode - Actuators are replaced with `BB.Sim.Actuator` which publishes `BeginMotion` messages with timing based on joint velocity limits - Controllers and bridges support per-component `simulation` option: - `:omit` (default) - don't start in simulation - `:mock` - start a no-op mock - `:start` - start the real component - `BB.Robot.Runtime.simulation_mode/1` to check current mode - Safety system works normally (must arm before commands) - `OpenLoopPositionEstimator` works unchanged for position feedback New modules: - `BB.Sim.Actuator` - kinematic simulation actuator - `BB.Sim.Controller` - no-op mock controller - `BB.Sim.Bridge` - no-op mock bridge Documentation: - New tutorial: `10-simulation.md` - Updated AGENTS.md with simulation mode info --- .formatter.exs | 1 + AGENTS.md | 22 ++ documentation/dsls/DSL-BB.md | 8 + documentation/tutorials/10-simulation.md | 267 ++++++++++++++++++ lib/bb/bridge_supervisor.ex | 20 +- lib/bb/controller_supervisor.ex | 58 +++- lib/bb/dsl.ex | 15 +- lib/bb/dsl/bridge.ex | 7 +- lib/bb/dsl/controller.ex | 7 +- lib/bb/dsl/verifiers/validate_child_specs.ex | 24 +- lib/bb/joint_supervisor.ex | 12 +- lib/bb/link_supervisor.ex | 9 +- lib/bb/process.ex | 48 +++- lib/bb/robot/runtime.ex | 27 +- lib/bb/sensor_supervisor.ex | 4 +- lib/bb/sim/actuator.ex | 156 +++++++++++ lib/bb/sim/bridge.ex | 78 ++++++ lib/bb/sim/controller.ex | 37 +++ test/bb/sim/simulation_test.exs | 269 +++++++++++++++++++ test/support/mock_bridge.ex | 30 +++ test/support/mock_controller.ex | 25 ++ 21 files changed, 1095 insertions(+), 29 deletions(-) create mode 100644 documentation/tutorials/10-simulation.md create mode 100644 lib/bb/sim/actuator.ex create mode 100644 lib/bb/sim/bridge.ex create mode 100644 lib/bb/sim/controller.ex create mode 100644 test/bb/sim/simulation_test.exs create mode 100644 test/support/mock_bridge.ex create mode 100644 test/support/mock_controller.ex diff --git a/.formatter.exs b/.formatter.exs index 7bcd920..2e9a853 100644 --- a/.formatter.exs +++ b/.formatter.exs @@ -80,6 +80,7 @@ spark_locals_without_parens = [ scale: 1, sensor: 2, sensor: 3, + simulation: 1, sphere: 0, sphere: 1, supervisor_module: 1, diff --git a/AGENTS.md b/AGENTS.md index af970c3..e17ee1c 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -24,6 +24,8 @@ See `documentation/tutorials/` for guided tutorials: 6. `06-urdf-export.md` - exporting to URDF for ROS tools 7. `07-parameters.md` - runtime-adjustable configuration 8. `08-parameter-bridges.md` - bidirectional parameter access with remote systems +9. `09-inverse-kinematics.md` - solving inverse kinematics +10. `10-simulation.md` - running robots in simulation mode The DSL reference is in `documentation/dsls/DSL-BB.md`. @@ -100,6 +102,26 @@ Transformers run in sequence to process DSL at compile-time: **URDF Export** (`lib/bb/urdf/exporter.ex`): Converts robot definitions to URDF XML format for use with ROS tools like RViz and Gazebo. Available via `mix bb.to_urdf`. +### Simulation Mode + +Robots can run in simulation mode without hardware: + +```elixir +# Start in kinematic simulation +MyRobot.start_link(simulation: :kinematic) + +# Check simulation mode +BB.Robot.Runtime.simulation_mode(MyRobot) # => :kinematic or nil +``` + +In simulation mode: +- Actuators are replaced with `BB.Sim.Actuator` which publishes `BeginMotion` messages with timing based on joint velocity limits +- Controllers are omitted by default (configurable per-controller with `simulation: :omit | :mock | :start`) +- Safety system still requires arming before commands work +- `OpenLoopPositionEstimator` works unchanged for position feedback + +See `documentation/tutorials/10-simulation.md` for details. + ### Safety System (CRITICAL) See `documentation/topics/safety.md` for comprehensive safety documentation. diff --git a/documentation/dsls/DSL-BB.md b/documentation/dsls/DSL-BB.md index 73384c0..5c343ff 100644 --- a/documentation/dsls/DSL-BB.md +++ b/documentation/dsls/DSL-BB.md @@ -918,7 +918,11 @@ A controller process at the robot level. |------|------|---------|------| | [`name`](#controllers-controller-name){: #controllers-controller-name .spark-required} | `atom` | | A unique name for the controller | | [`child_spec`](#controllers-controller-child_spec){: #controllers-controller-child_spec .spark-required} | `module \| {module, keyword}` | | The child specification for the controller process. Either a module or `{module, keyword_list}` | +### Options +| Name | Type | Default | Docs | +|------|------|---------|------| +| [`simulation`](#controllers-controller-simulation){: #controllers-controller-simulation } | `:omit \| :mock \| :start` | `:omit` | Behaviour in simulation mode: :omit (don't start), :mock (start no-op mock), :start (start real controller) | @@ -1182,7 +1186,11 @@ end |------|------|---------|------| | [`name`](#parameters-bridge-name){: #parameters-bridge-name .spark-required} | `atom` | | A unique name for the bridge | | [`child_spec`](#parameters-bridge-child_spec){: #parameters-bridge-child_spec .spark-required} | `module \| {module, keyword}` | | The child specification for the bridge process. Either a module or `{module, keyword_list}` | +### Options +| Name | Type | Default | Docs | +|------|------|---------|------| +| [`simulation`](#parameters-bridge-simulation){: #parameters-bridge-simulation } | `:omit \| :mock \| :start` | `:omit` | Behaviour in simulation mode: :omit (don't start), :mock (start no-op mock), :start (start real bridge) | diff --git a/documentation/tutorials/10-simulation.md b/documentation/tutorials/10-simulation.md new file mode 100644 index 0000000..92f7705 --- /dev/null +++ b/documentation/tutorials/10-simulation.md @@ -0,0 +1,267 @@ + + +# Simulation Mode + +Beam Bots supports running robots in simulation mode, allowing you to develop and test robot behaviour without physical hardware. A single robot definition works for both hardware and simulation - you just change how you start it. + +## Prerequisites + +Complete [Your First Robot](01-first-robot.md) and [Starting and Stopping](02-starting-and-stopping.md) first. + +## Starting in Simulation Mode + +Start your robot in kinematic simulation mode by passing the `simulation` option: + +```elixir +iex> {:ok, pid} = MyRobot.start_link(simulation: :kinematic) +{:ok, #PID<0.234.0>} +``` + +The robot is now running entirely in software. Actuators receive commands and publish motion messages, but no hardware communication occurs. + +## Checking Simulation Mode + +You can check whether a robot is running in simulation mode: + +```elixir +iex> BB.Robot.Runtime.simulation_mode(MyRobot) +:kinematic + +# Hardware mode returns nil +iex> BB.Robot.Runtime.simulation_mode(MyRobot) +nil +``` + +## How Simulation Works + +In simulation mode: + +1. **Actuators are replaced** - Real actuator modules are swapped for `BB.Sim.Actuator` +2. **Controllers are omitted** - By default, hardware controllers don't start +3. **Messages flow normally** - Commands, `BeginMotion`, and `JointState` messages work as usual +4. **Safety system is active** - You must still arm the robot before sending commands + +The simulated actuator: + +- Receives position commands via the normal API +- Calculates motion timing from joint velocity limits in your DSL +- Publishes `BeginMotion` messages with realistic timing +- Clamps positions to joint limits + +The existing `OpenLoopPositionEstimator` sensor works unchanged, estimating position from `BeginMotion` messages. + +## Example: Testing Motion + +```elixir +# Start in simulation +{:ok, _pid} = MyRobot.start_link(simulation: :kinematic) + +# Arm the robot (required even in simulation) +:ok = BB.Safety.arm(MyRobot) + +# Send a position command +BB.Actuator.set_position!(MyRobot, :shoulder_motor, 1.57) + +# The OpenLoopPositionEstimator will estimate position over time +Process.sleep(500) +position = BB.Robot.Runtime.joint_position(MyRobot, :shoulder) +``` + +## Controller Behaviour in Simulation + +By default, controllers are omitted in simulation mode. You can customise this per-controller using the `simulation` option in the DSL: + +```elixir +controllers do + # Won't start in simulation (default) + controller :pca9685, {BB.Servo.PCA9685.Controller, bus: "i2c-1"}, + simulation: :omit + + # Starts a mock controller that accepts but ignores commands + controller :dynamixel, {BB.Servo.Robotis.Controller, port: "/dev/ttyUSB0"}, + simulation: :mock + + # Starts the real controller (for external simulator integration) + controller :gazebo_bridge, {MyApp.GazeboBridge, url: "localhost:11345"}, + simulation: :start +end +``` + +The three options are: + +| Option | Behaviour | +|--------|-----------| +| `:omit` | Controller not started (default) | +| `:mock` | Mock controller started - accepts commands but does nothing | +| `:start` | Real controller started | + +### When to Use Each Option + +- **`:omit`** - Most hardware controllers (I2C, serial, GPIO). The simulated actuator doesn't need them. +- **`:mock`** - When actuators query the controller for state during initialisation. +- **`:start`** - For external simulator bridges (Gazebo, MuJoCo) that need to run in simulation. + +## Bridge Behaviour in Simulation + +Parameter bridges also support the `simulation` option, with the same three modes: + +```elixir +parameters do + # Won't start in simulation (default) + bridge :mavlink, {BBMavLink.ParameterBridge, conn: "/dev/ttyACM0"}, + simulation: :omit + + # Starts a mock bridge that accepts but ignores operations + bridge :gcs, {MyApp.GCSBridge, url: "ws://gcs.local/socket"}, + simulation: :mock + + # Starts the real bridge (for external system integration) + bridge :phoenix, {BBPhoenix.ParameterBridge, url: "ws://localhost:4000/socket"}, + simulation: :start +end +``` + +| Option | Behaviour | +|--------|-----------| +| `:omit` | Bridge not started (default) | +| `:mock` | Mock bridge started - accepts operations but does nothing | +| `:start` | Real bridge started | + +## Kinematic Simulation + +The `:kinematic` simulation mode provides position/velocity interpolation without physics: + +- Positions are clamped to joint limits (`lower`, `upper`) +- Travel time is calculated from velocity limits +- No acceleration, inertia, or gravity simulation + +This is sufficient for: + +- Testing control logic and state machines +- Verifying command sequences +- UI development without hardware +- Integration testing + +## Future Simulation Modes + +The simulation option is an atom to allow future expansion: + +```elixir +# Current: kinematic simulation +MyRobot.start_link(simulation: :kinematic) + +# Future: external physics engine +MyRobot.start_link(simulation: :external) + +# Future: built-in physics +MyRobot.start_link(simulation: :physics) +``` + +## Environment-Based Mode Selection + +To switch between hardware and simulation based on environment: + +```elixir +# In your application.ex +defmodule MyApp.Application do + use Application + + @impl true + def start(_type, _args) do + simulation_mode = + if Application.get_env(:my_app, :simulate, false) do + :kinematic + else + nil + end + + children = [ + {MyRobot, simulation: simulation_mode} + ] + + Supervisor.start_link(children, strategy: :one_for_one) + end +end +``` + +Then in your config: + +```elixir +# config/dev.exs +config :my_app, simulate: true + +# config/prod.exs (or target.exs for Nerves) +config :my_app, simulate: false +``` + +## Testing with Simulation + +Simulation mode is useful for integration tests: + +```elixir +defmodule MyRobotTest do + use ExUnit.Case + + test "robot moves to home position" do + {:ok, pid} = MyRobot.start_link(simulation: :kinematic) + + :ok = BB.Safety.arm(MyRobot) + :ok = BB.Command.execute(MyRobot, :home) + + # Verify the robot reached home position + assert_eventually fn -> + pos = BB.Robot.Runtime.joint_position(MyRobot, :shoulder) + abs(pos - 0.0) < 0.01 + end + + Supervisor.stop(pid) + end +end +``` + +## Subscribing to Simulated Motion + +You can subscribe to motion messages from simulated actuators: + +```elixir +# Subscribe to actuator messages +BB.PubSub.subscribe(MyRobot, [:actuator, :base, :shoulder, :motor]) + +# Send a command +BB.Actuator.set_position!(MyRobot, :motor, 1.0) + +# Receive the BeginMotion message +receive do + {:bb, _path, %BB.Message{payload: %BB.Message.Actuator.BeginMotion{} = motion}} -> + IO.puts("Moving from #{motion.initial_position} to #{motion.target_position}") + IO.puts("Expected arrival: #{motion.expected_arrival}ms") +end +``` + +## Limitations + +Kinematic simulation doesn't model: + +- Physics (gravity, inertia, friction, collisions) +- Sensor noise or latency +- Hardware-specific behaviour +- External disturbances + +For high-fidelity simulation, consider integrating with an external physics engine like Gazebo or MuJoCo using `simulation: :start` controllers. + +## What's Next? + +You now know how to: + +- Run robots in simulation mode +- Configure controller behaviour in simulation +- Use simulation for development and testing + +For more advanced topics, see: + +- [Safety](../topics/safety.md) - Understanding the safety system +- [Parameters](07-parameters.md) - Runtime-adjustable configuration diff --git a/lib/bb/bridge_supervisor.ex b/lib/bb/bridge_supervisor.ex index 7ef9412..4c0c08d 100644 --- a/lib/bb/bridge_supervisor.ex +++ b/lib/bb/bridge_supervisor.ex @@ -22,15 +22,29 @@ defmodule BB.BridgeSupervisor do end @impl true - def init({robot_module, _opts}) do + def init({robot_module, opts}) do + simulation_mode = Keyword.get(opts, :simulation) + children = robot_module |> Info.parameters() |> Enum.filter(&is_struct(&1, Bridge)) - |> Enum.map(fn bridge -> - BB.Process.bridge_child_spec(robot_module, bridge.name, bridge.child_spec, []) + |> Enum.flat_map(fn bridge -> + build_bridge_child(robot_module, bridge, simulation_mode) end) Supervisor.init(children, strategy: :one_for_one) end + + defp build_bridge_child(robot_module, bridge, nil = _simulation_mode) do + [BB.Process.bridge_child_spec(robot_module, bridge.name, bridge.child_spec, [])] + end + + defp build_bridge_child(robot_module, bridge, _simulation_mode) do + case bridge.simulation do + :omit -> [] + :mock -> [BB.Process.bridge_child_spec(robot_module, bridge.name, BB.Sim.Bridge, [])] + :start -> [BB.Process.bridge_child_spec(robot_module, bridge.name, bridge.child_spec, [])] + end + end end diff --git a/lib/bb/controller_supervisor.ex b/lib/bb/controller_supervisor.ex index baaf120..c989a8f 100644 --- a/lib/bb/controller_supervisor.ex +++ b/lib/bb/controller_supervisor.ex @@ -22,20 +22,60 @@ defmodule BB.ControllerSupervisor do end @impl true - def init({robot_module, _opts}) do + def init({robot_module, opts}) do + simulation_mode = Keyword.get(opts, :simulation) + children = robot_module |> Info.controllers() - |> Enum.map(fn controller -> - BB.Process.child_spec( - robot_module, - controller.name, - controller.child_spec, - [], - :controller - ) + |> Enum.flat_map(fn controller -> + build_controller_child(robot_module, controller, simulation_mode, opts) end) Supervisor.init(children, strategy: :one_for_one) end + + defp build_controller_child(robot_module, controller, nil = _simulation_mode, opts) do + [ + BB.Process.child_spec( + robot_module, + controller.name, + controller.child_spec, + [], + :controller, + opts + ) + ] + end + + defp build_controller_child(robot_module, controller, _simulation_mode, opts) do + case controller.simulation do + :omit -> + [] + + :mock -> + [ + BB.Process.child_spec( + robot_module, + controller.name, + BB.Sim.Controller, + [], + :controller, + opts + ) + ] + + :start -> + [ + BB.Process.child_spec( + robot_module, + controller.name, + controller.child_spec, + [], + :controller, + opts + ) + ] + end + end end diff --git a/lib/bb/dsl.ex b/lib/bb/dsl.ex index e43022b..c29124e 100644 --- a/lib/bb/dsl.ex +++ b/lib/bb/dsl.ex @@ -662,6 +662,12 @@ defmodule BB.Dsl do required: true, doc: "The child specification for the controller process. Either a module or `{module, keyword_list}`" + ], + simulation: [ + type: {:in, [:omit, :mock, :start]}, + default: :omit, + doc: + "Behaviour in simulation mode: :omit (don't start), :mock (start no-op mock), :start (start real controller)" ] ] } @@ -669,7 +675,8 @@ defmodule BB.Dsl do @controllers %Section{ name: :controllers, describe: "Robot-level controllers", - entities: [@controller] + entities: [@controller], + imports: [BB.Dsl.ParamRef] } @command_argument %Entity{ @@ -816,6 +823,12 @@ defmodule BB.Dsl do required: true, doc: "The child specification for the bridge process. Either a module or `{module, keyword_list}`" + ], + simulation: [ + type: {:in, [:omit, :mock, :start]}, + default: :omit, + doc: + "Behaviour in simulation mode: :omit (don't start), :mock (start no-op mock), :start (start real bridge)" ] ] } diff --git a/lib/bb/dsl/bridge.ex b/lib/bb/dsl/bridge.ex index d5287d8..0a3b405 100644 --- a/lib/bb/dsl/bridge.ex +++ b/lib/bb/dsl/bridge.ex @@ -8,16 +8,19 @@ defmodule BB.Dsl.Bridge do defstruct __identifier__: nil, __spark_metadata__: nil, name: nil, - child_spec: nil + child_spec: nil, + simulation: :omit alias Spark.Dsl.Entity @type child_spec :: module | {module, keyword()} + @type simulation_mode :: :omit | :mock | :start @type t :: %__MODULE__{ __identifier__: any, __spark_metadata__: Entity.spark_meta(), name: atom, - child_spec: child_spec + child_spec: child_spec, + simulation: simulation_mode } end diff --git a/lib/bb/dsl/controller.ex b/lib/bb/dsl/controller.ex index 909ad4a..981fa12 100644 --- a/lib/bb/dsl/controller.ex +++ b/lib/bb/dsl/controller.ex @@ -8,16 +8,19 @@ defmodule BB.Dsl.Controller do defstruct __identifier__: nil, __spark_metadata__: nil, name: nil, - child_spec: nil + child_spec: nil, + simulation: :omit alias Spark.Dsl.Entity @type child_spec :: module | {module, Keyword.t()} + @type simulation_mode :: :omit | :mock | :start @type t :: %__MODULE__{ __identifier__: any, __spark_metadata__: Entity.spark_meta(), name: atom, - child_spec: child_spec + child_spec: child_spec, + simulation: simulation_mode } end diff --git a/lib/bb/dsl/verifiers/validate_child_specs.ex b/lib/bb/dsl/verifiers/validate_child_specs.ex index df02359..75b5811 100644 --- a/lib/bb/dsl/verifiers/validate_child_specs.ex +++ b/lib/bb/dsl/verifiers/validate_child_specs.ex @@ -189,9 +189,11 @@ defmodule BB.Dsl.Verifiers.ValidateChildSpecs do has_schema? -> schema = module.options_schema() + param_ref_keys = get_param_ref_keys(opts) + schema_for_validation = mark_keys_as_optional(schema, param_ref_keys) opts_without_param_refs = filter_param_refs(opts) - case Spark.Options.validate(opts_without_param_refs, schema) do + case Spark.Options.validate(opts_without_param_refs, schema_for_validation) do {:ok, _validated} -> :ok @@ -220,6 +222,26 @@ defmodule BB.Dsl.Verifiers.ValidateChildSpecs do Enum.reject(opts, fn {_key, value} -> is_struct(value, ParamRef) end) end + defp get_param_ref_keys(opts) do + opts + |> Enum.filter(fn {_key, value} -> is_struct(value, ParamRef) end) + |> Keyword.keys() + end + + defp mark_keys_as_optional(%Spark.Options{schema: schema} = spark_opts, keys) do + %{spark_opts | schema: mark_keys_as_optional(schema, keys)} + end + + defp mark_keys_as_optional(schema, keys) when is_list(schema) do + Enum.map(schema, fn {key, opts} -> + if key in keys do + {key, Keyword.put(opts, :required, false)} + else + {key, opts} + end + end) + end + defp format_schema(%Spark.Options{schema: schema}) do format_schema(schema) end diff --git a/lib/bb/joint_supervisor.ex b/lib/bb/joint_supervisor.ex index 6327d0f..9605419 100644 --- a/lib/bb/joint_supervisor.ex +++ b/lib/bb/joint_supervisor.ex @@ -48,7 +48,14 @@ defmodule BB.JointSupervisor do sensor_children = Enum.map(joint.sensors, fn sensor -> - BB.Process.child_spec(robot_module, sensor.name, sensor.child_spec, joint_path, :sensor) + BB.Process.child_spec( + robot_module, + sensor.name, + sensor.child_spec, + joint_path, + :sensor, + opts + ) end) actuator_children = @@ -58,7 +65,8 @@ defmodule BB.JointSupervisor do actuator.name, actuator.child_spec, joint_path, - :actuator + :actuator, + opts ) end) diff --git a/lib/bb/link_supervisor.ex b/lib/bb/link_supervisor.ex index a0a0482..675886e 100644 --- a/lib/bb/link_supervisor.ex +++ b/lib/bb/link_supervisor.ex @@ -47,7 +47,14 @@ defmodule BB.LinkSupervisor do sensor_children = Enum.map(link.sensors, fn sensor -> - BB.Process.child_spec(robot_module, sensor.name, sensor.child_spec, link_path, :sensor) + BB.Process.child_spec( + robot_module, + sensor.name, + sensor.child_spec, + link_path, + :sensor, + opts + ) end) joint_children = diff --git a/lib/bb/process.ex b/lib/bb/process.ex index c2d57fd..63138e9 100644 --- a/lib/bb/process.ex +++ b/lib/bb/process.ex @@ -13,7 +13,7 @@ defmodule BB.Process do Build a child_spec that registers the process in the robot's registry. The resulting child spec uses the appropriate wrapper GenServer based on type: - - `:actuator` → `BB.Actuator.Server` + - `:actuator` → `BB.Actuator.Server` (or `BB.Sim.Actuator` in simulation mode) - `:sensor` → `BB.Sensor.Server` - `:controller` → `BB.Controller.Server` @@ -23,9 +23,32 @@ defmodule BB.Process do The process is registered by its name (which must be globally unique across the robot). The full path is passed to the process in its init args for context, but is not used for registration. + + ## Options + + - `:simulation` - when set (e.g., `:kinematic`), actuators use `BB.Sim.Actuator` + instead of the real actuator module """ - @spec child_spec(module, atom, module | {module, Keyword.t()}, [atom], process_type) :: map - def child_spec(robot_module, name, user_child_spec, path, type) do + @spec child_spec( + module, + atom, + module | {module, Keyword.t()}, + [atom], + process_type, + Keyword.t() + ) :: + map + def child_spec(robot_module, name, user_child_spec, path, type, opts \\ []) do + simulation_mode = Keyword.get(opts, :simulation) + + if simulation_mode && type == :actuator do + build_simulated_actuator_spec(robot_module, name, path) + else + build_real_child_spec(robot_module, name, user_child_spec, path, type) + end + end + + defp build_real_child_spec(robot_module, name, user_child_spec, path, type) do {callback_module, user_args} = normalize_child_spec(user_child_spec) full_path = path ++ [name] @@ -47,6 +70,25 @@ defmodule BB.Process do } end + defp build_simulated_actuator_spec(robot_module, name, path) do + full_path = path ++ [name] + + init_arg = [ + bb: %{robot: robot_module, path: full_path}, + __callback_module__: BB.Sim.Actuator + ] + + %{ + id: name, + start: + {BB.Actuator.Server, :start_link, + [ + init_arg, + [name: via(robot_module, name)] + ]} + } + end + defp wrapper_for_type(:actuator), do: BB.Actuator.Server defp wrapper_for_type(:sensor), do: BB.Sensor.Server defp wrapper_for_type(:controller), do: BB.Controller.Server diff --git a/lib/bb/robot/runtime.ex b/lib/bb/robot/runtime.ex index 160d8e9..6dde4a3 100644 --- a/lib/bb/robot/runtime.ex +++ b/lib/bb/robot/runtime.ex @@ -63,10 +63,12 @@ defmodule BB.Robot.Runtime do :current_command_name, :current_execution_id, :parameter_store, - :parameter_store_state + :parameter_store_state, + :simulation_mode ] @type robot_state :: :disarmed | :disarming | :idle | :executing | :error + @type simulation_mode :: nil | :kinematic | :external @type t :: %__MODULE__{ robot_module: module(), robot: BB.Robot.t(), @@ -78,7 +80,8 @@ defmodule BB.Robot.Runtime do current_command_name: atom() | nil, current_execution_id: reference() | nil, parameter_store: module() | nil, - parameter_store_state: term() | nil + parameter_store_state: term() | nil, + simulation_mode: simulation_mode() } @doc """ @@ -123,6 +126,17 @@ defmodule BB.Robot.Runtime do end end + @doc """ + Get the simulation mode for a robot. + + Returns `nil` if running in hardware mode, or the simulation mode atom + (e.g., `:kinematic`, `:external`) if running in simulation. + """ + @spec simulation_mode(module()) :: simulation_mode() + def simulation_mode(robot_module) do + GenServer.call(via(robot_module), :get_simulation_mode) + end + @doc """ Transition the robot to a new state. """ @@ -302,6 +316,8 @@ defmodule BB.Robot.Runtime do # Internal state tracks :idle/:executing (not :disarmed) # The armed/disarmed state is owned by SafetyController + simulation_mode = Keyword.get(opts, :simulation) + state = %__MODULE__{ robot_module: robot_module, robot: robot, @@ -313,7 +329,8 @@ defmodule BB.Robot.Runtime do current_command_name: nil, current_execution_id: nil, parameter_store: store_module, - parameter_store_state: store_state + parameter_store_state: store_state, + simulation_mode: simulation_mode } # Register DSL-defined parameters (applies defaults) @@ -380,6 +397,10 @@ defmodule BB.Robot.Runtime do {:reply, state.robot, state} end + def handle_call(:get_simulation_mode, _from, state) do + {:reply, state.simulation_mode, state} + end + def handle_call({:execute, command_name, goal, execution_id}, _from, state) do case Map.fetch(state.commands, command_name) do {:ok, command} -> diff --git a/lib/bb/sensor_supervisor.ex b/lib/bb/sensor_supervisor.ex index ef0eae0..93f4e76 100644 --- a/lib/bb/sensor_supervisor.ex +++ b/lib/bb/sensor_supervisor.ex @@ -22,12 +22,12 @@ defmodule BB.SensorSupervisor do end @impl true - def init({robot_module, _opts}) do + def init({robot_module, opts}) do children = robot_module |> Info.sensors() |> Enum.map(fn sensor -> - BB.Process.child_spec(robot_module, sensor.name, sensor.child_spec, [], :sensor) + BB.Process.child_spec(robot_module, sensor.name, sensor.child_spec, [], :sensor, opts) end) Supervisor.init(children, strategy: :one_for_one) diff --git a/lib/bb/sim/actuator.ex b/lib/bb/sim/actuator.ex new file mode 100644 index 0000000..be12f7b --- /dev/null +++ b/lib/bb/sim/actuator.ex @@ -0,0 +1,156 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Sim.Actuator do + @moduledoc """ + Simulated actuator for kinematic simulation mode. + + This actuator is automatically used in place of real actuators when the robot + is started with `simulation: :kinematic`. It: + + - Receives position commands via pubsub, cast, and call + - Calculates motion timing from joint velocity limits + - Publishes `BeginMotion` messages for position estimation + - Clamps positions to joint limits + + Works with `BB.Sensor.OpenLoopPositionEstimator` for position feedback. + + ## Example + + # Start robot in simulation mode + MyRobot.start_link(simulation: :kinematic) + + # Commands work identically to hardware mode + BB.Actuator.set_position(MyRobot, [:base, :shoulder, :motor], 1.57) + """ + + use BB.Actuator, options_schema: [] + + alias BB.Message + alias BB.Message.Actuator.BeginMotion + alias BB.Message.Actuator.Command + alias BB.PubSub + alias BB.Robot + + defstruct [:bb, :joint, :current_position, :name, :joint_name] + + @impl BB.Actuator + def disarm(_opts), do: :ok + + @impl BB.Actuator + def init(opts) do + bb = Keyword.fetch!(opts, :bb) + [name, joint_name | _] = Enum.reverse(bb.path) + robot = bb.robot.robot() + + joint = Robot.get_joint(robot, joint_name) + + initial_position = calculate_initial_position(joint) + + state = %__MODULE__{ + bb: bb, + joint: joint, + current_position: initial_position, + name: name, + joint_name: joint_name + } + + {:ok, state} + end + + @impl BB.Actuator + def handle_info({:bb, _path, %Message{payload: %Command.Position{} = cmd}}, state) do + {:noreply, do_set_position(cmd.position, cmd.command_id, state)} + end + + def handle_info({:bb, _path, %Message{payload: %Command.Stop{}}}, state) do + {:noreply, state} + end + + def handle_info({:bb, _path, %Message{payload: %Command.Hold{}}}, state) do + {:noreply, state} + end + + def handle_info({:bb, _path, _message}, state) do + {:noreply, state} + end + + @impl BB.Actuator + def handle_cast({:command, %Message{payload: %Command.Position{} = cmd}}, state) do + {:noreply, do_set_position(cmd.position, cmd.command_id, state)} + end + + def handle_cast({:command, _message}, state) do + {:noreply, state} + end + + @impl BB.Actuator + def handle_call({:command, %Message{payload: %Command.Position{} = cmd}}, _from, state) do + new_state = do_set_position(cmd.position, cmd.command_id, state) + {:reply, {:ok, :accepted}, new_state} + end + + def handle_call({:command, _message}, _from, state) do + {:reply, {:ok, :accepted}, state} + end + + defp do_set_position(target_position, command_id, state) do + clamped = clamp_position(target_position, state.joint) + travel_time_ms = calculate_travel_time(state.current_position, clamped, state.joint) + expected_arrival = System.monotonic_time(:millisecond) + travel_time_ms + + message_opts = [ + initial_position: state.current_position, + target_position: clamped, + expected_arrival: expected_arrival, + command_type: :position + ] + + message_opts = + if command_id do + Keyword.put(message_opts, :command_id, command_id) + else + message_opts + end + + {:ok, message} = Message.new(BeginMotion, state.joint_name, message_opts) + PubSub.publish(state.bb.robot, [:actuator | state.bb.path], message) + + %{state | current_position: clamped} + end + + defp calculate_initial_position(nil), do: 0.0 + + defp calculate_initial_position(%{limits: nil}), do: 0.0 + + defp calculate_initial_position(%{limits: limits}) do + lower = limits.lower || 0.0 + upper = limits.upper || 0.0 + (lower + upper) / 2 + end + + defp clamp_position(position, nil), do: position + defp clamp_position(position, %{limits: nil}), do: position + + defp clamp_position(position, %{limits: limits}) do + position + |> clamp_lower(limits.lower) + |> clamp_upper(limits.upper) + end + + defp clamp_lower(position, nil), do: position + defp clamp_lower(position, lower), do: max(position, lower) + + defp clamp_upper(position, nil), do: position + defp clamp_upper(position, upper), do: min(position, upper) + + defp calculate_travel_time(_from, _to, nil), do: 0 + defp calculate_travel_time(_from, _to, %{limits: nil}), do: 0 + defp calculate_travel_time(_from, _to, %{limits: %{velocity: nil}}), do: 0 + + defp calculate_travel_time(from, to, %{limits: %{velocity: velocity}}) do + distance = abs(to - from) + round(distance / velocity * 1000) + end +end diff --git a/lib/bb/sim/bridge.ex b/lib/bb/sim/bridge.ex new file mode 100644 index 0000000..3600b8d --- /dev/null +++ b/lib/bb/sim/bridge.ex @@ -0,0 +1,78 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Sim.Bridge do + @moduledoc """ + Mock bridge for simulation mode. + + Accepts all operations but does nothing. Useful when actuators or other + components query the bridge during initialisation in simulation mode. + """ + use BB.Bridge, options_schema: [] + + @impl GenServer + def init(opts) do + {:ok, %{bb: opts[:bb]}} + end + + @impl BB.Bridge + def handle_change(_robot, _changed, state) do + {:ok, state} + end + + @impl BB.Bridge + def list_remote(state) do + {:ok, [], state} + end + + @impl BB.Bridge + def get_remote(_param_id, state) do + {:error, :not_found, state} + end + + @impl BB.Bridge + def set_remote(_param_id, _value, state) do + {:ok, state} + end + + @impl BB.Bridge + def subscribe_remote(_param_id, state) do + {:ok, state} + end + + @impl GenServer + def handle_call(:list_remote, _from, state) do + {:ok, params, state} = list_remote(state) + {:reply, {:ok, params}, state} + end + + def handle_call({:get_remote, param_id}, _from, state) do + {:error, reason, state} = get_remote(param_id, state) + {:reply, {:error, reason}, state} + end + + def handle_call({:set_remote, param_id, value}, _from, state) do + {:ok, state} = set_remote(param_id, value, state) + {:reply, :ok, state} + end + + def handle_call({:subscribe_remote, param_id}, _from, state) do + {:ok, state} = subscribe_remote(param_id, state) + {:reply, :ok, state} + end + + def handle_call(_request, _from, state) do + {:reply, :ok, state} + end + + @impl GenServer + def handle_cast(_request, state) do + {:noreply, state} + end + + @impl GenServer + def handle_info(_msg, state) do + {:noreply, state} + end +end diff --git a/lib/bb/sim/controller.ex b/lib/bb/sim/controller.ex new file mode 100644 index 0000000..d94c4a1 --- /dev/null +++ b/lib/bb/sim/controller.ex @@ -0,0 +1,37 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Sim.Controller do + @moduledoc """ + Mock controller for simulation mode. + + Used when a controller is configured with `simulation: :mock`. Accepts all + commands but does nothing with them. Useful when actuators need to call + controllers for state queries but no hardware is present. + + ## Example + + controllers do + controller :pca9685, {BB.Servo.PCA9685.Controller, bus: "i2c-1"}, + simulation: :mock + end + """ + + use BB.Controller, options_schema: [] + + @impl BB.Controller + def init(opts) do + {:ok, %{bb: opts[:bb]}} + end + + @impl BB.Controller + def handle_call(_request, _from, state) do + {:reply, :ok, state} + end + + @impl BB.Controller + def handle_cast(_request, state) do + {:noreply, state} + end +end diff --git a/test/bb/sim/simulation_test.exs b/test/bb/sim/simulation_test.exs new file mode 100644 index 0000000..0619732 --- /dev/null +++ b/test/bb/sim/simulation_test.exs @@ -0,0 +1,269 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Sim.SimulationTest do + use ExUnit.Case, async: false + + alias BB.Message + alias BB.Message.Actuator.BeginMotion + alias BB.PubSub + alias BB.Robot.Runtime + + describe "simulation mode" do + defmodule SimModeRobot do + @moduledoc false + use BB + + topology do + link :base_link do + joint :shoulder do + type(:revolute) + + limit do + lower(~u(-90 degree)) + upper(~u(90 degree)) + velocity(~u(60 degree_per_second)) + effort(~u(10 newton_meter)) + end + + actuator(:motor, ServoMotor) + sensor(:estimator, {BB.Sensor.OpenLoopPositionEstimator, actuator: :motor}) + + link :arm do + end + end + end + end + end + + setup do + on_exit(fn -> + case Process.whereis(SimModeRobot.Registry) do + nil -> :ok + _pid -> :ok + end + end) + + :ok + end + + test "robot starts in simulation mode with simulation: :kinematic" do + {:ok, pid} = SimModeRobot.start_link(simulation: :kinematic) + assert is_pid(pid) + assert Runtime.simulation_mode(SimModeRobot) == :kinematic + Supervisor.stop(pid) + end + + test "robot starts in hardware mode by default" do + {:ok, pid} = SimModeRobot.start_link() + assert is_pid(pid) + assert Runtime.simulation_mode(SimModeRobot) == nil + Supervisor.stop(pid) + end + + test "simulated actuator publishes BeginMotion on position command" do + Process.flag(:trap_exit, true) + {:ok, pid} = SimModeRobot.start_link(simulation: :kinematic) + + PubSub.subscribe(SimModeRobot, [:actuator, :base_link, :shoulder, :motor]) + + :ok = BB.Safety.arm(SimModeRobot) + + target_position = 0.5 + BB.Actuator.set_position!(SimModeRobot, :motor, target_position) + + assert_receive {:bb, [:actuator, :base_link, :shoulder, :motor], + %Message{payload: %BeginMotion{target_position: ^target_position}}}, + 1000 + + Supervisor.stop(pid) + end + + test "simulated actuator clamps position to joint limits" do + Process.flag(:trap_exit, true) + {:ok, pid} = SimModeRobot.start_link(simulation: :kinematic) + + PubSub.subscribe(SimModeRobot, [:actuator, :base_link, :shoulder, :motor]) + + :ok = BB.Safety.arm(SimModeRobot) + + over_limit = 3.0 + BB.Actuator.set_position!(SimModeRobot, :motor, over_limit) + + upper_limit = :math.pi() / 2 + + assert_receive {:bb, [:actuator, :base_link, :shoulder, :motor], + %Message{payload: %BeginMotion{target_position: target}}}, + 1000 + + assert_in_delta target, upper_limit, 0.001 + + Supervisor.stop(pid) + end + + test "synchronous position command returns acknowledgement" do + {:ok, pid} = SimModeRobot.start_link(simulation: :kinematic) + + :ok = BB.Safety.arm(SimModeRobot) + + result = BB.Actuator.set_position_sync(SimModeRobot, :motor, 0.5) + + assert result == {:ok, :accepted} + + Supervisor.stop(pid) + end + end + + describe "controller simulation options" do + defmodule CtrlOmitRobot do + @moduledoc false + use BB + + controllers do + controller(:mock_ctrl, {BB.Test.MockController, []}, simulation: :omit) + end + + topology do + link :base do + end + end + end + + defmodule CtrlMockRobot do + @moduledoc false + use BB + + controllers do + controller(:mock_ctrl, {BB.Test.MockController, []}, simulation: :mock) + end + + topology do + link :base do + end + end + end + + defmodule CtrlStartRobot do + @moduledoc false + use BB + + controllers do + controller(:mock_ctrl, {BB.Test.MockController, []}, simulation: :start) + end + + topology do + link :base do + end + end + end + + test "controller with simulation: :omit is not started in simulation mode" do + {:ok, pid} = CtrlOmitRobot.start_link(simulation: :kinematic) + + assert BB.Process.whereis(CtrlOmitRobot, :mock_ctrl) == :undefined + + Supervisor.stop(pid) + end + + test "controller with simulation: :omit is started in hardware mode" do + {:ok, pid} = CtrlOmitRobot.start_link() + + refute BB.Process.whereis(CtrlOmitRobot, :mock_ctrl) == :undefined + + Supervisor.stop(pid) + end + + test "controller with simulation: :mock starts mock controller" do + {:ok, pid} = CtrlMockRobot.start_link(simulation: :kinematic) + + refute BB.Process.whereis(CtrlMockRobot, :mock_ctrl) == :undefined + + Supervisor.stop(pid) + end + + test "controller with simulation: :start starts real controller" do + {:ok, pid} = CtrlStartRobot.start_link(simulation: :kinematic) + + refute BB.Process.whereis(CtrlStartRobot, :mock_ctrl) == :undefined + + Supervisor.stop(pid) + end + end + + describe "bridge simulation options" do + defmodule BridgeOmitRobot do + @moduledoc false + use BB + + parameters do + bridge(:mock_bridge, {BB.Test.MockBridge, []}, simulation: :omit) + end + + topology do + link :base do + end + end + end + + defmodule BridgeMockRobot do + @moduledoc false + use BB + + parameters do + bridge(:mock_bridge, {BB.Test.MockBridge, []}, simulation: :mock) + end + + topology do + link :base do + end + end + end + + defmodule BridgeStartRobot do + @moduledoc false + use BB + + parameters do + bridge(:mock_bridge, {BB.Test.MockBridge, []}, simulation: :start) + end + + topology do + link :base do + end + end + end + + test "bridge with simulation: :omit is not started in simulation mode" do + {:ok, pid} = BridgeOmitRobot.start_link(simulation: :kinematic) + + assert BB.Process.whereis(BridgeOmitRobot, :mock_bridge) == :undefined + + Supervisor.stop(pid) + end + + test "bridge with simulation: :omit is started in hardware mode" do + {:ok, pid} = BridgeOmitRobot.start_link() + + refute BB.Process.whereis(BridgeOmitRobot, :mock_bridge) == :undefined + + Supervisor.stop(pid) + end + + test "bridge with simulation: :mock starts mock bridge" do + {:ok, pid} = BridgeMockRobot.start_link(simulation: :kinematic) + + refute BB.Process.whereis(BridgeMockRobot, :mock_bridge) == :undefined + + Supervisor.stop(pid) + end + + test "bridge with simulation: :start starts real bridge" do + {:ok, pid} = BridgeStartRobot.start_link(simulation: :kinematic) + + refute BB.Process.whereis(BridgeStartRobot, :mock_bridge) == :undefined + + Supervisor.stop(pid) + end + end +end diff --git a/test/support/mock_bridge.ex b/test/support/mock_bridge.ex new file mode 100644 index 0000000..277fae4 --- /dev/null +++ b/test/support/mock_bridge.ex @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Test.MockBridge do + @moduledoc """ + Minimal mock bridge for testing. + """ + use BB.Bridge, options_schema: [] + + @impl GenServer + def init(opts) do + {:ok, %{bb: opts[:bb]}} + end + + @impl BB.Bridge + def handle_change(_robot, _changed, state) do + {:ok, state} + end + + @impl GenServer + def handle_call(_request, _from, state) do + {:reply, :ok, state} + end + + @impl GenServer + def handle_cast(_request, state) do + {:noreply, state} + end +end diff --git a/test/support/mock_controller.ex b/test/support/mock_controller.ex new file mode 100644 index 0000000..f1c315f --- /dev/null +++ b/test/support/mock_controller.ex @@ -0,0 +1,25 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Test.MockController do + @moduledoc """ + Minimal mock controller for testing. + """ + use BB.Controller, options_schema: [] + + @impl BB.Controller + def init(opts) do + {:ok, %{bb: opts[:bb]}} + end + + @impl BB.Controller + def handle_call(_request, _from, state) do + {:reply, :ok, state} + end + + @impl BB.Controller + def handle_cast(_request, state) do + {:noreply, state} + end +end