From f08c65fd8c3d94478d3beb7624cf07e73d26e3cf Mon Sep 17 00:00:00 2001 From: James Harton Date: Tue, 19 May 2026 08:17:39 +0000 Subject: [PATCH 1/4] feature(transmission): model URDF transmissions in the DSL Adds a `transmission` block to joints in the BB DSL, mirroring URDF's `` element for the simple 1:1 case (one joint, one actuator). The block carries `reduction` (gear ratio), `offset` (zero-point calibration in radians or metres depending on joint type), and `reversed?` (polarity). All three fields accept `param/1` references for runtime adjustment. Runtime model: `BB.Actuator.Server` (the wrapper around every actuator driver) resolves the joint's transmission at init and applies it to incoming `Command.Position`, `Command.Velocity`, `Command.Effort`, and `Command.Trajectory` messages before delegating to the user driver. User drivers operate purely in motor-space. The transmission is exposed to callbacks via `BB.Actuator.current_transmission/0` for the outbound direction (e.g. publishing joint-space JointState from motor-space hardware readings). `BB.Sim.Actuator` follows the same pipeline: receives motor-space from the wrapper, calls `BB.Transmission.unapply_position/2` to recover joint-space, then runs the existing clamp + motion-timing logic and publishes joint-space `BeginMotion` as before. Simulated motion matches real-hardware motion through the transmission. Coupled transmissions (differential, four-bar) and the top-level `transmissions` section are deliberately out of scope for this PR; the internal data model keeps the runtime as a per-joint concept so they can land as non-breaking additions later. Refs: beam-bots/bb#108 --- lib/bb/actuator.ex | 38 +++ lib/bb/actuator/server.ex | 104 ++++++-- lib/bb/dsl.ex | 39 ++- lib/bb/dsl/joint.ex | 4 +- lib/bb/dsl/transmission.ex | 28 +++ .../dsl/validate_limit_units_transformer.ex | 64 +++-- lib/bb/dsl/verifiers/validate_param_refs.ex | 17 +- lib/bb/robot/builder.ex | 59 ++++- lib/bb/robot/joint.ex | 22 ++ lib/bb/sim/actuator.ex | 17 +- lib/bb/transmission.ex | 144 +++++++++++ lib/bb/transmission/resolver.ex | 120 +++++++++ test/bb/actuator/server_transmission_test.exs | 137 ++++++++++ test/bb/dsl/transmission_test.exs | 233 ++++++++++++++++++ test/bb/sim/actuator_transmission_test.exs | 66 +++++ test/bb/transmission_test.exs | 112 +++++++++ test/support/recording_actuator.ex | 52 ++++ 17 files changed, 1212 insertions(+), 44 deletions(-) create mode 100644 lib/bb/dsl/transmission.ex create mode 100644 lib/bb/transmission.ex create mode 100644 lib/bb/transmission/resolver.ex create mode 100644 test/bb/actuator/server_transmission_test.exs create mode 100644 test/bb/dsl/transmission_test.exs create mode 100644 test/bb/sim/actuator_transmission_test.exs create mode 100644 test/bb/transmission_test.exs create mode 100644 test/support/recording_actuator.ex diff --git a/lib/bb/actuator.ex b/lib/bb/actuator.ex index 528ad47..9f1580e 100644 --- a/lib/bb/actuator.ex +++ b/lib/bb/actuator.ex @@ -291,6 +291,44 @@ defmodule BB.Actuator do alias BB.Message alias BB.Message.Actuator.Command + # ---------------------------------------------------------------------------- + # Transmission access + # ---------------------------------------------------------------------------- + + @doc """ + Returns the joint's resolved transmission, or `nil` if the joint has no + transmission block. + + Only valid from inside an actuator callback (i.e. running in the + wrapper's process). The wrapper resolves the transmission at init, + stores it in the process dictionary, and updates it on parameter + changes. Callbacks that publish JointState in joint-space should use + this to unapply the transmission to motor-space readings. + + defmodule MyDriver do + use BB.Actuator + + def init(opts), do: {:ok, ...} + + def handle_info(:tick, state) do + motor_radians = read_hardware(state) + joint_radians = + case BB.Actuator.current_transmission() do + nil -> motor_radians + t -> BB.Transmission.unapply_position(motor_radians, t) + end + + publish_joint_state(joint_radians) + {:noreply, state} + end + end + + Controllers and other processes outside the wrapper should use + `BB.Transmission.Resolver.resolve_and_subscribe/2` directly. + """ + @spec current_transmission() :: BB.Transmission.t() | nil + def current_transmission, do: Process.get(:bb_transmission) + # ---------------------------------------------------------------------------- # Position Commands # ---------------------------------------------------------------------------- diff --git a/lib/bb/actuator/server.ex b/lib/bb/actuator/server.ex index 8d096e2..750b740 100644 --- a/lib/bb/actuator/server.ex +++ b/lib/bb/actuator/server.ex @@ -18,14 +18,20 @@ defmodule BB.Actuator.Server do use GenServer + alias BB.Message alias BB.Parameter.Changed, as: ParameterChanged alias BB.Server.ParamResolution + alias BB.Transmission + alias BB.Transmission.Resolver, as: TransmissionResolver defstruct [ :callback_module, :resolved_opts, :raw_opts, :param_subscriptions, + :transmission, + :transmission_subscriptions, + :joint_name, :bb, :user_state ] @@ -35,6 +41,9 @@ defmodule BB.Actuator.Server do resolved_opts: keyword(), raw_opts: keyword(), param_subscriptions: %{[atom()] => atom()}, + transmission: Transmission.t() | nil, + transmission_subscriptions: %{atom() => [atom()]}, + joint_name: atom() | nil, bb: %{robot: module(), path: [atom()]}, user_state: term() } @@ -58,28 +67,34 @@ defmodule BB.Actuator.Server do {param_subscriptions, resolved_opts} = ParamResolution.resolve_and_subscribe(raw_opts, bb.robot) + joint_name = joint_for_actuator(bb) + + {transmission, transmission_subscriptions} = + if joint_name do + TransmissionResolver.resolve_and_subscribe(bb.robot, joint_name) + else + {nil, %{}} + end + + Process.put(:bb_transmission, transmission) + + base = %__MODULE__{ + callback_module: callback_module, + resolved_opts: resolved_opts, + raw_opts: raw_opts, + param_subscriptions: param_subscriptions, + transmission: transmission, + transmission_subscriptions: transmission_subscriptions, + joint_name: joint_name, + bb: bb + } + case callback_module.init(resolved_opts) do {:ok, user_state} -> - {:ok, - %__MODULE__{ - callback_module: callback_module, - resolved_opts: resolved_opts, - raw_opts: raw_opts, - param_subscriptions: param_subscriptions, - bb: bb, - user_state: user_state - }} + {:ok, %{base | user_state: user_state}} {:ok, user_state, timeout_or_continue} -> - {:ok, - %__MODULE__{ - callback_module: callback_module, - resolved_opts: resolved_opts, - raw_opts: raw_opts, - param_subscriptions: param_subscriptions, - bb: bb, - user_state: user_state - }, timeout_or_continue} + {:ok, %{base | user_state: user_state}, timeout_or_continue} {:stop, reason} -> {:stop, reason} @@ -89,8 +104,34 @@ defmodule BB.Actuator.Server do end end + defp joint_for_actuator(%{robot: robot_module, path: path}) do + actuator_name = List.last(path) + robot = robot_module.robot() + + case Map.get(robot.actuators, actuator_name) do + %{joint: joint_name} -> joint_name + _ -> nil + end + end + @impl GenServer def handle_info({:bb, [:param | param_path], %{payload: %ParameterChanged{}}}, state) do + state = + case TransmissionResolver.handle_change( + param_path, + state.transmission, + state.transmission_subscriptions, + state.bb.robot, + state.joint_name + ) do + {:changed, new_transmission} -> + Process.put(:bb_transmission, new_transmission) + %{state | transmission: new_transmission} + + :ignored -> + state + end + case ParamResolution.handle_change( param_path, state.param_subscriptions, @@ -111,7 +152,14 @@ defmodule BB.Actuator.Server do end end - def handle_info(msg, state) do + def handle_info({:bb, topic, %Message{} = message}, state) do + transformed = Transmission.apply_to_command(message, state.transmission) + delegate_handle_info({:bb, topic, transformed}, state) + end + + def handle_info(msg, state), do: delegate_handle_info(msg, state) + + defp delegate_handle_info(msg, state) do case state.callback_module.handle_info(msg, state.user_state) do {:noreply, new_user_state} -> {:noreply, %{state | user_state: new_user_state}} @@ -125,7 +173,14 @@ defmodule BB.Actuator.Server do end @impl GenServer - def handle_call(request, from, state) do + def handle_call({:command, %Message{} = message}, from, state) do + transformed = Transmission.apply_to_command(message, state.transmission) + delegate_handle_call({:command, transformed}, from, state) + end + + def handle_call(request, from, state), do: delegate_handle_call(request, from, state) + + defp delegate_handle_call(request, from, state) do case state.callback_module.handle_call(request, from, state.user_state) do {:reply, reply, new_user_state} -> {:reply, reply, %{state | user_state: new_user_state}} @@ -148,7 +203,14 @@ defmodule BB.Actuator.Server do end @impl GenServer - def handle_cast(request, state) do + def handle_cast({:command, %Message{} = message}, state) do + transformed = Transmission.apply_to_command(message, state.transmission) + delegate_handle_cast({:command, transformed}, state) + end + + def handle_cast(request, state), do: delegate_handle_cast(request, state) + + defp delegate_handle_cast(request, state) do case state.callback_module.handle_cast(request, state.user_state) do {:noreply, new_user_state} -> {:noreply, %{state | user_state: new_user_state}} diff --git a/lib/bb/dsl.ex b/lib/bb/dsl.ex index f67f097..443d03e 100644 --- a/lib/bb/dsl.ex +++ b/lib/bb/dsl.ex @@ -206,6 +206,42 @@ defmodule BB.Dsl do ] } + @transmission %Entity{ + name: :transmission, + describe: """ + A mechanical transmission between the joint and its actuator(s). + + Captures the relationship between joint-space command and motor-space + command: gear reduction, zero-offset, and polarity. The URDF equivalent + is ``. + """, + target: BB.Dsl.Transmission, + identifier: {:auto, :unique_integer}, + imports: [BB.Unit, BB.Dsl.ParamRef], + schema: [ + reduction: [ + type: {:or, [:float, {:struct, BB.Dsl.ParamRef}]}, + doc: + "Gear ratio between actuator and joint. A reduction of `n` means the actuator rotates `n` times for one rotation of the joint. Defaults to `1.0` (direct drive). May be a `param/1` reference.", + required: false, + default: 1.0 + ], + offset: [ + type: {:or, [unit_type(compatible: :degree), unit_type(compatible: :meter)]}, + doc: + "Zero-point offset between joint frame and actuator frame: the joint angle (or linear position) corresponding to the actuator's zero. May be a `param/1` reference.", + required: false + ], + reversed?: [ + type: {:or, [:boolean, {:struct, BB.Dsl.ParamRef}]}, + doc: + "Whether actuator motion is reversed relative to joint motion. May be a `param/1` reference.", + required: false, + default: false + ] + ] + } + @sensor %Entity{ name: :sensor, describe: "A sensor attached to the robot, a link, or a joint.", @@ -281,11 +317,12 @@ defmodule BB.Dsl do ], dynamics: [@dynamics], limit: [@limit], + transmission: [@transmission], sensors: [@sensor], actuators: [@actuator] ], recursive_as: :joints, - singleton_entity_keys: [:dynamics, :origin, :axis, :link, :limit], + singleton_entity_keys: [:dynamics, :origin, :axis, :link, :limit, :transmission], schema: [ name: [ type: :atom, diff --git a/lib/bb/dsl/joint.ex b/lib/bb/dsl/joint.ex index d2c6df3..66c9360 100644 --- a/lib/bb/dsl/joint.ex +++ b/lib/bb/dsl/joint.ex @@ -15,10 +15,11 @@ defmodule BB.Dsl.Joint do link: nil, dynamics: nil, limit: nil, + transmission: nil, sensors: [], actuators: [] - alias BB.Dsl.{Actuator, Axis, Dynamics, Limit, Link, Origin, Sensor} + alias BB.Dsl.{Actuator, Axis, Dynamics, Limit, Link, Origin, Sensor, Transmission} alias Spark.Dsl.Entity @type t :: %__MODULE__{ @@ -31,6 +32,7 @@ defmodule BB.Dsl.Joint do link: Link.t(), dynamics: nil | Dynamics.t(), limit: nil | Limit.t(), + transmission: nil | Transmission.t(), sensors: [Sensor.t()], actuators: [Actuator.t()] } diff --git a/lib/bb/dsl/transmission.ex b/lib/bb/dsl/transmission.ex new file mode 100644 index 0000000..34c4a51 --- /dev/null +++ b/lib/bb/dsl/transmission.ex @@ -0,0 +1,28 @@ +# SPDX-FileCopyrightText: 2025 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Dsl.Transmission do + @moduledoc """ + Mechanical transmission between a joint and its actuator(s). + + Captures the relationship between joint-space command and motor-space + command — gear reduction, zero-offset, and polarity. The URDF equivalent + is ``. + """ + defstruct __identifier__: nil, + __spark_metadata__: nil, + reduction: 1.0, + offset: nil, + reversed?: false + + alias Spark.Dsl.Entity + + @type t :: %__MODULE__{ + __identifier__: any, + __spark_metadata__: Entity.spark_meta(), + reduction: number, + offset: nil | Localize.Unit.t(), + reversed?: boolean + } +end diff --git a/lib/bb/dsl/validate_limit_units_transformer.ex b/lib/bb/dsl/validate_limit_units_transformer.ex index 0ad5386..3edc1c4 100644 --- a/lib/bb/dsl/validate_limit_units_transformer.ex +++ b/lib/bb/dsl/validate_limit_units_transformer.ex @@ -23,7 +23,7 @@ defmodule BB.Dsl.ValidateLimitUnitsTransformer do use Spark.Dsl.Transformer - alias BB.Dsl.{Joint, Limit, Link, ParamRef} + alias BB.Dsl.{Joint, Limit, Link, ParamRef, Transmission} alias BB.Unit alias Spark.Dsl.Transformer alias Spark.Error.DslError @@ -89,45 +89,79 @@ defmodule BB.Dsl.ValidateLimitUnitsTransformer do end) end - defp validate_joint(%Joint{limit: nil}, _path, _module), do: :ok - - defp validate_joint(%Joint{type: type, limit: %Limit{} = limit}, path, module) + defp validate_joint(%Joint{type: type} = joint, path, module) when type in @rotational_types do - check_fields(limit, @rotational_units, path, module, type) + with :ok <- check_limit(joint.limit, @rotational_units, path, module, type) do + check_transmission_offset(joint.transmission, :degree, path, module, type) + end end - defp validate_joint(%Joint{type: type, limit: %Limit{} = limit}, path, module) + defp validate_joint(%Joint{type: type} = joint, path, module) when type in @linear_types do - check_fields(limit, @linear_units, path, module, type) + with :ok <- check_limit(joint.limit, @linear_units, path, module, type) do + check_transmission_offset(joint.transmission, :meter, path, module, type) + end end defp validate_joint(%Joint{}, _path, _module), do: :ok + defp check_limit(nil, _expected_units, _path, _module, _joint_type), do: :ok + + defp check_limit(%Limit{} = limit, expected_units, path, module, joint_type) do + check_fields(limit, expected_units, path, module, joint_type) + end + + defp check_transmission_offset(nil, _expected_unit, _path, _module, _joint_type), do: :ok + + defp check_transmission_offset( + %Transmission{offset: offset}, + expected_unit, + path, + module, + joint_type + ) do + case check_field(offset, :transmission, :offset, expected_unit, path, module, joint_type) do + {:cont, :ok} -> :ok + {:halt, error} -> error + end + end + defp check_fields(limit, expected_units, path, module, joint_type) do Enum.reduce_while(expected_units, :ok, fn {field, expected_unit}, :ok -> - check_field(Map.get(limit, field), field, expected_unit, path, module, joint_type) + check_field(Map.get(limit, field), :limit, field, expected_unit, path, module, joint_type) end) end - defp check_field(nil, _field, _expected_unit, _path, _module, _joint_type), do: {:cont, :ok} + defp check_field(nil, _section, _field, _expected_unit, _path, _module, _joint_type), + do: {:cont, :ok} - defp check_field(%ParamRef{}, _field, _expected_unit, _path, _module, _joint_type), + defp check_field(%ParamRef{}, _section, _field, _expected_unit, _path, _module, _joint_type), do: {:cont, :ok} - defp check_field(%Localize.Unit{} = value, field, expected_unit, path, module, joint_type) do + defp check_field( + %Localize.Unit{} = value, + section, + field, + expected_unit, + path, + module, + joint_type + ) do if Unit.compatible?(value, expected_unit) do {:cont, :ok} else - {:halt, {:error, mismatch_error(module, path, field, value, expected_unit, joint_type)}} + {:halt, + {:error, mismatch_error(module, path, section, field, value, expected_unit, joint_type)}} end end - defp check_field(_value, _field, _expected_unit, _path, _module, _joint_type), do: {:cont, :ok} + defp check_field(_value, _section, _field, _expected_unit, _path, _module, _joint_type), + do: {:cont, :ok} - defp mismatch_error(module, path, field, value, expected_unit, joint_type) do + defp mismatch_error(module, path, section, field, value, expected_unit, joint_type) do DslError.exception( module: module, - path: path ++ [:limit, field], + path: path ++ [section, field], message: """ The unit `#{value.name}` provided for `#{field}` is not compatible with a `#{joint_type}` joint. diff --git a/lib/bb/dsl/verifiers/validate_param_refs.ex b/lib/bb/dsl/verifiers/validate_param_refs.ex index 626c723..af9f3e3 100644 --- a/lib/bb/dsl/verifiers/validate_param_refs.ex +++ b/lib/bb/dsl/verifiers/validate_param_refs.ex @@ -13,7 +13,7 @@ defmodule BB.Dsl.Verifiers.ValidateParamRefs do use Spark.Dsl.Verifier - alias BB.Dsl.{Axis, Dynamics, Inertia, Inertial, Joint, Limit, Link, Origin, ParamRef} + alias BB.Dsl.{Axis, Dynamics, Inertia, Inertial, Joint, Limit, Link, Origin, ParamRef, Transmission} alias BB.Unit alias Spark.Dsl.Verifier alias Spark.Error.DslError @@ -70,6 +70,7 @@ defmodule BB.Dsl.Verifiers.ValidateParamRefs do axis_refs = collect_from_axis(joint.axis, joint_path ++ [:axis]) limit_refs = collect_from_limit(joint.limit, joint_path ++ [:limit]) dynamics_refs = collect_from_dynamics(joint.dynamics, joint_path ++ [:dynamics]) + transmission_refs = collect_from_transmission(joint.transmission, joint_path ++ [:transmission]) nested_refs = case joint.link do @@ -77,7 +78,7 @@ defmodule BB.Dsl.Verifiers.ValidateParamRefs do nested_link -> collect_from_entity(nested_link, path_prefix) end - origin_refs ++ axis_refs ++ limit_refs ++ dynamics_refs ++ nested_refs + origin_refs ++ axis_refs ++ limit_refs ++ dynamics_refs ++ transmission_refs ++ nested_refs end defp collect_from_entity(_entity, _path_prefix), do: [] @@ -118,6 +119,18 @@ defmodule BB.Dsl.Verifiers.ValidateParamRefs do end) end + defp collect_from_transmission(nil, _path), do: [] + + defp collect_from_transmission(%Transmission{} = transmission, path) do + [:reduction, :offset, :reversed?] + |> Enum.flat_map(fn field -> + case Map.get(transmission, field) do + %ParamRef{} = ref -> [{ref, path ++ [field]}] + _ -> [] + end + end) + end + defp collect_from_dynamics(nil, _path), do: [] defp collect_from_dynamics(%Dynamics{} = dynamics, path) do diff --git a/lib/bb/robot/builder.ex b/lib/bb/robot/builder.ex index 5e9eeae..dc0fa7f 100644 --- a/lib/bb/robot/builder.ex +++ b/lib/bb/robot/builder.ex @@ -132,6 +132,9 @@ defmodule BB.Robot.Builder do {limits, limits_subs} = convert_limits(dsl_joint.limit, dsl_joint.type, joint_name) {dynamics, dynamics_subs} = convert_dynamics(dsl_joint.dynamics, dsl_joint.type, joint_name) + {transmission, transmission_subs} = + convert_transmission(dsl_joint.transmission, dsl_joint.type, joint_name) + joint = %Joint{ name: joint_name, type: dsl_joint.type, @@ -141,14 +144,68 @@ defmodule BB.Robot.Builder do axis: axis, limits: limits, dynamics: dynamics, + transmission: transmission, sensors: Enum.map(dsl_joint.sensors, & &1.name), actuators: Enum.map(dsl_joint.actuators, & &1.name) } - param_subs = origin_subs ++ axis_subs ++ limits_subs ++ dynamics_subs + param_subs = + origin_subs ++ axis_subs ++ limits_subs ++ dynamics_subs ++ transmission_subs + {joint, param_subs} end + defp convert_transmission(nil, _type, _joint_name), do: {nil, []} + + defp convert_transmission(%Dsl.Transmission{} = transmission, type, joint_name) do + offset_converter = offset_converter_for(type) + + {reduction, reduction_subs} = + convert_with_default( + transmission.reduction, + & &1, + 1.0, + joint_name, + [:transmission, :reduction] + ) + + {offset, offset_subs} = + convert_with_default( + transmission.offset, + offset_converter, + 0.0, + joint_name, + [:transmission, :offset] + ) + + {reversed?, reversed_subs} = + convert_with_default( + transmission.reversed?, + & &1, + false, + joint_name, + [:transmission, :reversed?] + ) + + converted = %{reduction: reduction, offset: offset, reversed?: reversed?} + {converted, reduction_subs ++ offset_subs ++ reversed_subs} + end + + defp convert_with_default(nil, _converter, default, _joint_name, _field_path), + do: {default, []} + + defp convert_with_default(%ParamRef{path: path}, _converter, _default, joint_name, field_path), + do: {nil, [{path, {:joint, joint_name, field_path}}]} + + defp convert_with_default(value, converter, _default, _joint_name, _field_path), + do: {converter.(value), []} + + defp offset_converter_for(type) when type in [:revolute, :continuous], + do: &Units.to_radians/1 + + defp offset_converter_for(:prismatic), do: &Units.to_meters/1 + defp offset_converter_for(_), do: &Units.to_radians/1 + defp convert_mass(nil), do: nil defp convert_mass(%Dsl.Inertial{mass: nil}), do: nil defp convert_mass(%Dsl.Inertial{mass: mass}), do: Units.to_kilograms(mass) diff --git a/lib/bb/robot/joint.ex b/lib/bb/robot/joint.ex index 4af0fe2..6bbb05c 100644 --- a/lib/bb/robot/joint.ex +++ b/lib/bb/robot/joint.ex @@ -19,6 +19,7 @@ defmodule BB.Robot.Joint do :axis, :limits, :dynamics, + :transmission, :sensors, :actuators ] @@ -34,6 +35,7 @@ defmodule BB.Robot.Joint do axis: axis() | nil, limits: limits() | nil, dynamics: dynamics() | nil, + transmission: transmission() | nil, sensors: [atom()], actuators: [atom()] } @@ -92,6 +94,26 @@ defmodule BB.Robot.Joint do friction: float() | nil } + @typedoc """ + Mechanical transmission between this joint and its actuator(s). + + - `reduction`: gear ratio (dimensionless). `1.0` means direct drive. + - `offset`: joint-space value (radians for rotational joints, metres for + linear joints) corresponding to the actuator's zero. `nil` means no + offset is applied. + - `reversed?`: whether actuator motion is reversed relative to joint + motion. + + Any field may be `nil` when its DSL value is a `ParamRef` awaiting + resolution at runtime; the `BB.Transmission.Server` resolves and stores + the live values. + """ + @type transmission :: %{ + reduction: float() | nil, + offset: float() | nil, + reversed?: boolean() | nil + } + @doc """ Check if this joint is rotational (revolute or continuous). """ diff --git a/lib/bb/sim/actuator.ex b/lib/bb/sim/actuator.ex index 0070286..12cec6e 100644 --- a/lib/bb/sim/actuator.ex +++ b/lib/bb/sim/actuator.ex @@ -32,6 +32,7 @@ defmodule BB.Sim.Actuator do alias BB.Message.Actuator.Command alias BB.PubSub alias BB.Robot + alias BB.Transmission defstruct [ :bb, @@ -72,7 +73,7 @@ defmodule BB.Sim.Actuator do @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)} + {:noreply, do_set_position(joint_position(cmd.position), cmd.command_id, state)} end def handle_info({:bb, _path, %Message{payload: %Command.Stop{}}}, state) do @@ -89,7 +90,7 @@ defmodule BB.Sim.Actuator do @impl BB.Actuator def handle_cast({:command, %Message{payload: %Command.Position{} = cmd}}, state) do - {:noreply, do_set_position(cmd.position, cmd.command_id, state)} + {:noreply, do_set_position(joint_position(cmd.position), cmd.command_id, state)} end def handle_cast({:command, _message}, state) do @@ -98,7 +99,7 @@ defmodule BB.Sim.Actuator do @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) + new_state = do_set_position(joint_position(cmd.position), cmd.command_id, state) {:reply, {:ok, :accepted}, new_state} end @@ -316,4 +317,14 @@ defmodule BB.Sim.Actuator do defp clamp_upper(position, nil), do: position defp clamp_upper(position, upper), do: min(position, upper) + + # Recover joint-space from the motor-space command the wrapper handed us. + # When the joint has no transmission, the wrapper passes the value through + # unchanged and there is nothing to undo. + defp joint_position(motor_position) do + case BB.Actuator.current_transmission() do + nil -> motor_position + transmission -> Transmission.unapply_position(motor_position, transmission) + end + end end diff --git a/lib/bb/transmission.ex b/lib/bb/transmission.ex new file mode 100644 index 0000000..553fc52 --- /dev/null +++ b/lib/bb/transmission.ex @@ -0,0 +1,144 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Transmission do + @moduledoc """ + Mechanical transmission math: convert quantities between joint-space and + motor-space. + + A transmission captures three properties of the linkage between a joint + and its actuator: + + - `reduction` — the gear ratio. A reduction of `n` means the actuator + rotates `n` times for one rotation of the joint, so motor angular + motion is `n` times the joint motion, and motor torque is `1 / n` + times the joint torque. + - `offset` — the joint-space value (radians for rotational joints, metres + for linear joints) corresponding to the actuator's zero position. + Applies to positions only — velocities and accelerations have no + offset since they are rates. + - `reversed?` — whether actuator motion is reversed relative to joint + motion. + + All values are floats in SI base units (radians, metres, rad/s, m/s, + N·m, N). + + ## Equations + + Position (joint → motor): `motor = sign × reduction × (joint − offset)` + Position (motor → joint): `joint = sign × motor / reduction + offset` + Rate (velocity, acceleration): `motor = sign × reduction × joint` + Effort (torque, force): `motor = sign × joint / reduction` + + Where `sign = -1` if `reversed?`, otherwise `+1`. Each pair of + apply/unapply is an exact inverse within float precision. + """ + + @type t :: %{ + required(:reduction) => float, + required(:offset) => float, + required(:reversed?) => boolean + } + + @doc "Convert a joint-space position into a motor-space position." + @spec apply_position(float, t) :: float + def apply_position(value, %{reduction: r, offset: o, reversed?: rev}), + do: sign(rev) * r * (value - o) + + @doc "Convert a motor-space position into a joint-space position." + @spec unapply_position(float, t) :: float + def unapply_position(value, %{reduction: r, offset: o, reversed?: rev}), + do: sign(rev) * value / r + o + + @doc """ + Convert a joint-space rate (velocity or acceleration) into motor-space. + + No offset is applied since the offset is a constant in position space. + """ + @spec apply_rate(float, t) :: float + def apply_rate(value, %{reduction: r, reversed?: rev}), + do: sign(rev) * r * value + + @doc "Convert a motor-space rate into a joint-space rate." + @spec unapply_rate(float, t) :: float + def unapply_rate(value, %{reduction: r, reversed?: rev}), + do: sign(rev) * value / r + + @doc """ + Convert a joint-space effort (torque for rotational, force for linear) + into motor-space. + + A gear reduction multiplies position and velocity but divides effort: + a 50:1 reduction means the motor needs `1 / 50` of the joint torque. + """ + @spec apply_effort(float, t) :: float + def apply_effort(value, %{reduction: r, reversed?: rev}), + do: sign(rev) * value / r + + @doc "Convert a motor-space effort into a joint-space effort." + @spec unapply_effort(float, t) :: float + def unapply_effort(value, %{reduction: r, reversed?: rev}), + do: sign(rev) * value * r + + defp sign(true), do: -1.0 + defp sign(false), do: 1.0 + + # ---------------------------------------------------------------------------- + # Message-level helpers + # ---------------------------------------------------------------------------- + + alias BB.Message + alias BB.Message.Actuator.Command + + @doc """ + Apply a transmission to an actuator command message, returning a new + message with the payload values transformed into motor-space. + + Position, velocity, and effort commands are transformed; hold and stop + commands have no values to transform and are returned unchanged. + Trajectory waypoints are transformed pointwise. + + When `transmission` is `nil`, the message is returned unchanged. + """ + @spec apply_to_command(Message.t(), t() | nil) :: Message.t() + def apply_to_command(message, nil), do: message + + def apply_to_command(%Message{payload: payload} = message, transmission) do + %{message | payload: apply_to_payload(payload, transmission)} + end + + defp apply_to_payload(%Command.Position{} = cmd, t) do + %{ + cmd + | position: apply_position(cmd.position, t), + velocity: maybe_apply_rate(cmd.velocity, t) + } + end + + defp apply_to_payload(%Command.Velocity{velocity: v} = cmd, t) do + %{cmd | velocity: apply_rate(v, t)} + end + + defp apply_to_payload(%Command.Effort{effort: e} = cmd, t) do + %{cmd | effort: apply_effort(e, t)} + end + + defp apply_to_payload(%Command.Trajectory{waypoints: waypoints} = cmd, t) do + %{cmd | waypoints: Enum.map(waypoints, &apply_to_waypoint(&1, t))} + end + + defp apply_to_payload(other, _t), do: other + + defp apply_to_waypoint(%{position: p, velocity: v, acceleration: a} = wp, t) do + %{ + wp + | position: apply_position(p, t), + velocity: apply_rate(v, t), + acceleration: apply_rate(a, t) + } + end + + defp maybe_apply_rate(nil, _t), do: nil + defp maybe_apply_rate(value, t), do: apply_rate(value, t) +end diff --git a/lib/bb/transmission/resolver.ex b/lib/bb/transmission/resolver.ex new file mode 100644 index 0000000..f8ed299 --- /dev/null +++ b/lib/bb/transmission/resolver.ex @@ -0,0 +1,120 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Transmission.Resolver do + @moduledoc """ + Resolve a joint's transmission against the runtime parameter store. + + Transmissions can carry `BB.Dsl.ParamRef` values for any of their fields + (`reduction`, `offset`, `reversed?`). At compile time those fields appear + as `nil` on the optimised joint struct and the parameter paths are + recorded in `BB.Robot.param_subscriptions`. This module fills the nil + fields in with current parameter values at process startup and + re-resolves when the parameter changes. + """ + + alias BB.PubSub + alias BB.Robot.State, as: RobotState + alias BB.Robot.Units + alias BB.Robot.Runtime + + @type field :: :reduction | :offset | :reversed? + @type subscriptions :: %{field() => [atom()]} + + @doc """ + Resolve a joint's transmission for use by a server process and subscribe + to parameter changes for any parameterised fields. + + Returns `{resolved_transmission_or_nil, subscriptions_map}`. The + subscriptions map maps each transmission field that came from a `param/1` + reference to its parameter path; the empty map means everything was a + literal at compile time. + + When the joint has no transmission, returns `{nil, %{}}` and subscribes + to nothing. + """ + @spec resolve_and_subscribe(module(), atom()) :: + {BB.Transmission.t() | nil, subscriptions()} + def resolve_and_subscribe(robot_module, joint_name) do + robot = robot_module.robot() + + case robot.joints[joint_name].transmission do + nil -> + {nil, %{}} + + transmission -> + subscriptions = collect_subscriptions(robot.param_subscriptions, joint_name) + robot_state = Runtime.get_robot_state(robot_module) + resolved = resolve_fields(transmission, subscriptions, robot, joint_name, robot_state) + + for path <- Map.values(subscriptions) do + PubSub.subscribe(robot_module, [:param | path]) + end + + {resolved, subscriptions} + end + end + + @doc """ + Re-resolve the transmission after a parameter change. + + Call from a server's `handle_info` when a `[:param | path]` message + arrives. Returns `{:changed, new_transmission}` if the change affects + this transmission, or `:ignored` otherwise. + """ + @spec handle_change( + [atom()], + BB.Transmission.t(), + subscriptions(), + module(), + atom() + ) :: {:changed, BB.Transmission.t()} | :ignored + def handle_change(param_path, current, subscriptions, robot_module, joint_name) do + if param_path in Map.values(subscriptions) do + robot = robot_module.robot() + robot_state = Runtime.get_robot_state(robot_module) + + new_transmission = + resolve_fields(current, subscriptions, robot, joint_name, robot_state) + + {:changed, new_transmission} + else + :ignored + end + end + + defp collect_subscriptions(param_subscriptions, joint_name) do + Enum.flat_map(param_subscriptions, fn {param_path, locations} -> + Enum.flat_map(locations, fn + {:joint, ^joint_name, [:transmission, field]} -> [{field, param_path}] + _ -> [] + end) + end) + |> Enum.into(%{}) + end + + defp resolve_fields(transmission, subscriptions, robot, joint_name, robot_state) do + joint_type = robot.joints[joint_name].type + + Enum.reduce(subscriptions, transmission, fn {field, path}, acc -> + {:ok, raw_value} = RobotState.get_parameter(robot_state, path) + Map.put(acc, field, convert_for_field(field, raw_value, joint_type)) + end) + end + + defp convert_for_field(:reduction, value, _joint_type) when is_number(value), + do: value * 1.0 + + defp convert_for_field(:reversed?, value, _joint_type) when is_boolean(value), do: value + + defp convert_for_field(:offset, %Localize.Unit{} = value, type) + when type in [:revolute, :continuous], + do: Units.to_radians(value) + + defp convert_for_field(:offset, %Localize.Unit{} = value, :prismatic), + do: Units.to_meters(value) + + defp convert_for_field(:offset, %Localize.Unit{} = value, _), + do: Units.to_radians(value) +end diff --git a/test/bb/actuator/server_transmission_test.exs b/test/bb/actuator/server_transmission_test.exs new file mode 100644 index 0000000..64e57a6 --- /dev/null +++ b/test/bb/actuator/server_transmission_test.exs @@ -0,0 +1,137 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Actuator.ServerTransmissionTest do + use ExUnit.Case + + alias BB.Message + alias BB.Message.Actuator.Command + + defmodule WithTransmission do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reduction 50.0 + offset(~u(45 degree)) + reversed? true + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + actuator :motor, BB.Test.RecordingActuator + + link :arm + end + end + end + end + + defmodule WithoutTransmission do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + actuator :motor, BB.Test.RecordingActuator + + link :arm + end + end + end + end + + @transmission %{reduction: 50.0, offset: :math.pi() / 4, reversed?: true} + + defp start_robot(robot_module) do + :persistent_term.put({BB.Test.RecordingActuator, robot_module}, self()) + start_supervised!(robot_module) + on_exit(fn -> :persistent_term.erase({BB.Test.RecordingActuator, robot_module}) end) + end + + describe "with a transmission" do + setup do + start_robot(WithTransmission) + :ok + end + + test "transforms a Position command from joint-space to motor-space (pubsub)" do + :ok = + BB.publish( + WithTransmission, + [:actuator, :base, :shoulder, :motor], + Message.new!(Command.Position, :motor, position: :math.pi() / 4 + 0.01) + ) + + assert_receive {:received, :info, %Message{payload: %Command.Position{} = cmd}}, 500 + + expected = BB.Transmission.apply_position(:math.pi() / 4 + 0.01, @transmission) + assert_in_delta cmd.position, expected, 1.0e-9 + end + + test "transforms a Position command via cast" do + message = Message.new!(Command.Position, :motor, position: :math.pi() / 4 + 0.01) + :ok = BB.cast(WithTransmission, :motor, {:command, message}) + + assert_receive {:received, :cast, %Message{payload: %Command.Position{} = cmd}}, 500 + expected = BB.Transmission.apply_position(:math.pi() / 4 + 0.01, @transmission) + assert_in_delta cmd.position, expected, 1.0e-9 + end + + test "transforms a Position command via call" do + message = Message.new!(Command.Position, :motor, position: :math.pi() / 4 + 0.01) + {:ok, :accepted} = BB.call(WithTransmission, :motor, {:command, message}, 500) + + assert_receive {:received, :call, %Message{payload: %Command.Position{} = cmd}}, 500 + expected = BB.Transmission.apply_position(:math.pi() / 4 + 0.01, @transmission) + assert_in_delta cmd.position, expected, 1.0e-9 + end + + test "transforms the velocity hint on a Position command" do + message = + Message.new!(Command.Position, :motor, position: :math.pi() / 4, velocity: 0.1) + + :ok = BB.cast(WithTransmission, :motor, {:command, message}) + + assert_receive {:received, :cast, %Message{payload: %Command.Position{} = cmd}}, 500 + assert_in_delta cmd.velocity, -50.0 * 0.1, 1.0e-9 + end + + test "passes Hold commands through unchanged" do + hold = Message.new!(Command.Hold, :motor, []) + :ok = BB.cast(WithTransmission, :motor, {:command, hold}) + + assert_receive {:received, :cast, %Message{payload: %Command.Hold{}}}, 500 + end + end + + describe "without a transmission" do + setup do + start_robot(WithoutTransmission) + :ok + end + + test "position commands flow through unchanged" do + message = Message.new!(Command.Position, :motor, position: 1.23) + :ok = BB.cast(WithoutTransmission, :motor, {:command, message}) + + assert_receive {:received, :cast, %Message{payload: %Command.Position{position: p}}}, 500 + assert p == 1.23 + end + end +end diff --git a/test/bb/dsl/transmission_test.exs b/test/bb/dsl/transmission_test.exs new file mode 100644 index 0000000..5c182b2 --- /dev/null +++ b/test/bb/dsl/transmission_test.exs @@ -0,0 +1,233 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Dsl.TransmissionTest do + use ExUnit.Case, async: true + + describe "literal values" do + test "joint without a transmission block has nil transmission on the robot" do + defmodule NoTransmission do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :arm + end + end + end + end + + joint = BB.Robot.get_joint(NoTransmission.robot(), :shoulder) + assert joint.transmission == nil + end + + test "transmission block converts to SI floats on the optimised robot" do + defmodule Literal do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reduction 50.0 + offset(~u(45 degree)) + reversed? true + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :arm + end + end + end + end + + transmission = BB.Robot.get_joint(Literal.robot(), :shoulder).transmission + assert transmission.reduction == 50.0 + assert_in_delta transmission.offset, :math.pi() / 4, 1.0e-9 + assert transmission.reversed? == true + end + + test "prismatic joint converts offset to metres" do + defmodule Prismatic do + use BB + + topology do + link :base do + joint :slider do + type :prismatic + + transmission do + reduction 2.0 + offset(~u(10 millimeter)) + end + + limit do + effort(~u(5 newton)) + velocity(~u(0.1 meter_per_second)) + end + + link :child + end + end + end + end + + transmission = BB.Robot.get_joint(Prismatic.robot(), :slider).transmission + assert_in_delta transmission.offset, 0.01, 1.0e-9 + assert transmission.reversed? == false + assert transmission.reduction == 2.0 + end + + test "defaults applied when only some fields are given" do + defmodule PartialDefaults do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reversed? true + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :arm + end + end + end + end + + transmission = BB.Robot.get_joint(PartialDefaults.robot(), :shoulder).transmission + assert transmission.reduction == 1.0 + assert transmission.offset == 0.0 + assert transmission.reversed? == true + end + end + + describe "joint-type unit validation" do + test "rejects a metre offset on a revolute joint" do + assert_raise Spark.Error.DslError, ~r/not compatible with a `revolute`/sm, fn -> + defmodule BadOffset do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + offset(~u(10 millimeter)) + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :arm + end + end + end + end + end + end + + test "rejects a degree offset on a prismatic joint" do + assert_raise Spark.Error.DslError, ~r/not compatible with a `prismatic`/sm, fn -> + defmodule BadOffsetPrismatic do + use BB + + topology do + link :base do + joint :slider do + type :prismatic + + transmission do + offset(~u(10 degree)) + end + + limit do + effort(~u(5 newton)) + velocity(~u(0.1 meter_per_second)) + end + + link :child + end + end + end + end + end + end + end + + describe "parameter references" do + test "reduction, offset, and reversed? all accept param/1" do + defmodule Parameterised do + use BB + + parameters do + group :tx do + param :reduction, type: :float, default: 50.0 + param :offset, type: {:unit, :degree}, default: ~u(45 degree) + param :reversed?, type: :boolean, default: true + end + end + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reduction(param([:tx, :reduction])) + offset(param([:tx, :offset])) + reversed?(param([:tx, :reversed?])) + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + link :arm + end + end + end + end + + robot = Parameterised.robot() + transmission = BB.Robot.get_joint(robot, :shoulder).transmission + + assert transmission.reduction == nil + assert transmission.offset == nil + assert transmission.reversed? == nil + + subs = robot.param_subscriptions + + assert Map.has_key?(subs, [:tx, :reduction]) + assert Map.has_key?(subs, [:tx, :offset]) + assert Map.has_key?(subs, [:tx, :reversed?]) + end + + end +end diff --git a/test/bb/sim/actuator_transmission_test.exs b/test/bb/sim/actuator_transmission_test.exs new file mode 100644 index 0000000..5b2efa8 --- /dev/null +++ b/test/bb/sim/actuator_transmission_test.exs @@ -0,0 +1,66 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Sim.ActuatorTransmissionTest do + use ExUnit.Case, async: false + + alias BB.Message + alias BB.Message.Actuator.BeginMotion + alias BB.PubSub + + describe "sim honours the joint's transmission" do + defmodule SimTxRobot do + @moduledoc false + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reduction 50.0 + offset(~u(45 degree)) + reversed? true + end + + limit do + lower(~u(-180 degree)) + upper(~u(180 degree)) + effort(~u(10 newton_meter)) + velocity(~u(60 degree_per_second)) + acceleration(~u(120 degree_per_square_second)) + end + + actuator :motor, ServoMotor + sensor :estimator, {BB.Sensor.OpenLoopPositionEstimator, actuator: :motor} + + link :arm + end + end + end + end + + test "BeginMotion target_position is joint-space, not motor-space" do + Process.flag(:trap_exit, true) + {:ok, pid} = SimTxRobot.start_link(simulation: :kinematic) + + PubSub.subscribe(SimTxRobot, [:actuator, :base, :shoulder, :motor]) + :ok = BB.Safety.arm(SimTxRobot) + + joint_target = :math.pi() / 4 + 0.01 + BB.Actuator.set_position!(SimTxRobot, :motor, joint_target) + + assert_receive {:bb, _path, + %Message{ + payload: %BeginMotion{target_position: target} + }}, + 1000 + + assert_in_delta target, joint_target, 1.0e-9 + + Supervisor.stop(pid) + end + end +end diff --git a/test/bb/transmission_test.exs b/test/bb/transmission_test.exs new file mode 100644 index 0000000..6a6070e --- /dev/null +++ b/test/bb/transmission_test.exs @@ -0,0 +1,112 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.TransmissionTest do + use ExUnit.Case, async: true + + alias BB.Transmission + + @identity %{reduction: 1.0, offset: 0.0, reversed?: false} + + describe "apply_position/2" do + test "identity transmission is a no-op" do + assert Transmission.apply_position(0.5, @identity) == 0.5 + assert Transmission.apply_position(-1.2, @identity) == -1.2 + end + + test "reduction scales the motor angle by the gear ratio" do + t = %{@identity | reduction: 50.0} + assert Transmission.apply_position(0.01, t) == 0.5 + end + + test "reversed? flips the sign" do + t = %{@identity | reversed?: true} + assert Transmission.apply_position(0.5, t) == -0.5 + end + + test "offset shifts the joint zero" do + t = %{@identity | offset: 0.1} + assert_in_delta Transmission.apply_position(0.1, t), 0.0, 1.0e-9 + assert_in_delta Transmission.apply_position(0.3, t), 0.2, 1.0e-9 + end + + test "reduction, offset, and reversed? combine" do + t = %{reduction: 50.0, offset: 0.1, reversed?: true} + assert_in_delta Transmission.apply_position(0.3, t), -10.0, 1.0e-9 + end + end + + describe "unapply_position/2" do + test "is the left inverse of apply_position/2" do + t = %{reduction: 50.0, offset: 0.1, reversed?: true} + + for v <- [-1.0, -0.5, 0.0, 0.1, 0.5, 1.0, 3.14] do + assert_in_delta Transmission.unapply_position(Transmission.apply_position(v, t), t), + v, + 1.0e-9 + end + end + + test "is the right inverse of apply_position/2" do + t = %{reduction: 30.0, offset: -0.05, reversed?: false} + + for v <- [-100.0, -1.0, 0.0, 1.0, 100.0] do + assert_in_delta Transmission.apply_position(Transmission.unapply_position(v, t), t), + v, + 1.0e-9 + end + end + end + + describe "apply_rate/2 and unapply_rate/2" do + test "rate has no offset component" do + t = %{reduction: 50.0, offset: 100.0, reversed?: false} + assert Transmission.apply_rate(0.01, t) == 0.5 + assert Transmission.unapply_rate(0.5, t) == 0.01 + end + + test "reduction scales rate" do + t = %{@identity | reduction: 50.0} + assert Transmission.apply_rate(0.1, t) == 5.0 + assert Transmission.unapply_rate(5.0, t) == 0.1 + end + + test "reversed? flips sign" do + t = %{@identity | reversed?: true, reduction: 2.0} + assert Transmission.apply_rate(1.0, t) == -2.0 + assert Transmission.unapply_rate(-2.0, t) == 1.0 + end + + test "apply ∘ unapply is identity" do + t = %{reduction: 17.5, offset: 0.3, reversed?: true} + + for v <- [-3.0, -0.5, 0.0, 0.5, 3.0] do + assert_in_delta Transmission.unapply_rate(Transmission.apply_rate(v, t), t), v, 1.0e-9 + end + end + end + + describe "apply_effort/2 and unapply_effort/2" do + test "effort divides by reduction (inverse of position)" do + t = %{@identity | reduction: 50.0} + assert Transmission.apply_effort(10.0, t) == 0.2 + assert Transmission.unapply_effort(0.2, t) == 10.0 + end + + test "reversed? flips sign for effort too" do + t = %{@identity | reversed?: true, reduction: 2.0} + assert Transmission.apply_effort(10.0, t) == -5.0 + end + + test "apply ∘ unapply is identity" do + t = %{reduction: 17.5, offset: 0.3, reversed?: true} + + for v <- [-3.0, -0.5, 0.0, 0.5, 3.0] do + assert_in_delta Transmission.unapply_effort(Transmission.apply_effort(v, t), t), + v, + 1.0e-9 + end + end + end +end diff --git a/test/support/recording_actuator.ex b/test/support/recording_actuator.ex new file mode 100644 index 0000000..cec7139 --- /dev/null +++ b/test/support/recording_actuator.ex @@ -0,0 +1,52 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +defmodule BB.Test.RecordingActuator do + @moduledoc """ + Actuator that forwards every received command message to a test process. + + Before starting the robot, put the recipient pid into persistent term: + + :persistent_term.put({BB.Test.RecordingActuator, MyRobot}, self()) + + The actuator looks up the recipient at init and sends + `{:received, kind, message}` for each incoming command. `kind` is one of + `:info`, `:cast`, or `:call`. + """ + use BB.Actuator, options_schema: [] + + @impl BB.Actuator + def disarm(_opts), do: :ok + + @impl BB.Actuator + def init(opts) do + bb = Keyword.fetch!(opts, :bb) + recipient = :persistent_term.get({__MODULE__, bb.robot}, nil) + BB.subscribe(bb.robot, [:actuator | bb.path]) + {:ok, %{recipient: recipient}} + end + + @impl BB.Actuator + def handle_info({:bb, _path, %BB.Message{} = message}, state) do + forward(state.recipient, :info, message) + {:noreply, state} + end + + def handle_info(_msg, state), do: {:noreply, state} + + @impl BB.Actuator + def handle_cast({:command, %BB.Message{} = message}, state) do + forward(state.recipient, :cast, message) + {:noreply, state} + end + + @impl BB.Actuator + def handle_call({:command, %BB.Message{} = message}, _from, state) do + forward(state.recipient, :call, message) + {:reply, {:ok, :accepted}, state} + end + + defp forward(nil, _kind, _message), do: :ok + defp forward(pid, kind, message), do: send(pid, {:received, kind, message}) +end From 330b7c6307dc76a91def0dd2893a1f52fd885f8c Mon Sep 17 00:00:00 2001 From: James Harton Date: Tue, 19 May 2026 08:26:10 +0000 Subject: [PATCH 2/4] feature(urdf): parse and emit `` for SimpleTransmission MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The URDF parser now captures `` blocks (URDF-standard SimpleTransmission only — coupled types warn and skip as before) and exposes them on the parsed robot keyed by joint name. The importer emits a `transmission do reduction N end` block inside each joint with a non-1.0 mechanical reduction. The exporter emits `` XML for joints with a non-1.0 reduction. URDF does not have a standard place for the BB-specific `offset` and `reversed?` fields, so transmissions with only those extensions are deliberately not round-tripped through URDF — that's a documented limitation rather than a bug. `BB.Urdf.Xml.element/3` now also accepts a binary text body for elements like `...` and `...`. Refs: beam-bots/bb#108 --- .formatter.exs | 5 + lib/bb/urdf/exporter.ex | 47 ++++++++- lib/bb/urdf/importer.ex | 40 ++++++-- lib/bb/urdf/parser.ex | 105 ++++++++++++++++++-- lib/bb/urdf/xml.ex | 15 ++- test/bb/urdf/exporter_test.exs | 68 +++++++++++++ test/bb/urdf/importer_test.exs | 29 ++++++ test/bb/urdf/parser_test.exs | 41 ++++++++ test/fixtures/urdf/simple_transmission.urdf | 22 ++++ 9 files changed, 351 insertions(+), 21 deletions(-) create mode 100644 test/fixtures/urdf/simple_transmission.urdf diff --git a/.formatter.exs b/.formatter.exs index 3142e38..613bc33 100644 --- a/.formatter.exs +++ b/.formatter.exs @@ -74,6 +74,7 @@ spark_locals_without_parens = [ mesh: 1, min: 1, name: 1, + offset: 1, origin: 0, origin: 1, param: 1, @@ -82,9 +83,11 @@ spark_locals_without_parens = [ pitch: 1, radius: 1, red: 1, + reduction: 1, registry_module: 1, registry_options: 1, required: 1, + reversed?: 1, roll: 1, scale: 1, sensor: 2, @@ -100,6 +103,8 @@ spark_locals_without_parens = [ timeout: 1, topology_max_restarts: 1, topology_max_seconds: 1, + transmission: 0, + transmission: 1, type: 1, upper: 1, velocity: 1, diff --git a/lib/bb/urdf/exporter.ex b/lib/bb/urdf/exporter.ex index 965a735..a11b77d 100644 --- a/lib/bb/urdf/exporter.ex +++ b/lib/bb/urdf/exporter.ex @@ -29,11 +29,17 @@ defmodule BB.Urdf.Exporter do """ @spec export_robot(Robot.t()) :: {:ok, String.t()} def export_robot(%Robot{} = robot) do + joints_in_order = Robot.joints_in_order(robot) links = Robot.links_in_order(robot) |> Enum.map(&build_link_element/1) - joints = Robot.joints_in_order(robot) |> Enum.map(&build_joint_element/1) + joints = Enum.map(joints_in_order, &build_joint_element/1) + transmissions = Enum.flat_map(joints_in_order, &build_transmission_elements(&1, robot)) xml = - Xml.element(:robot, [name: format_robot_name(robot.name)], links ++ joints) + Xml.element( + :robot, + [name: format_robot_name(robot.name)], + links ++ joints ++ transmissions + ) |> Xml.to_string() {:ok, xml} @@ -246,4 +252,41 @@ defmodule BB.Urdf.Exporter do if Enum.empty?(attrs), do: nil, else: Xml.element(:dynamics, attrs) end + + # URDF's `` standard only carries `mechanicalReduction`. + # `offset` and `reversed?` are BB extensions and have no round-trip + # representation. When a transmission has only those non-URDF properties + # (reduction == 1.0), no `` block is emitted. + defp build_transmission_elements(%Joint{transmission: nil}, _robot), do: [] + + defp build_transmission_elements(%Joint{transmission: %{reduction: 1.0}}, _robot), do: [] + + defp build_transmission_elements(%Joint{} = joint, robot) do + actuator_name = + case joint.actuators do + [first | _] -> first + [] -> :"#{joint.name}_motor" + end + + transmission_name = "#{joint.name}_trans" + + actuator_attrs = [name: Atom.to_string(actuator_name)] + + actuator_children = [ + Xml.element( + :mechanicalReduction, + [], + Xml.format_float(joint.transmission.reduction) + ) + ] + + children = [ + Xml.element(:type, [], "transmission_interface/SimpleTransmission"), + Xml.element(:joint, [name: Atom.to_string(joint.name)], []), + Xml.element(:actuator, actuator_attrs, actuator_children) + ] + + _ = robot + [Xml.element(:transmission, [name: transmission_name], children)] + end end diff --git a/lib/bb/urdf/importer.ex b/lib/bb/urdf/importer.ex index 44c2368..05dc328 100644 --- a/lib/bb/urdf/importer.ex +++ b/lib/bb/urdf/importer.ex @@ -52,7 +52,12 @@ if Code.ensure_loaded?(Sourceror) do def to_quoted(robot, module_name) when is_atom(module_name) do with {:ok, prepared} <- prepare(robot) do topology = - render_topology(prepared.root, prepared.links_by_name, prepared.joints_by_parent) + render_topology( + prepared.root, + prepared.links_by_name, + prepared.joints_by_parent, + prepared.transmissions + ) settings = render_settings(prepared.name) @@ -84,7 +89,12 @@ if Code.ensure_loaded?(Sourceror) do def to_topology_quoted(robot) do with {:ok, prepared} <- prepare(robot) do topology = - render_topology(prepared.root, prepared.links_by_name, prepared.joints_by_parent) + render_topology( + prepared.root, + prepared.links_by_name, + prepared.joints_by_parent, + prepared.transmissions + ) {:ok, topology, prepared.warnings} end @@ -108,6 +118,7 @@ if Code.ensure_loaded?(Sourceror) do root: root, links_by_name: links_by_name, joints_by_parent: joints_by_parent, + transmissions: Map.get(robot, :transmissions, %{}), name: robot.name, warnings: robot.warnings }} @@ -242,17 +253,17 @@ if Code.ensure_loaded?(Sourceror) do call(:settings, [], [call(:name, [atom_name(name)])]) end - defp render_topology(root, links_by_name, joints_by_parent) do - call(:topology, [], [render_link(root, links_by_name, joints_by_parent)]) + defp render_topology(root, links_by_name, joints_by_parent, transmissions) do + call(:topology, [], [render_link(root, links_by_name, joints_by_parent, transmissions)]) end - defp render_link(link, links_by_name, joints_by_parent) do + defp render_link(link, links_by_name, joints_by_parent, transmissions) do children = render_inertial(link.inertial) ++ render_visual(link.visual) ++ Enum.map(link.collisions, &render_collision/1) ++ Enum.flat_map(Map.get(joints_by_parent, link.name, []), fn joint -> - [render_joint(joint, links_by_name, joints_by_parent)] + [render_joint(joint, links_by_name, joints_by_parent, transmissions)] end) call(:link, [atom_name(link.name)], children) @@ -348,7 +359,7 @@ if Code.ensure_loaded?(Sourceror) do call(:texture, [], [call(:filename, [filename])]) end - defp render_joint(joint, links_by_name, joints_by_parent) do + defp render_joint(joint, links_by_name, joints_by_parent, transmissions) do child_link = Map.fetch!(links_by_name, joint.child) body = @@ -357,12 +368,21 @@ if Code.ensure_loaded?(Sourceror) do render_axis(joint) ++ render_limit(joint) ++ render_dynamics(joint.dynamics) ++ + render_transmission(Map.get(transmissions, joint.name)) ++ render_mimic(joint) ++ - [render_link(child_link, links_by_name, joints_by_parent)] + [render_link(child_link, links_by_name, joints_by_parent, transmissions)] call(:joint, [atom_name(joint.name)], body) end + defp render_transmission(nil), do: [] + + defp render_transmission(%{reduction: 1.0}), do: [] + + defp render_transmission(%{reduction: reduction}) do + [call(:transmission, [], [call(:reduction, [reduction * 1.0])])] + end + defp render_mimic(%{mimic: nil}), do: [] defp render_mimic(%{name: joint_name, mimic: mimic}) do @@ -588,6 +608,8 @@ if Code.ensure_loaded?(Sourceror) do pitch: 1, radius: 1, red: 1, + reduction: 1, + reversed?: 1, roll: 1, scale: 1, sensor: 2, @@ -598,6 +620,8 @@ if Code.ensure_loaded?(Sourceror) do texture: 0, texture: 1, topology: 1, + transmission: 0, + transmission: 1, type: 1, upper: 1, velocity: 1, diff --git a/lib/bb/urdf/parser.ex b/lib/bb/urdf/parser.ex index 25bc387..5156fe0 100644 --- a/lib/bb/urdf/parser.ex +++ b/lib/bb/urdf/parser.ex @@ -38,9 +38,15 @@ defmodule BB.Urdf.Parser do name: String.t(), links: [link], joints: [joint], + transmissions: %{optional(String.t()) => transmission}, materials: %{optional(String.t()) => material}, warnings: [String.t()] } + @type transmission :: %{ + name: String.t() | nil, + joint: String.t(), + reduction: float + } @type link :: %{ name: String.t(), visual: visual | nil, @@ -136,28 +142,109 @@ defmodule BB.Urdf.Parser do {joint, warnings ++ acc} end) - other_warnings = + {transmissions, transmission_warnings} = children - |> Enum.flat_map(fn - xml_element(name: :transmission, attributes: attrs) -> - ["skipping #{attr_value(attrs, :name, "")}"] + |> filter_by_name(:transmission) + |> Enum.map_reduce([], fn el, acc -> + {parsed, warnings} = parse_transmission(el) + {parsed, warnings ++ acc} + end) - xml_element(name: :gazebo) -> - ["skipping extension block"] + transmissions_by_joint = + transmissions + |> Enum.reject(&is_nil/1) + |> Enum.into(%{}, fn t -> {t.joint, t} end) - _ -> - [] + gazebo_warnings = + children + |> Enum.flat_map(fn + xml_element(name: :gazebo) -> ["skipping extension block"] + _ -> [] end) %{ name: to_string(name), links: links, joints: joints, + transmissions: transmissions_by_joint, materials: materials, - warnings: Enum.reverse(link_warnings) ++ Enum.reverse(joint_warnings) ++ other_warnings + warnings: + Enum.reverse(link_warnings) ++ + Enum.reverse(joint_warnings) ++ + Enum.reverse(transmission_warnings) ++ + gazebo_warnings } end + defp parse_transmission(xml_element(name: :transmission) = element) do + name = attr(element, :name, nil) + children = children(element) + + type = + case first_by_name(children, :type) do + nil -> nil + el -> el |> text_content() |> String.trim() + end + + joint_name = + case first_by_name(children, :joint) do + nil -> nil + el -> el |> attr(:name) |> to_string() + end + + actuators = filter_by_name(children, :actuator) + + cond do + joint_name == nil -> + {nil, ["skipping #{inspect(to_string(name || ""))}: missing "]} + + type != nil and not simple_transmission?(type) -> + {nil, + [ + "skipping #{inspect(to_string(name || ""))}: type #{inspect(type)} is not supported (only SimpleTransmission)" + ]} + + length(actuators) > 1 -> + {nil, + [ + "skipping #{inspect(to_string(name || ""))}: coupled transmissions (multiple ) are not supported" + ]} + + true -> + reduction = + case actuators do + [] -> 1.0 + [actuator | _] -> extract_reduction(actuator) + end + + {%{ + name: name && to_string(name), + joint: joint_name, + reduction: reduction + }, []} + end + end + + defp simple_transmission?(type) do + String.ends_with?(type, "SimpleTransmission") + end + + defp extract_reduction(actuator_element) do + case actuator_element |> children() |> first_by_name(:mechanicalReduction) do + nil -> 1.0 + el -> el |> text_content() |> String.trim() |> parse_float() + end + end + + defp text_content(xml_element(content: content)) do + content + |> Enum.map(fn + {:xmlText, _, _, _, value, _} -> to_string(value) + _ -> "" + end) + |> Enum.join("") + end + defp parse_top_level_materials(children) do children |> filter_by_name(:material) diff --git a/lib/bb/urdf/xml.ex b/lib/bb/urdf/xml.ex index 893e555..3966586 100644 --- a/lib/bb/urdf/xml.ex +++ b/lib/bb/urdf/xml.ex @@ -23,8 +23,19 @@ defmodule BB.Urdf.Xml do Attributes are converted to charlists as required by xmerl. Nil children are filtered out. """ - @spec element(atom(), keyword(), list()) :: tuple() - def element(name, attrs \\ [], children \\ []) do + @spec element(atom(), keyword(), list() | binary()) :: tuple() + def element(name, attrs \\ [], children \\ []) + + def element(name, attrs, text) when is_binary(text) do + xmerl_attrs = + attrs + |> Enum.reject(fn {_k, v} -> is_nil(v) end) + |> Enum.map(fn {k, v} -> {k, to_charlist(v)} end) + + {name, xmerl_attrs, [String.to_charlist(text)]} + end + + def element(name, attrs, children) when is_list(children) do xmerl_attrs = attrs |> Enum.reject(fn {_k, v} -> is_nil(v) end) diff --git a/test/bb/urdf/exporter_test.exs b/test/bb/urdf/exporter_test.exs index 6a0002a..5cd1b9e 100644 --- a/test/bb/urdf/exporter_test.exs +++ b/test/bb/urdf/exporter_test.exs @@ -352,4 +352,72 @@ defmodule BB.Urdf.ExporterTest do assert xml =~ ~r/.*<\/robot>/s end end + + describe "transmission export" do + defmodule RobotWithTransmission do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reduction 101.0 + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + actuator :motor, BB.Test.MockActuator + + link :arm + end + end + end + end + + defmodule RobotWithoutGearReduction do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + transmission do + reversed? true + end + + limit do + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + + actuator :motor, BB.Test.MockActuator + + link :arm + end + end + end + end + + test "emits a block with mechanicalReduction" do + {:ok, xml} = Exporter.export(RobotWithTransmission) + + assert xml =~ ~s() + assert xml =~ ~s(transmission_interface/SimpleTransmission) + assert xml =~ ~s() + assert xml =~ ~s() + assert xml =~ ~r{101(\.0)?} + end + + test "skips when reduction is 1.0 (URDF has no place for offset or reversed?)" do + {:ok, xml} = Exporter.export(RobotWithoutGearReduction) + + refute xml =~ " + + + + + + + + transmission_interface/SimpleTransmission + + 1 + + + """ + + {:ok, parsed} = Parser.parse_string(xml) + {:ok, source, _} = Importer.to_source(parsed, MyApp.TxGenIdentity) + + refute source =~ "transmission do" + end + test "errors out when a joint references an undefined link" do xml = """ diff --git a/test/bb/urdf/parser_test.exs b/test/bb/urdf/parser_test.exs index 4b787ff..c56dfee 100644 --- a/test/bb/urdf/parser_test.exs +++ b/test/bb/urdf/parser_test.exs @@ -68,6 +68,47 @@ defmodule BB.Urdf.ParserTest do test "returns an error for missing files" do assert {:error, :enoent} = Parser.parse_file("/no/such/file.urdf") end + + test "parses a SimpleTransmission into the transmissions map keyed by joint" do + {:ok, robot} = Parser.parse_file(Path.join(@fixture_dir, "simple_transmission.urdf")) + + assert robot.transmissions == %{ + "shoulder_pan" => %{ + name: "shoulder_pan_trans", + joint: "shoulder_pan", + reduction: 101.0 + } + } + end + + test "warns and skips coupled transmissions" do + xml = """ + + + + + + + + + + + + + + + transmission_interface/DifferentialTransmission + + 2 + 3 + + + """ + + {:ok, robot} = Parser.parse_string(xml) + assert robot.transmissions == %{} + assert Enum.any?(robot.warnings, &(&1 =~ "DifferentialTransmission")) + end end describe "parse_string/1" do diff --git a/test/fixtures/urdf/simple_transmission.urdf b/test/fixtures/urdf/simple_transmission.urdf new file mode 100644 index 0000000..92911f2 --- /dev/null +++ b/test/fixtures/urdf/simple_transmission.urdf @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 101 + + + From 9a868d2b033216c31c28a9ec10dfc99c182a41a1 Mon Sep 17 00:00:00 2001 From: James Harton Date: Tue, 19 May 2026 09:13:31 +0000 Subject: [PATCH 3/4] feature(igniter): add BB.Igniter.Transmission upgrade helper Provides the shared Igniter logic each driver package's upgrade task delegates to. Given a driver module and a `lift_offset?` flag, walks every module in the project and for each `joint :name do ... end` block: - Finds `actuator :_, {Driver, opts}` calls (also matches the Sourceror-wrapped form `{:__block__, _, [{Driver, opts}]}`). - Removes the `reverse?:` key from `opts`. - If `reverse?: true` was present, inserts a `transmission do reversed? true end` block after the joint's `limit` block. - With `lift_offset?: true` (Feetech and Robotis), additionally computes `offset = (lower + upper) / 2` from the joint's literal limits and emits it in the transmission, preserving the auto-centering the drivers used to do internally for asymmetric joints. Refs: beam-bots/bb#108 --- lib/bb/igniter/transmission.ex | 345 ++++++++++++++++++++++++++ test/bb/igniter/transmission_test.exs | 184 ++++++++++++++ 2 files changed, 529 insertions(+) create mode 100644 lib/bb/igniter/transmission.ex create mode 100644 test/bb/igniter/transmission_test.exs diff --git a/lib/bb/igniter/transmission.ex b/lib/bb/igniter/transmission.ex new file mode 100644 index 0000000..81b2811 --- /dev/null +++ b/lib/bb/igniter/transmission.ex @@ -0,0 +1,345 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +if Code.ensure_loaded?(Igniter) do + defmodule BB.Igniter.Transmission do + @moduledoc """ + Shared Igniter upgrade logic for lifting actuator-driver `reverse?` options + into joint-level `transmission` blocks. + + Used by the upgrade tasks in each driver package (Feetech, Robotis, + PCA9685, Pigpio). The transformation is: + + Before: + + joint :shoulder do + type :revolute + limit do + lower(~u(-10 degree)) + upper(~u(190 degree)) + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + actuator :motor, {BB.Servo.Feetech.Actuator, + servo_id: 1, controller: :feetech, reverse?: true + } + link :arm + end + + After (with `lift_offset?: true`): + + joint :shoulder do + type :revolute + limit do + lower(~u(-10 degree)) + upper(~u(190 degree)) + effort(~u(10 newton_meter)) + velocity(~u(180 degree_per_second)) + end + transmission do + offset(~u(90.0 degree)) + reversed? true + end + actuator :motor, {BB.Servo.Feetech.Actuator, + servo_id: 1, controller: :feetech + } + link :arm + end + + The `offset` is computed as `(lower + upper) / 2`, preserving the + auto-centering behaviour the Feetech/Robotis drivers used to derive + internally. `lift_offset?: false` skips the offset computation — used by + the PCA9685 and Pigpio upgraders, which never auto-centered. + """ + + alias Sourceror.Zipper + + @doc """ + Lift `reverse?` opts on the given driver's actuator child-specs into + joint-level `transmission` blocks across the project. + + Options: + + * `:lift_offset?` (default `false`) — when true, also compute and emit + an `offset(...)` value derived from the enclosing joint's `lower` and + `upper` limits, preserving the implicit centering that Feetech and + Robotis drivers used to do internally. + """ + @spec lift_reverse_question(Igniter.t(), module(), keyword()) :: Igniter.t() + def lift_reverse_question(igniter, driver_module, opts \\ []) do + lift_offset? = Keyword.get(opts, :lift_offset?, false) + + {igniter, modules} = + Igniter.Project.Module.find_all_matching_modules(igniter, fn _mod, _zipper -> true end) + + Enum.reduce(modules, igniter, fn mod, ig -> + case Igniter.Project.Module.find_and_update_module(ig, mod, fn zipper -> + {:ok, update_module(zipper, driver_module, lift_offset?)} + end) do + {:ok, ig} -> ig + {:error, ig} -> ig + end + end) + end + + # Apply the transmission lift to every `joint :name do ... end` block in + # the module body. We work directly on the quoted form rather than the + # zipper since we need recursive transformation of nested calls. + defp update_module(zipper, driver, lift_offset?) do + current = Zipper.node(zipper) + new_node = Macro.prewalk(current, &transform_joint(&1, driver, lift_offset?)) + Zipper.replace(zipper, new_node) + end + + defp transform_joint({:joint, meta, [name, do_block]} = node, driver, lift_offset?) + when is_list(do_block) do + case do_block_body(do_block) do + {:ok, body, wrapper} -> + updated_body = rewrite_joint_body(body, driver, lift_offset?) + {:joint, meta, [name, rebuild_do(wrapper, updated_body)]} + + :error -> + node + end + end + + defp transform_joint(other, _driver, _lift_offset?), do: other + + defp do_block_body([{{:__block__, _, [:do]}, body} = pair]), do: {:ok, body, {:block, pair}} + defp do_block_body([do: body]), do: {:ok, body, :plain} + defp do_block_body(_), do: :error + + defp rebuild_do({:block, {key, _}}, body), do: [{key, body}] + defp rebuild_do(:plain, body), do: [do: body] + + # The joint's `do` body can be a single call or a `:__block__` of calls. + # Normalise to a list, transform, and pack back. + defp rewrite_joint_body({:__block__, meta, stmts}, driver, lift_offset?) do + {new_stmts, joint_state} = scan_and_modify(stmts, driver, lift_offset?) + new_stmts = insert_transmission(new_stmts, joint_state) + {:__block__, meta, new_stmts} + end + + defp rewrite_joint_body(stmt, driver, lift_offset?) do + {new_stmts, joint_state} = scan_and_modify([stmt], driver, lift_offset?) + new_stmts = insert_transmission(new_stmts, joint_state) + + case new_stmts do + [single] -> single + many -> {:__block__, [], many} + end + end + + # Walk statements in the joint body. For each one: + # - actuator(...) calls matching driver: strip `reverse?:` opt, capture + # whether it was true. + # - limit do ... end blocks: capture lower/upper for offset computation. + # - transmission do ... end blocks: capture so we can merge into the + # existing block instead of inserting a new one. + defp scan_and_modify(stmts, driver, lift_offset?) do + Enum.map_reduce( + stmts, + %{ + reverse?: false, + touched_actuator?: false, + existing_transmission_index: nil, + limits: nil, + lift_offset?: lift_offset? + }, + fn stmt, acc -> + process_stmt(stmt, driver, acc) + end + ) + end + + defp process_stmt({:actuator, meta, [name, child_spec]}, driver, acc) do + case strip_reverse_question(child_spec, driver) do + {:matched, was_true?, new_child_spec} -> + new_acc = %{acc | reverse?: acc.reverse? or was_true?, touched_actuator?: true} + {{:actuator, meta, [name, new_child_spec]}, new_acc} + + :unmatched -> + {{:actuator, meta, [name, child_spec]}, acc} + end + end + + defp process_stmt({:limit, _meta, [[{{:__block__, _, [:do]}, body}]]} = stmt, _driver, acc) do + {stmt, %{acc | limits: extract_limits(body)}} + end + + defp process_stmt({:limit, _meta, [[do: body]]} = stmt, _driver, acc) do + {stmt, %{acc | limits: extract_limits(body)}} + end + + defp process_stmt(stmt, _driver, acc), do: {stmt, acc} + + # Look for `{Driver, opts}` (or `{Driver, opts}` aliased) and strip `reverse?:`. + defp strip_reverse_question( + {:{}, meta, [driver_alias, opts]}, + driver + ) do + strip_from_tuple(driver_alias, opts, driver, fn new_opts -> + {:{}, meta, [driver_alias, new_opts]} + end) + end + + defp strip_reverse_question({driver_alias, opts}, driver) do + strip_from_tuple(driver_alias, opts, driver, fn new_opts -> + {driver_alias, new_opts} + end) + end + + defp strip_reverse_question({:__block__, meta, [inner]}, driver) do + case strip_reverse_question(inner, driver) do + {:matched, was_true?, new_inner} -> {:matched, was_true?, {:__block__, meta, [new_inner]}} + :unmatched -> :unmatched + end + end + + defp strip_reverse_question(_, _), do: :unmatched + + defp strip_from_tuple(driver_alias, opts, driver, rebuild) do + if alias_matches?(driver_alias, driver) and is_list(opts) do + case pop_reverse_question(opts) do + :not_present -> + :unmatched + + {value, new_opts} -> + was_true? = literal_true?(value) + {:matched, was_true?, rebuild.(new_opts)} + end + else + :unmatched + end + end + + defp alias_matches?({:__aliases__, _, parts}, driver_module) do + Module.concat(parts) == driver_module + end + + defp alias_matches?(_, _), do: false + + defp pop_reverse_question(opts) do + case Enum.split_with(opts, fn + {{:__block__, _, [:reverse?]}, _} -> true + {:reverse?, _} -> true + _ -> false + end) do + {[], _} -> + :not_present + + {[{_, value} | _], rest} -> + {value, rest} + end + end + + defp literal_true?({:__block__, _, [true]}), do: true + defp literal_true?(true), do: true + defp literal_true?(_), do: false + + # Pull `lower` and `upper` calls out of a limit body so we can compute an offset. + defp extract_limits({:__block__, _, stmts}), do: extract_limits(stmts) + + defp extract_limits(stmts) when is_list(stmts) do + Enum.reduce(stmts, %{lower: nil, upper: nil}, fn + {:lower, _, [value]}, acc -> %{acc | lower: value} + {:upper, _, [value]}, acc -> %{acc | upper: value} + _, acc -> acc + end) + end + + defp extract_limits(_), do: nil + + defp insert_transmission(stmts, %{touched_actuator?: false}), do: stmts + + defp insert_transmission(stmts, %{reverse?: false, lift_offset?: false}), do: stmts + + defp insert_transmission(stmts, state) do + case transmission_source(state) do + nil -> + stmts + + source -> + new_node = Sourceror.parse_string!(source) + place_after_limit(stmts, new_node) + end + end + + defp transmission_source(state) do + offset_line = + if state.lift_offset? do + offset_line_from_limits(state.limits) + end + + reversed_line = + if state.reverse? do + "reversed? true" + end + + case Enum.reject([offset_line, reversed_line], &is_nil/1) do + [] -> + nil + + lines -> + "transmission do\n " <> Enum.join(lines, "\n ") <> "\nend" + end + end + + defp offset_line_from_limits(nil), do: nil + defp offset_line_from_limits(%{lower: nil}), do: nil + defp offset_line_from_limits(%{upper: nil}), do: nil + + defp offset_line_from_limits(%{lower: lower_ast, upper: upper_ast}) do + with {lower_value, unit} <- unit_literal(lower_ast), + {upper_value, ^unit} <- unit_literal(upper_ast), + midpoint when midpoint != 0.0 <- (lower_value + upper_value) / 2 do + "offset ~u(#{format_number(midpoint)} #{unit})" + else + _ -> nil + end + end + + # Match `~u(-110 degree)` / `~u(190 degree)` / etc. and pull out the numeric + # value and unit name. Returns nil for non-literal expressions (param refs, + # arithmetic, etc.) so the upgrader gives up rather than guessing. + defp unit_literal({:sigil_u, _, [{:<<>>, _, [text]}, _]}) when is_binary(text) do + case String.split(text, " ", parts: 2) do + [num_str, unit] -> + case Float.parse(num_str) do + {value, ""} -> + {value, unit} + + _ -> + case Integer.parse(num_str) do + {value, ""} -> {value * 1.0, unit} + _ -> nil + end + end + + _ -> + nil + end + end + + defp unit_literal(_), do: nil + + defp format_number(n) when is_float(n) do + if n == Float.round(n, 0), do: :erlang.float_to_binary(n, decimals: 1), else: to_string(n) + end + + defp format_number(n), do: to_string(n) + + # Place a new node immediately after the `limit do ... end` call if one is + # present, otherwise prepend it. + defp place_after_limit(stmts, new_node) do + case Enum.find_index(stmts, &limit_call?/1) do + nil -> [new_node | stmts] + idx -> List.insert_at(stmts, idx + 1, new_node) + end + end + + defp limit_call?({:limit, _, _}), do: true + defp limit_call?(_), do: false + end +end diff --git a/test/bb/igniter/transmission_test.exs b/test/bb/igniter/transmission_test.exs new file mode 100644 index 0000000..63b5523 --- /dev/null +++ b/test/bb/igniter/transmission_test.exs @@ -0,0 +1,184 @@ +# SPDX-FileCopyrightText: 2026 James Harton +# +# SPDX-License-Identifier: Apache-2.0 + +if Code.ensure_loaded?(Igniter) do + defmodule BB.Igniter.TransmissionTest do + use ExUnit.Case, async: true + + import Igniter.Test + + defmodule FakeDriver do + @moduledoc false + end + + defp run(source, opts \\ []) do + igniter = + test_project() + |> Igniter.create_new_file("lib/my_robot.ex", source) + |> BB.Igniter.Transmission.lift_reverse_question(FakeDriver, opts) + + source = Map.fetch!(igniter.rewrite.sources, "lib/my_robot.ex") + Rewrite.Source.get(source, :content) + end + + test "strips reverse?: true and inserts a transmission block with reversed? true" do + output = + run(""" + defmodule MyRobot do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + limit do + lower ~u(-90 degree) + upper ~u(90 degree) + effort ~u(10 newton_meter) + velocity ~u(180 degree_per_second) + end + + actuator :motor, {BB.Igniter.TransmissionTest.FakeDriver, + servo_id: 1, reverse?: true + } + + link :arm + end + end + end + end + """) + + assert output =~ "transmission do" + assert output =~ "reversed?(true)" + refute output =~ "reverse?:" + end + + test "drops reverse?: false silently, no transmission block added" do + output = + run(""" + defmodule MyRobot do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + actuator :motor, {BB.Igniter.TransmissionTest.FakeDriver, + servo_id: 1, reverse?: false + } + + link :arm + end + end + end + end + """) + + refute output =~ "reverse?:" + refute output =~ "transmission do" + end + + test "with lift_offset?: true, computes offset from asymmetric limits" do + output = + run( + """ + defmodule MyRobot do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + limit do + lower ~u(-10 degree) + upper ~u(190 degree) + effort ~u(10 newton_meter) + velocity ~u(180 degree_per_second) + end + + actuator :motor, {BB.Igniter.TransmissionTest.FakeDriver, + servo_id: 1, reverse?: true + } + + link :arm + end + end + end + end + """, + lift_offset?: true + ) + + assert output =~ "transmission do" + assert output =~ "offset(~u(90.0 degree))" + assert output =~ "reversed?(true)" + refute output =~ "reverse?:" + end + + test "with lift_offset?: true, no offset for symmetric limits" do + output = + run( + """ + defmodule MyRobot do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + limit do + lower ~u(-90 degree) + upper ~u(90 degree) + effort ~u(10 newton_meter) + velocity ~u(180 degree_per_second) + end + + actuator :motor, {BB.Igniter.TransmissionTest.FakeDriver, + servo_id: 1, reverse?: true + } + + link :arm + end + end + end + end + """, + lift_offset?: true + ) + + assert output =~ "transmission do" + assert output =~ "reversed?(true)" + refute output =~ "offset(" + end + + test "leaves untouched joints with no matching actuator" do + input = """ + defmodule MyRobot do + use BB + + topology do + link :base do + joint :shoulder do + type :revolute + + actuator :motor, {SomeOther.Driver, opts: 1} + + link :arm + end + end + end + end + """ + + output = run(input) + + refute output =~ "transmission do" + end + end +end From e80472a9726afa278cfc26921245b10d852d9a3e Mon Sep 17 00:00:00 2001 From: James Harton Date: Wed, 20 May 2026 02:50:28 +0000 Subject: [PATCH 4/4] chore: format, credo, dialyzer cleanups - Format the verifier and Igniter upgrader to satisfy mix format. - Reorder aliases in BB.Transmission.Resolver alphabetically. - Refactor BB.Urdf.Parser.parse_transmission/1 (split a cond branch out into smaller classify/build_transmission heads to drop the cyclomatic complexity below the credo threshold). - Refactor BB.Igniter.Transmission.lift_reverse_question/3 (extracted lift_in_module/4) and unit_literal/1 (collapsed the nested case statements via with) so neither nests past two levels. - Use Enum.map_join/3 in BB.Urdf.Parser.text_content/1. - Drop the unreachable format_number/1 fallback clause flagged by dialyzer. Refs: beam-bots/bb#108 --- lib/bb/dsl/verifiers/validate_param_refs.ex | 18 +++- lib/bb/igniter/transmission.ex | 56 ++++++----- lib/bb/transmission/resolver.ex | 2 +- lib/bb/urdf/parser.ex | 103 +++++++++++--------- test/bb/dsl/transmission_test.exs | 1 - test/bb/igniter/transmission_test.exs | 6 +- 6 files changed, 109 insertions(+), 77 deletions(-) diff --git a/lib/bb/dsl/verifiers/validate_param_refs.ex b/lib/bb/dsl/verifiers/validate_param_refs.ex index af9f3e3..8f89f0a 100644 --- a/lib/bb/dsl/verifiers/validate_param_refs.ex +++ b/lib/bb/dsl/verifiers/validate_param_refs.ex @@ -13,7 +13,19 @@ defmodule BB.Dsl.Verifiers.ValidateParamRefs do use Spark.Dsl.Verifier - alias BB.Dsl.{Axis, Dynamics, Inertia, Inertial, Joint, Limit, Link, Origin, ParamRef, Transmission} + alias BB.Dsl.{ + Axis, + Dynamics, + Inertia, + Inertial, + Joint, + Limit, + Link, + Origin, + ParamRef, + Transmission + } + alias BB.Unit alias Spark.Dsl.Verifier alias Spark.Error.DslError @@ -70,7 +82,9 @@ defmodule BB.Dsl.Verifiers.ValidateParamRefs do axis_refs = collect_from_axis(joint.axis, joint_path ++ [:axis]) limit_refs = collect_from_limit(joint.limit, joint_path ++ [:limit]) dynamics_refs = collect_from_dynamics(joint.dynamics, joint_path ++ [:dynamics]) - transmission_refs = collect_from_transmission(joint.transmission, joint_path ++ [:transmission]) + + transmission_refs = + collect_from_transmission(joint.transmission, joint_path ++ [:transmission]) nested_refs = case joint.link do diff --git a/lib/bb/igniter/transmission.ex b/lib/bb/igniter/transmission.ex index 81b2811..a577e8a 100644 --- a/lib/bb/igniter/transmission.ex +++ b/lib/bb/igniter/transmission.ex @@ -73,14 +73,16 @@ if Code.ensure_loaded?(Igniter) do {igniter, modules} = Igniter.Project.Module.find_all_matching_modules(igniter, fn _mod, _zipper -> true end) - Enum.reduce(modules, igniter, fn mod, ig -> - case Igniter.Project.Module.find_and_update_module(ig, mod, fn zipper -> - {:ok, update_module(zipper, driver_module, lift_offset?)} - end) do - {:ok, ig} -> ig - {:error, ig} -> ig - end - end) + Enum.reduce(modules, igniter, &lift_in_module(&1, &2, driver_module, lift_offset?)) + end + + defp lift_in_module(module, igniter, driver_module, lift_offset?) do + updater = fn zipper -> {:ok, update_module(zipper, driver_module, lift_offset?)} end + + case Igniter.Project.Module.find_and_update_module(igniter, module, updater) do + {:ok, ig} -> ig + {:error, ig} -> ig + end end # Apply the transmission lift to every `joint :name do ... end` block in @@ -107,7 +109,7 @@ if Code.ensure_loaded?(Igniter) do defp transform_joint(other, _driver, _lift_offset?), do: other defp do_block_body([{{:__block__, _, [:do]}, body} = pair]), do: {:ok, body, {:block, pair}} - defp do_block_body([do: body]), do: {:ok, body, :plain} + defp do_block_body(do: body), do: {:ok, body, :plain} defp do_block_body(_), do: :error defp rebuild_do({:block, {key, _}}, body), do: [{key, body}] @@ -304,32 +306,34 @@ if Code.ensure_loaded?(Igniter) do # value and unit name. Returns nil for non-literal expressions (param refs, # arithmetic, etc.) so the upgrader gives up rather than guessing. defp unit_literal({:sigil_u, _, [{:<<>>, _, [text]}, _]}) when is_binary(text) do - case String.split(text, " ", parts: 2) do - [num_str, unit] -> - case Float.parse(num_str) do - {value, ""} -> - {value, unit} - - _ -> - case Integer.parse(num_str) do - {value, ""} -> {value * 1.0, unit} - _ -> nil - end - end - - _ -> - nil + with [num_str, unit] <- String.split(text, " ", parts: 2), + {value, ""} <- parse_number(num_str) do + {value, unit} + else + _ -> nil end end defp unit_literal(_), do: nil + defp parse_number(str) do + case Float.parse(str) do + {_, ""} = ok -> ok + _ -> integer_to_float(str) + end + end + + defp integer_to_float(str) do + case Integer.parse(str) do + {value, ""} -> {value * 1.0, ""} + other -> other + end + end + defp format_number(n) when is_float(n) do if n == Float.round(n, 0), do: :erlang.float_to_binary(n, decimals: 1), else: to_string(n) end - defp format_number(n), do: to_string(n) - # Place a new node immediately after the `limit do ... end` call if one is # present, otherwise prepend it. defp place_after_limit(stmts, new_node) do diff --git a/lib/bb/transmission/resolver.ex b/lib/bb/transmission/resolver.ex index f8ed299..5cf928e 100644 --- a/lib/bb/transmission/resolver.ex +++ b/lib/bb/transmission/resolver.ex @@ -15,9 +15,9 @@ defmodule BB.Transmission.Resolver do """ alias BB.PubSub + alias BB.Robot.Runtime alias BB.Robot.State, as: RobotState alias BB.Robot.Units - alias BB.Robot.Runtime @type field :: :reduction | :offset | :reversed? @type subscriptions :: %{field() => [atom()]} diff --git a/lib/bb/urdf/parser.ex b/lib/bb/urdf/parser.ex index 5156fe0..6a92cc3 100644 --- a/lib/bb/urdf/parser.ex +++ b/lib/bb/urdf/parser.ex @@ -177,54 +177,69 @@ defmodule BB.Urdf.Parser do end defp parse_transmission(xml_element(name: :transmission) = element) do - name = attr(element, :name, nil) - children = children(element) + parsed = %{ + name: attr(element, :name, nil), + children: children(element) + } - type = - case first_by_name(children, :type) do - nil -> nil - el -> el |> text_content() |> String.trim() - end + parsed + |> Map.merge(transmission_metadata(parsed.children)) + |> classify_transmission() + end - joint_name = - case first_by_name(children, :joint) do - nil -> nil - el -> el |> attr(:name) |> to_string() - end + defp transmission_metadata(children) do + %{ + type: transmission_type(first_by_name(children, :type)), + joint_name: transmission_joint(first_by_name(children, :joint)), + actuators: filter_by_name(children, :actuator) + } + end + + defp transmission_type(nil), do: nil + defp transmission_type(el), do: el |> text_content() |> String.trim() + + defp transmission_joint(nil), do: nil + defp transmission_joint(el), do: el |> attr(:name) |> to_string() - actuators = filter_by_name(children, :actuator) - - cond do - joint_name == nil -> - {nil, ["skipping #{inspect(to_string(name || ""))}: missing "]} - - type != nil and not simple_transmission?(type) -> - {nil, - [ - "skipping #{inspect(to_string(name || ""))}: type #{inspect(type)} is not supported (only SimpleTransmission)" - ]} - - length(actuators) > 1 -> - {nil, - [ - "skipping #{inspect(to_string(name || ""))}: coupled transmissions (multiple ) are not supported" - ]} - - true -> - reduction = - case actuators do - [] -> 1.0 - [actuator | _] -> extract_reduction(actuator) - end - - {%{ - name: name && to_string(name), - joint: joint_name, - reduction: reduction - }, []} + defp classify_transmission(%{joint_name: nil} = parsed) do + {nil, [skip_warning(parsed.name, "missing ")]} + end + + defp classify_transmission(%{type: type} = parsed) + when not is_nil(type) do + if simple_transmission?(type) do + build_transmission(parsed) + else + {nil, + [skip_warning(parsed.name, "type #{inspect(type)} is not supported (only SimpleTransmission)")]} end end + defp classify_transmission(parsed), do: build_transmission(parsed) + + defp build_transmission(%{actuators: actuators} = parsed) when length(actuators) > 1 do + {nil, + [skip_warning(parsed.name, "coupled transmissions (multiple ) are not supported")]} + end + + defp build_transmission(parsed) do + reduction = + case parsed.actuators do + [] -> 1.0 + [actuator | _] -> extract_reduction(actuator) + end + + {%{ + name: parsed.name && to_string(parsed.name), + joint: parsed.joint_name, + reduction: reduction + }, []} + end + + defp skip_warning(name, reason) do + "skipping #{inspect(to_string(name || ""))}: #{reason}" + end + defp simple_transmission?(type) do String.ends_with?(type, "SimpleTransmission") end @@ -237,12 +252,10 @@ defmodule BB.Urdf.Parser do end defp text_content(xml_element(content: content)) do - content - |> Enum.map(fn + Enum.map_join(content, "", fn {:xmlText, _, _, _, value, _} -> to_string(value) _ -> "" end) - |> Enum.join("") end defp parse_top_level_materials(children) do diff --git a/test/bb/dsl/transmission_test.exs b/test/bb/dsl/transmission_test.exs index 5c182b2..9d13cda 100644 --- a/test/bb/dsl/transmission_test.exs +++ b/test/bb/dsl/transmission_test.exs @@ -228,6 +228,5 @@ defmodule BB.Dsl.TransmissionTest do assert Map.has_key?(subs, [:tx, :offset]) assert Map.has_key?(subs, [:tx, :reversed?]) end - end end diff --git a/test/bb/igniter/transmission_test.exs b/test/bb/igniter/transmission_test.exs index 63b5523..3af78aa 100644 --- a/test/bb/igniter/transmission_test.exs +++ b/test/bb/igniter/transmission_test.exs @@ -12,14 +12,16 @@ if Code.ensure_loaded?(Igniter) do @moduledoc false end + alias Rewrite.Source + defp run(source, opts \\ []) do igniter = test_project() |> Igniter.create_new_file("lib/my_robot.ex", source) |> BB.Igniter.Transmission.lift_reverse_question(FakeDriver, opts) - source = Map.fetch!(igniter.rewrite.sources, "lib/my_robot.ex") - Rewrite.Source.get(source, :content) + generated_source = Map.fetch!(igniter.rewrite.sources, "lib/my_robot.ex") + Source.get(generated_source, :content) end test "strips reverse?: true and inserts a transmission block with reversed? true" do