Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .formatter.exs
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ spark_locals_without_parens = [
scale: 1,
sensor: 2,
sensor: 3,
simulation: 1,
sphere: 0,
sphere: 1,
supervisor_module: 1,
Expand Down
22 changes: 22 additions & 0 deletions AGENTS.md
Original file line number Diff line number Diff line change
Expand Up @@ -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`.

Expand Down Expand Up @@ -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.
Expand Down
8 changes: 8 additions & 0 deletions documentation/dsls/DSL-BB.md
Original file line number Diff line number Diff line change
Expand Up @@ -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) |



Expand Down Expand Up @@ -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) |



Expand Down
267 changes: 267 additions & 0 deletions documentation/tutorials/10-simulation.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,267 @@
<!--
SPDX-FileCopyrightText: 2025 James Harton

SPDX-License-Identifier: Apache-2.0
-->

# 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
20 changes: 17 additions & 3 deletions lib/bb/bridge_supervisor.ex
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading