commit 8be13016bc08115359227c3e3ce9dd8f773e1316 Author: Upstream Snapshot Date: Fri Aug 28 15:42:08 2026 +0800 Import upstream snapshot 57d13ead53206a6bf0db3d66f86506ae8c2ce01a Upstream: https://github.com/pollen-robotics/bam Upstream-Commit: 57d13ead53206a6bf0db3d66f86506ae8c2ce01a Upstream-Branch: mjlab_frictionloss diff --git a/.github/workflows/run_uv.yml b/.github/workflows/run_uv.yml new file mode 100644 index 0000000..156a233 --- /dev/null +++ b/.github/workflows/run_uv.yml @@ -0,0 +1,25 @@ +name: UV run + +on: + push: + branches: + - "master" + +jobs: + run_with_uv: + strategy: + matrix: + os: [macos-latest, ubuntu-latest, ubuntu-24.04-arm, macos-15-intel, windows-latest] + pyver: [cp38, cp39, cp310, cp311, cp312, cp313, cp314] + + name: Wheel ${{ matrix.os }}, ${{ matrix.pyver }} + runs-on: ${{ matrix.os }} + + steps: + - uses: actions/checkout@v4 + + - name: Install uv + uses: astral-sh/setup-uv@v6 + + - name: Syncing UV + run: uv sync diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml new file mode 100644 index 0000000..326ebc8 --- /dev/null +++ b/.github/workflows/wheels.yml @@ -0,0 +1,38 @@ +name: Build and upload to PyPI + +on: + release: + types: + - published + +jobs: + build: + name: Build sdist and wheel + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Build sdist and wheel + run: pipx run build + + - uses: actions/upload-artifact@v4 + with: + name: cibw-dist + path: dist/* + + upload_pypi: + needs: [build] + runs-on: ubuntu-latest + environment: pypi + permissions: + id-token: write + if: github.event_name == 'release' && github.event.action == 'published' + steps: + - uses: actions/download-artifact@v4 + with: + # unpacks all CIBW artifacts into dist/ + pattern: cibw-* + path: dist + merge-multiple: true + + - uses: pypa/gh-action-pypi-publish@release/v1 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0c071ae --- /dev/null +++ b/.gitignore @@ -0,0 +1,16 @@ +**/__pycache__ +trash/ +data/ +**.pyc +**.swp +params.json +build/ +dist/ +bam.egg-info/ +better_actuator_models.egg-info/ +docs/_build/ +docs/autoapi/ +data_raw/ +data_processed/ +uv.lock +notes.md \ No newline at end of file diff --git a/.readthedocs.yaml b/.readthedocs.yaml new file mode 100644 index 0000000..3750e73 --- /dev/null +++ b/.readthedocs.yaml @@ -0,0 +1,18 @@ +# Read the Docs configuration file +# See https://docs.readthedocs.io/en/stable/config-file/v2.html + +version: 2 + +build: + os: ubuntu-24.04 + tools: + python: "3.12" + +sphinx: + configuration: docs/conf.py + +# autoapi scans bam/ via AST, so the package itself does not need to be +# installed — only the Sphinx toolchain is required to build the docs. +python: + install: + - requirements: docs/requirements.txt diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..7a4a3ea --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..a930010 --- /dev/null +++ b/README.md @@ -0,0 +1,55 @@ +

+ BAM logo +

+ +# BAM: Better Actuator Models + +Accurate models of servo actuators are essential for the simulation of robotic systems. It is particularly important while performing Reinforcement Learning (RL) on real robots, as the precision of the model impacts directly the transferability of the learned policy. + +The friction model generally implemented in widely used simulators like MuJoCo or IsaacGym is the Coulomb-Viscous, which is too simplistic to accurately represent complex friction phenomena like Stribeck, load-dependence or quadratic effects (read [this article](https://arxiv.org/pdf/2410.08650v1) for more details). + +**BAM** aims at making servo-actuator simulation more faithful by: + +- proposing an identification pipeline to fit friction models from recorded trajectories, +- providing a set of extended friction models that capture complex friction phenomena, +- sharing a library of identified models for common servos: + - Dynamixel MX-64 + - Dynamixel MX-106 + - Dynamixel XL-320 + - Dynamixel XL330-288-T + - eRob80:50 + - eRob80:100 + - Feetech STS3215 +- providing a simple API to use these models in MuJoCo CPU and MuJoCo Warp. + +## 📖 Documentation + +Please refer to the [documentation](https://bam.readthedocs.io/en/latest/) for more details on how to use BAM and the provided friction models. + +## 📚 Citation + +If you use BAM or the friction models provided in this repository for your scientific work, please cite the following publication: + +```bibtex +@inproceedings{duclusaud2025extended, + title={Extended Friction Models for the Physics Simulation of Servo Actuators}, + author={Duclusaud, Marc and Passault, Gr{\'e}goire and Padois, Vincent and Ly, Olivier}, + booktitle={2025 IEEE International Conference on Robotics and Automation (ICRA)}, + pages={12091--12097}, + year={2025}, + organization={IEEE} +} +``` + +If you also wish to cite the software directly, you may use: + +```bibtex +@software{BAM, + title = {{BAM: Better Actuator Models}}, + author = {Duclusaud, Marc and Passault, Grégoire}, + license = {Apache-2.0}, + url = {https://github.com/Rhoban/bam}, + version = {0.0.1}, + year = {2024} +} +``` diff --git a/bam/__init__.py b/bam/__init__.py new file mode 100644 index 0000000..2ff1b05 --- /dev/null +++ b/bam/__init__.py @@ -0,0 +1,8 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + diff --git a/bam/actuator.py b/bam/actuator.py new file mode 100644 index 0000000..4253188 --- /dev/null +++ b/bam/actuator.py @@ -0,0 +1,377 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +from __future__ import annotations + +from typing import TYPE_CHECKING, Union + +import numpy as np +from .testbench import Testbench +from bam.parameter import Parameter + +if TYPE_CHECKING: + import torch + +# Anything the control law / torque equations can operate on elementwise. The +# concrete backend (numpy or torch, see :class:`Backend`) decides how ``clamp`` +# behaves; the arithmetic itself broadcasts identically over scalars, numpy +# arrays and torch tensors. +ArrayLike = Union[float, np.ndarray, "torch.Tensor"] + + +class Backend: + """Abstracts the array library so the actuator math is vectorization-agnostic. + + Only operations that differ between numpy and torch (currently ``clamp``) + live here; plain arithmetic (``+``, ``*``, ``**``, ``np.sign`` …) broadcasts + the same way for both, so it is written directly in the actuator methods. + """ + + def clamp(self, x: ArrayLike, low: ArrayLike, high: ArrayLike) -> ArrayLike: + raise NotImplementedError + + +class NumpyBackend(Backend): + def clamp(self, x: ArrayLike, low: ArrayLike, high: ArrayLike) -> ArrayLike: + return np.clip(x, low, high) + + +class TorchBackend(Backend): + def clamp(self, x: ArrayLike, low: ArrayLike, high: ArrayLike) -> ArrayLike: + import torch + + return torch.clamp(x, low, high) + + +class Actuator: + """Abstract base class for all BAM actuator models. + + Subclasses implement the firmware control law and the motor torque equation + for a specific actuator family. An actuator is always attached to a + :class:`~bam.model.Model` via :meth:`~bam.model.Model.set_actuator`, which + calls :meth:`initialize` to create the motor parameters (``kt``, ``R``, …) + on the model. + + :param testbench_class: Class used to instantiate the testbench when a log + is loaded (typically :class:`~bam.testbench.Pendulum`). + """ + + def __init__(self, testbench_class: Testbench): + self.testbench_class = testbench_class + self.testbench: Testbench | None = None + self.backend: Backend = NumpyBackend() + + def set_model(self, model): + """Attach this actuator to a model and run :meth:`initialize`. + + :param model: :class:`~bam.model.Model` instance to attach to. + """ + self.model = model + self.initialize() + + def reset(self): + pass + + def load_log(self, log: dict): + """Called when a log is loaded. + + Instantiates the testbench from the log metadata (mass, length, …) + and updates actuator settings (kp, vin) from the log. + + :param log: Log dict as loaded from a processed JSON file. + """ + self.testbench = self.testbench_class(log) + + def initialize(self): + """Create motor parameters on the attached model. + + Called automatically by :meth:`set_model`. Must create at minimum + ``self.model.kt`` and ``self.model.R`` as :class:`~bam.parameter.Parameter` + instances. + """ + raise NotImplementedError + + def control_unit(self) -> str: + """Return the physical unit of the control signal (e.g. ``"volts"``, ``"amps"``).""" + raise NotImplementedError + + def compute_control( + self, q_target: ArrayLike, q: ArrayLike, dq: ArrayLike, dt: float + ) -> ArrayLike | None: + """Compute the control signal from the current state and target. + + The state arguments (``q_target``, ``q``, ``dq``) may be Python floats, + numpy arrays or torch tensors; the computation is elementwise and the + result matches their (broadcast) type. Use :class:`TorchBackend` for a + fully vectorized, autograd-friendly evaluation. + + :param q_target: Target joint angle(s) [rad]. + :param q: Current joint angle(s) [rad]. + :param dq: Current joint velocity(ies) [rad/s]. + :param dt: Timestep [s]. + :returns: Control signal in the unit given by :meth:`control_unit`. + """ + raise NotImplementedError + + def compute_torque( + self, + control: ArrayLike | None, + torque_enable: bool, + q: ArrayLike, + dq: ArrayLike, + ) -> ArrayLike: + """Compute the motor torque from the control signal and current state. + + As with :meth:`compute_control`, the array arguments accept floats, numpy + arrays or torch tensors and the result follows their (broadcast) type. + + :param control: Control signal (volts, amps, or Nm depending on the actuator). + :param torque_enable: Whether the actuator is powered. + :param q: Current joint angle(s) [rad]. + :param dq: Current joint velocity(ies) [rad/s]. + :returns: Motor torque [Nm]. + """ + raise NotImplementedError + + def get_extra_inertia(self) -> float: + """Return the actuator's apparent inertia added to the load [kg·m²].""" + raise NotImplementedError + + +class DCMotorActuator(Actuator): + """Base class for DC motor actuators with a supply voltage and a P-gain. + + Loads ``kp`` and ``vin`` from the log metadata when :meth:`load_log` is called. + + :param testbench_class: Testbench class (see :class:`Actuator`). + :param vin: Default supply voltage [V]. + :param kp: Default firmware proportional gain. + """ + + def __init__( + self, + testbench_class: Testbench, + vin: float, + kp: float, + ): + super().__init__(testbench_class) + self.vin = vin + self.kp = kp + + def load_log(self, log: dict): + super().load_log(log) + self.kp = log["kp"] + if "vin" in log: + self.vin = log["vin"] + + def initialize(self): + """Create ``kt`` and ``R`` parameters on the attached model.""" + # Torque constant [Nm/A] or [V/(rad/s)] + self.model.kt = Parameter(1.0, 0.0, 10.0) + + # Motor resistance [Ohm] + self.model.R = Parameter(1.0, 0.0, 10.0) + + +class VoltageControlledActuator(DCMotorActuator): + """Voltage-controlled servo with a firmware P-position controller. + + Implements the control law + ``duty_cycle = clip(error_gain * kp * Δq, -max_pwm, max_pwm)`` + and the DC motor torque equation with back-EMF. + + :param testbench_class: Testbench class (see :class:`Actuator`). + :param vin: Supply voltage [V]. + :param kp: Firmware proportional gain. + :param error_gain: Converts ``kp * Δq`` to a duty cycle in [−1, 1]. + Depends on the servo's internal encoder resolution and gain scaling. + :param max_pwm: Maximum duty cycle magnitude (default 1.0). + :param max_current: Firmware current limit [A]. If not None, the firmware + limiter is modelled in :meth:`compute_control` as a constraint on the PWM + duty cycle that *attempts* to keep the motor current within + ``[-max_current, max_current]``. Because the firmware can only bound the + duty cycle (not synthesize arbitrary voltage), the limit is only reached + when the battery voltage allows it; at high speed the back-EMF can make + it unreachable. ``None`` (default) → no current limiting. + """ + + def __init__( + self, + testbench_class: Testbench, + vin: float, + kp: float, + error_gain: float = 1.0, + max_pwm: float = 1.0, + max_current: float | None = None, + ): + super().__init__(testbench_class, vin, kp) + self.error_gain = error_gain + self.max_pwm = max_pwm + self.max_current = max_current + + def control_unit(self) -> str: + return "volts" + + def compute_control( + self, q_target: ArrayLike, q: ArrayLike, dq: ArrayLike, dt: float + ) -> ArrayLike | None: + """Compute the voltage command from position error. + + When ``max_current`` is set, the firmware current limiter is modelled as + a constraint on the duty cycle rather than a clamp on the output torque. + The firmware can only act on the PWM duty cycle, so the achievable + current is bounded by the battery voltage: solving :math:`|I| \\le + I_\\text{max}` for :math:`I = (\\text{duty} \\cdot v_\\text{in} - k_t + \\dot{q}) / R` gives the duty window + + .. math:: + + \\frac{k_t \\dot{q} - R\\,I_\\text{max}}{v_\\text{in}} + \\le \\text{duty} \\le + \\frac{k_t \\dot{q} + R\\,I_\\text{max}}{v_\\text{in}} + + The commanded duty is clamped to this window (the limiter *attempt*) and + then to the physical ``[-max_pwm, max_pwm]`` range (the battery reality), + applied last. When back-EMF is large the window can fall outside the + physical range, so the limiter saturates without actually holding the + current at ``max_current`` — exactly as the real firmware behaves. + + :param q_target: Target joint angle(s) [rad]. + :param q: Current joint angle(s) [rad]. + :param dq: Current joint velocity(ies) [rad/s]. Used by the current + limiter (back-EMF term); otherwise unused. + :param dt: Timestep [s] (unused here). + :returns: Voltage [V] sent to the motor. + """ + duty_cycle = (q_target - q) * self.kp * self.error_gain + + # Firmware current limiter: bound the duty cycle so the motor current + # I = (duty * vin - kt * dq) / R stays within [-max_current, max_current]. + # This is only an attempt: the physical PWM clamp below is applied last, + # so if the required duty falls outside [-max_pwm, max_pwm] the current + # limit is not actually reached (the battery cannot supply the voltage). + if self.max_current is not None: + back_emf = self.model.kt.value * dq + duty_span = self.model.R.value * self.max_current / self.vin + duty_center = back_emf / self.vin + duty_cycle = self.backend.clamp( + duty_cycle, duty_center - duty_span, duty_center + duty_span + ) + + # Physical PWM limit (voltage bounded by the battery) — applied last. + duty_cycle = self.backend.clamp(duty_cycle, -self.max_pwm, self.max_pwm) + + return self.vin * duty_cycle + + def compute_torque( + self, + control: ArrayLike | None, + torque_enable: bool, + q: ArrayLike, + dq: ArrayLike, + ) -> ArrayLike: + """Compute motor torque using the DC motor equation with back-EMF. + + :math:`\\tau = k_t V / R - k_t^2 \\dot{q} / R` + + The firmware current limit is *not* applied here: it is modelled as a + duty-cycle constraint in :meth:`compute_control`, so the voltage + ``control`` already reflects the (possibly saturated) current limiter and + the torque follows directly from the DC motor equation. + + :param control: Voltage(s) [V]. + :param torque_enable: If ``False``, returns zero torque. + :param q: Current joint angle(s) [rad] (unused here). + :param dq: Current joint velocity(ies) [rad/s]. + :returns: Motor torque [Nm]. + """ + volts = control + torque = self.model.kt.value * volts / self.model.R.value + torque -= (self.model.kt.value**2) * dq / self.model.R.value + return torque * torque_enable + + +class CurrentControlledActuator(DCMotorActuator): + """Current-controlled servo with a firmware P-position controller. + + The controller outputs a target current proportional to position error, + clipped by both the voltage limits and a user-configurable current cap. + + :param testbench_class: Testbench class (see :class:`Actuator`). + :param vin: Supply voltage [V], used to compute the current saturation. + :param kp: Firmware proportional gain. + :param error_gain: Converts ``kp * Δq`` to a target current [A]. + """ + + def __init__( + self, testbench_class: Testbench, vin: float, kp: float, error_gain: float = 1.0 + ): + super().__init__(testbench_class, vin, kp) + self.error_gain = error_gain + + def control_unit(self) -> str: + return "amps" + + def initialize(self): + super().initialize() + self.model.current_limit = Parameter(1.5, 0, 3) + + def compute_control( + self, q_target: ArrayLike, q: ArrayLike, dq: ArrayLike, dt: float + ) -> ArrayLike | None: + """Compute the current command from position error. + + Clips the P-controller output by both the back-EMF voltage limit and + the ``current_limit`` parameter. The voltage-limit bounds are themselves + elementwise (they depend on ``dq``), so with the torch backend the clamp + broadcasts per environment. + + :param q_target: Target joint angle(s) [rad]. + :param q: Current joint angle(s) [rad]. + :param dq: Current joint velocity(ies) [rad/s]. + :param dt: Timestep [s] (unused here). + :returns: Target current [A]. + """ + # Target current using simple P controller + current = (q_target - q) * self.kp * self.error_gain + + # Maximum allowable current due to voltage limits + current_limit_low = (1 / self.model.R.value) * ( + self.vin - self.model.kt.value * dq + ) + current_limit_high = (1 / self.model.R.value) * ( + -self.vin - self.model.kt.value * dq + ) + current = self.backend.clamp(current, current_limit_high, current_limit_low) + + # Maximum current allowed by the user to avoid heating + current = self.backend.clamp( + current, -self.model.current_limit.value, self.model.current_limit.value + ) + + return current + + def compute_torque( + self, + control: ArrayLike | None, + torque_enable: bool, + q: ArrayLike, + dq: ArrayLike, + ) -> ArrayLike: + """Compute motor torque from current command. + + :math:`\\tau = k_t I` + + :param control: Current command(s) [A]. + :param torque_enable: If ``False``, returns zero torque. + :param q: Current joint angle(s) [rad] (unused here). + :param dq: Current joint velocity(ies) [rad/s] (unused here). + :returns: Motor torque [Nm]. + """ + torque = self.model.kt.value * control + return torque * torque_enable diff --git a/bam/actuators.py b/bam/actuators.py new file mode 100644 index 0000000..d01d1d3 --- /dev/null +++ b/bam/actuators.py @@ -0,0 +1,35 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +from .testbench import Pendulum +from .erob.actuator import ErobActuator +from .dynamixel.actuator import ( + MXActuator, + XL320Actuator, + XL330Actuator, + XL330CurrentActuator, +) +from .feetech.actuator import STS3215Actuator +from .unitree.actuator import UnitreeGo1Actuator + +actuators = { + # Dynamixel MX series + "mx64": lambda: MXActuator(Pendulum), + "mx106": lambda: MXActuator(Pendulum), + # Dynamixel XL series + "xl320": lambda: XL320Actuator(Pendulum), + "xl330": lambda: XL330Actuator(Pendulum), + "xl330i": lambda: XL330CurrentActuator(Pendulum), + # eRob actuators with custom PD controller + "erob80_100": lambda: ErobActuator(Pendulum, damping=2.0), + "erob80_50": lambda: ErobActuator(Pendulum, damping=1.0), + # Feetech STS3215 + "sts3215": lambda: STS3215Actuator(Pendulum), + # Unitree Go1 + "unitree_go1": lambda: UnitreeGo1Actuator(Pendulum), +} diff --git a/bam/animate.py b/bam/animate.py new file mode 100644 index 0000000..539e057 --- /dev/null +++ b/bam/animate.py @@ -0,0 +1,430 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +"""Animated version of ``bam.plot``. + +Renders, side by side, an animated pendulum (left) and the identification +plots (right). The plots are revealed in real time as the simulation plays. +The pendulum shows the *simulated* state (solid) overlaid on the *measured* +state (alpha=0.5). + +When several models are given (e.g. ``m1 m6``), they play *sequentially*: each +model's phase animates the pendulum and grows that model's curve on the plots, +while previously played models stay on screen. The final frame is identical to +what ``bam.plot`` renders statically. +""" + +import argparse +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.gridspec import GridSpec +from matplotlib.patches import Circle, FancyBboxPatch, Rectangle +from matplotlib.animation import FuncAnimation, FFMpegWriter, PillowWriter + +import json + +from .model import load_model, DummyModel +from .actuators import actuators +from . import simulate + +# --- Flat "poster" palette (matches the reference illustration) --- +COL_ARM = "#c9a94e" +COL_MASS = "#8a8a93" +COL_RING = "#26262f" +COL_PILLAR = "#d8cba6" +COL_FLOOR = "#6f6f7a" +COL_PIVOT = "#26262f" + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument( + "--log", type=str, required=True, help="Path to a single processed JSON log." +) +arg_parser.add_argument("--params", type=str, default=["params.json"], nargs="+") +arg_parser.add_argument("--actuator", type=str, required=True) +arg_parser.add_argument("--reset_period", default=None, type=float) +arg_parser.add_argument( + "--fps", + type=int, + default=25, + help="Frames per second (drives both playback pacing and output). " + "The log is downsampled so that, at --speed 1, playback is real time.", +) +arg_parser.add_argument( + "--speed", + type=float, + default=1.0, + help="Playback speed factor (1.0 = real time).", +) +arg_parser.add_argument( + "--angle-offset", + dest="angle_offset", + type=float, + default=0.0, + help="Offset [rad] added to the joint angle when drawing the arm " + "(0 = arm points to the right at q=0).", +) +arg_parser.add_argument( + "--save", + type=str, + default=None, + help="Save to this path (.mp4 or .gif) instead of showing a window.", +) +args = arg_parser.parse_args() + + +def load_log(): + with open(args.log) as f: + log = json.load(f) + log["filename"] = args.log + if "arm_mass" not in log: + log["arm_mass"] = 0.0 + return log + + +def compute_series(log): + """Reproduce the data ``bam.plot`` computes, plus per-model rollouts.""" + ts = np.arange(len(log["entries"])) * log["dt"] + q = np.array([e["position"] for e in log["entries"]]) + goal_q = np.array([e["goal_position"] for e in log["entries"]]) + speed = np.array([e["speed"] if "speed" in e else 0.0 for e in log["entries"]]) + has_speed = any("speed" in e for e in log["entries"]) + + # Recorded control (reference), computed exactly like bam.plot. + dummy = DummyModel() + dummy.set_actuator(actuators[args.actuator]()) + _, __, controls = simulate.Simulator(dummy).rollout_log(log, simulate_control=False) + controls = np.array([0.0 if c is None else c for c in controls]) + torque_enable = np.array([e["torque_enable"] for e in log["entries"]]) + + models = [] + for model_name in args.params: + model = load_model(model_name) + sim_q, sim_speed, sim_controls = simulate.Simulator(model).rollout_log( + log, reset_period=args.reset_period, simulate_control=True + ) + models.append( + { + "name": model.name or model_name, + "q": np.array(sim_q), + "speed": np.array(sim_speed), + "controls": np.array(sim_controls, dtype=float), + } + ) + + return { + "ts": ts, + "q": q, + "goal_q": goal_q, + "speed": speed, + "has_speed": has_speed, + "controls": controls, + "torque_enable": torque_enable, + "control_unit": dummy.actuator.control_unit(), + "models": models, + } + + +class Pendulum: + """Draws the flat-style pendulum and updates it to a given angle. + + Convention: ``q=0`` points straight *down* (the pendulum's rest position); + positive ``q`` swings the arm counter-clockwise. The pivot is elevated and + the arm hangs from it, mounted on a gallows-style stand. + """ + + def __init__(self, ax, angle_offset=0.0): + self.ax = ax + self.offset = angle_offset + self.L = 1.2 + self.pivot = np.array([0.0, 0.95]) + + ax.set_xlim(-1.45, 1.45) + ax.set_ylim(-1.45, 1.45) + ax.set_aspect("equal") + ax.axis("off") + + px, py = self.pivot + + # Floor, vertical stand and top beam holding the pivot (static). + ax.add_patch(Rectangle((-1.45, -1.45), 2.9, 0.34, color=COL_FLOOR, zorder=0)) + ax.add_patch( + FancyBboxPatch( + (-1.30, -1.11), + 0.22, + py + 1.20, + boxstyle="round,pad=0,rounding_size=0.08", + fc=COL_PILLAR, + ec="none", + zorder=1, + ) + ) + ax.add_patch( + FancyBboxPatch( + (-1.24, py - 0.09), + 1.24 + px, + 0.18, + boxstyle="round,pad=0,rounding_size=0.08", + fc=COL_PILLAR, + ec="none", + zorder=1, + ) + ) + + # Two arms + masses: index 0 = measured (alpha), 1 = simulated (solid). + self.arms = [] + self.masses = [] + for alpha, z in ((0.5, 2), (1.0, 3)): + (arm,) = ax.plot( + [], + [], + color=COL_ARM, + lw=22, + solid_capstyle="round", + alpha=alpha, + zorder=z, + ) + mass = Circle( + (0, 0), + 0.20, + fc=COL_MASS, + ec=COL_RING, + lw=7, + alpha=alpha, + zorder=z + 0.1, + ) + ax.add_patch(mass) + self.arms.append(arm) + self.masses.append(mass) + ax.add_patch(Circle(tuple(self.pivot), 0.06, fc=COL_PIVOT, zorder=5)) + + def _tip(self, q): + theta = q + self.offset + # q=0 -> straight down; positive q -> counter-clockwise. + return ( + self.pivot[0] + self.L * np.sin(theta), + self.pivot[1] - self.L * np.cos(theta), + ) + + def set_measured(self, q): + self._set(0, q, visible=True) + + def set_simulated(self, q, visible=True): + self._set(1, q, visible=visible) + + def _set(self, i, q, visible): + x, y = self._tip(q) + self.arms[i].set_data([self.pivot[0], x], [self.pivot[1], y]) + self.arms[i].set_visible(visible) + self.masses[i].center = (x, y) + self.masses[i].set_visible(visible) + + def artists(self): + return self.arms + self.masses + + +def build_figure(data): + has_speed = data["has_speed"] + # Larger fonts and thicker lines so the figure stays legible on small + # devices / when the video is scaled down. + plt.rcParams.update( + { + "font.size": 18, + "axes.titlesize": 20, + "axes.labelsize": 18, + "xtick.labelsize": 15, + "ytick.labelsize": 15, + "legend.fontsize": 15, + "lines.linewidth": 3.0, + "axes.linewidth": 1.8, + "grid.linewidth": 1.2, + } + ) + + n_right = 3 if has_speed else 2 + fig = plt.figure(figsize=(16, 9)) + gs = GridSpec(n_right, 2, width_ratios=[1, 1.6], figure=fig) + + ax_pend = fig.add_subplot(gs[:, 0]) + pendulum = Pendulum(ax_pend, angle_offset=args.angle_offset) + + right_axes = [] + ax1 = fig.add_subplot(gs[0, 1]) + right_axes.append(ax1) + if has_speed: + ax2 = fig.add_subplot(gs[1, 1], sharex=ax1) + ax3 = fig.add_subplot(gs[2, 1], sharex=ax1) + right_axes += [ax2, ax3] + else: + ax2 = None + ax3 = fig.add_subplot(gs[1, 1], sharex=ax1) + right_axes.append(ax3) + + ts = data["ts"] + unit = data["control_unit"] + lines = {} + + # --- ax1: angle --- + # Read (measured) curves are drawn in full from the start; only the + # simulated curves are revealed progressively. + (lines["q"],) = ax1.plot(ts, data["q"], label="q") + (lines["goal_q"],) = ax1.plot( + ts, data["goal_q"], label="goal_q", color="black", linestyle="--" + ) + lines["sim_q"] = [ + ax1.plot([], [], label=f"{m['name']}_q")[0] for m in data["models"] + ] + ax1.set_title( + f"{data_log['motor']}, {data_log['trajectory']}, " + f"m={data_log['mass']}, l={data_log['length']}, k={data_log['kp']}" + ) + ax1.set_ylabel("angle [rad]") + + # --- ax2: speed --- + if has_speed: + (lines["speed"],) = ax2.plot(ts, data["speed"], label="speed") + lines["sim_speed"] = [ + ax2.plot([], [], label=f"{m['name']}_speed")[0] for m in data["models"] + ] + ax2.set_ylabel("speed [rad/s]") + + # --- ax3: control --- + (lines["control"],) = ax3.plot(ts, data["controls"], label=unit) + lines["sim_control"] = [ + ax3.plot([], [], label=f"{m['name']}_{unit}")[0] for m in data["models"] + ] + # torque-off shading (static context). + cmin = float(np.min(data["controls"])) - 0.02 + cmax = float(np.max(data["controls"])) + 0.02 + ax3.fill_between( + ts, + cmin, + cmax, + where=~data["torque_enable"], + color="red", + alpha=0.3, + label="torque off", + ) + ax3.set_ylabel(unit) + ax3.set_xlabel(f"time [s] / simulator: reference") + + # Fixed limits so nothing rescales mid-animation, and vertical time cursor. + cursors = [] + for ax in right_axes: + ax.set_xlim(ts[0], ts[-1]) + ax.grid(True) + ax.legend(loc="upper right") + cursors.append(ax.axvline(ts[0], color="0.4", lw=2.2, alpha=0.0)) + + _set_ylim(ax1, [data["q"], data["goal_q"]] + [m["q"] for m in data["models"]]) + if has_speed: + _set_ylim(ax2, [data["speed"]] + [m["speed"] for m in data["models"]]) + ax3.set_ylim(cmin, cmax) + + fig.tight_layout() + return fig, pendulum, lines, cursors + + +def _set_ylim(ax, arrays): + lo = min(float(np.min(a)) for a in arrays if len(a)) + hi = max(float(np.max(a)) for a in arrays if len(a)) + pad = 0.05 * (hi - lo + 1e-9) + ax.set_ylim(lo - pad, hi + pad) + + +def main(): + global data_log + data_log = load_log() + print(f"Animating {data_log['filename']}") + data = compute_series(data_log) + + fig, pendulum, lines, cursors = build_figure(data) + + ts = data["ts"] + n = len(ts) + n_models = max(1, len(data["models"])) + dt = data_log["dt"] + # Downsample so that, at --speed 1, one output frame advances real time + # by 1/fps seconds (i.e. real-time playback). + step = max(1, round(args.speed / (args.fps * dt))) + idxs = list(range(0, n, step)) + if idxs[-1] != n - 1: + idxs.append(n - 1) + per_phase = len(idxs) + total_frames = n_models * per_phase + + def reveal(line, xs, ys, upto): + line.set_data(xs[:upto], ys[:upto]) + + def full(line, xs, ys): + line.set_data(xs, ys) + + def update(frame): + phase = min(frame // per_phase, n_models - 1) + i = idxs[frame % per_phase] + upto = i + 1 + + # --- Pendulum --- + pendulum.set_measured(data["q"][i]) + if data["models"]: + sim_q = data["models"][phase]["q"] + pendulum.set_simulated(sim_q[i]) + # Cumulated MAE (rad) between simulated and measured angle so far. + mae = float(np.mean(np.abs(sim_q[:upto] - data["q"][:upto]))) + pendulum.ax.set_title( + f"{data['models'][phase]['name']} (MAE: {mae:.3f} rad)", + fontsize=24, + fontweight="bold", + ) + else: + pendulum.set_simulated(data["q"][i], visible=False) + + # Read (measured) curves are static (drawn in full at build time). + + # --- Per-model sim curves --- + for k, m in enumerate(data["models"]): + grp = [ + (lines["sim_q"][k], m["q"]), + (lines["sim_control"][k], m["controls"]), + ] + if data["has_speed"]: + grp.append((lines["sim_speed"][k], m["speed"])) + for line, ys in grp: + if k < phase: + full(line, ts, ys) + elif k == phase: + reveal(line, ts, ys, upto) + else: + line.set_data([], []) + + # --- Time cursor --- + for c in cursors: + c.set_xdata([ts[i], ts[i]]) + c.set_alpha(0.6) + + return [] + + interval_ms = 1000.0 / args.fps + anim = FuncAnimation( + fig, update, frames=total_frames, interval=interval_ms, blit=False + ) + + if args.save: + fps = args.fps + if args.save.lower().endswith(".gif"): + writer = PillowWriter(fps=fps) + else: + writer = FFMpegWriter(fps=fps, bitrate=4000) + print(f"Saving {total_frames} frames to {args.save} at {fps} fps...") + anim.save(args.save, writer=writer) + print("Done.") + else: + plt.show() + + +if __name__ == "__main__": + main() diff --git a/bam/drive_backdrive.py b/bam/drive_backdrive.py new file mode 100644 index 0000000..fcd6023 --- /dev/null +++ b/bam/drive_backdrive.py @@ -0,0 +1,89 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import numpy as np +import matplotlib.pyplot as plt +from .model import load_model + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--params", type=str, default="params.json") +arg_parser.add_argument("--max_torque", type=float, required=True) +args = arg_parser.parse_args() + +plt.figure(figsize=(5, 4)) + +model = load_model(args.params) +for velocity in range(10): + torques = np.linspace(0, args.max_torque, 500) + lows = [] + highs = [] + for motor_torque in torques: + external_torque_low = None + external_torque_high = None + for external_torque in torques: + frictions, _ = model.compute_frictions( + motor_torque, -external_torque, velocity + ) + + if motor_torque - frictions > external_torque: + if external_torque_low is None or external_torque > external_torque_low: + external_torque_low = external_torque + + if ( + external_torque_high is None + and motor_torque + frictions < external_torque + ): + external_torque_high = external_torque + + lows.append(external_torque_low) + highs.append(external_torque_high) + + plt.plot( + torques, + lows, + color="tab:blue", + label=r"$\tau_{drive}(\tau_m)$" if velocity == 0 else None, + alpha=np.exp(-velocity / 2), + ) + plt.plot( + torques, + highs, + color="tab:red", + label=r"$\tau_{backdrive}(\tau_m)$" if velocity == 0 else None, + alpha=np.exp(-velocity / 2), + ) + +plt.plot( + [0, args.max_torque], + [0, args.max_torque], + color="black", + linestyle="--", + label=r"$\tau_f = 0$", +) +plt.xlabel(r"$\tau_m$ [N.m]") +plt.ylabel(r"$-\tau_e$ [N.m]") + +title = r"$\mathcal{M}_1$" +if "M2" in model.title: + title = r"$\mathcal{M}_2$" +elif "M3" in model.title: + title = r"$\mathcal{M}_3$" +elif "M4" in model.title: + title = r"$\mathcal{M}_4$" +elif "M5" in model.title: + title = r"$\mathcal{M}_5$" +elif "M6" in model.title: + title = r"$\mathcal{M}_6$" + +plt.title(title) +plt.legend(ncol=3) +plt.xlim(0, args.max_torque) +plt.ylim(0, args.max_torque) +plt.grid() +plt.show() diff --git a/bam/dynamixel/__init__.py b/bam/dynamixel/__init__.py new file mode 100644 index 0000000..2ff1b05 --- /dev/null +++ b/bam/dynamixel/__init__.py @@ -0,0 +1,8 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + diff --git a/bam/dynamixel/actuator.py b/bam/dynamixel/actuator.py new file mode 100644 index 0000000..1010539 --- /dev/null +++ b/bam/dynamixel/actuator.py @@ -0,0 +1,131 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np +from bam.message import yellow, print_parameter, bright +from bam.actuator import VoltageControlledActuator, CurrentControlledActuator +from bam.parameter import Parameter +from bam.testbench import Testbench, Pendulum + + +class MXActuator(VoltageControlledActuator): + """ + Represents a Dynamixel MX-64 or MX-106 actuator + """ + + def __init__(self, testbench_class: Testbench): + super().__init__( + testbench_class, + vin=15.0, + kp=32.0, + # This gain, if multiplied by a position error and firmware KP, gives duty cycle + # It was determined using an oscilloscope and MX actuators + error_gain=0.158, + # Maximum allowable duty cycle, also determined with oscilloscope + max_pwm=0.9625, + ) + + def initialize(self): + # Torque constant [Nm/A] or [V/(rad/s)] + self.model.kt = Parameter(1.6, 1.0, 3.0) + + # Motor resistance [Ohm] + self.model.R = Parameter(2.0, 1.0, 5.0) + + # Motor armature / apparent inertia [kg m^2] + self.model.armature = Parameter(0.005, 0.001, 0.05) + + def get_extra_inertia(self) -> float: + return self.model.armature.value + + +class XL320Actuator(VoltageControlledActuator): + """ + Represents a Dynamixel XL-320 actuator + """ + + def __init__(self, testbench_class: Testbench): + super().__init__( + testbench_class, + vin=7.5, + kp=32.0, + # This gain, if multiplied by a position error and firmware KP, gives duty cycle + # It was determined using an oscilloscope and XL-320 actuators + error_gain=0.05048199, + # Maximum allowable duty cycle, also determined with oscilloscope + max_pwm=1.0, + ) + + def initialize(self): + # Torque constant [Nm/A] or [V/(rad/s)] + self.model.kt = Parameter(0.7, 0.25, 1.5) + + # Motor resistance [Ohm] + self.model.R = Parameter(5.0, 3.0, 40.0) + + # Motor armature / apparent inertia [kg m^2] + self.model.armature = Parameter(0.0005, 0.0001, 0.01) + + self.model.max_load_friction = 1.0 + + def get_extra_inertia(self) -> float: + return self.model.armature.value + + +XL330_ENCODER_COUNTS_PER_REV = 4096 +XL330_KP_DIVISOR = 256 # Empirically observed for XL330 (manual mentions 128) +XL330_PWM_LIMIT = 885 # Default Present PWM limit for XL330 + + +class XL330Actuator(VoltageControlledActuator): + """ + Represents a Dynamixel XL330 actuator. + """ + + def __init__(self, testbench_class: Testbench): + super().__init__( + testbench_class, + vin=7.5, + kp=400, + error_gain=(XL330_ENCODER_COUNTS_PER_REV / (2 * np.pi)) + / (XL330_KP_DIVISOR * XL330_PWM_LIMIT), + max_pwm=1.0, + # Firmware current limit [A] for the XL330 + max_current=1.75, + ) + + def initialize(self): + self.model.kt = Parameter(1.6, 0.1, 3.0) + self.model.R = Parameter(2.6, 2.0, 5.0) + self.model.armature = Parameter(0.005, 0.0001, 0.05) + + def get_extra_inertia(self) -> float: + return self.model.armature.value + + +class XL330CurrentActuator(CurrentControlledActuator): + """ + Represents a Dynamixel XL330 actuator controlled in current position mode. + """ + + def __init__(self, testbench_class: Testbench): + super().__init__( + testbench_class, + vin=7.5, + kp=400, + error_gain=0.01, + ) + + def initialize(self): + self.model.kt = Parameter(1.6, 0.1, 3.0) + self.model.R = Parameter(2.6, 2.0, 5.0) + self.model.armature = Parameter(0.005, 0.0001, 0.05) + self.model.current_limit = Parameter(1.5, 1.0, 3.0) + + def get_extra_inertia(self) -> float: + return self.model.armature.value diff --git a/bam/dynamixel/all_record.py b/bam/dynamixel/all_record.py new file mode 100644 index 0000000..a3089de --- /dev/null +++ b/bam/dynamixel/all_record.py @@ -0,0 +1,61 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import os +import time + + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--arm-mass", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--motor", type=str, required=True) +arg_parser.add_argument("--port", type=str, default="/dev/ttyUSB0") +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--vin", type=float, required=False, default=15.0) +arg_parser.add_argument("--speak", action="store_true") +args = arg_parser.parse_args() + +KPS = { + "mx64": [4, 8, 16, 32], + "mx106": [4, 8, 16, 32], + "xl320": [4, 8, 16, 32], + "xl330": [50, 100, 200, 300, 400], + "xl330i": [50, 100, 200, 300, 400], +} + +if args.motor not in KPS: + raise ValueError(f"Unknown motor '{args.motor}'. Known motors: {list(KPS.keys())}") + +kps = KPS[args.motor] +trajectories = ["sin_sin", "lift_and_drop", "up_and_down", "sin_time_square"] + +command_base = f"uv run -m bam.dynamixel.record --mass {args.mass} --arm-mass {args.arm_mass} --length {args.length}" +command_base += ( + f" --port {args.port} --logdir {args.logdir} --motor {args.motor} --vin {args.vin}" +) + + +for kp in kps: + for trajectory in trajectories: + sentence = f"Kp {kp}, trajectory {trajectory.replace('_', ' ')}" + print(sentence) + + if args.speak: + from gtts import gTTS + + myobj = gTTS(text=sentence, lang="en", slow=False) + myobj.save("/tmp/message.mp3") + os.system("mpg321 /tmp/message.mp3") + + command = f"{command_base} --kp {kp} --trajectory {trajectory}" + os.system(command) + + if trajectory == "sin_time_square": + time.sleep(3) diff --git a/bam/dynamixel/dynamixel.py b/bam/dynamixel/dynamixel.py new file mode 100644 index 0000000..d0b47d9 --- /dev/null +++ b/bam/dynamixel/dynamixel.py @@ -0,0 +1,314 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import os +import numpy as np +from dynamixel_sdk import * + +# ============================================================================= +# Control table – Protocol V1 (MX / AX series) +# ============================================================================= +# Torque enable +ADDR_TORQUE_ENABLE = 24 +# P Gain +ADDR_P_GAIN = 28 +# Goal position +ADDR_GOAL_POSITION = 30 +# Present position (2 bytes) +ADDR_PRESENT_POSITION = 36 +# Present speed (2 bytes) +ADDR_PRESENT_SPEED = 38 +# Present load (2 bytes) +ADDR_PRESENT_LOAD = 40 +# Present voltage (1 byte) +ADDR_PRESENT_VOLTAGE = 42 +# Present temperature (1 byte) +ADDR_PRESENT_TEMPERATURE = 43 + +# ============================================================================= +# Control table – XL-320 (Protocol V2 only) +# ============================================================================= +# --- EEPROM --- +XL320_ADDR_CW_ANGLE_LIMIT = 6 # 2 bytes +XL320_ADDR_CCW_ANGLE_LIMIT = 8 # 2 bytes +XL320_ADDR_CONTROL_MODE = 11 # 1 byte (1=Wheel, 2=Joint) +XL320_ADDR_MAX_TORQUE = 15 # 2 bytes +# --- RAM --- +XL320_ADDR_TORQUE_ENABLE = 24 # 1 byte +XL320_ADDR_LED = 25 # 1 byte (RGB bitmask: R=1, G=2, B=4) +XL320_ADDR_D_GAIN = 27 # 1 byte +XL320_ADDR_I_GAIN = 28 # 1 byte +XL320_ADDR_P_GAIN = 29 # 1 byte +XL320_ADDR_GOAL_POSITION = 30 # 2 bytes (0–1023, 0.29°/step, range 0–300°) +XL320_ADDR_MOVING_SPEED = 32 # 2 bytes (0–1023 CCW, 1024–2047 CW, 0.111 rpm/step) +XL320_ADDR_TORQUE_LIMIT = 35 # 2 bytes +XL320_ADDR_PRESENT_POSITION = 37 # 2 bytes +XL320_ADDR_PRESENT_SPEED = 39 # 2 bytes +XL320_ADDR_PRESENT_LOAD = 41 # 2 bytes +XL320_ADDR_PRESENT_VOLTAGE = 45 # 1 byte +XL320_ADDR_PRESENT_TEMPERATURE = 46 # 1 byte +XL320_ADDR_MOVING = 49 # 1 byte (0=stopped, 1=moving) +XL320_ADDR_HW_ERROR_STATUS = 50 # 1 byte + +# XL-320 position resolution +XL320_RESOLUTION = 1023 # 10-bit +XL320_RANGE_DEG = 300.0 # degrees +XL320_CENTER = 512 # raw value for 0 rad (150°) + + +class DynamixelActuatorV1: + def __init__(self, port: str, id: int = 1): + self.id = id + + result = os.system(f"setserial {port} low_latency") + if result != 0: + raise Exception( + "Failed to set low latency mode (you can try: sudo apt install setserial)" + ) + + self.portHandler = PortHandler(port) + self.packetHandler = PacketHandler(1.0) + + self.portHandler.openPort() + self.portHandler.setBaudRate(1000000) + + def set_p_gain(self, gain: int): + # Set P gain + self.packetHandler.write2ByteTxOnly( + self.portHandler, self.id, ADDR_P_GAIN, gain + ) + + def set_torque(self, enable: bool): + # Enable torque + self.packetHandler.write1ByteTxOnly( + self.portHandler, self.id, ADDR_TORQUE_ENABLE, 1 if enable else 0 + ) + + def set_goal_position(self, position: float): + # Position is a 12-bit value + position = int(4096 * (position / (2 * np.pi) + 0.5)) + + # Set goal position + self.packetHandler.write2ByteTxOnly( + self.portHandler, self.id, ADDR_GOAL_POSITION, position + ) + + def read_data(self): + # Reading position, speed, load, voltage and temperature + data, result, error = self.packetHandler.readTxRx( + self.portHandler, self.id, ADDR_PRESENT_POSITION, 8 + ) + + # Position is a 12-bit value + position = (data[1] << 8) | data[0] + position = 2 * np.pi * ((position / 4096) - 0.5) + + # Speed is a 10-bit value, units are 0.11 rpm per step + speed = (data[3] << 8) | data[2] + if speed > 1024: + speed = -(speed - 1024) + speed = speed * 0.11 * 2 * np.pi / 60.0 + + # Applied "load" + load = (data[5] << 8) | data[4] + if load > 1024: + load = -(load - 1024) + + # Voltage is a byte value, units are 0.1 V + volts = data[6] / 10.0 + + # Temperature are °C + temp = data[7] + + return { + "position": position, + "speed": speed, + "load": load, + "input_volts": volts, + "temp": temp, + } + + +class DynamixelXL320: + """ + Controller for the XL-320 servo motor. + + The XL-320 uses **Protocol 2.0 only** and has a 10-bit position encoder + covering a 300° range (0 to 1023 raw, centre = 512 ≡ 0 rad). + + Args: + port: Serial port, e.g. ``"/dev/ttyUSB0"``. + id: DYNAMIXEL ID (default 1). + """ + + def __init__(self, port: str, id: int = 1): + self.id = id + + result = os.system(f"setserial {port} low_latency") + if result != 0: + raise Exception( + "Failed to set low latency mode " + "(you can try: sudo apt install setserial)" + ) + + self.portHandler = PortHandler(port) + self.packetHandler = PacketHandler(2.0) # Protocol 2.0 mandatory + + self.portHandler.openPort() + self.portHandler.setBaudRate(1000000) + + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ + + @staticmethod + def _rad_to_raw(position: float) -> int: + """Convert radians to XL-320 raw position (0–1023, centre = 0 rad).""" + raw = int( + position * (XL320_RESOLUTION / (XL320_RANGE_DEG * np.pi / 180.0)) + + XL320_CENTER + ) + return int(np.clip(raw, 0, XL320_RESOLUTION)) + + @staticmethod + def _raw_to_rad(raw: int) -> float: + """Convert XL-320 raw position to radians.""" + return ( + (raw - XL320_CENTER) * (XL320_RANGE_DEG * np.pi / 180.0) / XL320_RESOLUTION + ) + + # ------------------------------------------------------------------ + # Write commands + # ------------------------------------------------------------------ + + def set_torque(self, enable: bool): + """Enable or disable motor torque.""" + self.packetHandler.write1ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_TORQUE_ENABLE, 1 if enable else 0 + ) + + def set_p_gain(self, gain: int): + """Set the proportional (P) gain (0–254).""" + self.packetHandler.write1ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_P_GAIN, gain + ) + + def set_i_gain(self, gain: int): + """Set the integral (I) gain (0–254).""" + self.packetHandler.write1ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_I_GAIN, gain + ) + + def set_d_gain(self, gain: int): + """Set the derivative (D) gain (0–254).""" + self.packetHandler.write1ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_D_GAIN, gain + ) + + def set_pid_gains(self, p: int, i: int, d: int): + """Set P, I and D gains in one call (values 0–254 each).""" + self.set_p_gain(p) + self.set_i_gain(i) + self.set_d_gain(d) + + def set_goal_position(self, position: float): + """ + Set goal position. + + Args: + position: Target angle in radians. Acceptable range: + ``[-150°, +150°]`` → ``[-2.618, +2.618]`` rad. + """ + raw = self._rad_to_raw(position) + self.packetHandler.write2ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_GOAL_POSITION, raw + ) + + def set_moving_speed(self, speed_rpm: float): + """ + Set moving speed in Joint Mode. + + Args: + speed_rpm: Speed in RPM (0 = maximum, 114 = approx. max). + The sign is ignored; always positive in joint mode. + """ + raw = int(abs(speed_rpm) / 0.111) + raw = int(np.clip(raw, 0, 1023)) + self.packetHandler.write2ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_MOVING_SPEED, raw + ) + + def set_led(self, color: int): + """ + Set the LED colour using a RGB bitmask. + + Args: + color: Bitmask – Red=1, Green=2, Blue=4 (combinations allowed, 0=off). + """ + self.packetHandler.write1ByteTxOnly( + self.portHandler, self.id, XL320_ADDR_LED, color & 0x07 + ) + + # ------------------------------------------------------------------ + # Read feedback + # ------------------------------------------------------------------ + + def read_data(self) -> dict: + """ + Read present position, speed, load, input voltage and temperature. + + Returns a dict with keys: + ``position`` (rad), ``speed`` (rad/s), ``load`` (signed, 0.1 % units), + ``input_volts`` (V), ``temp`` (°C). + + Note: addresses 37–42 are contiguous; 43–44 are unused; 45–46 are + voltage and temperature. Two separate reads are performed. + """ + # Read position (2 B), speed (2 B), load (2 B) → 6 bytes from addr 37 + data_psl, result, error = self.packetHandler.readTxRx( + self.portHandler, self.id, XL320_ADDR_PRESENT_POSITION, 6 + ) + + # Present position – 10-bit, 0.29°/step, centre = 512 + position_raw = (data_psl[1] << 8) | data_psl[0] + position = self._raw_to_rad(position_raw) + + # Present speed – 11-bit value, bit 10 = direction (0=CCW, 1=CW) + # Unit: 0.111 rpm/step + speed_raw = (data_psl[3] << 8) | data_psl[2] + if speed_raw > 1023: + speed = -(speed_raw - 1024) + else: + speed = speed_raw + speed = speed * 0.111 * 2 * np.pi / 60.0 # rad/s + + # Present load – 11-bit, bit 10 = direction, unit 0.1 % + load_raw = (data_psl[5] << 8) | data_psl[4] + if load_raw > 1023: + load = -(load_raw - 1024) + else: + load = load_raw + + # Read voltage (1 B) and temperature (1 B) → 2 bytes from addr 45 + data_vt, result, error = self.packetHandler.readTxRx( + self.portHandler, self.id, XL320_ADDR_PRESENT_VOLTAGE, 2 + ) + + # Voltage: unit is 0.1 V + volts = data_vt[0] / 10.0 + + # Temperature in °C + temp = data_vt[1] + + return { + "position": position, + "speed": speed, + "load": load, + "input_volts": volts, + "temp": temp, + } diff --git a/bam/dynamixel/record.py b/bam/dynamixel/record.py new file mode 100644 index 0000000..d55cf9c --- /dev/null +++ b/bam/dynamixel/record.py @@ -0,0 +1,150 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import json +import datetime +import os +import numpy as np +import argparse +import time +from .dynamixel import DynamixelActuatorV1, DynamixelXL320 +from bam.trajectory import * + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--arm-mass", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--port", type=str, default="/dev/ttyUSB0") +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--trajectory", type=str, default="lift_and_drop") +arg_parser.add_argument("--motor", type=str, required=True) +arg_parser.add_argument("--kp", type=int, default=32) +arg_parser.add_argument("--vin", type=float, default=15.0) +args = arg_parser.parse_args() + +if args.trajectory not in trajectories: + raise ValueError(f"Unknown trajectory: {args.trajectory}") + +_XL330_MOTORS = {"xl330", "xl330i"} + +# XL330-specific conversion helpers +_XL330_RADS_PER_SEC_PER_COUNT = 0.229 * (2.0 * np.pi / 60.0) +_XL330_PWM_LIMIT = 885 # XL330 Present PWM limit (counts) + + +def convert_xl330_velocity(raw_signed: float) -> float: + return float(raw_signed) * _XL330_RADS_PER_SEC_PER_COUNT + + +def convert_xl330_pwm_to_duty(raw: float) -> float: + x = float(raw) + if x > 2**15 - 1: + x -= 2**16 + return np.clip(x / _XL330_PWM_LIMIT, -1.0, 1.0) + + +trajectory = trajectories[args.trajectory] + +# ── Controller initialisation ──────────────────────────────────────────────── +if args.motor in _XL330_MOTORS: + from rustypot import Xl330PyController + + c = Xl330PyController(args.port, baudrate=1000000, timeout=0.01) + ID = 1 + + start = time.time() + while time.time() - start < 1.0: + goal_position, torque_enable = trajectory(0) + if torque_enable: + c.write_goal_position(ID, goal_position) + c.write_torque_enable(ID, torque_enable) + c.write_position_p_gain(ID, args.kp) +else: + dxl = DynamixelActuatorV1(args.port) + + start = time.time() + while time.time() - start < 1.0: + goal_position, torque_enable = trajectory(0) + if torque_enable: + dxl.set_goal_position(goal_position) + dxl.set_torque(torque_enable) + dxl.set_p_gain(args.kp) + +# ── Recording loop ─────────────────────────────────────────────────────────── +start = time.time() +data = { + "mass": args.mass, + "arm-mass": args.arm_mass, + "length": args.length, + "kp": args.kp, + "vin": args.vin, + "motor": args.motor, + "trajectory": args.trajectory, + "entries": [], +} + +while time.time() - start < trajectory.duration: + t = time.time() - start + goal_position, new_torque_enable = trajectory(t) + if new_torque_enable != torque_enable: + if args.motor in _XL330_MOTORS: + c.write_torque_enable(ID, new_torque_enable) + else: + dxl.set_torque(new_torque_enable) + torque_enable = new_torque_enable + time.sleep(0.001) + if torque_enable: + if args.motor in _XL330_MOTORS: + c.write_goal_position(ID, goal_position) + else: + dxl.set_goal_position(goal_position) + time.sleep(0.001) + + t0 = time.time() - start + + if args.motor in _XL330_MOTORS: + entry = {} + entry["position"] = c.read_present_position(ID)[0] + entry["speed"] = convert_xl330_velocity(c.read_present_velocity(ID)[0]) + entry["load"] = convert_xl330_pwm_to_duty(c.read_present_pwm(ID)[0]) + entry["input_volts"] = c.read_present_input_voltage(ID)[0] / 10.0 + entry["temp"] = c.read_present_temperature(ID)[0] + else: + entry = dxl.read_data() + + t1 = time.time() - start + + entry["timestamp"] = (t0 + t1) / 2.0 + entry["goal_position"] = goal_position + entry["torque_enable"] = torque_enable + data["entries"].append(entry) + +# ── Return to zero ─────────────────────────────────────────────────────────── +goal_position = data["entries"][-1]["position"] +return_dt = 0.01 +max_variation = return_dt * 1.0 +while abs(goal_position) > 0: + if goal_position > 0: + goal_position = max(0, goal_position - max_variation) + else: + goal_position = min(0, goal_position + max_variation) + if args.motor in _XL330_MOTORS: + c.write_goal_position(ID, goal_position) + else: + dxl.set_goal_position(goal_position) + time.sleep(return_dt) + +if args.motor in _XL330_MOTORS: + c.write_torque_enable(ID, False) +else: + dxl.set_torque(False) + +# ── Save ───────────────────────────────────────────────────────────────────── +date = datetime.datetime.now().strftime("%Y-%m-%d_%Hh%Mm%S") +filename = f"{args.logdir}/{date}.json" +json.dump(data, open(filename, "w")) diff --git a/bam/erob/.gitignore b/bam/erob/.gitignore new file mode 100644 index 0000000..959b9a2 --- /dev/null +++ b/bam/erob/.gitignore @@ -0,0 +1 @@ +*pb2.py diff --git a/bam/erob/__init__.py b/bam/erob/__init__.py new file mode 100644 index 0000000..2ff1b05 --- /dev/null +++ b/bam/erob/__init__.py @@ -0,0 +1,8 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + diff --git a/bam/erob/actuator.py b/bam/erob/actuator.py new file mode 100644 index 0000000..d989823 --- /dev/null +++ b/bam/erob/actuator.py @@ -0,0 +1,95 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +from bam.actuator import Actuator +from bam.parameter import Parameter +from bam.testbench import Testbench, Pendulum + +if TYPE_CHECKING: + from bam.actuator import ArrayLike + + +class ErobActuator(Actuator): + def __init__(self, testbench_class: Testbench, damping=2.0): + super().__init__(testbench_class) + + # Maximum current [A] + self.max_amps = 12.0 + + # Maximum input voltage [V] + self.max_volts = 48.0 + + # Damping factor + self.damping = damping + + def initialize(self): + # Torque constant [Nm/A] or [V/(rad/s)] + self.model.kt = Parameter(1.6, 1.0, 15.0) + + # Motor resistance [Ohm] + self.model.R = Parameter(2.0, 0.1, 3.5) + + # Motor armature / apparent inertia [kg m^2] + self.model.armature = Parameter(0.005, 0.001, 2.0) + + # Adjusting upper bounds for identification + self.model.max_friction_base = 10.0 + self.model.max_load_friction = 1.0 + self.model.max_viscous_friction = 30.0 + + def load_log(self, log: dict): + super().load_log(log) + + self.kp = log["kp"] + + if "damping" in log: + self.damping = log["damping"] + + def control_unit(self) -> str: + return "amps" + + def compute_control( + self, q_target: ArrayLike, q: ArrayLike, dq: ArrayLike, dt: float + ) -> ArrayLike | None: + # Target velocity is assumed to be 0 + amps = (q_target - q) * self.kp + self.damping * np.sqrt(self.kp) * (0.0 - dq) + amps = self.backend.clamp(amps, -self.max_amps, self.max_amps) + + return amps + + def compute_torque( + self, + control: ArrayLike | None, + torque_enable: bool, + q: ArrayLike, + dq: ArrayLike, + ) -> ArrayLike: + # Computing the torque given the control signal + # With eRob, control=None actually meany amps=0, and not a disconnection of the motor + amps = control * torque_enable + torque = self.model.kt.value * amps + + # Computing the torque boundaries given the maximum voltage and the back EMF + volts_bounded_torque = ( + self.model.kt.value / self.model.R.value + ) * self.max_volts + emf = (self.model.kt.value**2) * dq / self.model.R.value + + min_torque = -volts_bounded_torque - emf + max_torque = volts_bounded_torque - emf + torque = self.backend.clamp(torque, min_torque, max_torque) + + return torque + + def get_extra_inertia(self) -> float: + return self.model.armature.value diff --git a/bam/erob/all_record.py b/bam/erob/all_record.py new file mode 100644 index 0000000..3c565a4 --- /dev/null +++ b/bam/erob/all_record.py @@ -0,0 +1,49 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import os +import time + + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--host", type=str, default="127.0.0.1") +arg_parser.add_argument("--offset", type=float, required=True) +arg_parser.add_argument("--damping", type=float, required=True) +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--arm_mass", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--motor", type=str, required=True) +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--speak", action="store_true") +args = arg_parser.parse_args() + +kps = [5, 10, 25, 50] +trajectories = ["sin_sin", "lift_and_drop", "up_and_down", "sin_time_square"] + +command_base = f"python3 -m bam.erob.record --offset {args.offset} --mass {args.mass} --arm_mass {args.arm_mass} --length {args.length} --damping {args.damping}" +command_base += f" --host {args.host} --logdir {args.logdir} --motor {args.motor}" + + +for kp in kps: + for trajectory in trajectories: + sentence = f"Kp {kp}, trajectory {trajectory.replace('_', ' ')}" + print(sentence) + + if args.speak: + from gtts import gTTS + + myobj = gTTS(text=sentence, lang="en", slow=False) + myobj.save("/tmp/message.mp3") + os.system("mpg321 /tmp/message.mp3") + + command = f"{command_base} --kp {kp} --trajectory {trajectory}" + os.system(command) + + if trajectory == "sin_time_square": + time.sleep(3) diff --git a/bam/erob/etherban.proto b/bam/erob/etherban.proto new file mode 100644 index 0000000..e51f92a --- /dev/null +++ b/bam/erob/etherban.proto @@ -0,0 +1,36 @@ +syntax = "proto2"; +package etherban_server; + +enum OperationMode { + POSITION = 0; + VELOCITY = 1; + TORQUE = 2; +} + +// Status of a given device +message Status { + required int32 device = 1; + + required double target_position = 2; + required double velocity_demand = 3; + required double torque_demand = 4; + + required double present_position = 5; + required double present_velocity = 6; + required double present_current = 7; +} + +message Order { + required int32 device = 1; + required OperationMode operation_mode = 2; + required double target = 3; +} + +message Request { + repeated Order orders = 1; +} + +message Response { + repeated Status statuses = 1; + required bool has_fault = 2; +} \ No newline at end of file diff --git a/bam/erob/etherban.py b/bam/erob/etherban.py new file mode 100644 index 0000000..678a1e6 --- /dev/null +++ b/bam/erob/etherban.py @@ -0,0 +1,176 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import zmq +import copy +import numpy as np +import threading +import os +import time +from . import etherban_pb2 as messages +from bam.trajectory import cubic_interpolate + + +class Client: + def __init__(self, host: str): + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REQ) + self.socket.connect(f"tcp://{host}:7332") + self.running = True + self.lock = threading.Lock() + self.wait_lock = threading.Condition() + + self.orders = {} + self.statuses = {} + + def set_order(self, index: int, mode="position", target=0.0): + self.lock.acquire() + if index not in self.orders: + self.orders[index] = messages.Order() + + order = self.orders[index] + order.device = index + + if mode == "position": + order.operation_mode = messages.POSITION + elif mode == "velocity": + order.operation_mode = messages.VELOCITY + elif mode == "torque": + order.operation_mode = messages.TORQUE + order.target = target + self.lock.release() + + def position_control( + self, + index: int, + target_position: float, + target_velocity: float = 0.0, + kp: float = 10.0, + damping: float = 2.0, + max_amps: float = 12.0, + ): + status = self.statuses[index] + position_error = target_position - status["position"] + velocity_error = target_velocity - status["velocity"] + amps = position_error * kp + damping * np.sqrt(kp) * velocity_error + amps = max(-max_amps, min(max_amps, amps)) + self.set_order(index, "torque", amps) + + def stop(self, index: int): + self.set_order(index, "torque", 0.0) + + def send(self): + self.lock.acquire() + message = messages.Request() + + # Adding orders to the packet + for index in self.orders: + order = self.orders[index] + message.orders.append(order) + + # Send the message + self.socket.send(message.SerializeToString()) + + # Receive the response + response = self.socket.recv() + response_message = messages.Response() + response_message.ParseFromString(response) + + for status in response_message.statuses: + self.statuses[status.device] = { + "target_position": status.target_position, + "torque_demand": status.torque_demand, + "velocity_demand": status.velocity_demand, + "position": status.present_position, + "velocity": status.present_velocity, + "current": status.present_current, + } + + self.lock.release() + + if response_message.has_fault: + print("Server has fault, clearing it and aborting") + exit(0) + + def get_statuses(self): + self.lock.acquire() + statuses = copy.deepcopy(self.statuses) + self.lock.release() + + return statuses + + def run(self): + t0 = time.time() + while self.running: + while time.time() - t0 < 0.001: + time.sleep(1e-4) + t0 += 0.001 + + self.send() + + self.wait_lock.acquire() + self.wait_lock.notify_all() + self.wait_lock.release() + + def sync(self): + self.wait_lock.acquire() + self.wait_lock.wait() + self.wait_lock.release() + + def wait_stability(self, index: int): + positions = [] + while True: + status = self.statuses[index] + positions.append(status["position"]) + positions = positions[-100:] + if len(positions) == 100 and np.std(positions) < 1e-5: + break + self.sync() + + def goto_safe(self, index: int, target: float, duration: float = 3.0): + status = self.statuses[index] + start_pos = status["position"] + t0 = time.time() + t = 0 + + while t < duration: + t = time.time() - t0 + current_target = cubic_interpolate( + [[0.0, start_pos, 0], [duration, target, 0]], t + ) + self.set_order(0, "position", current_target) + self.sync() + + def run_background(self): + thread = threading.Thread(target=self.run) + thread.start() + + self.sync() + + def stop(self): + self.running = False + + +if __name__ == "__main__": + client = Client("localhost") + start = time.time() + client.run_background() + + while True: + os.system("clear") + print(f"{len(client.statuses)} devices") + for k in client.statuses: + status = client.statuses[k] + print(f"Device {k}") + print(f" - Target Position: {status['target_position']}") + print(f" - Torque Demand: {status['torque_demand']}") + print(f" - Velocity Demand: {status['velocity_demand']}") + print(f" - Position: {status['position']}") + print(f" - Velocity: {status['velocity']}") + print(f" - Current: {status['current']}") + time.sleep(0.1) diff --git a/bam/erob/generate_protobuf.sh b/bam/erob/generate_protobuf.sh new file mode 100644 index 0000000..3836bb8 --- /dev/null +++ b/bam/erob/generate_protobuf.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +# Re-generates the python protobuf +protoc --python_out=. etherban.proto diff --git a/bam/erob/record.py b/bam/erob/record.py new file mode 100644 index 0000000..60c89b8 --- /dev/null +++ b/bam/erob/record.py @@ -0,0 +1,93 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import json +import datetime +import argparse +import time +from .etherban import Client +from bam.trajectory import * + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--host", type=str, default="127.0.0.1") +arg_parser.add_argument( + "--offset", + type=float, + required=True, + help="Offset in radians for the zero position", +) +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--arm_mass", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--trajectory", type=str, default="sin_time_square") +arg_parser.add_argument("--motor", type=str, required=True) +arg_parser.add_argument("--kp", type=int, default=10.0) +arg_parser.add_argument("--damping", type=float, required=True) +args = arg_parser.parse_args() + +if args.trajectory not in trajectories: + raise ValueError(f"Unknown trajectory: {args.trajectory}") + + +def angle_wrap(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi + + +trajectory = trajectories[args.trajectory] + +eth = Client(args.host) +eth.run_background() + +goal_position, torque_enable = trajectory(0) +eth.wait_stability(0) +eth.goto_safe(0, args.offset + goal_position) + +start = time.time() +data = { + "mass": args.mass, + "arm_mass": args.arm_mass, + "length": args.length, + "kp": args.kp, + "damping": args.damping, + "motor": args.motor, + "trajectory": args.trajectory, + "entries": [], +} + +while time.time() - start < trajectory.duration: + t = time.time() - start + goal_position, torque_enable = trajectory(t) + + if torque_enable: + eth.position_control(0, args.offset + goal_position, 0.0, args.kp, args.damping) + else: + eth.set_order(0, "torque", 0.0) + eth.sync() + + status = eth.get_statuses()[0] + entry = { + "position": angle_wrap(status["position"] - args.offset), + "speed": status["velocity"], + "torque_demand": status["torque_demand"], + "control": status["current"], + "timestamp": time.time() - start, + "goal_position": goal_position, + "torque_enable": torque_enable, + } + + data["entries"].append(entry) + +eth.set_order(0, "torque", 0.0) +eth.stop() + +# Format YYYY-MM-DD_HH:mm:ss" +date = datetime.datetime.now().strftime("%Y-%m-%d_%Hh%Mm%S") + +filename = f"{args.logdir}/{date}.json" +json.dump(data, open(filename, "w")) diff --git a/bam/erob/static.py b/bam/erob/static.py new file mode 100644 index 0000000..1371cba --- /dev/null +++ b/bam/erob/static.py @@ -0,0 +1,73 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import json +import time +import datetime +import os +import argparse +import erob.etherban as etherban + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--offset", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--port", type=str, default="/dev/ttyUSB0") +args = arg_parser.parse_args() + +data = {} + + +def read_float(prompt): + try: + return float(input(prompt)) + except ValueError: + return None + + +data["mass"] = args.mass +data["length"] = args.length +amps = read_float("Amps: ") + +client = etherban.Client("localhost") +client.set_order(0, "torque", amps) +client.run_background() + +has_value = False +for pos in "low", "high": + if input(f"Snapshot the {pos} position [y/n] ? ") == "y": + has_value = True + status = client.get_statuses()[0] + data[pos] = { + "amps": status["current"], + "position": status["position"] - args.offset, + } + else: + data[pos] = {"amps": None, "position": None} + +# Slowly decreaing amps +while amps > 0: + client.set_order(0, "torque", amps) + amps = max(0, amps - 0.25) + time.sleep(0.1) + +if has_value: + # Reading static.json if it exists + if os.path.exists("static.json"): + with open("static.json", "r") as f: + static_data = json.load(f) + else: + static_data = [] + + static_data.append(data) + + # Writing to static.json + with open("static.json", "w") as f: + json.dump(static_data, f) + +client.stop() diff --git a/bam/feetech/__init__.py b/bam/feetech/__init__.py new file mode 100644 index 0000000..2ff1b05 --- /dev/null +++ b/bam/feetech/__init__.py @@ -0,0 +1,8 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + diff --git a/bam/feetech/actuator.py b/bam/feetech/actuator.py new file mode 100644 index 0000000..89fbed7 --- /dev/null +++ b/bam/feetech/actuator.py @@ -0,0 +1,94 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +from __future__ import annotations + +import numpy as np +from typing import TYPE_CHECKING, Union +from bam.message import yellow, print_parameter, bright +from bam.actuator import VoltageControlledActuator +from bam.parameter import Parameter +from bam.testbench import Testbench, Pendulum + +if TYPE_CHECKING: + from bam.actuator import ArrayLike + + +class STS3215Actuator(VoltageControlledActuator): + """ + Feetech STS3215 7.4v + """ + + def __init__(self, testbench_class: Testbench): + super().__init__( + testbench_class, + vin=7.4, + kp=32, + # This gain, if multiplied by a position error and firmware KP, gives duty cycle + # It was determined using an oscilloscope and STS3215 actuators + # here, firmware_kp = kp + error_gain=0.166, + # self.error_gain = 0.001 * np.rad2deg(1.0) + # Maximum allowable duty cycle, also determined with oscilloscope + max_pwm=0.97, # TODO, but can we assume 1.0 ? + ) + + self.default_max_velocity = (3400 * 2 * np.pi) / 4096 + + def get_extra_inertia(self) -> float: + return self.model.armature.value + + def load_log(self, log: dict): + super().load_log(log) + + self.q_target_smooth = np.zeros_like(self.kp) + + def initialize(self): + # Torque constant [Nm/A] or [V/(rad/s)] + self.model.kt = Parameter(0.784532, 0.05, 2.5) # docs says 8 kg.cm / A + + self.model.error_gain_ratio = Parameter(1.0, 0.1, 10.0) + + # Motor resistance [Ohm] + self.model.R = Parameter(2.0, 0.1, 10.0) + + # Motor armature / apparent inertia [kg m^2] + self.model.armature = Parameter(0.0001, 0.00001, 0.04) + + self.model.q_offset = Parameter(0, -0.2, 0.2) + + self.model.max_velocity = Parameter( + self.default_max_velocity, + 0.1 * self.default_max_velocity, + 10.0 * self.default_max_velocity, + ) + + def compute_control( + self, q_target: ArrayLike, q: ArrayLike, dq: ArrayLike, dt: float + ) -> ArrayLike | None: + """ + Assumes the motor is using a kp controller + This can be overloaded if more custom behaviour is used + """ + + # Internal target position is clipped using maximum velocity + self.q_target_smooth = self.backend.clamp( + q_target, + self.q_target_smooth - self.model.max_velocity.value * dt, + self.q_target_smooth + self.model.max_velocity.value * dt, + ) + + duty_cycle = ( + (self.q_target_smooth - q) + * self.kp + * self.error_gain + * self.model.error_gain_ratio.value + ) + duty_cycle = self.backend.clamp(duty_cycle, -self.max_pwm, self.max_pwm) + + return self.vin * duty_cycle diff --git a/bam/feetech/all_record.py b/bam/feetech/all_record.py new file mode 100644 index 0000000..6f9d480 --- /dev/null +++ b/bam/feetech/all_record.py @@ -0,0 +1,51 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import os +import time + + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--motor", type=str, required=True) +arg_parser.add_argument("--port", type=str, default="/dev/ttyUSB0") +arg_parser.add_argument("--id", type=int, required=True) +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--vin", type=float, default=5.1) +arg_parser.add_argument("--speak", action="store_true") +args = arg_parser.parse_args() + +# kps = [130, 275, 550, 1100] +kps = [4, 8, 16, 32] +trajectories = ["brutal", "sin_sin", "lift_and_drop", "up_and_down", "sin_time_square"] + +command_base = ( + f"python3 -m bam.feetech.record --mass {args.mass} --length {args.length}" +) +command_base += f" --port {args.port} --logdir {args.logdir} --motor {args.motor} --id {args.id} --vin {args.vin}" + + +for kp in kps: + for trajectory in trajectories: + sentence = f"Kp {kp}, trajectory {trajectory.replace('_', ' ')}" + print(sentence) + + if args.speak: + from gtts import gTTS + + myobj = gTTS(text=sentence, lang="en", slow=False) + myobj.save("/tmp/message.mp3") + os.system("mpg321 /tmp/message.mp3") + + command = f"{command_base} --kp {kp} --trajectory {trajectory}" + os.system(command) + + if trajectory == "sin_time_square": + time.sleep(3) diff --git a/bam/feetech/record.py b/bam/feetech/record.py new file mode 100644 index 0000000..b238cbb --- /dev/null +++ b/bam/feetech/record.py @@ -0,0 +1,171 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +# from bam.feetech.feetech_pwm_control import FeetechPWMControl +from pypot.feetech import FeetechSTS3215IO +import rustypot +import json +import datetime +import os +import numpy as np +import argparse +import time +from bam.trajectory import * + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--mass", type=float, required=True) +arg_parser.add_argument("--length", type=float, required=True) +arg_parser.add_argument("--port", type=str, default="/dev/ttyUSB0") +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--trajectory", type=str, default="lift_and_drop") +arg_parser.add_argument("--motor", type=str, required=True) +arg_parser.add_argument("--kp", type=int, default=32) +arg_parser.add_argument("--vin", type=float, default=15.0) +arg_parser.add_argument("--id", type=int, required=True) +args = arg_parser.parse_args() + +os.makedirs(args.logdir, exist_ok=True) + +if args.trajectory not in trajectories: + raise ValueError(f"Unknown trajectory: {args.trajectory}") + +# motors = { +# "test": (1, "sts3215"), +# } +ids = [1] + +# motor = FeetechPWMControl(id=args.id) +io = FeetechSTS3215IO("/dev/ttyACM0") +io.set_mode({1: 0}) +# control = rustypot.feetech("/dev/ttyACM0", 1000000) +# control.set_mode(ids, 0) + +trajectory = trajectories[args.trajectory] + +start = time.time() +while time.time() - start < 1.0: + goal_position, torque_enable = trajectory(0) + if torque_enable: + io.set_goal_position({1: np.rad2deg(goal_position)}) + # control.write_goal_position(ids, [goal_position]) + io.enable_torque([1]) + # control.enable_torque(ids) + # motor.goal_position = np.rad2deg(goal_position) + # motor.enable_torque() + else: + io.disable_torque([1]) + # control.disable_torque(ids) + # motor.disable_torque() + # control.set_kps(ids, [32]) + io.set_P_coefficient({1: args.kp}) + io.set_D_coefficient({1: 0}) + + # motor.kp = args.kp + + +start = time.time() +data = { + "mass": args.mass, + "length": args.length, + "kp": args.kp, + "vin": args.vin, + "motor": args.motor, + "trajectory": args.trajectory, + "entries": [], +} + + +def read_data(): + # position = control.read_present_position(ids)[0] + position = np.deg2rad(io.get_present_position([1])[0]) + # position = np.deg2rad(motor.io.get_present_position([motor.id])[0]) + + # speed = np.deg2rad(motor.io.get_present_speed([motor.id])[0]) # TODO convert + # speed = motor.get_present_speed() + # speed = control.read_present_velocity(ids)[0] + speed = np.deg2rad(io.get_present_speed([1])[0]) + + load = 0 # TMP + + volts = io.get_present_voltage([1])[0] * 0.1 + # volts = 0 + + # temp = motor.io.get_present_temperature([motor.id])[0] + temp = 0 + + return { + "position": float(position), + "speed": float(speed), + "load": float(load), + "input_volts": float(volts), + "temp": float(temp), + } + + +while time.time() - start < trajectory.duration: + t = time.time() - start + goal_position, new_torque_enable = trajectory(t) + if new_torque_enable != torque_enable: + if new_torque_enable: + # control.enable_torque(ids) + # motor.enable_torque() + io.enable_torque([1]) + else: + # control.disable_torque(ids) + io.disable_torque([1]) + # motor.disable_torque() + torque_enable = new_torque_enable + time.sleep(0.001) + if torque_enable: + # control.write_goal_position(ids, [goal_position]) + io.set_goal_position({1: np.rad2deg(goal_position)}) + + # motor.goal_position = np.rad2deg(goal_position) + time.sleep(0.001) + + t0 = time.time() - start + entry = read_data() + + t1 = time.time() - start + + entry["timestamp"] = (t0 + t1) / 2.0 + entry["goal_position"] = goal_position + entry["torque_enable"] = torque_enable + data["entries"].append(entry) + +goal_position = data["entries"][-1]["position"] +return_dt = 0.01 +max_variation = return_dt * 1.0 +while abs(goal_position) > 0: + if goal_position > 0: + goal_position = max(0, goal_position - max_variation) + else: + goal_position = min(0, goal_position + max_variation) + + # control.write_goal_position(ids, [goal_position]) + # motor.goal_position = np.rad2deg(goal_position) + io.set_goal_position({1: np.rad2deg(goal_position)}) + + time.sleep(return_dt) + +# control.write_goal_position(ids, [0]) +io.set_goal_position({1: 0}) +# motor.goal_position = 0 +time.sleep(1) + +# control.disable_torque(ids) +io.disable_torque([1]) +# motor.disable_torque() + + +# Format YYYY-MM-DD_HH:mm:ss" +date = datetime.datetime.now().strftime("%Y-%m-%d_%Hh%Mm%S") + +filename = f"{args.logdir}/{date}.json" + +json.dump(data, open(filename, "w")) diff --git a/bam/fit.py b/bam/fit.py new file mode 100644 index 0000000..c02abb3 --- /dev/null +++ b/bam/fit.py @@ -0,0 +1,221 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import socket +from datetime import datetime +import sys +from multiprocessing import Process +import numpy as np +import json +from copy import deepcopy +import json +import time +import optuna +import wandb + +from .logs import Logs +from .model import models, Model, load_model +from . import message +from . import simulate +from .actuators import actuators + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--output", type=str, default="params.json") +arg_parser.add_argument("--method", type=str, default="cmaes") +arg_parser.add_argument("--actuator", type=str, required=True) +arg_parser.add_argument("--model", type=str, required=True) +arg_parser.add_argument("--trials", type=int, default=100_000) +arg_parser.add_argument("--workers", type=int, default=1) +arg_parser.add_argument("--load-study", type=str, default=None) +arg_parser.add_argument("--reset_period", default=None, type=float) +arg_parser.add_argument("--wandb", action="store_true") +arg_parser.add_argument("--set", type=str, default="") +arg_parser.add_argument("--validation_kp", type=int, default=0) +arg_parser.add_argument("--eval", action="store_true") +args = arg_parser.parse_args() + +if not args.eval: + # Json params file + params_json_filename = args.output + if not params_json_filename.endswith(".json"): + params_json_filename = f"output/params_{params_json_filename}.json" + json.dump({}, open(params_json_filename, "w")) + +logs = Logs(args.logdir) +if not args.eval and args.validation_kp > 0: + validation_logs = logs.split(args.validation_kp) + validation_batch = validation_logs.make_batch() + print(f"{len(validation_logs.logs)} logs splitted for validation") + if len(validation_logs.logs) == 0: + raise ValueError("No logs for validation") +logs_batch = logs.make_batch() + + +def compute_score(model: Model, log: dict) -> float: + simulator = simulate.Simulator(model) + result = simulator.rollout_log( + log, reset_period=args.reset_period, simulate_control=True + ) + positions = np.array(result[0]) + log_positions = np.array([entry["position"] for entry in log["entries"]]) + + return np.mean(np.abs(positions - log_positions)) + + +def compute_scores(model: Model, compute_logs=None): + scores = 0 + for log in compute_logs.logs: + # t0 = time.time() + scores += compute_score(model, log) + # t1 = time.time() + # elapsed = t1 - t0 + # print(f"Durations: {elapsed:.6f} s") + + return scores / len(compute_logs.logs) + + +def make_model() -> Model: + model = models[args.model]() + model.set_actuator(actuators[args.actuator]()) + + if args.set != "": + parameters = model.get_parameters() + values = eval(args.set) + for key in values: + parameters[key].value = values[key] + parameters[key].optimize = False + + return model + + +def objective(trial): + model = make_model() + + parameters = model.get_parameters() + for name in parameters: + parameter = parameters[name] + if parameter.optimize: + parameter.value = trial.suggest_float(name, parameter.min, parameter.max) + + return compute_score(model, logs_batch) + + +last_log = time.time() +wandb_run = None + + +def monitor(study, trial): + global last_log, wandb_run + elapsed = time.time() - last_log + + if args.wandb and wandb_run is None: + wandb_run = wandb.init( + name=f"{args.output}_{args.model}_{args.logdir}", + # Set the project where this run will be logged + project=f"{args.actuator}_identification", + # Track hyperparameters and run metadata + config={ + "logdir": args.logdir, + "model": args.model, + "hostname": socket.gethostname(), + }, + ) + + if elapsed > 0.2: + last_log = time.time() + data = deepcopy(study.best_params) + trial_number = trial.number + best_value = study.best_value + wandb_log = { + "optim/best_value": best_value, + "optim/trial_number": trial_number, + } + + model = make_model() + model_parameters = model.get_parameters() + for key in model_parameters: + if key not in data: + data[key] = model_parameters[key].value + data["model"] = args.model + data["actuator"] = args.actuator + + json.dump(data, open(params_json_filename, "w")) + + if args.validation_kp > 0: + val_model = load_model(params_json_filename) + val_best_value = compute_score(val_model, validation_batch) + wandb_log["optim/val_best_value"] = val_best_value + + print() + message.bright(f"[Trial {trial_number}, Best score: {best_value}]") + print( + message.emphasis(f"Best params found (saved to {params_json_filename}): ") + ) + for key in data: + infos, warning = None, None + + if key in model_parameters: + if model_parameters[key].optimize: + infos = f"min: {model_parameters[key].min}, max: {model_parameters[key].max}" + else: + warning = "not optimized" + + message.print_parameter(key, data[key], infos, warning) + + if type(data[key]) == float: + wandb_log[f"params/{key}"] = data[key] + + if wandb_run is not None: + wandb.log(wandb_log) + sys.stdout.flush() + + +if args.eval: + model = load_model("params.json") + print(f"Score: {compute_scores(model, logs)}") +else: + study_name = f"study_{datetime.now().strftime('%Y%m%d_%H%M%S')}" + + # Study URL (when multiple workers are used) + study_url = f"sqlite:///study.db" + # study_url = f"mysql://root:root@127.0.0.1:6033/optuna" + + if args.method == "cmaes": + sampler = optuna.samplers.CmaEsSampler( + # x0=model.get_parameter_values(), + restart_strategy="bipop" + ) + elif args.method == "random": + sampler = optuna.samplers.RandomSampler() + elif args.method == "nsgaii": + sampler = optuna.samplers.NSGAIISampler() + else: + raise ValueError(f"Unknown method: {args.method}") + + def optuna_run(enable_monitoring=True): + if args.workers > 1: + study = optuna.load_study(study_name=study_name, storage=study_url) + else: + study = optuna.create_study(sampler=sampler) + optuna.logging.set_verbosity(optuna.logging.WARNING) + callbacks = [] + if enable_monitoring: + callbacks = [monitor] + study.optimize(objective, n_trials=args.trials, n_jobs=1, callbacks=callbacks) + + if args.workers > 1: + optuna.create_study(study_name=study_name, storage=study_url, sampler=sampler) + + # Running the other workers + for k in range(args.workers - 1): + p = Process(target=optuna_run, args=(False,)) + p.start() + + optuna_run(True) diff --git a/bam/jitter.py b/bam/jitter.py new file mode 100644 index 0000000..d431e65 --- /dev/null +++ b/bam/jitter.py @@ -0,0 +1,48 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import glob +import os +import numpy as np +import json + +arg_parser = argparse.ArgumentParser(description="Show jitter histogram across logs") +arg_parser.add_argument( + "--logdir", type=str, required=True, help="Directory containing raw log json files" +) +args = arg_parser.parse_args() + +filenames = sorted(glob.glob(os.path.join(args.logdir, "*.json"))) +if len(filenames) == 0: + print(f"No json files found in {args.logdir}") + exit(1) + +dts = [] +for filename in filenames: + data = json.load(open(filename)) + ts = [entry["timestamp"] for entry in data["entries"]] + dts.append(np.diff(ts)) + +dt = np.concatenate(dts) + +mean = np.mean(dt) +std = np.std(dt) +print(f"Loaded {len(filenames)} logs, {len(dt)} intervals") +print(f"Mean: {mean:.6f} s, Std: {std:.6f} s") + +import matplotlib.pyplot as plt + +plt.hist(dt, bins=100) +plt.axvline(mean, color="red", linestyle="--", label=f"Mean: {mean:.6f} s") +plt.title(f"Histogram of time between samples\nMean: {mean:.6f} s, Std: {std:.6f} s") +plt.xlabel("Time (s)") +plt.ylabel("Count") +plt.legend() +plt.grid() +plt.show() diff --git a/bam/logs.py b/bam/logs.py new file mode 100644 index 0000000..394ca19 --- /dev/null +++ b/bam/logs.py @@ -0,0 +1,105 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import glob +import numpy as np +import copy +import random +import json +from . import message + + +class Logs: + """Collection of processed trajectory logs used for identification. + + Loads all JSON files found in a directory (produced by ``python -m bam.process``) + and exposes them as a list of log dicts. Each log dict contains the pendulum + configuration (mass, arm_mass, length, kp, vin) and a list of timestep entries with + position, velocity, and control values. + + :param directory: Path to a directory of processed JSON log files. + """ + + def __init__(self, directory: str): + # Directories + self.directory: str = directory + self.json_files = glob.glob(f"{self.directory}/*.json") + + self.logs = [] + for json_file in self.json_files: + with open(json_file) as f: + data = json.load(f) + data["filename"] = json_file + if "arm_mass" not in data: + data["arm_mass"] = 0.0 + self.logs.append(data) + + def split(self, selector_kp: int) -> "Logs": + """Split logs by P-gain value to create a validation set. + + Removes all logs recorded with ``kp == selector_kp`` from this object + and returns them as a new :class:`Logs` instance. Modifies ``self`` + in place. + + :param selector_kp: P-gain value used to select the held-out logs. + :returns: A new :class:`Logs` object containing only the held-out logs. + """ + indices = [] + for k, log in enumerate(self.logs): + if log["kp"] == selector_kp: + indices.append(k) + + other_logs = copy.deepcopy(self) + + self.json_files = [ + self.json_files[i] for i in range(len(self.json_files)) if i not in indices + ] + self.logs = [self.logs[i] for i in range(len(self.logs)) if i not in indices] + + other_logs.json_files = [ + other_logs.json_files[i] + for i in range(len(other_logs.json_files)) + if i in indices + ] + other_logs.logs = [ + other_logs.logs[i] for i in range(len(other_logs.logs)) if i in indices + ] + + return other_logs + + def make_batch(self) -> dict: + """ + Make a batch log from all the logs. In a batch log, all entries are vectorized. + For example, batch["mass"] is a vector of all masses + batch["entries"][0]["position"] will be a vector of all positions + """ + batch: dict = {"entries": []} + + for key in self.logs[0]: + if key != "entries": + batch[key] = np.array([log[key] for log in self.logs]) + + entries_min_length = min([len(log["entries"]) for log in self.logs]) + entries_max_length = max([len(log["entries"]) for log in self.logs]) + if entries_max_length > entries_min_length + 1: + print( + message.yellow( + f"WARNING: logs have significantly different lengths ({entries_min_length} to {entries_max_length})" + ) + ) + + entry_keys = self.logs[0]["entries"][0].keys() + for k in range(entries_min_length): + batch["entries"].append( + { + key: np.array([log["entries"][k][key] for log in self.logs]) + for key in entry_keys + } + ) + + return batch diff --git a/bam/mae.py b/bam/mae.py new file mode 100644 index 0000000..68a2527 --- /dev/null +++ b/bam/mae.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +# Copyright 2025 Marc Duclusaud & Grégoire Passault +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: +# +# http://www.apache.org/licenses/LICENSE-2.0 + +"""Compare BAM models by mean absolute error (MAE) on a log directory. + +Example:: + + uv run python mae.py --params params/xl330/ --logdir data_processed/ +""" + +import argparse +import json +from pathlib import Path + +import numpy as np +import matplotlib.pyplot as plt + +from bam.logs import Logs +from bam.model import load_model +from bam import simulate + +arg_parser = argparse.ArgumentParser(description="Compare BAM model MAEs") +arg_parser.add_argument( + "--params", type=str, required=True, help="Directory containing *.json param files" +) +arg_parser.add_argument( + "--logdir", type=str, required=True, help="Directory containing log files" +) +arg_parser.add_argument( + "--reset_period", + type=float, + default=None, + help="Reset period for simulation rollouts (s)", +) +arg_parser.add_argument( + "--sort", + action="store_true", + default=False, + help="Sort bars by MAE (default: keep evaluation order)", +) +arg_parser.add_argument("--no-sort", dest="sort", action="store_false") +arg_parser.add_argument( + "--json", + type=str, + default=None, + help="Write results to this JSON file instead of plotting", +) +arg_parser.add_argument( + "--mujoco", + action="store_true", + help="Use the MuJoCo (CPU) simulator backend instead of the reference one", +) +arg_parser.add_argument( + "--mjlab", + action="store_true", + help="Use the mjlab (MuJoCo Warp / GPU) simulator backend, vectorized over all logs", +) +args = arg_parser.parse_args() + +if args.mujoco: + # Imported lazily so the default (reference) backend doesn't require MuJoCo. + from bam import mujoco as mujoco_backend + +if args.mjlab: + # Imported lazily so other backends don't require mjlab. + from bam import mjlab as mjlab_backend + +# ── Load logs ───────────────────────────────────────────────────────────────── +logs = Logs(args.logdir) +print(f"Loaded {len(logs.logs)} logs from {args.logdir}") + +# ── Discover param files ────────────────────────────────────────────────────── +params_dir = Path(args.params) +param_files = sorted(params_dir.glob("*.json")) +if not param_files: + raise FileNotFoundError(f"No *.json files found in {params_dir}") +print(f"Found {len(param_files)} param files: {[p.name for p in param_files]}") + + +# ── MAE computation ─────────────────────────────────────────────────────────── +def _mae(positions, log: dict) -> float: + log_positions = np.array([entry["position"] for entry in log["entries"]]) + return float(np.mean(np.abs(np.array(positions) - log_positions))) + + +def compute_mae(model, log: dict) -> float: + if args.mujoco: + simulator = mujoco_backend.Simulator(model) + positions, _, _ = simulator.rollout_log(log, reset_period=args.reset_period) + else: + simulator = simulate.Simulator(model) + positions, _, _ = simulator.rollout_log( + log, reset_period=args.reset_period, simulate_control=True + ) + return _mae(positions, log) + + +def compute_maes_mjlab(param_file, all_logs: list) -> list: + """Vectorized MAEs for one param file over all logs, using the mjlab GPU backend. + + Logs are grouped by ``dt`` (a batch must share a single MuJoCo timestep) and + each group is rolled out in a single parallel ``rollout_logs`` call. + """ + simulator = mjlab_backend.Simulator(json_path=str(param_file)) + groups: dict[float, list[int]] = {} + for i, log in enumerate(all_logs): + groups.setdefault(log["dt"], []).append(i) + + maes: list[float | None] = [None] * len(all_logs) + for indices in groups.values(): + batch = [all_logs[i] for i in indices] + positions, _, _ = simulator.rollout_logs(batch, reset_period=args.reset_period) + for j, i in enumerate(indices): + maes[i] = _mae(positions[j], all_logs[i]) + return maes + + +results = {} # name → list of per-log MAEs + +for param_file in param_files: + try: + model = load_model(str(param_file)) + except Exception as e: + print(f" {'[SKIP] ' + param_file.stem:30s} ({e})") + continue + label = param_file.stem # e.g. "m6", "m4", ... + print(f" {label:30s}", end="", flush=True) + + if args.mjlab: + maes = compute_maes_mjlab(param_file, logs.logs) + else: + maes = [compute_mae(model, log) for log in logs.logs] + + mean_mae = float(np.mean(maes)) + std_mae = float(np.std(maes)) + results[label] = {"mean": mean_mae, "std": std_mae, "per_log": maes} + print(f"MAE = {mean_mae * 1000:.2f} ± {std_mae * 1000:.2f} mrad") + + +# ── JSON output ─────────────────────────────────────────────────────────────── +if args.json is not None: + with open(args.json, "w") as f: + json.dump(results, f, indent=2) + print(f"Wrote results for {len(results)} models to {args.json}") + raise SystemExit(0) + + +# ── Box plot ────────────────────────────────────────────────────────────────── +labels = list(results.keys()) +per_log = [np.array(results[k]["per_log"]) * 1000 for k in labels] # → mrad +means = np.array([results[k]["mean"] for k in labels]) * 1000 +medians = np.array([np.median(d) for d in per_log]) + +if args.sort: + order = np.argsort(means) + labels = [labels[i] for i in order] + per_log = [per_log[i] for i in order] + means = means[order] + medians = medians[order] + +fig, ax = plt.subplots(figsize=(max(6, len(labels) * 0.9 + 1), 5)) +positions = np.arange(1, len(labels) + 1) +bp = ax.boxplot( + per_log, + positions=positions, + widths=0.6, + showmeans=True, + meanline=True, + patch_artist=True, + medianprops=dict(color="black"), + meanprops=dict(color="firebrick", linestyle="--"), +) +for patch in bp["boxes"]: + patch.set_facecolor("steelblue") + patch.set_alpha(0.6) + patch.set_edgecolor("black") + patch.set_linewidth(0.7) + +# Custom annotation: median MAE next to each box, at the median line's level. +for pos, median in zip(positions, medians): + ax.text( + pos + 0.35, + median, + f"{median:.1f}", + ha="left", + va="center", + fontsize=8, + color="black", + ) + +ax.set_xticks(positions, labels) +ax.set_ylabel("MAE [mrad]") +ax.set_xlabel("Model") +ax.set_title(f"Model comparison — {len(logs.logs)} logs from {Path(args.logdir).name}") +ax.grid(axis="y", linestyle="--", alpha=0.5) +plt.tight_layout() +plt.show() diff --git a/bam/message.py b/bam/message.py new file mode 100644 index 0000000..ff28ee6 --- /dev/null +++ b/bam/message.py @@ -0,0 +1,56 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import os +from colorama import Fore, Back, Style + + +def error(message): + print(Style.BRIGHT + Fore.RED + message + Style.RESET_ALL) + + +def bright(message): + print("") + print(Style.BRIGHT + message + Style.RESET_ALL) + + +def run_or_fail(cmd): + if os.system(cmd) != 0: + die("Error while running " + cmd) + + +def emphasis(text): + return Fore.BLUE + text + Fore.RESET + + +def yellow(text): + return Fore.YELLOW + text + Fore.RESET + + +def red(text): + return Fore.RED + text + Fore.RESET + + +def success(text): + return Fore.GREEN + text + Fore.RESET + + +def die(message): + print(error(message)) + exit() + + +def print_parameter(name, value, infos=None, warning=None): + result = f"- {success(name)}: {yellow(str(value))}" + + if infos is not None: + result += f" ({infos})" + if warning is not None: + result += red(f" ({warning})") + + print(result) diff --git a/bam/mjlab.py b/bam/mjlab.py new file mode 100644 index 0000000..6797235 --- /dev/null +++ b/bam/mjlab.py @@ -0,0 +1,998 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +"""BAM actuator models for mjlab (MuJoCo Warp). + +Wraps any BAM Model (m1–m6) into mjlab's actuator framework. All friction +computations are fully vectorized over the (num_envs, num_joints) batch +dimension using PyTorch. + +Usage example — bundled motor:: + + from bam.mjlab import make_bam_actuator_cfg + + actuator_cfg = make_bam_actuator_cfg( + motor_name="xl330", + model="m6", + target_names_expr=(r".*",), + ) + +Usage example — custom JSON path:: + + actuator_cfg = make_bam_actuator_cfg( + json_path="params/xl330/m6.json", + target_names_expr=(r".*",), + ) +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING + +import numpy as np +import mujoco +import mujoco_warp as mjwarp +import torch + +from mjlab.actuator.actuator import Actuator, ActuatorCfg, ActuatorCmd, CommandField +from mjlab.utils.spec import create_motor_actuator +from mjlab.scene import Scene, SceneCfg +from mjlab.sim import MujocoCfg, Simulation, SimulationCfg +from mjlab.entity import EntityArticulationInfoCfg, EntityCfg +from mjlab.managers.event_manager import RecomputeLevel, requires_model_fields + +from .actuator import TorchBackend, VoltageControlledActuator +from .model import Model, load_model, _resolve_json_path +from .testbench_mujoco import Pendulum + +if TYPE_CHECKING: + from mjlab.entity import Entity + + +@requires_model_fields("dof_frictionloss", "dof_damping") +def bam_init(env, env_ids=None) -> None: + """Startup event that expands BamActuator's per-world friction fields. + + :class:`BamActuator` writes a per-environment friction budget into MuJoCo's + ``dof_frictionloss`` and ``dof_damping`` model fields. Those fields must be + expanded per world (otherwise they alias one shared buffer and per-env writes + are invalid). Add this as a ``startup`` event and mjlab expands them for you:: + + from bam.mjlab import bam_init + from mjlab.managers.event_manager import EventTermCfg + + events["bam_init"] = EventTermCfg(func=bam_init, mode="startup") + + The body is intentionally a no-op: the ``@requires_model_fields`` decorator + does the work by registering the fields in the EventManager, which the env + then hands to ``sim.expand_model_fields()`` during setup. + """ + del env, env_ids + + +# MuJoCo constraint-type id for a per-DOF friction constraint. In MuJoCo Warp the +# ``efc.id`` of such a constraint is the DOF index and its Jacobian is a unit row +# on that DOF, so it contributes exactly ``efc.force`` to ``qfrc_constraint``. +_FRICTION_DOF_CONSTRAINT = int(mujoco.mjtConstraint.mjCNSTR_FRICTION_DOF) + + +@dataclass(kw_only=True) +class BamActuatorCfg(ActuatorCfg): + """Configuration for a BAM actuator compatible with mjlab. + + Specify the model with **one** of two mutually exclusive approaches: + + * **Bundled motor**: set ``motor_name`` (e.g. "xl330") and ``model`` + (e.g. "m6"). The path is resolved automatically from the ``params/`` + directory bundled with the library. + * **Custom JSON**: set ``json_path`` to a BAM params JSON file produced by + ``bam.fit``. + + :param motor_name: Name of the bundled motor. Currently supported: "xl330", "xl320", "mx106", "mx64", "erob80:50", and "erob80:100". Mutually exclusive with ``json_path``. + :param model: Model variant to use with ``motor_name``, one of "m1"–"m6". Mutually exclusive with ``json_path``. + :param json_path: Path to a custom BAM params JSON file produced by ``bam.fit``. Mutually exclusive with ``motor_name`` and ``model``. + :param target_names_expr: Tuple of regex patterns to match actuated joint names. + :param vin: Supply voltage override [V]. ``None`` → uses the value in the JSON. + :param kp_fw: Firmware P-gain override. ``None`` → uses the value in the JSON. + :param vin_range: If set, a per-env battery voltage is sampled uniformly from this + range at startup and held constant across resets. Takes precedence over ``vin``. + :param vin_drop_resistance_range: If set, a per-env battery + wire resistance [Ohm] is sampled + uniformly from this range at startup. Models the voltage drop V_drop = R * I due to + battery + cable resistance, where the current I [A] is estimated from the actuator + torques as Σ|τ| / Kt. Held constant across resets. + :param vin_min: Hard lower bound on the effective supply voltage [V] after applying the + voltage drop. Ensures ``vin`` never falls below this value regardless of the load. + ``None`` → no lower bound. + :param delay_min_lag: Minimum command delay in simulation steps. Models the latency + between the policy output and the motor response. ``0`` → no delay. + :param delay_max_lag: Maximum command delay in simulation steps. Set greater than + ``delay_min_lag`` to randomize the delay across environments. + :param delay_hold_prob: Probability of keeping the same lag value at each step. + ``0.0`` → lag is resampled every ``delay_update_period`` steps. + :param delay_update_period: Number of steps between lag updates. ``0`` → updated + every step. + :param delay_per_env_phase: Whether each environment starts with an independent + delay phase offset. ``True`` → environments are not synchronized. + :param stiff_frictionloss: When ``True`` (default), stiffen the joint-friction + constraint (``solref_friction`` / ``solimp_friction``). MuJoCo Warp has no + noslip solver, so the frictionloss constraint stays soft and a statically-held + joint creeps; this is the GPU-side substitute. Uses MuJoCo's direct + (timestep-independent) solref form so it works regardless of the sim ``dt``. + Set ``False`` to keep MuJoCo's soft defaults. + """ + + motor_name: str | None = None + model: str | None = None + json_path: str | None = None + vin: float | None = None + kp_fw: float | None = None + vin_range: tuple[float, float] | None = None + vin_drop_resistance_range: tuple[float, float] | None = None + vin_min: float | None = None + stiff_frictionloss: bool = True + + def __post_init__(self) -> None: + if self.json_path is not None and ( + self.motor_name is not None or self.model is not None + ): + raise ValueError( + "Specify either json_path OR (motor_name + model), not both." + ) + object.__setattr__( + self, + "_resolved_json_path", + _resolve_json_path(self.json_path, self.motor_name, self.model), + ) + + def build( + self, + entity: "Entity", + target_ids: list[int], + target_names: list[str], + ) -> "BamActuator": + return BamActuator(self, entity, target_ids, target_names) + + +class BamActuator(Actuator): + """BAM actuator for mjlab — fully vectorized over all parallel environments. + + Implements the BAM torque pipeline: + + 1. **Voltage control law** — firmware P-controller (position error → duty + cycle → voltage). + 2. **DC motor torque** — back-EMF equation (voltage → torque). + 3. **Friction budget** — BAM m1–m6 friction model (Coulomb, Stribeck, + load-dependent, directional, quadratic). + + Rather than injecting a passive friction torque into the returned motor + torque, the friction budget is written into MuJoCo's native + ``dof_frictionloss`` (dry friction) and ``dof_damping`` (viscous) each step, + exactly like :class:`bam.mujoco.MujocoController`. MuJoCo's constraint solver + then applies the static-friction clipping (BAM Algorithm 1) itself. + + .. important:: + Because every environment carries a different friction budget, the + ``dof_frictionloss`` and ``dof_damping`` model fields must be expanded + per world *before* stepping. The simplest way is to add the + :func:`bam_init` startup event:: + + from bam.mjlab import bam_init + events["bam_init"] = EventTermCfg(func=bam_init, mode="startup") + + (equivalently, call ``env.sim.expand_model_fields(("dof_frictionloss", + "dof_damping"))`` after building the environment). Otherwise the fields + alias a single shared buffer and per-env writes are invalid. + + Per-environment gain scaling is supported via :meth:`set_gains`. + """ + + cfg: BamActuatorCfg + + def __init__( + self, + cfg: BamActuatorCfg, + entity: "Entity", + target_ids: list[int], + target_names: list[str], + ) -> None: + super().__init__(cfg, entity, target_ids, target_names) + + # Load the BAM model from JSON (one instance per actuator) + self._bam_model: Model = load_model(cfg._resolved_json_path) + + if cfg.vin is not None: + self._bam_model.actuator.vin = cfg.vin + if cfg.kp_fw is not None: + self._bam_model.actuator.kp = cfg.kp_fw + + if not isinstance(self._bam_model.actuator, VoltageControlledActuator): + raise NotImplementedError( + f"BamActuator only supports VoltageControlledActuator, " + f"got {type(self._bam_model.actuator).__name__}" + ) + + self._mjwarp_model: mjwarp.Model | None = None + self._data: mjwarp.Data | None = None + self._device: str = "cpu" + self._dof_ids: torch.Tensor | None = None + + self.vin_tensor: torch.Tensor | None = None + self.vin_drop_resistance: torch.Tensor | None = None + + self.kp_scale: torch.Tensor | None = None + self.kd_scale: torch.Tensor | None = None + self.default_kp_scale: torch.Tensor | None = None + self.default_kd_scale: torch.Tensor | None = None + + self._base_kp: float = 0.0 + self._dt: float = 0.0 + + self._num_envs: int = 0 + self._friction_fields_checked: bool = False + + # ───────────────────────────────────────────────────────────────────────── + # mjlab interface + # ───────────────────────────────────────────────────────────────────────── + + # Stiff joint-friction constraint, in MuJoCo's direct (timestep-independent) + # solref form: solref = (-stiffness, -damping), solimp with dmax→1. Applied + # when cfg.stiff_frictionloss is True to counter the lack of a noslip solver + # in MuJoCo Warp (matches a (2*dt, 1.0) timeconst solref at dt≈5 ms). + _STIFF_SOLREF_FRICTION = (-5.0e4, -2.0e2) + _STIFF_SOLIMP_FRICTION = (0.99, 0.9999, 0.001, 0.5, 2.0) + + def _set_friction_stiffness(self, joint: "mujoco.MjsJoint") -> None: + """Stiffen a joint's friction constraint if ``cfg.stiff_frictionloss``.""" + if self.cfg.stiff_frictionloss: + joint.solref_friction = self._STIFF_SOLREF_FRICTION + joint.solimp_friction = self._STIFF_SOLIMP_FRICTION + + def edit_spec(self, spec: mujoco.MjSpec, target_names: list[str]) -> None: + """Convert position actuators to motor mode and zero MuJoCo friction. + + The ``frictionloss`` and ``damping`` are zeroed here only as an initial + value; :meth:`compute` rewrites them every step with the BAM friction + budget so MuJoCo's solver applies the friction natively. When + ``cfg.stiff_frictionloss`` is set, the friction constraint's + ``solref_friction`` / ``solimp_friction`` are stiffened here too (see + :meth:`_set_friction_stiffness`). + """ + bam = self._bam_model + act = bam.actuator + kt = bam.kt.value + R = bam.R.value + armature = act.get_extra_inertia() + # Use upper bound of vin_range for force_limit so MuJoCo's forcerange + # is always a safe ceiling regardless of per-env voltage. + vin_for_limit = ( + max(self.cfg.vin_range) if self.cfg.vin_range is not None else act.vin + ) + force_limit = vin_for_limit * kt / R + + target_set = set(target_names) + converted: set[str] = set() + + for mjact in spec.actuators: + tgt = mjact.target + tgt_name = tgt.name if hasattr(tgt, "name") else (str(tgt) if tgt else None) + if tgt_name in target_set: + mjact.set_to_motor() + mjact.forcelimited = True + mjact.forcerange = (-force_limit, force_limit) + mjact.gear = [1.0, 0, 0, 0, 0, 0] + for joint in spec.joints: + if joint.name == tgt_name: + joint.armature = float(armature) + joint.damping = np.zeros((3, 1)) + joint.frictionloss = 0.0 + self._set_friction_stiffness(joint) + break + self._mjs_actuators.append(mjact) + converted.add(tgt_name) + + for target_name in target_names: + if target_name not in converted: + mjact = create_motor_actuator( + spec, + target_name, + effort_limit=force_limit, + armature=armature, + frictionloss=0.0, + transmission_type=self.cfg.transmission_type, + ) + self._mjs_actuators.append(mjact) + for joint in spec.joints: + if joint.name == target_name: + joint.damping = np.zeros((3, 1)) + joint.frictionloss = 0.0 + self._set_friction_stiffness(joint) + break + + def initialize( + self, + mj_model: mujoco.MjModel, + model: mjwarp.Model, + data: mjwarp.Data, + device: str, + ) -> None: + super().initialize(mj_model, model, data, device) + self._mjwarp_model = model + self._data = data + self._device = device + + # Map local target indices → global joint indices → DOF addresses + jnt_dofadr = mj_model.jnt_dofadr + entity_joint_ids = self.entity.indexing.joint_ids + dof_ids = [ + jnt_dofadr[entity_joint_ids[tid].item()] for tid in self._target_ids_list + ] + self._dof_ids = torch.tensor(dof_ids, dtype=torch.long, device=device) + + num_envs = data.nworld + self._num_envs = num_envs + self._friction_fields_checked = False + num_joints = len(self._dof_ids) + self.kp_scale = torch.ones(num_envs, 1, dtype=torch.float32, device=device) + self.kd_scale = torch.ones(num_envs, 1, dtype=torch.float32, device=device) + self.default_kp_scale = self.kp_scale.clone() + self.default_kd_scale = self.kd_scale.clone() + + bam = self._bam_model + act = bam.actuator + + # Delegate the control law / torque equation to the BAM actuator, run on + # the Torch backend so its clamps are vectorized over (num_envs, num_joints). + act.backend = TorchBackend() + # Base firmware gain and physics timestep, captured before compute() starts + # overwriting act.kp / act.vin with per-env tensors each step. + self._base_kp = float(act.kp) + self._dt = float(mj_model.opt.timestep) + + # vin_tensor: (N, 1) — per-env battery voltage, constant across resets + if self.cfg.vin_range is not None: + self.vin_tensor = torch.empty( + num_envs, 1, dtype=torch.float32, device=device + ).uniform_(*self.cfg.vin_range) + else: + self.vin_tensor = torch.full( + (num_envs, 1), act.vin, dtype=torch.float32, device=device + ) + + # vin_drop_resistance: (N, 1) — per-env battery + wire resistance [Ohm], constant across resets + if self.cfg.vin_drop_resistance_range is not None: + self.vin_drop_resistance = torch.empty( + num_envs, 1, dtype=torch.float32, device=device + ).uniform_(*self.cfg.vin_drop_resistance_range) + else: + self.vin_drop_resistance = None + + vin_repr = ( + f"range={self.cfg.vin_range}" + if self.cfg.vin_range is not None + else f"{act.vin:.1f}V" + ) + drop_repr = ( + f"drop_resistance_range={self.cfg.vin_drop_resistance_range}" + if self.cfg.vin_drop_resistance_range is not None + else "no drop" + ) + vin_for_limit = ( + max(self.cfg.vin_range) if self.cfg.vin_range is not None else act.vin + ) + force_limit = vin_for_limit * bam.kt.value / bam.R.value + print( + f"[BamActuator] model={bam.name!r} " + f"joints={num_joints} " + f"kt={bam.kt.value:.4f} R={bam.R.value:.4f} " + f"vin={vin_repr} {drop_repr} force_limit=±{force_limit:.2f}Nm " + f"friction_base={bam.friction_base.value:.4f} " + f"friction_viscous={bam.friction_viscous.value:.4f} " + f"envs={num_envs} device={device}" + ) + + def reset(self, env_ids: torch.Tensor | slice | None = None) -> None: + super().reset(env_ids) + # vin_tensor and vin_drop_resistance are startup-randomized: do NOT re-sample on reset. + + @property + def command_field(self) -> CommandField: + return "position" + + # ───────────────────────────────────────────────────────────────────────── + # Gain scaling (for domain randomization) + # ───────────────────────────────────────────────────────────────────────── + + def set_gains( + self, + env_ids: torch.Tensor | slice, + kp_scale: torch.Tensor | None = None, + kd_scale: torch.Tensor | None = None, + ) -> None: + """Scale firmware gains for a subset of environments.""" + if kp_scale is not None: + assert self.kp_scale is not None + self.kp_scale[env_ids] = kp_scale + if kd_scale is not None: + assert self.kd_scale is not None + self.kd_scale[env_ids] = kd_scale + + def reset_gains(self, env_ids: torch.Tensor | slice) -> None: + """Restore default gains for a subset of environments.""" + assert self.kp_scale is not None and self.default_kp_scale is not None + assert self.kd_scale is not None and self.default_kd_scale is not None + self.kp_scale[env_ids] = self.default_kp_scale[env_ids] + self.kd_scale[env_ids] = self.default_kd_scale[env_ids] + + # ───────────────────────────────────────────────────────────────────────── + # BAM friction budget (m1–m6 unified, vectorized) + # ───────────────────────────────────────────────────────────────────────── + + def _compute_friction_budget( + self, + motor_torque: torch.Tensor, + external_torque: torch.Tensor, + stribeck_coeff: torch.Tensor, + ) -> torch.Tensor: + """Velocity-independent friction budget — shape ``(N, J)``. + + Covers all BAM models m1–m6 by reading flags from the stored Model: + + * **m1**: Coulomb only (``friction_base``) + * **m2**: + Stribeck (``stribeck=True``) + * **m3**: + non-directional load friction (``load_dependent=True``) + * **m4**: m3 + Stribeck load friction + * **m5**: directional load friction (``directional=True``) + * **m6**: m5 + quadratic Stribeck load term (``quadratic=True``) + """ + bam = self._bam_model + frictionloss = torch.full_like(motor_torque, bam.friction_base.value) + + if bam.stribeck: + frictionloss = frictionloss + stribeck_coeff * bam.friction_stribeck.value + + if bam.load_dependent: + if bam.directional: + # m5/m6 — directional gearbox torque + gearbox_torque = torch.abs( + external_torque * bam.load_friction_external.value + - motor_torque * bam.load_friction_motor.value + ) + frictionloss = frictionloss + gearbox_torque + + if bam.stribeck: + gearbox_torque_stribeck = torch.abs( + external_torque * bam.load_friction_external_stribeck.value + - motor_torque * bam.load_friction_motor_stribeck.value + ) + frictionloss = ( + frictionloss + stribeck_coeff * gearbox_torque_stribeck + ) + + if bam.quadratic: + # m6 — quadratic term; directional: motor-side vs external-side + abs_ext = torch.abs(external_torque) + abs_mot = torch.abs(motor_torque) + drive_mask = (abs_mot > abs_ext).to(motor_torque.dtype) + backdrive_mask = 1.0 - drive_mask + quad_term = ( + drive_mask + * bam.load_friction_external_quad.value + * abs_ext**2 + + backdrive_mask + * bam.load_friction_motor_quad.value + * abs_mot**2 + ) + frictionloss = frictionloss + stribeck_coeff * quad_term + else: + # m3/m4 — non-directional gearbox torque + gearbox_torque = torch.abs(external_torque - motor_torque) + frictionloss = ( + frictionloss + bam.load_friction_base.value * gearbox_torque + ) + + if bam.stribeck: + frictionloss = ( + frictionloss + + stribeck_coeff + * bam.load_friction_stribeck.value + * gearbox_torque + ) + + return frictionloss + + def _write_frictions(self, frictionloss: torch.Tensor, damping: float) -> None: + """Write the friction budget into MuJoCo's per-DOF friction fields. + + ``frictionloss`` (shape ``(N, J)``) and ``damping`` (scalar viscous + coefficient) are written into ``dof_frictionloss`` and ``dof_damping`` + of the controlled DOFs, for every environment. The fields are accessed + fresh each call so that a later :meth:`~mjlab.sim.Sim.expand_model_fields` + (which reallocates the arrays and clears the bridge cache) is picked up. + """ + assert self._mjwarp_model is not None and self._dof_ids is not None + + fl_field = self._mjwarp_model.dof_frictionloss + damping_field = self._mjwarp_model.dof_damping + + if not self._friction_fields_checked: + # Per-env writes require truly per-world storage. A non-expanded + # field aliases a single (1, nv) buffer (stride 0 on the world axis), + # so writing distinct per-env values would be invalid. + if self._num_envs > 1 and ( + fl_field.stride(0) == 0 or damping_field.stride(0) == 0 + ): + raise RuntimeError( + "BamActuator writes per-environment dof_frictionloss/" + "dof_damping, but these model fields are not expanded per " + "world. Add the bam_init startup event so mjlab expands them " + "automatically:\n" + " from bam.mjlab import bam_init\n" + " events['bam_init'] = EventTermCfg(func=bam_init, " + "mode='startup')\n" + "Alternatively, after building the environment, call " + "sim.expand_model_fields(('dof_frictionloss', " + "'dof_damping'))." + ) + self._friction_fields_checked = True + + fl_field[:, self._dof_ids] = frictionloss + damping_field[:, self._dof_ids] = damping + + def _as_tensor(self, x) -> torch.Tensor: + """Normalize a mjwarp/bridge field to a real ``torch.Tensor``. + + Handles mjlab's ``TorchArray`` bridge wrapper (indexing it returns the + underlying tensor), plain tensors, and numpy fallbacks. + """ + if isinstance(x, torch.Tensor): + return x + if hasattr(x, "_tensor"): # mjlab TorchArray bridge wrapper + return x[...] + return torch.as_tensor(x, device=self._device) + + def _dof_friction_force(self, nv: int) -> torch.Tensor: + """Per-DOF force produced by our own ``dof_frictionloss`` constraints. + + Scans the active constraint rows (``efc``) of the previous solve, keeps + only the DOF-friction constraints, and scatters their constraint-space + force onto the DOF they act on. Returns shape ``(N, nv)``. + + This is the MuJoCo-Warp equivalent of the ``efc``-scan done in + :class:`bam.mujoco.MujocoController`, used to strip the friction + contribution out of ``qfrc_constraint``. + """ + assert self._data is not None + efc = self._data.efc + efc_type = self._as_tensor(efc.type) # (N, njmax) int + efc_id = self._as_tensor(efc.id) # (N, njmax) int + efc_force = self._as_tensor(efc.force) # (N, njmax) float + nefc = self._as_tensor(self._data.nefc) # (N,) int + + n_worlds, n_max = efc_type.shape + # Only the first nefc[w] rows of each world are valid; the rest is stale. + valid = torch.arange(n_max, device=efc_type.device).unsqueeze( + 0 + ) < nefc.unsqueeze(1) + is_fric = valid & (efc_type == _FRICTION_DOF_CONSTRAINT) # (N, njmax) + + contrib = torch.where(is_fric, efc_force, torch.zeros_like(efc_force)) + # Masked-out rows scatter a zero contribution into DOF 0 (harmless). + idx = torch.where(is_fric, efc_id, torch.zeros_like(efc_id)).long() + + qfrc_friction = torch.zeros( + n_worlds, nv, dtype=efc_force.dtype, device=efc_force.device + ) + qfrc_friction.scatter_add_(1, idx, contrib) + return qfrc_friction + + # ───────────────────────────────────────────────────────────────────────── + # Main compute — shape (num_envs, num_joints) throughout + # ───────────────────────────────────────────────────────────────────────── + + def compute(self, cmd: ActuatorCmd) -> torch.Tensor: + """Compute output torques for all environments — shape ``(N, J)``. + + The firmware control law and the DC-motor torque equation are delegated + to the underlying BAM actuator's :meth:`~bam.actuator.Actuator.compute_control` + and :meth:`~bam.actuator.Actuator.compute_torque` (running on the Torch + backend, so their clamps vectorize over ``(N, J)``). This backend only + injects the mjlab-specific per-environment quantities around those calls: + + * ``vin`` (with the optional battery drop) → ``actuator.vin`` + * ``kp * kp_scale`` → ``actuator.kp`` + * ``kd_scale`` → applied to the back-EMF via a scaled velocity + """ + bam = self._bam_model + act = bam.actuator + + assert self.vin_tensor is not None + assert self.kp_scale is not None and self.kd_scale is not None + assert self._data is not None and self._dof_ids is not None + assert self._mjwarp_model is not None + + # Actuator torque applied on the PREVIOUS solve (lagged one step). The BAM + # friction budget uses this (not the freshly-computed motor_torque) as the + # motor-side load, and the battery voltage-drop model reuses it as the + # current-draw proxy. Mirrors bam.mujoco.MujocoController. + prev_actuator_torque = self._as_tensor(self._data.qfrc_actuator)[ + :, self._dof_ids + ] # (N, J) + + # ── Per-env supply voltage (with optional battery drop) ────────────── + # V_drop = R * I, with the current I estimated from the actuator torques + # as Σ|τ| / kt across the controlled joints. + vin = self.vin_tensor # (N, 1) + if self.vin_drop_resistance is not None: + current = ( + prev_actuator_torque.abs().sum(dim=-1, keepdim=True) / bam.kt.value + ) # (N, 1) + vin = vin - self.vin_drop_resistance * current # (N, 1), broadcast safe + if self.cfg.vin_min is not None: + vin = torch.clamp(vin, min=self.cfg.vin_min) + + friction_viscous = bam.friction_viscous.value + vel = cmd.vel # (N, J) + + # ── 1-2. Firmware control law + DC-motor torque (delegated) ────────── + # Inject the per-env firmware parameters: compute_control multiplies by + # actuator.kp and actuator.vin, so writing per-env tensors there yields + # the vectorized firmware controller with no re-implementation. + act.vin = vin # (N, 1) + act.kp = self._base_kp * self.kp_scale # (N, 1) + # kd_scale scales the electrical (back-EMF) damping only. Both + # compute_control (current-limiter back-EMF) and compute_torque + # (motor-torque back-EMF) use the velocity solely in that term, so + # feeding them a scaled velocity applies kd_scale consistently. The + # firmware current limit is modelled inside compute_control as a + # duty-cycle constraint (torque_enable handled by the Simulator). + scaled_vel = vel * self.kd_scale + control = act.compute_control( + cmd.position_target, cmd.pos, scaled_vel, self._dt + ) + motor_torque = act.compute_torque(control, True, cmd.pos, scaled_vel) + + # ── 3. External torque (gravity + Coriolis + constraints) ───────────── + # The external load on the gearbox is the gravity/Coriolis bias plus the + # constraint forces (contacts, joint limits, …), but NOT the DOF-friction + # constraint force we injected via dof_frictionloss on the previous solve + # — otherwise the load-dependent friction terms would feed back on + # themselves. This mirrors bam.mujoco.MujocoController. + qfrc_bias = self._as_tensor(self._data.qfrc_bias) # (N, nv) + qfrc_constraint = self._as_tensor(self._data.qfrc_constraint) # (N, nv) + nv = qfrc_bias.shape[-1] + qfrc_friction = self._dof_friction_force(nv) # (N, nv) + external_torque = ( + -qfrc_bias[:, self._dof_ids] + + qfrc_constraint[:, self._dof_ids] + - qfrc_friction[:, self._dof_ids] + ) # (N, J) + + # ── 4. Stribeck coefficient ─────────────────────────────────────────── + # (N, J); zero tensor when model has no stribeck (unused in budget) + abs_vel = torch.abs(vel) + if bam.stribeck: + dtheta_stribeck = bam.dtheta_stribeck.value + alpha = bam.alpha.value + stribeck_coeff = torch.exp(-torch.pow(abs_vel / dtheta_stribeck, alpha)) + else: + stribeck_coeff = torch.zeros_like(vel) + + # ── 5. Friction budget → MuJoCo dof_frictionloss / dof_damping ──────── + # Velocity-independent (Coulomb + Stribeck + load) part → frictionloss. + # Viscous part → damping. MuJoCo's constraint solver then performs the + # static-friction clipping (BAM Algorithm 1) itself, so we do NOT add a + # passive friction torque to the returned motor torque. + frictionloss = self._compute_friction_budget( + prev_actuator_torque, external_torque, stribeck_coeff + ) # (N, J) + self._write_frictions(frictionloss, friction_viscous) + + return motor_torque + + +# ───────────────────────────────────────────────────────────────────────────── +# GPU (MuJoCo Warp) simulator — mirrors bam.simulate / bam.mujoco Simulator +# ───────────────────────────────────────────────────────────────────────────── + +_ENTITY_NAME = "pendulum" +_JOINT_NAME = "pendulum" + + +class Simulator: + """mjlab (MuJoCo Warp) GPU simulator, mirroring :class:`bam.simulate.Simulator`. + + Builds the *same* pendulum spec as :class:`bam.testbench_mujoco.Pendulum`, + drives the hinge with a :class:`BamActuator` (via :class:`BamActuatorCfg`), + and steps everything on mjlab's vectorized MuJoCo-Warp pipeline. + + The point of this backend is to exploit mjlab's GPU parallelization: a whole + batch of logs is rolled out **at once**, one environment per log. Every + environment shares the same motor model (kt, R, friction budget) but carries + its own pendulum parameters (``mass``/``arm_mass``/``length``) and firmware + gains (``kp``/``vin``). This is exactly what makes evaluating a model over a + directory of logs (as in :mod:`bam.mae`) embarrassingly parallel here. + + How the per-environment "specs" are made different + -------------------------------------------------- + mjlab replicates one compiled model across all worlds, so per-log physical + parameters are injected *after* building the sim by expanding the relevant + model fields per world and writing each environment's values: + + * ``body_mass`` / ``body_ipos`` / ``body_inertia`` — the pendulum inertial, + computed by :meth:`bam.testbench_mujoco.Pendulum.inertial_params`, followed + by :meth:`mjlab.sim.Simulation.recompute_constants` to refresh the derived + mass matrix. + * ``kp`` — applied through the actuator's per-env ``kp_scale``. + * ``vin`` — written into the actuator's per-env ``vin_tensor``. + + :param json_path: Path to a BAM params JSON file (produced by ``bam.fit``). + Mutually exclusive with ``motor_name`` / ``model``. + :param motor_name: Bundled motor name (e.g. ``"mx106"``). Combine with ``model``. + :param model: Bundled model variant (``"m1"``–``"m6"``). Combine with ``motor_name``. + :param device: Torch device, defaults to ``"cuda"`` when available else ``"cpu"``. + :param integrator: MuJoCo integrator, ``"euler"`` (default, matching the CPU + backend) or ``"implicitfast"``. + :param stiff_frictionloss: Forwarded to :class:`BamActuatorCfg`. When ``True`` + (default), the joint-friction constraint is stiffened to counter MuJoCo + Warp's lack of a noslip solver (see :attr:`BamActuatorCfg.stiff_frictionloss`). + """ + + def __init__( + self, + json_path: str | None = None, + *, + motor_name: str | None = None, + model: str | None = None, + device: str | None = None, + integrator: str = "euler", + stiff_frictionloss: bool = True, + ) -> None: + self._params_path = _resolve_json_path(json_path, motor_name, model) + # Default supply voltage from the JSON, used for logs that don't carry vin. + self._default_vin = load_model(self._params_path).actuator.vin + if device is None: + device = "cuda" if torch.cuda.is_available() else "cpu" + self.device = device + self.integrator = integrator + self.stiff_frictionloss = stiff_frictionloss + + # ── Public API (mirrors the reference / CPU simulators) ────────────────── + + def rollout_log(self, log: dict, reset_period: float | None = None) -> tuple: + """Roll out a single log. See :meth:`rollout_logs`. + + :returns: Tuple ``(positions, velocities, controls)`` — lists over timesteps. + """ + positions, velocities, controls = self.rollout_logs( + [log], reset_period=reset_period + ) + return positions[0], velocities[0], controls[0] + + def rollout_logs( + self, logs: list[dict], reset_period: float | None = None + ) -> tuple[list, list, list]: + """Roll out a batch of logs in parallel — one environment per log. + + All logs must share the same timestep ``dt`` (a single MuJoCo timestep is + used for the whole batch) but may otherwise differ in pendulum parameters, + firmware gains, initial state, goal trajectory and length. Logs of + different lengths are padded (holding the last command) and their outputs + trimmed back to their own length. + + :param logs: List of processed log dicts. + :param reset_period: If set, re-synchronize each environment's state to its + log at this interval [s] (mirrors the reference simulator). + :returns: Tuple ``(positions, velocities, controls)`` — each a list (one + entry per log) of per-timestep values. ``controls`` are the applied + joint torques [Nm] (the BAM actuator drives MuJoCo in motor mode). + """ + n = len(logs) + if n == 0: + return [], [], [] + + dts = {log["dt"] for log in logs} + if len(dts) != 1: + raise ValueError( + f"All logs must share the same dt for the mjlab backend, got {sorted(dts)}" + ) + dt = dts.pop() + lengths = [len(log["entries"]) for log in logs] + max_len = max(lengths) + + # ── Pre-extract per-step arrays, padded by holding the last command ── + goals = np.zeros((max_len, n)) + torque_en = np.zeros((max_len, n), dtype=bool) + log_pos = np.zeros((max_len, n)) + log_speed = np.zeros((max_len, n)) + for i, log in enumerate(logs): + entries = log["entries"] + length = lengths[i] + + def _col(key, default=None): + return np.array( + [ + e.get(key, default) if default is not None else e[key] + for e in entries + ] + ) + + gi = _col("goal_position") + ti = _col("torque_enable") + pi = _col("position") + si = np.array([e.get("speed", 0.0) for e in entries]) + goals[:length, i], goals[length:, i] = gi, gi[-1] + torque_en[:length, i], torque_en[length:, i] = ti, ti[-1] + log_pos[:length, i], log_pos[length:, i] = pi, pi[-1] + log_speed[:length, i], log_speed[length:, i] = si, si[-1] + + # ── Build the scene / sim / actuator ───────────────────────────────── + scene, sim, entity, bam_act, body_id = self._build(logs[0], n, dt) + self._apply_per_env_params(sim, entity, bam_act, body_id, logs) + + dev = self.device + f32 = torch.float32 + + # ── Reset to each log's first entry ────────────────────────────────── + sim.reset() + scene.reset() + p0 = torch.as_tensor(log_pos[0], dtype=f32, device=dev).unsqueeze(1) + v0 = torch.as_tensor(log_speed[0], dtype=f32, device=dev).unsqueeze(1) + entity.write_joint_state_to_sim(p0, v0) + sim.forward() + + ctrl_ids = bam_act.ctrl_ids + + positions = np.zeros((max_len, n)) + velocities = np.zeros((max_len, n)) + controls = np.zeros((max_len, n)) + + reset_t = 0.0 + for k in range(max_len): + # Optional periodic re-sync to the log (mirrors the reference sim). + reset_t += dt + if reset_period is not None and reset_t > reset_period: + reset_t = 0.0 + pk = torch.as_tensor(log_pos[k], dtype=f32, device=dev).unsqueeze(1) + vk = torch.as_tensor(log_speed[k], dtype=f32, device=dev).unsqueeze(1) + entity.write_joint_state_to_sim(pk, vk) + sim.forward() + + positions[k] = entity.data.joint_pos[:, 0].detach().cpu().numpy() + velocities[k] = entity.data.joint_vel[:, 0].detach().cpu().numpy() + + goal = torch.as_tensor(goals[k], dtype=f32, device=dev).unsqueeze(1) + entity.set_joint_position_target(goal) + scene.write_data_to_sim() + + # Zero the applied torque where the log has torque disabled. + te = torque_en[k] + if not te.all(): + off = np.nonzero(~te)[0] + off_ids = torch.as_tensor(off, dtype=torch.long, device=dev) + zeros = torch.zeros( + (off_ids.numel(), ctrl_ids.numel()), dtype=f32, device=dev + ) + entity.write_ctrl_to_sim(zeros, env_ids=off_ids) + + controls[k] = sim.data.ctrl[:, ctrl_ids][:, 0].detach().cpu().numpy() + + sim.step() + scene.update(dt=dt) + + # ── Trim each environment's output back to its own length ──────────── + out_pos = [list(positions[: lengths[i], i]) for i in range(n)] + out_vel = [list(velocities[: lengths[i], i]) for i in range(n)] + out_ctrl = [list(controls[: lengths[i], i]) for i in range(n)] + return out_pos, out_vel, out_ctrl + + # ── Internals ──────────────────────────────────────────────────────────── + + def _build(self, base_log: dict, num_envs: int, dt: float): + """Build the scene, sim and actuator for ``num_envs`` pendulums.""" + bam_cfg = BamActuatorCfg( + json_path=self._params_path, + target_names_expr=(_JOINT_NAME,), + kp_fw=float(base_log["kp"]), + vin=base_log.get("vin"), + stiff_frictionloss=self.stiff_frictionloss, + ) + base = { + "mass": base_log["mass"], + "arm_mass": base_log.get("arm_mass", 0.0), + "length": base_log["length"], + } + ent_cfg = EntityCfg( + spec_fn=lambda: Pendulum(base).build_spec(_JOINT_NAME), + articulation=EntityArticulationInfoCfg(actuators=(bam_cfg,)), + init_state=EntityCfg.InitialStateCfg( + joint_pos={_JOINT_NAME: 0.0}, joint_vel={".*": 0.0} + ), + ) + scene = Scene( + SceneCfg(num_envs=num_envs, terrain=None, entities={_ENTITY_NAME: ent_cfg}), + self.device, + ) + mj_model = scene.compile() + # The joint-friction constraint stiffening (for the missing noslip solver) + # is applied by BamActuator.edit_spec via cfg.stiff_frictionloss. + sim = Simulation( + num_envs=num_envs, + cfg=SimulationCfg( + mujoco=MujocoCfg( + timestep=dt, + integrator=self.integrator, + gravity=(0.0, 0.0, -Pendulum.G), + ) + ), + model=mj_model, + device=self.device, + ) + scene.initialize(sim.mj_model, sim.model, sim.data) + # BamActuator writes per-env friction; those fields must be per-world. + sim.expand_model_fields( + ( + "body_mass", + "body_ipos", + "body_inertia", + "dof_frictionloss", + "dof_damping", + ) + ) + entity = scene[_ENTITY_NAME] + bam_act = entity.actuators[0] + body_id = mj_model.body(f"{_ENTITY_NAME}/{_JOINT_NAME}").id + return scene, sim, entity, bam_act, body_id + + def _apply_per_env_params(self, sim, entity, bam_act, body_id, logs): + """Inject per-environment pendulum inertial + firmware gains.""" + dev = self.device + f32 = torch.float32 + n = len(logs) + + masses, coms, inertias = [], [], [] + for log in logs: + pend = Pendulum( + { + "mass": log["mass"], + "arm_mass": log.get("arm_mass", 0.0), + "length": log["length"], + } + ) + total_mass, com_z, inertia_x = pend.inertial_params() + masses.append(total_mass) + coms.append(com_z) + inertias.append(inertia_x) + + sim.model.body_mass[:, body_id] = torch.as_tensor(masses, dtype=f32, device=dev) + ipos = sim.model.body_ipos + ipos[:, body_id, 0] = 0.0 + ipos[:, body_id, 1] = 0.0 + ipos[:, body_id, 2] = torch.as_tensor(coms, dtype=f32, device=dev) + inertia = sim.model.body_inertia + it = torch.as_tensor(inertias, dtype=f32, device=dev) + inertia[:, body_id, 0] = it + inertia[:, body_id, 1] = it + inertia[:, body_id, 2] = it + # Refresh derived quantities (mass matrix, subtree mass) after mass/inertia edits. + sim.recompute_constants(RecomputeLevel.set_const) + + # Firmware: kp via per-env kp_scale (relative to base kp), vin per-env. + base_kp = float(logs[0]["kp"]) + kp_scale = torch.as_tensor( + [[float(log["kp"]) / base_kp] for log in logs], dtype=f32, device=dev + ) + bam_act.kp_scale[:] = kp_scale + bam_act.default_kp_scale[:] = kp_scale + vin = torch.as_tensor( + [[float(log.get("vin", self._default_vin))] for log in logs], + dtype=f32, + device=dev, + ) + bam_act.vin_tensor[:] = vin diff --git a/bam/model.py b/bam/model.py new file mode 100644 index 0000000..6ba3392 --- /dev/null +++ b/bam/model.py @@ -0,0 +1,345 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np +import json +from pathlib import Path +from .actuator import Actuator +from .actuators import actuators +from .parameter import Parameter + + +class Model: + """Friction model for a servo actuator. + + Combines a motor model (set via :meth:`set_actuator`) with a friction budget + that maps joint state and torques to a maximum resistive torque + :math:`\\tau_{fm}`. The friction applied by the simulator is the stopping + torque clipped in :math:`[-\\tau_{fm}, \\tau_{fm}]`. + + Use :func:`load_model` to instantiate from a parameter file rather than + constructing directly. + + :param load_dependent: Enable load-dependent friction terms. + :param directional: Enable directional (motor-side vs external-side) load friction. + Requires ``load_dependent=True``. + :param stribeck: Enable Stribeck effect (higher friction near zero velocity). + :param quadratic: Enable quadratic load-friction coupling term. + Requires ``directional=True`` and ``stribeck=True``. + :param name: Model variant identifier, e.g. ``"m6"``. + :param title: Human-readable title used in plots. + """ + + def __init__( + self, + load_dependent: bool = False, + directional: bool = False, + stribeck: bool = False, + quadratic: bool = False, + name: str = None, + title: str = "", + ): + self.actuator_name = None + self.name = name + self.title = title + + # Model parameters + self.load_dependent: bool = load_dependent + self.directional: bool = directional + self.stribeck: bool = stribeck + self.quadratic: bool = quadratic + + self.max_friction_base = 0.2 + self.max_load_friction = 0.5 + self.max_viscous_friction = 1.0 + + def reset(self) -> None: + """ + Resets the model internal state + """ + self.actuator.reset() + + def set_actuator(self, actuator: Actuator) -> None: + """Attach an actuator to this model and initialize its parameters. + + :param actuator: Actuator instance to attach. Determines the motor + model (kt, R, kp, vin) and the set of friction parameters that + are created on this model. + """ + self.actuator = actuator + self.actuator.set_model(self) + + # Offset of the motor (testbench error) + self.q_offset = Parameter(0.0, -0.1, 0.1) + + # Base friction is always here, stribeck friction is added when not moving [Nm] + self.friction_base = Parameter(0.05, 0.0, self.max_friction_base) + if self.stribeck: + self.friction_stribeck = Parameter(0.05, 0.0, self.max_friction_base) + + # Load-dependent friction, again base is always here and stribeck is added when not moving [Nm] + if self.load_dependent: + if self.directional: + self.load_friction_motor = Parameter(0.05, 0.0, self.max_load_friction) + self.load_friction_external = Parameter( + 0.05, 0.0, self.max_load_friction + ) + else: + self.load_friction_base = Parameter(0.05, 0.0, self.max_load_friction) + + if self.stribeck: + if self.directional: + self.load_friction_motor_stribeck = Parameter(0.05, 0.0, 1.0) + self.load_friction_external_stribeck = Parameter(0.05, 0.0, 1.0) + else: + self.load_friction_stribeck = Parameter(0.05, 0.0, 1.0) + + if self.quadratic: + self.load_friction_motor_quad = Parameter(0.0, 0.0, 0.01) + self.load_friction_external_quad = Parameter(0.0, 0.0, 0.01) + + if self.stribeck: + # Stribeck velocity [rad/s] and curvature + self.dtheta_stribeck = Parameter(0.2, 0.01, 5.0) + self.alpha = Parameter(1.35, 0.5, 10.0) + + # Viscous friction [Nm/(rad/s)] + self.friction_viscous = Parameter(0.1, 0.0, self.max_viscous_friction) + + def compute_frictions( + self, motor_torque: float, external_torque: float, dtheta: float + ) -> tuple: + """Compute the friction budget for the current state. + + Returns the two MuJoCo friction parameters that implement the BAM + friction-budget formulation: ``frictionloss`` (Coulomb-like constant + term) and ``damping`` (viscous term). The simulator applies friction + by clipping the stopping torque in + :math:`[-\\text{frictionloss} - \\text{damping}\\cdot|\\dot{\\theta}|,\\ +\\ldots]`. + + :param motor_torque: Torque produced by the motor [Nm]. + :param external_torque: External (gravity/load) torque seen at the joint [Nm]. + :param dtheta: Joint velocity [rad/s]. + :returns: Tuple ``(frictionloss, damping)`` ready to be written into + ``mj_model.dof_frictionloss`` and ``mj_model.dof_damping``. + """ + # Torque applied to the gearbox + if self.directional: + gearbox_torque = np.abs( + external_torque * self.load_friction_external.value + - motor_torque * self.load_friction_motor.value + ) + if self.stribeck: + gearbox_torque_stribeck = np.abs( + external_torque * self.load_friction_external_stribeck.value + - motor_torque * self.load_friction_motor_stribeck.value + ) + else: + gearbox_torque = np.abs(external_torque - motor_torque) + + if self.stribeck: + # Stribeck coeff (1 when stopped to 0 when moving) + stribeck_coeff = np.exp( + -(np.abs(dtheta / self.dtheta_stribeck.value) ** self.alpha.value) + ) + + # Static friction + frictionloss = self.friction_base.value + if self.load_dependent: + if self.directional: + frictionloss += gearbox_torque + else: + frictionloss += self.load_friction_base.value * gearbox_torque + + if self.stribeck: + frictionloss += stribeck_coeff * self.friction_stribeck.value + + if self.load_dependent: + if self.directional: + frictionloss += gearbox_torque_stribeck * stribeck_coeff + else: + frictionloss += ( + self.load_friction_stribeck.value + * gearbox_torque + * stribeck_coeff + ) + + if self.quadratic: + enable_quadratic = np.sign(external_torque) != np.sign(motor_torque) + direction_motor = np.abs(external_torque) < np.abs(motor_torque) + direction_external = np.abs(external_torque) > np.abs(motor_torque) + + gearbox_torque2_motor = ( + self.load_friction_external_quad.value + * np.abs(external_torque) ** 2 + ) + gearbox_torque2_external = ( + self.load_friction_motor_quad.value * np.abs(motor_torque) ** 2 + ) + + frictionloss += ( + stribeck_coeff + * ( + direction_motor * gearbox_torque2_motor + + direction_external * gearbox_torque2_external + ) + * enable_quadratic + ) + + # Viscous friction + damping = self.friction_viscous.value + + return frictionloss, damping + + def get_parameters(self) -> dict: + """ + This returns the list of parameters that can be optimized. + """ + return { + name: param + for name, param in vars(self).items() + if isinstance(param, Parameter) + } + + def get_parameter_values(self) -> dict: + """ + Return a dict containing parameter values + """ + parameters = self.get_parameters() + x = {} + for name in parameters: + parameter = parameters[name] + if parameter.optimize: + x[name] = parameter.value + return x + + def load_parameters(self, json_file: str) -> list: + """ + Load parameters from a given filename + """ + with open(json_file) as f: + data = json.load(f) + self.load_parameters_from_dict(data) + + def load_parameters_from_dict(self, data: dict) -> list: + """ + Load parameters from a given dict + """ + parameters = self.get_parameters() + + for name in parameters: + if name in data: + parameters[name].value = data[name] + + +class DummyModel(Model): + def __init__(self): + super().__init__() + + +models = { + "m1": lambda: Model(name="m1", title="Coulomb (M1)"), + "m2": lambda: Model(name="m2", stribeck=True, title="Stribeck (M2)"), + "m3": lambda: Model(name="m3", load_dependent=True, title="Load-dependent (M3)"), + "m4": lambda: Model( + name="m4", + load_dependent=True, + stribeck=True, + title="Stribeck load-dependent (M4)", + ), + "m5": lambda: Model( + name="m5", + load_dependent=True, + stribeck=True, + directional=True, + title="Stribeck load-dependent directional (M5)", + ), + "m6": lambda: Model( + name="m6", + load_dependent=True, + stribeck=True, + directional=True, + quadratic=True, + title="Stribeck load-dependent directional quadratic (M6)", + ), +} + + +def _resolve_json_path( + json_file: str | None, motor_name: str | None, model: str | None +) -> str: + if json_file is not None: + return json_file + if motor_name is None or model is None: + raise ValueError("Provide either json_file or both motor_name and model.") + params_root = Path(__file__).parent / "params" + path = params_root / motor_name / f"{model}.json" + if not path.exists(): + motor_dir = params_root / motor_name + available_models = ( + sorted(p.stem for p in motor_dir.glob("*.json")) + if motor_dir.exists() + else [] + ) + available_motors = ( + sorted(d.name for d in params_root.iterdir() if d.is_dir()) + if params_root.exists() + else [] + ) + raise FileNotFoundError( + f"No bundled params for motor={motor_name!r} model={model!r}. " + f"Available models for this motor: {available_models}. " + f"Available motors: {available_motors}." + ) + return str(path) + + +def load_model( + json_file: str = None, *, motor_name: str = None, model: str = None +) -> Model: + """Load a BAM friction model from a parameter file. + + Specify the source with **one** of two mutually exclusive approaches: + + - **Bundled motor** — pass ``motor_name`` and ``model``:: + + model = load_model(motor_name="xl330", model="m6") + + - **Custom JSON** — pass ``json_file`` (output of ``bam.fit``):: + + model = load_model("params/my_motor/m6.json") + + :param json_file: Path to a BAM params JSON file. + :param motor_name: Name of a bundled motor (e.g. ``"xl330"``, ``"mx106"``). + Must be combined with ``model``. + :param model: Model variant for a bundled motor (``"m1"``–``"m6"``). + Must be combined with ``motor_name``. + :returns: A :class:`Model` instance with all parameters loaded. + :raises FileNotFoundError: If the requested bundled motor or model does not exist. + """ + path = _resolve_json_path(json_file, motor_name, model) + with open(path) as f: + data = json.load(f) + return load_model_from_dict(data) + + +def load_model_from_dict(data: dict) -> Model: + """Load a BAM friction model from a parameter dictionary. + + This is the low-level counterpart of :func:`load_model`, used when the + JSON data has already been parsed. + + :param data: Dictionary produced by ``json.load`` on a BAM params file. + Must contain at least ``"model"`` and ``"actuator"`` keys. + :returns: A :class:`Model` instance with all parameters loaded. + """ + model = models[data["model"]]() + model.set_actuator(actuators[data["actuator"]]()) + model.actuator_name = data["actuator"] + model.load_parameters_from_dict(data) + return model diff --git a/bam/mujoco.py b/bam/mujoco.py new file mode 100644 index 0000000..faf8890 --- /dev/null +++ b/bam/mujoco.py @@ -0,0 +1,382 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np +import mujoco +import json +from copy import copy +from .model import Model, load_model_from_dict +from .testbench_mujoco import Pendulum + + +class MujocoController: + """ + A MujocoController is a class allowing to apply the torque and update frictions + from the computed model during a simulation. + + :param bam.Model model: Model to use (can be loaded using load_model) + :param str actuator: Actuator to control. The actuated joint properties will be updated. This can be a list of actuators + :param mujoco.MjModel mujoco_model: The mujoco model + :param mujoco.MjData mujoco_data: The mujoco data + :param float | None vin_drop_resistance: The battery + wire resistance [Ohm], if not None the + voltage will be reduced by vin_drop_resistance * current, where the current [A] is estimated + from the actuator torques as sum(|torque|) / kt + :param float | None vin_min: the minimum voltage, if not None the voltage will not go below this value + """ + + def __init__( + self, + model: Model, + actuator: str, + mujoco_model: mujoco.MjModel, + mujoco_data: mujoco.MjData, + vin_drop_resistance: float | None = None, + vin_min: float | None = None, + ): + self.model = model + self.actuator = np.atleast_1d(actuator) + self.mujoco_model = mujoco_model + self.mujoco_data = mujoco_data + self.vin_drop_resistance = vin_drop_resistance + self.vin_min = vin_min + + self.dofs = [] + self.q_target = np.zeros(len(self.actuator)) + self.dof_to_q_target = {} + for i, name in enumerate(self.actuator): + self.dof_to_q_target[name] = i + + self.last_ts = mujoco_data.time + + # Actuator indexes (ctrl) + self.act_indexes = [ + self.mujoco_model.actuator(name).id for name in self.actuator + ] + # Joint indexes (efc_id) + # Retrieved using the first element of the trnid + self.joint_indexes = [ + self.mujoco_model.actuator(name).trnid[0] for name in self.actuator + ] + # Qpos indexes (qpos) + self.qpos_indexes = self.mujoco_model.jnt_qposadr[self.joint_indexes] + self.dof_indexes = self.mujoco_model.jnt_dofadr[self.joint_indexes] + + # Setting the armature + self.mujoco_model.dof_armature[self.dof_indexes] = ( + model.actuator.get_extra_inertia() + ) + mujoco.mj_setConst(self.mujoco_model, self.mujoco_data) + + def get_q_target(self, name: str) -> float: + """Return the current target position for a named actuator [rad]. + + :param name: Actuator name as passed to the constructor. + """ + return self.q_target[self.dof_to_q_target[name]] + + def set_q_target(self, name: str, q_target: float): + """Set the target position for a named actuator. + + :param name: Actuator name as passed to the constructor. + :param q_target: Desired joint angle [rad]. + """ + self.q_target[self.dof_to_q_target[name]] = q_target + + def update(self): + """ + Update the controlled actuator(s) data: + - Torque to apply + - Friction parameters + - Damping + """ + q = self.mujoco_data.qpos[self.qpos_indexes] + dq = self.mujoco_data.qvel[self.dof_indexes] + + # Apply the voltage drop across the battery + wire resistance. The current + # draw is estimated from the previous step's actuator torques (I = torque / kt). + act = self.model.actuator + vin_orig = act.vin + if self.vin_drop_resistance is not None: + load_torque = np.sum( + np.abs(self.mujoco_data.qfrc_actuator[self.dof_indexes]) + ) + current = load_torque / self.model.kt.value + vin_eff = vin_orig - self.vin_drop_resistance * current + if self.vin_min is not None: + vin_eff = max(vin_eff, self.vin_min) + act.vin = vin_eff + + # Computing the control signal (when the actuator defines a max_current, + # the firmware current limiter is applied here as a duty-cycle constraint) + dt = self.mujoco_data.time - self.last_ts + self.last_ts = self.mujoco_data.time + control = act.compute_control(self.q_target, q, dq, dt) + + # Computing the applied torque + torque = act.compute_torque(control, True, q, dq) + + # Restore original vin + if self.vin_drop_resistance is not None: + act.vin = vin_orig + + # Applying the torque + self.mujoco_data.ctrl[self.act_indexes] = torque + + # Updating friction parameters + torque_external = ( + -self.mujoco_data.qfrc_bias[self.dof_indexes] + + self.mujoco_data.qfrc_constraint[self.dof_indexes] + ) + + # Repeats the ids (now N_id x N_efc) + efc_id_repeated = np.repeat( + [self.mujoco_data.efc_id], len(self.actuator), axis=0 + ) + # Repeat the indexes (now N_id x N_efc) + id_repeated = np.repeat( + [self.joint_indexes], len(self.mujoco_data.efc_id), axis=0 + ).T + # Do the batched test (element wise) + selector = efc_id_repeated == id_repeated + # Use * as a logical and + selector = selector * ( + self.mujoco_data.efc_type == mujoco.mjtConstraint.mjCNSTR_FRICTION_DOF.value + ) + # Sum the forces + friction_force = np.sum(self.mujoco_data.efc_force * selector, axis=1) + + torque_external -= friction_force + torque_actuator = self.mujoco_data.qfrc_actuator[self.dof_indexes] + + # Updating friction parameters + frictionloss, damping = self.model.compute_frictions( + torque_actuator, torque_external, dq + ) + + # Updating damping and frictionloss + self.mujoco_model.dof_frictionloss[self.dof_indexes] = frictionloss + self.mujoco_model.dof_damping[self.dof_indexes] = damping + + +class Simulator: + """MuJoCo mirror of :class:`bam.simulate.Simulator`. + + Rolls out a BAM model against the same testbench, but uses MuJoCo physics + (from :class:`bam.testbench_mujoco.Pendulum`) instead of the hand-written + Euler integrator, with the actuator driven by a :class:`MujocoController`. + + Vectorization is handled the naive way: one independent ``(MjModel, MjData, + MujocoController)`` triplet is created per environment and they are ticked + one by one. This is intentionally not efficient — the point of this + simulator is to validate that the MuJoCo spec and the controller reproduce + the reference simulation. + + :param model: BAM friction model to simulate. + :param actuator: Name used for the hinge joint and the motor actuator in + the generated spec (and the name the :class:`MujocoController` controls). + """ + + def __init__(self, model: Model, actuator: str = "pendulum"): + self.model = model + self.actuator = actuator + # One entry per environment: (mujoco_model, mujoco_data, controller) + self.instances: list[tuple] = [] + self.t = 0.0 + + def _build_spec(self) -> mujoco.MjSpec: + testbench = self.model.actuator.testbench + if testbench is None: + raise RuntimeError( + "No testbench set on the actuator. Call model.actuator.load_log(log) " + "(or set model.actuator.testbench) before building the simulator." + ) + pendulum = Pendulum( + { + "mass": testbench.mass, + "arm_mass": testbench.arm_mass, + "length": testbench.length, + } + ) + return pendulum.build_spec(self.actuator) + + def reset(self, q: float = 0.0, dq: float = 0.0): + """(Re)build the environments and reset them to a given state. + + ``q`` and ``dq`` may be scalars or arrays; the number of environments is + the length of the (broadcast) inputs. + + :param q: Initial joint angle(s) [rad]. + :param dq: Initial joint velocity(ies) [rad/s]. + """ + q = np.atleast_1d(np.asarray(q, dtype=float)) + dq = np.atleast_1d(np.asarray(dq, dtype=float)) + n = max(len(q), len(dq)) + q = np.broadcast_to(q, (n,)) + dq = np.broadcast_to(dq, (n,)) + + self.model.reset() + spec = self._build_spec() + + self.instances = [] + for i in range(n): + mujoco_model = spec.compile() + mujoco_data = mujoco.MjData(mujoco_model) + controller = MujocoController( + self.model, self.actuator, mujoco_model, mujoco_data + ) + mujoco_data.qpos[controller.qpos_indexes] = q[i] + mujoco_data.qvel[controller.dof_indexes] = dq[i] + mujoco.mj_forward(mujoco_model, mujoco_data) + # Seed the target with the initial position so the arm starts at rest + controller.q_target = mujoco_data.qpos[controller.qpos_indexes] + self.instances.append((mujoco_model, mujoco_data, controller)) + + self.t = 0.0 + + def _pack(self, values: list): + """Return a scalar for a single environment, else a numpy array.""" + if len(values) == 1: + return values[0] + return np.array(values) + + @property + def q(self): + """Current joint angle(s) [rad] (scalar if a single environment).""" + return self._pack( + [data.qpos[ctrl.qpos_indexes][0] for _, data, ctrl in self.instances] + ) + + @property + def dq(self): + """Current joint velocity(ies) [rad/s] (scalar if a single environment).""" + return self._pack( + [data.qvel[ctrl.dof_indexes][0] for _, data, ctrl in self.instances] + ) + + def step(self, goal_position, torque_enable, dt: float): + """Advance every environment by one timestep. + + Unlike :meth:`bam.simulate.Simulator.step` (which is fed a raw control + signal), the actuator here is a position controller: the input is the + goal position and the :class:`MujocoController` computes the control and + the applied torque internally. + + :param goal_position: Target joint angle(s) [rad], scalar or per-environment. + :param torque_enable: Whether the actuator is powered (scalar or per-env). + When ``False`` the applied torque is zeroed and only gravity and + friction act. + :param dt: Timestep [s]. + """ + goal_position = np.broadcast_to( + np.atleast_1d(np.asarray(goal_position, dtype=float)), + (len(self.instances),), + ) + torque_enable = np.broadcast_to( + np.atleast_1d(np.asarray(torque_enable)), (len(self.instances),) + ) + + for i, (mujoco_model, mujoco_data, controller) in enumerate(self.instances): + mujoco_model.opt.timestep = dt + controller.set_q_target(self.actuator, goal_position[i]) + controller.update() + if not torque_enable[i]: + mujoco_data.ctrl[controller.act_indexes] = 0.0 + mujoco.mj_step(mujoco_model, mujoco_data) + + self.t += dt + + def rollout_log(self, log: dict, reset_period: float = None): + """Roll out the model against a recorded log and return predicted trajectories. + + Mirrors :meth:`bam.simulate.Simulator.rollout_log`, but drives the + actuator through the :class:`MujocoController` (position control from the + recorded ``goal_position``), so it is equivalent to the reference + simulator's ``simulate_control=True`` mode. + + :param log: Processed log dict (see :meth:`bam.logs.Logs.make_batch`). + :param reset_period: If set, re-synchronize the state to the log at this + interval [s]. + :returns: Tuple ``(positions, velocities, controls)`` — lists of values + at each timestep. ``controls`` are the voltages/currents computed by + the controller. + """ + positions = [] + velocities = [] + controls = [] + + dt = log["dt"] + self.model.actuator.load_log(log) + + first_entry = log["entries"][0] + self.reset( + first_entry["position"], + first_entry["speed"] if "speed" in first_entry else 0.0, + ) + + reset_period_t = 0.0 + for entry in log["entries"]: + reset_period_t += dt + if reset_period is not None and reset_period_t > reset_period: + reset_period_t = 0.0 + self.reset(entry["position"], entry["speed"]) + + positions.append(copy(self.q)) + velocities.append(copy(self.dq)) + + # Control recomputed the same way the controller does, for reference. + control = self.model.actuator.compute_control( + entry["goal_position"], self.q, self.dq, dt + ) + controls.append(copy(control)) + + self.step(entry["goal_position"], entry["torque_enable"], dt) + + return positions, velocities, controls + + +def load_config( + path: str, + mujoco_model: mujoco.MjModel, + mujoco_data: mujoco.MjData, + kp: float, + vin: float, +) -> tuple: + """ + Loads a BAM configuration file and returns the list of controllers and the mapping dicts. + + Args: + path (str): path to the configuration file + mujoco_model (mujoco.MjModel): the mujoco model + mujoco_data (mujoco.MjData): the mujoco data + kp (float): the proportional gain + vin (float): the input voltage + + Returns: + list: list of controllers, dofs to model mapping, dofs to id mapping + """ + bam_controllers = {} + dof_to_bam_controller = {} + with open(path) as f: + data = json.load(f) + for key, value in data.items(): + dofs = value["dofs"] + for dof in dofs: + dof_to_bam_controller[dof] = key + + model = load_model_from_dict(value["model"]) + model.actuator.kp = kp + model.actuator.vin = vin + model.actuator.error_gain = value["error_gain"] + model.actuator.max_pwm = value["max_pwm"] + + bam_controllers[key] = MujocoController( + model, dofs, mujoco_model, mujoco_data + ) + bam_controllers[key].dofs = dofs + + return bam_controllers, dof_to_bam_controller diff --git a/bam/parameter.py b/bam/parameter.py new file mode 100644 index 0000000..e2f2818 --- /dev/null +++ b/bam/parameter.py @@ -0,0 +1,27 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + + +class Parameter: + """A scalar model parameter with bounds and an optimization flag. + + Parameters are attached to a :class:`~bam.model.Model` by + :meth:`~bam.model.Model.set_actuator` and collected by + :meth:`~bam.model.Model.get_parameters` for optimization. + + :param value: Initial value. + :param min: Lower bound used by the optimizer. + :param max: Upper bound used by the optimizer. + :param optimize: If ``False`` the parameter is held fixed during fitting. + """ + + def __init__(self, value: float, min: float, max: float, optimize: bool = True): + self.value: float = value + self.min: float = min + self.max: float = max + self.optimize: bool = optimize diff --git a/bam/params/README.md b/bam/params/README.md new file mode 100644 index 0000000..8dfc74e --- /dev/null +++ b/bam/params/README.md @@ -0,0 +1,4 @@ +Parameters +========== + +In this directory, you can find the identifier parameters for various actuators. diff --git a/bam/params/erob80_100/m1.json b/bam/params/erob80_100/m1.json new file mode 100644 index 0000000..dfacd9e --- /dev/null +++ b/bam/params/erob80_100/m1.json @@ -0,0 +1 @@ +{"kt": 7.657864604656686, "armature": 0.9589533917163041, "friction_base": 5.610965892410754, "friction_viscous": 8.392324354671375, "model": "m1", "actuator": "erob80_100"} \ No newline at end of file diff --git a/bam/params/erob80_100/m2.json b/bam/params/erob80_100/m2.json new file mode 100644 index 0000000..3482cbe --- /dev/null +++ b/bam/params/erob80_100/m2.json @@ -0,0 +1 @@ +{"kt": 7.709563685616313, "armature": 0.9732017667563382, "friction_base": 1.4462862541212224e-10, "friction_stribeck": 5.455188967669691, "dtheta_stribeck": 2.079758511184917, "alpha": 2.99999673058633, "friction_viscous": 9.570659122263434, "model": "m2", "actuator": "erob80_100"} \ No newline at end of file diff --git a/bam/params/erob80_100/m3.json b/bam/params/erob80_100/m3.json new file mode 100644 index 0000000..7e5bd66 --- /dev/null +++ b/bam/params/erob80_100/m3.json @@ -0,0 +1 @@ +{"kt": 9.469248716117072, "armature": 0.9887557992291075, "friction_base": 4.83228100668814, "load_friction_base": 0.14798087840990956, "friction_viscous": 7.395263062846597, "model": "m3", "actuator": "erob80_100"} \ No newline at end of file diff --git a/bam/params/erob80_100/m4.json b/bam/params/erob80_100/m4.json new file mode 100644 index 0000000..bddc810 --- /dev/null +++ b/bam/params/erob80_100/m4.json @@ -0,0 +1 @@ +{"kt": 9.800442495732021, "armature": 0.9469802941293913, "friction_base": 0.0001250620234780261, "friction_stribeck": 4.381669778040507, "load_friction_base": 2.126815651171455e-06, "load_friction_stribeck": 0.1661425164008433, "dtheta_stribeck": 1.977086206494623, "alpha": 2.9999261411563274, "friction_viscous": 9.02901791728356, "model": "m4", "actuator": "erob80_100"} \ No newline at end of file diff --git a/bam/params/erob80_100/m5.json b/bam/params/erob80_100/m5.json new file mode 100644 index 0000000..e7b1851 --- /dev/null +++ b/bam/params/erob80_100/m5.json @@ -0,0 +1 @@ +{"kt": 8.7651105517004, "armature": 0.7444198326018837, "friction_base": 0.0008544416565456324, "friction_stribeck": 3.8971701985820273, "load_friction_motor": 0.0014386633405137944, "load_friction_external": 3.392293448680981e-06, "load_friction_motor_stribeck": 7.95025355007889e-05, "load_friction_external_stribeck": 0.2544739333707678, "dtheta_stribeck": 1.8499219470996635, "alpha": 2.999117024168752, "friction_viscous": 8.888393828048388, "model": "m5", "actuator": "erob80_100"} \ No newline at end of file diff --git a/bam/params/erob80_100/m6.json b/bam/params/erob80_100/m6.json new file mode 100644 index 0000000..48be539 --- /dev/null +++ b/bam/params/erob80_100/m6.json @@ -0,0 +1 @@ +{"kt": 9.39629578274985, "armature": 1.1350955632532613, "friction_base": 0.0004779665335836211, "friction_stribeck": 5.2264235087217585, "load_friction_motor": 0.048887727723564756, "load_friction_external": 7.626323416955297e-08, "load_friction_motor_stribeck": 8.823275901433639e-06, "load_friction_external_stribeck": 0.05116664604344119, "load_friction_motor_quad": 0.0027029166135035076, "load_friction_external_quad": 0.002556257557735874, "dtheta_stribeck": 2.0330344391978556, "alpha": 2.9999247237894795, "friction_viscous": 9.709332392870175, "model": "m6", "actuator": "erob80_100"} \ No newline at end of file diff --git a/bam/params/erob80_50/m1.json b/bam/params/erob80_50/m1.json new file mode 100644 index 0000000..ad4d0d4 --- /dev/null +++ b/bam/params/erob80_50/m1.json @@ -0,0 +1 @@ +{"kt": 4.54339886748877, "R": 2.5088214949606913, "armature": 0.3369387357725746, "friction_base": 4.096647502063686, "friction_viscous": 1.29805795295293, "model": "m1", "actuator": "erob80_50"} \ No newline at end of file diff --git a/bam/params/erob80_50/m2.json b/bam/params/erob80_50/m2.json new file mode 100644 index 0000000..02188a1 --- /dev/null +++ b/bam/params/erob80_50/m2.json @@ -0,0 +1 @@ +{"kt": 4.5436619148111745, "R": 2.521141651470365, "armature": 0.3345162630438232, "friction_base": 4.086230363954603, "friction_stribeck": 1.6504871346024715e-08, "dtheta_stribeck": 0.9617768749469398, "alpha": 1.438935728332229, "friction_viscous": 1.3131993446070485, "model": "m2", "actuator": "erob80_50"} \ No newline at end of file diff --git a/bam/params/erob80_50/m3.json b/bam/params/erob80_50/m3.json new file mode 100644 index 0000000..fef9e64 --- /dev/null +++ b/bam/params/erob80_50/m3.json @@ -0,0 +1 @@ +{"kt": 4.819245281951386, "R": 2.204653159105827, "armature": 0.32927931334382515, "friction_base": 3.46658012152533, "load_friction_base": 0.06292192512694414, "friction_viscous": 1.207272265635136, "model": "m3", "actuator": "erob80_50"} \ No newline at end of file diff --git a/bam/params/erob80_50/m4.json b/bam/params/erob80_50/m4.json new file mode 100644 index 0000000..692ef67 --- /dev/null +++ b/bam/params/erob80_50/m4.json @@ -0,0 +1 @@ +{"kt": 4.822323833607484, "R": 1.820386625418371, "armature": 0.3338305298602653, "friction_base": 3.453696652879759, "friction_stribeck": 9.218443879044224e-06, "load_friction_base": 0.06308728641180934, "load_friction_stribeck": 7.125720330621117e-07, "dtheta_stribeck": 0.34601387316221976, "alpha": 2.727164024161143, "friction_viscous": 1.216634943303125, "model": "m4", "actuator": "erob80_50"} \ No newline at end of file diff --git a/bam/params/erob80_50/m5.json b/bam/params/erob80_50/m5.json new file mode 100644 index 0000000..88ca059 --- /dev/null +++ b/bam/params/erob80_50/m5.json @@ -0,0 +1 @@ +{"kt": 4.818322648039187, "R": 0.8574854939440504, "armature": 0.3310758428281228, "friction_base": 2.3673298244088454, "friction_stribeck": 0.8379934972683746, "load_friction_motor": 0.03418184370369529, "load_friction_external": 0.00026790940844419897, "load_friction_motor_stribeck": 0.015404277930421264, "load_friction_external_stribeck": 0.08496269863014352, "dtheta_stribeck": 2.263893998051299, "alpha": 2.984725791735308, "friction_viscous": 1.6088820042966994, "model": "m5", "actuator": "erob80_50"} \ No newline at end of file diff --git a/bam/params/erob80_50/m6.json b/bam/params/erob80_50/m6.json new file mode 100644 index 0000000..5b136d3 --- /dev/null +++ b/bam/params/erob80_50/m6.json @@ -0,0 +1 @@ +{"kt": 4.896769365659586, "R": 1.0734946294308996, "armature": 0.36173986463800073, "friction_base": 3.258936577427873, "friction_stribeck": 0.32043116640097835, "load_friction_motor": 0.03566185222966088, "load_friction_external": 0.0008431659435708802, "load_friction_motor_stribeck": 0.003770985236415064, "load_friction_external_stribeck": 0.041438023645284434, "load_friction_motor_quad": 0.0018721192347735344, "load_friction_external_quad": 0.0015990991138724365, "dtheta_stribeck": 2.830385649014235, "alpha": 1.7864364677651365, "friction_viscous": 1.403394119501813, "model": "m6", "actuator": "erob80_50"} \ No newline at end of file diff --git a/bam/params/feetech_sts3215_7_4V/m1.json b/bam/params/feetech_sts3215_7_4V/m1.json new file mode 100644 index 0000000..1838a26 --- /dev/null +++ b/bam/params/feetech_sts3215_7_4V/m1.json @@ -0,0 +1,10 @@ +{ + "kt": 1.21164135295077, + "R": 2.6761663274455603, + "armature": 0.02840336348682085, + "q_offset": -0.05116067663549731, + "friction_base": 0.05239296084748866, + "friction_viscous": 0.05908515565091076, + "model": "m1", + "actuator": "sts3215" +} \ No newline at end of file diff --git a/bam/params/mx106/m1.json b/bam/params/mx106/m1.json new file mode 100644 index 0000000..2f526f3 --- /dev/null +++ b/bam/params/mx106/m1.json @@ -0,0 +1,9 @@ +{ + "kt": 2.1913757006745245, + "R": 2.9649903987776804, + "armature": 0.026609234235148084, + "friction_base": 0.10352026623606064, + "friction_viscous": 0.03520238029013507, + "model": "m1", + "actuator": "mx106" +} \ No newline at end of file diff --git a/bam/params/mx106/m2.json b/bam/params/mx106/m2.json new file mode 100644 index 0000000..56fa9d4 --- /dev/null +++ b/bam/params/mx106/m2.json @@ -0,0 +1,12 @@ +{ + "kt": 2.190958566263214, + "R": 2.9609045764726725, + "armature": 0.026838831911873175, + "friction_base": 2.768555173702711e-06, + "friction_stribeck": 0.11742121471961874, + "dtheta_stribeck": 1.8487805494299074, + "alpha": 1.7413892741330947, + "friction_viscous": 0.059589394457307716, + "model": "m2", + "actuator": "mx106" +} \ No newline at end of file diff --git a/bam/params/mx106/m3.json b/bam/params/mx106/m3.json new file mode 100644 index 0000000..48b10d1 --- /dev/null +++ b/bam/params/mx106/m3.json @@ -0,0 +1,10 @@ +{ + "kt": 2.204165397088512, + "R": 2.653346939680077, + "armature": 0.02484689556730746, + "friction_base": 0.1133532996239035, + "load_friction_base": 0.10824037267470683, + "friction_viscous": 0.01194450384263248, + "model": "m3", + "actuator": "mx106" +} \ No newline at end of file diff --git a/bam/params/mx106/m4.json b/bam/params/mx106/m4.json new file mode 100644 index 0000000..af590e9 --- /dev/null +++ b/bam/params/mx106/m4.json @@ -0,0 +1,14 @@ +{ + "kt": 2.23138890457851, + "R": 2.3717487904639545, + "armature": 0.02457397334336701, + "friction_base": 6.653380436533205e-13, + "friction_stribeck": 0.10491060771568866, + "load_friction_base": 0.009150334028953775, + "load_friction_stribeck": 0.22235912179095094, + "dtheta_stribeck": 1.6232949773728853, + "alpha": 1.323485668999259, + "friction_viscous": 0.05096082506010955, + "model": "m4", + "actuator": "mx106" +} \ No newline at end of file diff --git a/bam/params/mx106/m5.json b/bam/params/mx106/m5.json new file mode 100644 index 0000000..7b911dc --- /dev/null +++ b/bam/params/mx106/m5.json @@ -0,0 +1,16 @@ +{ + "kt": 2.2087822411811935, + "R": 2.0142427292798475, + "armature": 0.026018155628771758, + "friction_base": 1.8812712661235184e-10, + "friction_stribeck": 0.12242747179820507, + "load_friction_motor": 0.20763710226562626, + "load_friction_external": 0.022684350817709906, + "load_friction_motor_stribeck": 0.19519320110890795, + "load_friction_external_stribeck": 0.08490162305974122, + "dtheta_stribeck": 1.7457130513692263, + "alpha": 1.138313248724122, + "friction_viscous": 0.05066487276724427, + "model": "m5", + "actuator": "mx106" +} \ No newline at end of file diff --git a/bam/params/mx106/m6.json b/bam/params/mx106/m6.json new file mode 100644 index 0000000..39b8d70 --- /dev/null +++ b/bam/params/mx106/m6.json @@ -0,0 +1,18 @@ +{ + "kt": 2.209828156174588, + "R": 2.033661749312417, + "armature": 0.026016184370993383, + "friction_base": 1.0232526438525893e-05, + "friction_stribeck": 0.12195409579603411, + "load_friction_motor": 0.19800553725822115, + "load_friction_external": 0.022215529897348624, + "load_friction_motor_stribeck": 0.19859970173941574, + "load_friction_external_stribeck": 0.08710328264676681, + "load_friction_motor_quad": 0.009999930103371748, + "load_friction_external_quad": 7.326919843140997e-05, + "dtheta_stribeck": 1.7463251741328727, + "alpha": 1.142827788004191, + "friction_viscous": 0.05071566473605552, + "model": "m6", + "actuator": "mx106" +} \ No newline at end of file diff --git a/bam/params/mx64/m1.json b/bam/params/mx64/m1.json new file mode 100644 index 0000000..0fbd010 --- /dev/null +++ b/bam/params/mx64/m1.json @@ -0,0 +1,9 @@ +{ + "kt": 1.6224667906987444, + "R": 3.949433673232461, + "armature": 0.011951238325312509, + "friction_base": 0.09038677246291783, + "friction_viscous": 0.011691602145974832, + "model": "m1", + "actuator": "mx64" +} \ No newline at end of file diff --git a/bam/params/mx64/m2.json b/bam/params/mx64/m2.json new file mode 100644 index 0000000..a632573 --- /dev/null +++ b/bam/params/mx64/m2.json @@ -0,0 +1,12 @@ +{ + "kt": 1.6204381558934768, + "R": 3.8944216519292354, + "armature": 0.011923664899375061, + "friction_base": 0.05611931763075957, + "friction_stribeck": 0.08186411549068562, + "dtheta_stribeck": 1.1202509555217797, + "alpha": 2.9999999999758256, + "friction_viscous": 0.019180846406029385, + "model": "m2", + "actuator": "mx64" +} \ No newline at end of file diff --git a/bam/params/mx64/m3.json b/bam/params/mx64/m3.json new file mode 100644 index 0000000..35bb011 --- /dev/null +++ b/bam/params/mx64/m3.json @@ -0,0 +1,10 @@ +{ + "kt": 1.631491787799071, + "R": 3.5708899455080743, + "armature": 0.011237906702031345, + "friction_base": 0.10216727649779266, + "load_friction_base": 0.07580074806755707, + "friction_viscous": 0.0006300275797440622, + "model": "m3", + "actuator": "mx64" +} \ No newline at end of file diff --git a/bam/params/mx64/m4.json b/bam/params/mx64/m4.json new file mode 100644 index 0000000..9a626a5 --- /dev/null +++ b/bam/params/mx64/m4.json @@ -0,0 +1,14 @@ +{ + "kt": 1.6631556605652682, + "R": 3.1560000816605682, + "armature": 0.010961212217454795, + "friction_base": 0.061549466501679174, + "friction_stribeck": 0.02389528618121147, + "load_friction_base": 1.9611119490261663e-10, + "load_friction_stribeck": 0.23428996680337053, + "dtheta_stribeck": 1.5205782308422509, + "alpha": 1.3344007931759223, + "friction_viscous": 0.014754905791636505, + "model": "m4", + "actuator": "mx64" +} \ No newline at end of file diff --git a/bam/params/mx64/m5.json b/bam/params/mx64/m5.json new file mode 100644 index 0000000..5533ccb --- /dev/null +++ b/bam/params/mx64/m5.json @@ -0,0 +1,16 @@ +{ + "kt": 1.587432589063179, + "R": 2.4143487051532198, + "armature": 0.011728935143844849, + "friction_base": 0.07501958219691603, + "friction_stribeck": 0.022233160228709636, + "load_friction_motor": 0.3847995899952685, + "load_friction_external": 1.6148277731274518e-11, + "load_friction_motor_stribeck": 0.030123950049619153, + "load_friction_external_stribeck": 0.1658962964406309, + "dtheta_stribeck": 0.7461716797150073, + "alpha": 1.2930393293009845, + "friction_viscous": 0.014908872960620413, + "model": "m5", + "actuator": "mx64" +} \ No newline at end of file diff --git a/bam/params/mx64/m6.json b/bam/params/mx64/m6.json new file mode 100644 index 0000000..1a37d02 --- /dev/null +++ b/bam/params/mx64/m6.json @@ -0,0 +1,18 @@ +{ + "kt": 1.6016832750318712, + "R": 2.3164043271967447, + "armature": 0.012265893049213158, + "friction_base": 6.217919698787649e-07, + "friction_stribeck": 0.12269525980968875, + "load_friction_motor": 0.3114720375111298, + "load_friction_external": 2.0462014258729542e-08, + "load_friction_motor_stribeck": 0.18774381222411038, + "load_friction_external_stribeck": 0.014065853481003015, + "load_friction_motor_quad": 0.006908512882777861, + "load_friction_external_quad": 0.009999980627672081, + "dtheta_stribeck": 2.575277366960116, + "alpha": 0.8409018066017007, + "friction_viscous": 0.02533701181690149, + "model": "m6", + "actuator": "mx64" +} \ No newline at end of file diff --git a/bam/params/xl320/m1.json b/bam/params/xl320/m1.json new file mode 100644 index 0000000..8b297ee --- /dev/null +++ b/bam/params/xl320/m1.json @@ -0,0 +1 @@ +{"kt": 0.9869471336819953, "R": 35.609097347319235, "armature": 0.0011189506223419352, "q_offset": -0.020819378421668963, "friction_base": 0.027514504332133573, "friction_viscous": 0.003317607576942647, "model": "m1", "actuator": "xl320"} \ No newline at end of file diff --git a/bam/params/xl320/m2.json b/bam/params/xl320/m2.json new file mode 100644 index 0000000..33c5b35 --- /dev/null +++ b/bam/params/xl320/m2.json @@ -0,0 +1 @@ +{"kt": 0.9412943898835693, "R": 34.1760764247053, "armature": 0.0011083204599469999, "q_offset": -0.02329389200964553, "friction_base": 3.177176398057847e-13, "friction_stribeck": 0.029893896971168774, "dtheta_stribeck": 2.328630880018793, "alpha": 1.2291010093231018, "friction_viscous": 0.009148664963139992, "model": "m2", "actuator": "xl320"} \ No newline at end of file diff --git a/bam/params/xl320/m3.json b/bam/params/xl320/m3.json new file mode 100644 index 0000000..1a982c6 --- /dev/null +++ b/bam/params/xl320/m3.json @@ -0,0 +1 @@ +{"kt": 1.0408462056437213, "R": 27.543138077523064, "armature": 0.001498599811990824, "q_offset": -0.018630436297482997, "friction_base": 0.020630491373515955, "load_friction_base": 0.21898043608013565, "friction_viscous": 0.0022633483259072375, "model": "m3", "actuator": "xl320"} \ No newline at end of file diff --git a/bam/params/xl320/m4.json b/bam/params/xl320/m4.json new file mode 100644 index 0000000..a74ec2f --- /dev/null +++ b/bam/params/xl320/m4.json @@ -0,0 +1 @@ +{"kt": 0.9948534969472849, "R": 26.200017809258604, "armature": 0.001481958770010781, "q_offset": -0.02324030641673211, "friction_base": 6.522076622427767e-12, "friction_stribeck": 0.021503490738180312, "load_friction_base": 0.22138710692461464, "load_friction_stribeck": 8.459762897200913e-11, "dtheta_stribeck": 1.9672779167547327, "alpha": 1.836241835428265, "friction_viscous": 0.007794402213506581, "model": "m4", "actuator": "xl320"} \ No newline at end of file diff --git a/bam/params/xl320/m5.json b/bam/params/xl320/m5.json new file mode 100644 index 0000000..a92d117 --- /dev/null +++ b/bam/params/xl320/m5.json @@ -0,0 +1 @@ +{"kt": 1.0089902629814915, "R": 31.59537719366117, "armature": 0.0011623832707017194, "q_offset": -0.021243741085637008, "friction_base": 6.936756414963028e-09, "friction_stribeck": 0.016288653470458467, "load_friction_motor": 3.027099212974675e-08, "load_friction_external": 0.15805798326702472, "load_friction_motor_stribeck": 7.425588167327617e-09, "load_friction_external_stribeck": 0.24623407524685495, "dtheta_stribeck": 2.3026673245198115, "alpha": 1.7036615009066722, "friction_viscous": 0.007224488134308582, "model": "m5", "actuator": "xl320"} \ No newline at end of file diff --git a/bam/params/xl320/m6.json b/bam/params/xl320/m6.json new file mode 100644 index 0000000..b691cae --- /dev/null +++ b/bam/params/xl320/m6.json @@ -0,0 +1 @@ +{"kt": 1.0088269677439867, "R": 31.585877261639528, "armature": 0.0011629864934131363, "q_offset": -0.021178174404549527, "friction_base": 4.333784350288425e-07, "friction_stribeck": 0.016305511369376878, "load_friction_motor": 3.0882140904620497e-06, "load_friction_external": 0.158325301089096, "load_friction_motor_stribeck": 5.965719077390517e-09, "load_friction_external_stribeck": 0.24548304071888186, "load_friction_motor_quad": 0.0036164099909151977, "load_friction_external_quad": 0.00471092869180762, "dtheta_stribeck": 2.3010537332753387, "alpha": 1.7067361543333317, "friction_viscous": 0.007225172246502409, "model": "m6", "actuator": "xl320"} \ No newline at end of file diff --git a/bam/params/xl330/m1.json b/bam/params/xl330/m1.json new file mode 100644 index 0000000..f29749f --- /dev/null +++ b/bam/params/xl330/m1.json @@ -0,0 +1,10 @@ +{ + "kt": 0.3866957639281311, + "R": 4.017129222290597, + "armature": 0.0018141672784458866, + "q_offset": 0.030871000250528358, + "friction_base": 0.01283272036582759, + "friction_viscous": 0.003661672282824312, + "model": "m1", + "actuator": "xl330" +} \ No newline at end of file diff --git a/bam/params/xl330/m2.json b/bam/params/xl330/m2.json new file mode 100644 index 0000000..f28c9cf --- /dev/null +++ b/bam/params/xl330/m2.json @@ -0,0 +1,13 @@ +{ + "kt": 0.38528091566850053, + "R": 4.000263886971013, + "armature": 0.001819285317339707, + "q_offset": 0.03284548034270232, + "friction_base": 4.133833162325061e-15, + "friction_stribeck": 0.01265123828962108, + "dtheta_stribeck": 4.166521110955459, + "alpha": 1.3636445350385433, + "friction_viscous": 0.005620845480615652, + "model": "m2", + "actuator": "xl330" +} \ No newline at end of file diff --git a/bam/params/xl330/m3.json b/bam/params/xl330/m3.json new file mode 100644 index 0000000..627e6f4 --- /dev/null +++ b/bam/params/xl330/m3.json @@ -0,0 +1,11 @@ +{ + "kt": 0.38042022855161584, + "R": 3.380066954773944, + "armature": 0.001917772275220315, + "q_offset": 0.029439576907685533, + "friction_base": 0.008881712972862601, + "load_friction_base": 0.12166455532811028, + "friction_viscous": 0.004379099756757558, + "model": "m3", + "actuator": "xl330" +} \ No newline at end of file diff --git a/bam/params/xl330/m4.json b/bam/params/xl330/m4.json new file mode 100644 index 0000000..c7a9faf --- /dev/null +++ b/bam/params/xl330/m4.json @@ -0,0 +1,15 @@ +{ + "kt": 0.3861709980035537, + "R": 3.402572641173151, + "armature": 0.0018954552619850965, + "q_offset": 0.02807245876562081, + "friction_base": 0.008758627618365112, + "friction_stribeck": 7.508873888400355e-17, + "load_friction_base": 0.06803357334088644, + "load_friction_stribeck": 0.07381180834784251, + "dtheta_stribeck": 2.5577016869227474, + "alpha": 0.7601158768377835, + "friction_viscous": 0.004621034970462603, + "model": "m4", + "actuator": "xl330" +} \ No newline at end of file diff --git a/bam/params/xl330/m5.json b/bam/params/xl330/m5.json new file mode 100644 index 0000000..334d6bd --- /dev/null +++ b/bam/params/xl330/m5.json @@ -0,0 +1,17 @@ +{ + "kt": 0.36221278145533053, + "R": 2.7519133088033305, + "armature": 0.001846893632595922, + "q_offset": 0.02914237064352493, + "friction_base": 0.00036595207645983614, + "friction_stribeck": 0.008677887561102606, + "load_friction_motor": 0.2896521957827231, + "load_friction_external": 0.0, + "load_friction_motor_stribeck": 3.057449839759573e-05, + "load_friction_external_stribeck": 0.0586825054453192, + "dtheta_stribeck": 3.1889119963879637, + "alpha": 2.99040048918495, + "friction_viscous": 0.00634803186996739, + "model": "m5", + "actuator": "xl330" +} \ No newline at end of file diff --git a/bam/params/xl330/m6.json b/bam/params/xl330/m6.json new file mode 100644 index 0000000..706b644 --- /dev/null +++ b/bam/params/xl330/m6.json @@ -0,0 +1,19 @@ +{ + "kt": 0.36601349688984386, + "R": 2.8113923539223227, + "armature": 0.0018077432831600838, + "q_offset": 0.0271132870444849, + "friction_base": 0.004771183165566, + "friction_stribeck": 0.004676345799486616, + "load_friction_motor": 0.2667860954283698, + "load_friction_external": 8.515871897059342e-06, + "load_friction_motor_stribeck": 1.0722918395099123e-05, + "load_friction_external_stribeck": 0.08077928978935671, + "load_friction_motor_quad": 0.009972471242139415, + "load_friction_external_quad": 0.004902565732332559, + "dtheta_stribeck": 2.890372094130307, + "alpha": 8.683259907618984, + "friction_viscous": 0.005359668274599504, + "model": "m6", + "actuator": "xl330" +} \ No newline at end of file diff --git a/bam/plot.py b/bam/plot.py new file mode 100644 index 0000000..3b3be4b --- /dev/null +++ b/bam/plot.py @@ -0,0 +1,158 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import argparse +import numpy as np +import matplotlib.pyplot as plt + +from .model import load_model, DummyModel +from .actuators import actuators +from . import simulate +from . import logs + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--params", type=str, default=["params.json"], nargs="+") +arg_parser.add_argument("--actuator", type=str, required=True) +arg_parser.add_argument("--reset_period", default=None, type=float) +arg_parser.add_argument("--sim", action="store_true") +arg_parser.add_argument( + "--sim-mujoco", + dest="sim_mujoco", + action="store_true", + help="Same as --sim but rolls out with the MuJoCo (CPU) simulator backend", +) +arg_parser.add_argument( + "--sim-mjlab", + dest="sim_mjlab", + action="store_true", + help="Same as --sim but rolls out with the mjlab (MuJoCo Warp / GPU) simulator backend", +) +args = arg_parser.parse_args() + +# Whether to overlay a simulation, and which backend to use. +do_sim = args.sim or args.sim_mujoco or args.sim_mjlab +if args.sim_mujoco: + sim_name = "MuJoCo" +elif args.sim_mjlab: + sim_name = "mjlab" +else: + sim_name = "reference" + +logs = logs.Logs(args.logdir) + +if args.sim_mujoco: + # Imported lazily so --sim (or no sim) doesn't require MuJoCo. + from . import mujoco as mujoco_backend + +if args.sim_mjlab: + # Imported lazily so other backends don't require mjlab. + from . import mjlab as mjlab_backend + +if do_sim: + model_names = args.params + +for log in logs.logs: + print(log["filename"]) + all_sim_q = [] + all_sim_speeds = [] + all_sim_controls = [] + all_names = [] + + if do_sim: + for model_name in model_names: + model = load_model(model_name) + all_names.append(model.name) + if args.sim_mujoco: + simulator = mujoco_backend.Simulator(model) + sim_q, sim_speed, sim_controls = simulator.rollout_log( + log, reset_period=args.reset_period + ) + elif args.sim_mjlab: + simulator = mjlab_backend.Simulator(json_path=model_name) + sim_q, sim_speed, sim_controls = simulator.rollout_log( + log, reset_period=args.reset_period + ) + else: + simulator = simulate.Simulator(model) + sim_q, sim_speed, sim_controls = simulator.rollout_log( + log, reset_period=args.reset_period, simulate_control=True + ) + all_sim_q.append(np.array(sim_q)) + all_sim_speeds.append(np.array(sim_speed)) + all_sim_controls.append(np.array(sim_controls)) + + ts = np.arange(len(log["entries"])) * log["dt"] + q = [entry["position"] for entry in log["entries"]] + goal_q = [entry["goal_position"] for entry in log["entries"]] + speed = [entry["speed"] if "speed" in entry else 0.0 for entry in log["entries"]] + has_speed = any("speed" in entry for entry in log["entries"]) + + dummy = DummyModel() + dummy.set_actuator(actuators[args.actuator]()) + simulator = simulate.Simulator(dummy) + _, __, controls = simulator.rollout_log(log, simulate_control=False) + torque_enable = np.array([entry["torque_enable"] for entry in log["entries"]]) + + # Using 2 x-shared subplots + if has_speed: + f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True) + else: + f, (ax1, ax3) = plt.subplots(2, sharex=True) + + ax1.plot(ts, q, label="q") + ax1.plot(ts, goal_q, label="goal_q", color="black", linestyle="--") + if do_sim: + for model_name, sim_q in zip(all_names, all_sim_q): + ax1.plot(ts, sim_q, label=f"{model_name}_q") + ax1.legend() + title = f"{log['motor']}, {log['trajectory']}, m={log['mass']}, l={log['length']}, k={log['kp']}" + + ax1.set_title( + f"{log['motor']}, {log['trajectory']}, m={log['mass']}, l={log['length']}, k={log['kp']}" + ) + ax1.set_ylabel("angle [rad]") + ax1.grid() + + if has_speed: + ax2.plot(ts, speed, label="speed") + if do_sim: + for model_name, sim_speeds in zip(all_names, all_sim_speeds): + ax2.plot(ts, sim_speeds, label=f"{model_name}_speed") + ax2.set_ylabel("speed [rad/s]") + ax2.grid() + ax2.legend() + + # Using torque_enable color piecewise + ax3.plot(ts, controls, label=dummy.actuator.control_unit()) + if do_sim: + for model_name, sim_controls in zip(all_names, all_sim_controls): + ax3.plot( + ts, + sim_controls, + label=f"{model_name}_{dummy.actuator.control_unit()}", + ) + # Shading the areas where torque is False + ax3.fill_between( + ts, + min([0.0 if c is None else c for c in controls]) - 0.02, + max([0.0 if c is None else c for c in controls]) + 0.02, + where=[not torque for torque in torque_enable], + color="red", + alpha=0.3, + label="torque off", + ) + ax3.set_ylabel(f"{dummy.actuator.control_unit()}") + ax3.legend() + if do_sim: + plt.xlabel(f"time [s] / simulator: {sim_name}") + else: + plt.xlabel("time [s]") + + plt.grid() + plt.show() diff --git a/bam/process.py b/bam/process.py new file mode 100644 index 0000000..4d4d8e7 --- /dev/null +++ b/bam/process.py @@ -0,0 +1,56 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import glob +from copy import deepcopy +import os +import json +import numpy as np +import argparse + +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument("--raw", type=str, required=True) +arg_parser.add_argument("--logdir", type=str, required=True) +arg_parser.add_argument("--dt", type=float, default=0.005) +args = arg_parser.parse_args() + +for logfile in glob.glob(f"{args.raw}/*.json"): + data = json.load(open(logfile)) + data_output = deepcopy(data) + data_output["entries"] = [] + data_output["dt"] = args.dt + + duration = data["entries"][-1]["timestamp"] + print(f"* Processing {logfile} with duration {duration:.2f}s") + ts = np.arange(0.0, duration, args.dt) + frame = 0 + + for t in ts: + while t > data["entries"][frame + 1]["timestamp"]: + frame += 1 + entry_1 = data["entries"][frame] + entry_2 = data["entries"][frame + 1] + new_entry = {} + + for key in entry_1: + if key == "timestamp": + continue + new_entry[key] = entry_1[key] + (entry_2[key] - entry_1[key]) * ( + t - entry_1["timestamp"] + ) / (entry_2["timestamp"] - entry_1["timestamp"]) + + new_entry["torque_enable"] = ( + True if (new_entry["torque_enable"] > 0.5) else False + ) + new_entry["timestamp"] = t + + data_output["entries"].append(new_entry) + + filename = os.path.basename(logfile) + output_filename = f"{args.logdir}/{filename}" + json.dump(data_output, open(output_filename, "w")) diff --git a/bam/simulate.py b/bam/simulate.py new file mode 100644 index 0000000..2dd05f1 --- /dev/null +++ b/bam/simulate.py @@ -0,0 +1,134 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np +from copy import copy +from .model import Model + + +class Simulator: + """Single-axis pendulum simulator used during identification. + + Implements the BAM simulation loop: firmware control law → motor torque → + friction budget → stopping-torque clipping → Euler integration. Used by + ``bam.fit`` to roll out a model against recorded logs and compute the + identification loss. + + :param model: BAM friction model to simulate. + """ + + def __init__(self, model: Model): + self.screen = None + self.model = model + self.reset() + + def reset(self, q: float = 0.0, dq: float = 0.0): + """ + Resets the simulation to a given state + """ + self.q = copy(q) + self.dq = copy(dq) + self.t = 0.0 + + self.model.reset() + + def step(self, control: None | float, torque_enable: bool, dt: float): + """Advance the simulation by one timestep. + + :param control: Control signal sent to the actuator (voltage, current, + or torque depending on the actuator type). ``None`` is treated as zero. + :param torque_enable: Whether the actuator is powered. When ``False`` + the motor torque is zero and only gravity and friction act. + :param dt: Timestep [s]. + """ + bias_torque = self.model.actuator.testbench.compute_bias( + self.q + self.model.q_offset.value, self.dq + ) + motor_torque = self.model.actuator.compute_torque( + control, torque_enable, self.q + self.model.q_offset.value, self.dq + ) + frictionloss, damping = self.model.compute_frictions( + motor_torque, bias_torque, self.dq + ) + + inertia = ( + self.model.actuator.testbench.compute_mass( + self.q + self.model.q_offset.value, self.dq + ) + + self.model.actuator.get_extra_inertia() + ) + net_torque = motor_torque + bias_torque + + # Tau_stop is the torque required to stop the motor (reach a velocity of 0 after dt) + tau_stop = (inertia / dt) * self.dq + net_torque + static_friction = -np.sign(tau_stop) * np.min( + [np.abs(tau_stop), frictionloss + damping * np.abs(self.dq)], axis=0 + ) + net_torque += static_friction + + angular_acceleration = net_torque / inertia + + self.dq += angular_acceleration * dt + self.dq = np.clip(self.dq, -100.0, 100.0) + self.q += self.dq * dt + 0.5 * angular_acceleration * dt**2 + self.t += dt + + def rollout_log( + self, log: dict, reset_period: float = None, simulate_control: bool = False + ): + """Roll out the model against a recorded log and return predicted trajectories. + + :param log: Processed log dict as returned by :meth:`bam.logs.Logs.make_batch` + or loaded directly from a processed JSON file. + :param reset_period: If set, re-synchronize the simulator state to the + log at this interval [s]. Useful when error accumulation destabilizes + long rollouts. + :param simulate_control: If ``True``, recompute the control signal from + the simulated state using the firmware control law. If ``False``, + use the control values recorded in the log. + :returns: Tuple ``(positions, velocities, controls)`` — lists of values + at each timestep. + """ + positions = [] + velocities = [] + controls = [] + + reset_period_t = 0.0 + dt = log["dt"] + first_entry = log["entries"][0] + self.reset( + first_entry["position"], + first_entry["speed"] if "speed" in first_entry else 0.0, + ) + self.model.actuator.load_log(log) + + for entry in log["entries"]: + reset_period_t += dt + if reset_period is not None and reset_period_t > reset_period: + reset_period_t = 0.0 + self.reset(entry["position"], entry["speed"]) + positions.append(copy(self.q)) + velocities.append(copy(self.dq)) + + if simulate_control: + control = self.model.actuator.compute_control( + entry["goal_position"], self.q, self.dq, dt + ) + else: + if "control" in entry: + control = entry["control"] + else: + control = self.model.actuator.compute_control( + entry["goal_position"], entry["position"], self.dq, dt + ) + + controls.append(copy(control)) + + self.step(control, entry["torque_enable"], dt) + + return positions, velocities, controls diff --git a/bam/testbench.py b/bam/testbench.py new file mode 100644 index 0000000..8c7221f --- /dev/null +++ b/bam/testbench.py @@ -0,0 +1,75 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np + + +class Testbench: + """Abstract base class for identification testbenches. + + A testbench defines the rigid-body dynamics of the physical setup: + + .. math:: + + \\tau_m + \\tau_e(q) = M(q)\\ddot{q} + + where :math:`\\tau_m` is the motor torque, :math:`\\tau_e` the external + (bias) torque, and :math:`M(q)` the effective inertia. + """ + + def compute_mass(self, q: float, dq: float) -> float: + """Return the effective inertia at the current state [kg·m²]. + + :param q: Joint angle [rad]. + :param dq: Joint velocity [rad/s]. + """ + raise NotImplementedError + + def compute_bias(self, q: float, dq: float) -> float: + """Return the external (bias) torque at the current state [Nm]. + + :param q: Joint angle [rad]. + :param dq: Joint velocity [rad/s]. + """ + raise NotImplementedError + + +class Pendulum(Testbench): + """Single-axis pendulum testbench. + + A point mass at the tip of a uniform rod driven by the actuator output + shaft. The zero angle is the arm pointing downward; positive angles are + counter-clockwise. + + :param log: Log dict containing ``"mass"`` [kg], ``"arm_mass"`` [kg], and + ``"length"`` [m] keys. + """ + + def __init__(self, log: dict): + self.mass = log["mass"] + self.arm_mass = log["arm_mass"] + self.length = log["length"] + + def compute_mass(self, q: float, dq: float) -> float: + """Return the pendulum's rotational inertia about the pivot [kg·m²]. + + :param q: Joint angle [rad] (unused — inertia is constant here). + :param dq: Joint velocity [rad/s] (unused). + """ + inertia = self.mass * self.length**2 + inertia += (self.arm_mass / 3) * self.length**2 + return inertia + + def compute_bias(self, q: float, dq: float) -> float: + """Return the gravity torque at the current angle [Nm]. + + :param q: Joint angle [rad]. + :param dq: Joint velocity [rad/s] (unused). + """ + g = -9.80665 + return (self.mass + self.arm_mass / 2) * g * self.length * np.sin(q) diff --git a/bam/testbench_mujoco.py b/bam/testbench_mujoco.py new file mode 100644 index 0000000..af93e94 --- /dev/null +++ b/bam/testbench_mujoco.py @@ -0,0 +1,220 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np +import mujoco + + +class Pendulum: + """MuJoCo counterpart of :class:`bam.testbench.Pendulum`. + + Builds a MuJoCo spec whose rigid-body dynamics match the analytic pendulum + testbench: a point mass at the tip of a uniform rod driven by a single hinge. + The zero angle is the arm pointing downward; positive angles are + counter-clockwise (about the +x axis, with the default ``-z`` gravity). + + The body inertial is set explicitly so the inertia about the pivot equals + + .. math:: + + M(q) = m\\,l^2 + \\frac{m_a}{3}\\,l^2 + + and the gravity torque equals + + .. math:: + + \\tau_e(q) = \\left(m + \\frac{m_a}{2}\\right) g\\,l\\,\\sin(q) + + exactly reproducing :meth:`bam.testbench.Pendulum.compute_mass` and + :meth:`bam.testbench.Pendulum.compute_bias`. + + :param log: Log dict containing ``"mass"`` [kg], ``"arm_mass"`` [kg], and + ``"length"`` [m] keys. + """ + + #: Gravity magnitude, matching :data:`bam.testbench` (g = -9.80665). + G = 9.80665 + + def __init__(self, log: dict): + self.mass = log["mass"] + self.arm_mass = log["arm_mass"] + self.length = log["length"] + + def inertial_params(self) -> tuple[float, float, float]: + """Return the MuJoCo body inertial matching the analytic testbench. + + :returns: ``(total_mass, com_z, inertia_x)`` where ``total_mass`` [kg] + is the body mass, ``com_z`` [m] the (negative) center-of-mass offset + along the downward arm, and ``inertia_x`` [kg·m²] the principal + inertia about the center of mass. MuJoCo re-adds ``total_mass * + com_z²`` through the COM offset, so the inertia about the pivot stays + equal to :meth:`bam.testbench.Pendulum.compute_mass`. + """ + total_mass = self.mass + self.arm_mass + # Center of mass along the (downward) arm: rod contributes at l/2, the + # tip mass at l. + com_z = ( + -(self.mass * self.length + self.arm_mass * self.length / 2.0) / total_mass + ) + # Inertia about the pivot, shifted to the COM with the parallel axis theorem. + inertia_pivot = ( + self.mass * self.length**2 + (self.arm_mass / 3.0) * self.length**2 + ) + inertia_com = inertia_pivot - total_mass * com_z**2 + # A point mass has zero inertia about its own COM, which MuJoCo would + # reject; floor it. Only rotation about x (the hinge axis) matters. + inertia_x = max(inertia_com, 1e-9) + return total_mass, com_z, inertia_x + + def build_spec(self, name: str = "pendulum") -> mujoco.MjSpec: + """Build and return a MuJoCo spec for this pendulum. + + :param name: Name given to both the hinge joint and the (motor) actuator. + This is the name the :class:`~bam.mujoco.MujocoController` is created with. + :returns: A :class:`mujoco.MjSpec` with a single hinge joint and a + direct-torque (motor) actuator. + """ + spec = mujoco.MjSpec() + spec.option.gravity = [0.0, 0.0, -self.G] + # One noslip solver iteration to reduce friction slip on the CPU pipeline. + spec.option.noslip_iterations = 1 + # We set the inertial properties explicitly to match the analytic + # testbench, so geoms are only visual. + spec.compiler.inertiafromgeom = ( + mujoco.mjtInertiaFromGeom.mjINERTIAFROMGEOM_FALSE + ) + + length = self.length + + # ── Scenery (purely visual, no collisions so the pendulum dynamics are + # identical to the analytic testbench) ────────────────────────────── + spec.add_texture( + name="skybox", + type=mujoco.mjtTexture.mjTEXTURE_SKYBOX, + builtin=mujoco.mjtBuiltin.mjBUILTIN_GRADIENT, + width=256, + height=256, + rgb1=[0.3, 0.5, 0.7], + rgb2=[0.0, 0.0, 0.0], + ) + spec.add_texture( + name="grid", + type=mujoco.mjtTexture.mjTEXTURE_2D, + builtin=mujoco.mjtBuiltin.mjBUILTIN_CHECKER, + width=300, + height=300, + rgb1=[0.2, 0.2, 0.2], + rgb2=[0.3, 0.3, 0.3], + mark=mujoco.mjtMark.mjMARK_EDGE, + markrgb=[0.8, 0.8, 0.8], + ) + floor_material = spec.add_material( + name="grid", texrepeat=[5, 5], reflectance=0.2 + ) + floor_material.textures[mujoco.mjtTextureRole.mjTEXROLE_RGB] = "grid" + + # A bit of light for some shadows. + spec.worldbody.add_light( + pos=[0.4, 0.4, 1.2], dir=[-0.4, -0.4, -1.2], castshadow=True + ) + + # Floor, placed just below the pendulum's lowest reach. + floor = spec.worldbody.add_geom( + name="floor", + type=mujoco.mjtGeom.mjGEOM_PLANE, + pos=[0.0, 0.0, -(length + 0.12)], + size=[2.0, 2.0, 0.1], + material="grid", + contype=0, + conaffinity=0, + ) + + body = spec.worldbody.add_body(name=name, pos=[0.0, 0.0, 0.0]) + body.add_joint( + name=name, + type=mujoco.mjtJoint.mjJNT_HINGE, + axis=[1.0, 0.0, 0.0], + ) + + total_mass, com_z, inertia_x = self.inertial_params() + body.mass = total_mass + body.ipos = [0.0, 0.0, com_z] + body.inertia = [inertia_x, inertia_x, inertia_x] + + # Visual arm: a brown stick (no mass/collision, inertia set explicitly). + body.add_geom( + name="arm", + type=mujoco.mjtGeom.mjGEOM_CAPSULE, + fromto=[0.0, 0.0, 0.0, 0.0, 0.0, -length], + size=[max(length * 0.025, 1e-3), 0.0, 0.0], + mass=0.0, + contype=0, + conaffinity=0, + rgba=[0.55, 0.35, 0.18, 1.0], + ) + # Fake tip mass: a dark cylinder centered at the end of the arm. + bob_half = max(length * 0.06, 1e-3) + body.add_geom( + name="bob", + type=mujoco.mjtGeom.mjGEOM_CYLINDER, + fromto=[ + 0.0, + 0.0, + -length + bob_half, + 0.0, + 0.0, + -length - bob_half, + ], + size=[max(length * 0.13, 2e-3), 0.0, 0.0], + mass=0.0, + contype=0, + conaffinity=0, + rgba=[0.13, 0.13, 0.15, 1.0], + ) + + # Direct-torque (motor) actuator on the hinge: ctrl is the joint torque. + actuator = spec.add_actuator(name=name) + actuator.target = name + actuator.trntype = mujoco.mjtTrn.mjTRN_JOINT + + return spec + + +if __name__ == "__main__": + import argparse + import time + import mujoco.viewer + + arg_parser = argparse.ArgumentParser( + description="Open a MuJoCo viewer on a pendulum testbench (zero torque control)" + ) + arg_parser.add_argument("--mass", type=float, default=0.5, help="Tip mass [kg]") + arg_parser.add_argument("--arm_mass", type=float, default=0.1, help="Arm mass [kg]") + arg_parser.add_argument("--length", type=float, default=0.2, help="Arm length [m]") + arg_parser.add_argument( + "--q0", type=float, default=1.0, help="Initial joint angle [rad]" + ) + args = arg_parser.parse_args() + + pendulum = Pendulum( + {"mass": args.mass, "arm_mass": args.arm_mass, "length": args.length} + ) + model = pendulum.build_spec().compile() + data = mujoco.MjData(model) + data.qpos[0] = args.q0 + + with mujoco.viewer.launch_passive(model, data) as viewer: + while viewer.is_running(): + step_start = time.time() + # Zero torque control: let gravity and friction act freely. + data.ctrl[:] = 0.0 + mujoco.mj_step(model, data) + viewer.sync() + remaining = model.opt.timestep - (time.time() - step_start) + if remaining > 0: + time.sleep(remaining) diff --git a/bam/to_mujoco.py b/bam/to_mujoco.py new file mode 100644 index 0000000..96d2110 --- /dev/null +++ b/bam/to_mujoco.py @@ -0,0 +1,156 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +"""Export an identified model to MuJoCo's built-in position actuator. + +.. deprecated:: + This export is kept for convenience, but it is only an approximation: the + identified model is collapsed into the constant ``kp``, ``damping``, + ``frictionloss``, ``armature`` and ``forcerange`` of a MuJoCo position + actuator, which cannot reproduce the load-dependent friction or the + torque-dependent damping that BAM identifies. Prefer + :class:`bam.mujoco.MujocoController` (CPU) or + :class:`bam.mjlab.BamActuatorCfg` (GPU), which evaluate the actual model at + each simulation step. +""" + +import argparse + +from .actuator import ( + Actuator, + CurrentControlledActuator, + VoltageControlledActuator, +) +from .message import bright, print_parameter, yellow +from .model import load_model +from .unitree.actuator import UnitreeGo1Actuator + +DEPRECATION_NOTICE = """WARNING: to_mujoco is deprecated. + +It approximates the model with a MuJoCo position actuator, and cannot reproduce +the load-dependent effects BAM identifies. Prefer bam.mujoco.MujocoController +(CPU) or bam.mjlab.BamActuatorCfg (GPU), which evaluate the model at each step. +""" + + +def voltage_controlled_to_mujoco(actuator: VoltageControlledActuator) -> dict: + """Compute MuJoCo parameters for a voltage-controlled actuator. + + The back-EMF is folded into ``damping`` and the firmware P-gain into ``kp``, + so the joint only matches the identified actuator to first order. + """ + if actuator.vin == 0 or actuator.kp == 0: + print(yellow("WARNING: kp or vin are not set")) + + kt = actuator.model.kt.value + R = actuator.model.R.value + + return { + "forcerange": actuator.vin * kt / R, + "armature": actuator.model.armature.value, + "kp": actuator.error_gain + * actuator.kp + * actuator.vin + * actuator.max_pwm + * kt + / R, + "damping": actuator.model.friction_viscous.value + kt**2 / R, + "frictionloss": actuator.model.friction_base.value, + } + + +def current_controlled_to_mujoco(actuator: CurrentControlledActuator) -> dict: + """Compute MuJoCo parameters for a current-controlled actuator. + + ``forcerange`` accounts for both the voltage and the current limit. + """ + if actuator.vin == 0 or actuator.kp == 0: + print(yellow("WARNING: kp or vin are not set")) + + kt = actuator.model.kt.value + + forcerange = actuator.vin * kt / actuator.model.R.value + forcerange = min(forcerange, actuator.model.current_limit.value * kt) + + return { + "forcerange": forcerange, + "armature": actuator.model.armature.value, + "kp": actuator.error_gain * actuator.kp * kt, + "damping": actuator.model.friction_viscous.value, + "frictionloss": actuator.model.friction_base.value, + } + + +def unitree_go1_to_mujoco(actuator: UnitreeGo1Actuator) -> dict: + """Compute MuJoCo parameters for a Unitree Go1 actuator.""" + if actuator.kp == 0: + print(yellow("WARNING: kp is not set")) + + return { + "forcerange": actuator.model.max_torque.value, + "armature": actuator.model.armature.value, + "kp": actuator.kp * actuator.model.ratio.value, + "damping": actuator.model.friction_viscous.value, + "frictionloss": actuator.model.friction_base.value, + } + + +def to_mujoco(actuator: Actuator) -> dict: + """Compute the MuJoCo position actuator parameters for ``actuator``. + + :param actuator: An identified actuator, with its log already loaded. + :returns: A dict of MuJoCo attribute names to values. + :raises NotImplementedError: If the actuator has no MuJoCo export. + """ + if isinstance(actuator, CurrentControlledActuator): + return current_controlled_to_mujoco(actuator) + elif isinstance(actuator, VoltageControlledActuator): + return voltage_controlled_to_mujoco(actuator) + elif isinstance(actuator, UnitreeGo1Actuator): + return unitree_go1_to_mujoco(actuator) + + raise NotImplementedError(f"{type(actuator).__name__} doesn't support to_mujoco") + + +if __name__ == "__main__": + arg_parser = argparse.ArgumentParser() + arg_parser.add_argument("--params", type=str, default="params.json") + arg_parser.add_argument("--kp", type=float, default=0.0) + arg_parser.add_argument("--kd", type=float, default=0.0) + arg_parser.add_argument("--vin", type=float, default=0.0) + args = arg_parser.parse_args() + + print(yellow(DEPRECATION_NOTICE)) + + # Load the model + model = load_model(args.params) + + values = { + "kp": args.kp, + "kd": args.kd, + "vin": args.vin, + "mass": 0.0, + "arm_mass": 0.0, + "length": 0.0, + } + model.actuator.load_log(values) + + if model.name != "m1": + print( + yellow( + f"WARNING: Model other than m1 can't be exported exactly to MuJoCo (model is {model.name})" + ) + ) + + bright(f"Reading model of type {model.name} from {args.params}") + bright(f"Parameters export for MuJoCo, actuator {model.actuator_name}") + + for name, value in to_mujoco(model.actuator).items(): + print_parameter(name, value) + + print("") diff --git a/bam/trajectory.py b/bam/trajectory.py new file mode 100644 index 0000000..63cbecd --- /dev/null +++ b/bam/trajectory.py @@ -0,0 +1,160 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +import numpy as np + + +def cubic_interpolate(keyframes: list, t: float) -> float: + """Interpolate a scalar signal through a list of keyframes with cubic splines. + + Each keyframe is a triplet ``(t, x, x')`` where ``t`` is the time, ``x`` + the value, and ``x'`` the desired first derivative at that point. + + :param keyframes: List of ``[t, x, dx/dt]`` triplets, sorted by time. + :param t: Query time. + :returns: Interpolated value at time ``t``. + """ + if t < keyframes[0][0]: + return keyframes[0][1] + if t > keyframes[-1][0]: + return keyframes[-1][1] + + for i in range(len(keyframes) - 1): + if keyframes[i][0] <= t <= keyframes[i + 1][0]: + t0, x0, x0p = keyframes[i] + t1, x1, x1p = keyframes[i + 1] + + A = [ + [1, t0, t0**2, t0**3], + [0, 1, 2 * t0, 3 * t0**2], + [1, t1, t1**2, t1**3], + [0, 1, 2 * t1, 3 * t1**2], + ] + b = [x0, x0p, x1, x1p] + w = np.linalg.solve(A, b) + + return w[0] + w[1] * t + w[2] * t**2 + w[3] * t**3 + + +class Trajectory: + """Abstract base class for identification trajectories. + + A trajectory is a callable that maps a time ``t`` to a target angle and a + torque-enable flag. All built-in trajectories run for 6 seconds. + """ + + duration: float = None + + def __call__(self, t: float) -> tuple[float, bool]: + """Return ``(angle [rad], torque_enable)`` at time ``t``. + + :param t: Time since the start of the trajectory [s]. + """ + raise NotImplementedError + + +class LiftAndDrop(Trajectory): + """Cubic move to −π/2 over 2 s, then torque disabled (gravity drop). + + Useful for identifying backdrivability and Stribeck effects at very low + speed, as the arm falls freely under gravity after the motor is released. + """ + + duration = 6.0 + + def __call__(self, t: float) -> tuple[float, bool]: + keyframes = [[0.0, 0.0, 0.0], [2.0, -np.pi / 2, 0.0]] + angle = cubic_interpolate(keyframes, t) + enable = t < 2.0 + return angle, enable + + +class SinusTimeSquare(Trajectory): + """Progressively faster sinusoidal trajectory: :math:`\\sin(t^2)`. + + General-purpose trajectory that sweeps a wide velocity range in a single + run. Recommended as the primary identification trajectory. + """ + + duration = 6.0 + + def __call__(self, t: float) -> tuple[float, bool]: + return np.sin(t**2), True + + +class UpAndDown(Trajectory): + """Slow cubic path 0 → π/2 → 0.8·π/2. + + Emphasises static friction and load-dependent effects at low to medium + speed. + """ + + duration = 6.0 + + def __call__(self, t: float) -> tuple[float, bool]: + keyframes = [ + [0.0, 0.0, 0.0], + [3.0, np.pi / 2, 0.0], + [6.0, 0.8 * np.pi / 2, 0.0], + ] + return cubic_interpolate(keyframes, t), True + + +class SinSin(Trajectory): + """Multi-frequency trajectory: :math:`\\sin(t)\\cdot\\pi/2 + \\sin(5t)\\cdot 0.5\\cdot\\sin(2t)`. + + Rich spectral content covers a broad range of velocities and accelerations. + """ + + duration = 6.0 + + def __call__(self, t: float) -> tuple[float, bool]: + angle = np.sin(t) * np.pi / 2 + np.sin(5.0 * t) * 0.5 * np.sin(t * 2.0) + return angle, True + + +class Nothing(Trajectory): + """Zero torque for the full duration (pure gravity response). + + Useful to isolate backdrivability and measure passive dynamics. + """ + + duration = 6.0 + + def __call__(self, t: float) -> tuple[float, bool]: + return 0.0, False + + +#: Registry of all built-in trajectories, keyed by name. +#: Pass one of these names to the ``--trajectory`` argument of the recording scripts. +trajectories: dict[str, Trajectory] = { + "lift_and_drop": LiftAndDrop(), + "sin_time_square": SinusTimeSquare(), + "up_and_down": UpAndDown(), + "sin_sin": SinSin(), + "nothing": Nothing(), +} + +if __name__ == "__main__": + import matplotlib.pyplot as plt + + trajectory = LiftAndDrop() + + ts = np.linspace(0.0, 5.0, 1000) + xs = [trajectory(t)[0] for t in ts] + en = [trajectory(t)[1] for t in ts] + + plt.plot(ts, xs, label="Angle") + plt.fill_between( + ts, min(xs), max(xs), en, alpha=0.2, color="green", label="Torque enable" + ) + plt.xlabel("Time (s)") + plt.ylabel("Angle (rad)") + plt.legend() + plt.grid() + plt.show() diff --git a/bam/unitree/__init__.py b/bam/unitree/__init__.py new file mode 100644 index 0000000..2ff1b05 --- /dev/null +++ b/bam/unitree/__init__.py @@ -0,0 +1,8 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + diff --git a/bam/unitree/actuator.py b/bam/unitree/actuator.py new file mode 100644 index 0000000..db6547f --- /dev/null +++ b/bam/unitree/actuator.py @@ -0,0 +1,79 @@ +# Copyright 2025 Marc Duclusaud & Grégoire Passault + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at: + +# http://www.apache.org/licenses/LICENSE-2.0 + +from __future__ import annotations + +import numpy as np +from typing import TYPE_CHECKING, Union +from bam.actuator import Actuator +from bam.parameter import Parameter +from bam.testbench import Testbench, Pendulum + +if TYPE_CHECKING: + from bam.actuator import ArrayLike + + +class UnitreeGo1Actuator(Actuator): + def __init__(self, testbench_class: Testbench, damping=0.3): + super().__init__(testbench_class) + + # Damping factor + self.damping = damping + + def initialize(self): + # Torque multiplier + self.model.ratio = Parameter(1.0, 0.5, 2.0) + + # Maximum torque [N.m] + self.model.max_torque = Parameter(23.7, 20, 30) + + # Motor armature / apparent inertia [kg m^2] + self.model.armature = Parameter(0.005, 0.00001, 2.0) + + # Adjusting upper bounds for identification + self.model.max_friction_base = 5.0 + self.model.max_load_friction = 2.0 + self.model.max_viscous_friction = 1.0 + + def load_log(self, log: dict): + super().load_log(log) + + self.kp = log["kp"] + + if "damping" in log: + self.damping = log["damping"] + + def control_unit(self) -> str: + return "N.m" + + def compute_control( + self, q_target: ArrayLike, q: ArrayLike, dq: ArrayLike, dt: float + ) -> Union[ArrayLike, None]: + # Target velocity is assumed to be 0 + torque = (q_target - q) * self.kp * self.model.ratio.value + self.damping * ( + 0.0 - dq + ) + torque = self.backend.clamp( + torque, -self.model.max_torque.value, self.model.max_torque.value + ) + + return torque + + def compute_torque( + self, + control: ArrayLike | None, + torque_enable: bool, + q: ArrayLike, + dq: ArrayLike, + ) -> ArrayLike: + torques = control * torque_enable + + return torques + + def get_extra_inertia(self) -> float: + return self.model.armature.value diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000..94451f6 --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,28 @@ +# Minimal makefile for Sphinx documentation + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +VENV ?= .venv +SPHINXBUILD ?= $(if $(wildcard $(VENV)/bin/sphinx-build),$(VENV)/bin/sphinx-build,sphinx-build) +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile clean api + +clean: + rm -rf _build + +# Rebuild API reference from scratch (forces autoapi to re-scan bam/) +api: + rm -rf autoapi/ + @$(SPHINXBUILD) -M html "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/_static/BAM_logo.png b/docs/_static/BAM_logo.png new file mode 100644 index 0000000..1b21394 Binary files /dev/null and b/docs/_static/BAM_logo.png differ diff --git a/docs/_static/actuator_feetech_sts3215_7_4V.png b/docs/_static/actuator_feetech_sts3215_7_4V.png new file mode 100644 index 0000000..69af2e4 Binary files /dev/null and b/docs/_static/actuator_feetech_sts3215_7_4V.png differ diff --git a/docs/_static/actuator_mx106.png b/docs/_static/actuator_mx106.png new file mode 100644 index 0000000..346829d Binary files /dev/null and b/docs/_static/actuator_mx106.png differ diff --git a/docs/_static/actuator_mx64.png b/docs/_static/actuator_mx64.png new file mode 100644 index 0000000..fb0d335 Binary files /dev/null and b/docs/_static/actuator_mx64.png differ diff --git a/docs/_static/actuator_xl320.png b/docs/_static/actuator_xl320.png new file mode 100644 index 0000000..bdf339b Binary files /dev/null and b/docs/_static/actuator_xl320.png differ diff --git a/docs/_static/actuator_xl330.png b/docs/_static/actuator_xl330.png new file mode 100644 index 0000000..b4bcfaa Binary files /dev/null and b/docs/_static/actuator_xl330.png differ diff --git a/docs/_static/bam.css b/docs/_static/bam.css new file mode 100644 index 0000000..3de66ba --- /dev/null +++ b/docs/_static/bam.css @@ -0,0 +1,103 @@ +/* Minimal custom style for BAM docs */ +.bd-page-width { + max-width: 92rem; +} + +h1, h2 { + letter-spacing: 0.01em; +} + +/* Add breathing room above and below content images */ +.bd-article img { + margin-top: 1rem; + margin-bottom: 2rem; +} + +/* Center the left (trajectory name) column of the trajectories table */ +.traj-table td:first-child, +.traj-table th:first-child { + text-align: center; + vertical-align: middle; +} + +video { + width: 100%; + max-width:700px; + display: block; + margin-left: auto; + margin-right: auto; +} + +/* Getting started cards */ +.bam-getting-started { + margin-top: 1.5rem; + margin-bottom: 1rem; +} + +.bam-card { + border-radius: 0.6rem; + transition: transform 0.15s ease, box-shadow 0.15s ease, border-color 0.15s ease; +} + +.bam-card:hover { + transform: translateY(-4px); + box-shadow: 0 8px 24px rgba(0, 0, 0, 0.12); + border-color: var(--pst-color-primary); +} + +.bam-card .sd-card-title { + font-size: 1.15rem; +} + +/* Float the card footer link to the right */ +.bam-card .sd-card-footer { + text-align: right; +} + +/* "Use in simulation" buttons on the identified-actuators table */ +.actuators-table td { + vertical-align: middle; +} + +.motor-btn { + display: block; + width: 100%; + margin: 0.25rem 0; + white-space: nowrap; +} + +/* Compact inline warning that fits inside a table cell */ +.mini-warning { + margin: 0.5rem 0 0; + font-size: 0.8em; + line-height: 1.35; + color: var(--pst-color-warning, #966600); +} + +.mini-warning code { + font-size: 0.9em; +} + +/* Banner shown on usage pages indicating the motor the examples target */ +.motor-banner { + display: flex; + align-items: center; + gap: 1.25rem; + margin-bottom: 1.5rem; +} + +.motor-banner-img { + flex: 0 0 auto; + width: 90px; + height: auto; + margin: 0; + border-radius: 0.4rem; +} + +.motor-banner-text { + flex: 1 1 auto; +} + +.motor-banner-text p:last-child { + margin-bottom: 0; +} \ No newline at end of file diff --git a/docs/_static/drive_backdrive_m1.png b/docs/_static/drive_backdrive_m1.png new file mode 100644 index 0000000..f2a5af8 Binary files /dev/null and b/docs/_static/drive_backdrive_m1.png differ diff --git a/docs/_static/drive_backdrive_m2.png b/docs/_static/drive_backdrive_m2.png new file mode 100644 index 0000000..0678b47 Binary files /dev/null and b/docs/_static/drive_backdrive_m2.png differ diff --git a/docs/_static/drive_backdrive_m3.png b/docs/_static/drive_backdrive_m3.png new file mode 100644 index 0000000..fd1b151 Binary files /dev/null and b/docs/_static/drive_backdrive_m3.png differ diff --git a/docs/_static/drive_backdrive_m4.png b/docs/_static/drive_backdrive_m4.png new file mode 100644 index 0000000..82be019 Binary files /dev/null and b/docs/_static/drive_backdrive_m4.png differ diff --git a/docs/_static/drive_backdrive_m5.png b/docs/_static/drive_backdrive_m5.png new file mode 100644 index 0000000..76ce106 Binary files /dev/null and b/docs/_static/drive_backdrive_m5.png differ diff --git a/docs/_static/drive_backdrive_m6.png b/docs/_static/drive_backdrive_m6.png new file mode 100644 index 0000000..f3e2f75 Binary files /dev/null and b/docs/_static/drive_backdrive_m6.png differ diff --git a/docs/_static/favicon.ico b/docs/_static/favicon.ico new file mode 100644 index 0000000..72f51cd Binary files /dev/null and b/docs/_static/favicon.ico differ diff --git a/docs/_static/hardware_1.png b/docs/_static/hardware_1.png new file mode 100644 index 0000000..242bcc7 Binary files /dev/null and b/docs/_static/hardware_1.png differ diff --git a/docs/_static/hardware_2.png b/docs/_static/hardware_2.png new file mode 100644 index 0000000..f018c23 Binary files /dev/null and b/docs/_static/hardware_2.png differ diff --git a/docs/_static/hardware_3.png b/docs/_static/hardware_3.png new file mode 100644 index 0000000..f253bdb Binary files /dev/null and b/docs/_static/hardware_3.png differ diff --git a/docs/_static/motor_examples.js b/docs/_static/motor_examples.js new file mode 100644 index 0000000..79e1b1a --- /dev/null +++ b/docs/_static/motor_examples.js @@ -0,0 +1,103 @@ +// Fill the {actuator} placeholder in the usage code examples with the motor +// chosen via the ?motor= query parameter (set by the buttons on the +// "Identified actuators" page). When no motor is given, xl330 is used as the +// default. Only whitelisted motor names are ever inserted. +(function () { + "use strict"; + + // name -> human-readable label + var MOTORS = { + mx64: "MX-64", + mx106: "MX-106", + xl320: "XL-320", + xl330: "XL-330", + feetech_sts3215_7_4V: "STS3215 (7.4V)", + }; + var DEFAULT_MOTOR = "xl330"; + var PLACEHOLDER = "{actuator}"; + + function selectedMotor() { + var value = new URLSearchParams(window.location.search).get("motor"); + if (value && Object.prototype.hasOwnProperty.call(MOTORS, value)) { + return value; + } + return DEFAULT_MOTOR; + } + + function pageHasPlaceholder() { + var blocks = document.querySelectorAll("div.highlight pre"); + for (var i = 0; i < blocks.length; i++) { + if (blocks[i].textContent.indexOf(PLACEHOLDER) !== -1) return true; + } + return false; + } + + function updateCodeBlocks(motor) { + // The placeholder is a contiguous token inside a string literal, so a plain + // substring replace on innerHTML is enough (motor is whitelisted). + document.querySelectorAll("div.highlight pre").forEach(function (pre) { + if (pre.textContent.indexOf(PLACEHOLDER) === -1) return; + pre.innerHTML = pre.innerHTML.split(PLACEHOLDER).join(motor); + }); + } + + // Derive the "_static/" URL prefix from an existing asset link, so the motor + // image resolves correctly regardless of the current page's depth. + function staticBase() { + var link = document.querySelector('link[href*="_static/"]'); + if (link) { + var href = link.getAttribute("href"); + var idx = href.indexOf("_static/"); + if (idx !== -1) return href.slice(0, idx + "_static/".length); + } + return "_static/"; + } + + function showBanner(motor) { + var article = + document.querySelector("article") || + document.querySelector("main") || + document.body; + var heading = article.querySelector("h1"); + var img = staticBase() + "actuator_" + motor + ".png"; + + var banner = document.createElement("div"); + banner.className = "admonition tip motor-banner"; + banner.innerHTML = + '' + + MOTORS[motor] + + '' + + '
' + + '

Motor selected

' + + "

Examples on this page are shown for " + + '' + + motor + + " (" + + MOTORS[motor] + + "). " + + 'Choose a different motor.

' + + "
"; + + if (heading && heading.parentNode) { + heading.parentNode.insertBefore(banner, heading.nextSibling); + } else { + article.insertBefore(banner, article.firstChild); + } + } + + function apply() { + // Only act on pages that actually carry the {actuator} placeholder. + if (!pageHasPlaceholder()) return; + var motor = selectedMotor(); + updateCodeBlocks(motor); + showBanner(motor); + } + + if (document.readyState === "loading") { + document.addEventListener("DOMContentLoaded", apply); + } else { + apply(); + } +})(); diff --git a/docs/_static/traj_lift_and_drop.png b/docs/_static/traj_lift_and_drop.png new file mode 100644 index 0000000..0eae9de Binary files /dev/null and b/docs/_static/traj_lift_and_drop.png differ diff --git a/docs/_static/traj_sin_sin.png b/docs/_static/traj_sin_sin.png new file mode 100644 index 0000000..921c961 Binary files /dev/null and b/docs/_static/traj_sin_sin.png differ diff --git a/docs/_static/traj_sin_time_square.png b/docs/_static/traj_sin_time_square.png new file mode 100644 index 0000000..e2c195b Binary files /dev/null and b/docs/_static/traj_sin_time_square.png differ diff --git a/docs/_static/traj_up_and_down.png b/docs/_static/traj_up_and_down.png new file mode 100644 index 0000000..87e102e Binary files /dev/null and b/docs/_static/traj_up_and_down.png differ diff --git a/docs/_static/velocity_vs_torque.svg b/docs/_static/velocity_vs_torque.svg new file mode 100644 index 0000000..e69d1e2 --- /dev/null +++ b/docs/_static/velocity_vs_torque.svg @@ -0,0 +1,1361 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/_static/velocity_vs_torque_custom_tau_max.svg b/docs/_static/velocity_vs_torque_custom_tau_max.svg new file mode 100644 index 0000000..6fade23 --- /dev/null +++ b/docs/_static/velocity_vs_torque_custom_tau_max.svg @@ -0,0 +1,2009 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/api/index.rst b/docs/api/index.rst new file mode 100644 index 0000000..587c208 --- /dev/null +++ b/docs/api/index.rst @@ -0,0 +1,7 @@ +API Reference +============= + +.. toctree:: + :maxdepth: 2 + + ../autoapi/bam/index diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 0000000..1d6d606 --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,64 @@ +import os +import sys + +project = "BAM" +author = "Marc Duclusaud & Grégoire Passault" +copyright = "2026, Marc Duclusaud & Grégoire Passault" + +extensions = [ + "autoapi.extension", + "sphinx.ext.napoleon", + "sphinx.ext.mathjax", + "sphinx.ext.todo", + "sphinx_copybutton", + "sphinxcontrib.video", + "sphinx_design", +] + +templates_path = ["_templates"] +exclude_patterns = ["_build", ".venv", "Thumbs.db", ".DS_Store"] + +sys.path.insert(0, os.path.abspath("..")) + +# sphinx-autoapi — scans bam/ via AST, no runtime imports needed +autoapi_type = "python" +autoapi_dirs = ["../bam"] +autoapi_root = "autoapi" +autoapi_ignore = ["*/drive_backdrive.py", "*/plot.py", "*/jitter.py", "*/mae.py", "*/message.py", "*/process.py"] +autoapi_add_toctree_entry = False +autoapi_options = [ + "members", + "undoc-members", + "show-inheritance", + "show-module-summary", + "imported-members", +] +autoapi_python_class_content = "both" +autoapi_member_order = "bysource" + +napoleon_google_docstring = True +napoleon_numpy_docstring = False +todo_include_todos = True + +html_theme = "pydata_sphinx_theme" +pygments_style = "friendly" +pygments_dark_style = "monokai" +html_static_path = ["_static"] +html_css_files = ["bam.css"] +html_js_files = ["motor_examples.js"] +html_logo = "_static/BAM_logo.png" +html_favicon = "_static/favicon.ico" + +html_theme_options = { + "show_nav_level": 2, + "show_toc_level": 2, + "navigation_with_keys": True, + "icon_links": [ + { + "name": "GitHub", + "url": "https://github.com/Rhoban/bam", + "icon": "fa-brands fa-square-github", + "type": "fontawesome", + } + ], +} diff --git a/docs/identification/acquisition.rst b/docs/identification/acquisition.rst new file mode 100644 index 0000000..f975c83 --- /dev/null +++ b/docs/identification/acquisition.rst @@ -0,0 +1,241 @@ +Data Acquisition +================ + +Recording consists of running the actuator through a set of predefined +trajectories while logging position, velocity, and control signals. The +trajectories are played with different P-gain values and with each +mass/length combination. The resulting data is then processed to a fixed +timestep and stored in a structured format for later use in the identification +pipeline. + + +Installation +------------ + +First, you need to clone the `BAM repository `_. + +Then, install the extra dependencies for the identification pipeline using +``uv`` (`installation instructions `_): + +.. code-block:: text + + uv sync --extra identification + +Trajectories +---------------------- + +The trajectories are designed to excite different friction regimes. +Each trajectory runs for 6 seconds. + +.. list-table:: + :header-rows: 1 + :widths: 20 80 + :class: traj-table + + * - Name + - Description + * - ``sin_time_square`` + - :math:`\sin(t^2)` profile — progressively faster oscillations, good + general-purpose trajectory that covers a wide velocity range. + + |traj_sin_time_square| + * - ``lift_and_drop`` + - Cubic move to −π/2 over 2 s, then torque disabled — the arm falls + under gravity. Disabling the torque can allow to dissociate viscous friction + from the back-EMF induced damping. + + |traj_lift_and_drop| + * - ``up_and_down`` + - Cubic path 0 → π/2 → 0.8·π/2 — slower motion, emphasizes static + friction and load-dependent effects. + + |traj_up_and_down| + * - ``sin_sin`` + - :math:`\sin(t)\cdot\pi/2 + \sin(5t)\cdot 0.5\cdot\sin(2t)` — rich + multi-frequency content. + + |traj_sin_sin| + +.. |traj_sin_time_square| image:: /_static/traj_sin_time_square.png + :width: 100% + :alt: sin_time_square trajectory +.. |traj_lift_and_drop| image:: /_static/traj_lift_and_drop.png + :width: 100% + :alt: lift_and_drop trajectory +.. |traj_up_and_down| image:: /_static/traj_up_and_down.png + :width: 100% + :alt: up_and_down trajectory +.. |traj_sin_sin| image:: /_static/traj_sin_sin.png + :width: 100% + :alt: sin_sin trajectory + +Recording +--------- + +BAM already ships with support for several actuators (the ones whose models +are provided in the library), and is designed to be extended with new ones. +Each manufacturer is implemented in its own package under +``bam//``, exposing the same small interface. To record your own +actuator, you therefore either: + +- **extend an existing manufacturer package** if your motor's brand is already + supported — simply add your actuator class (for example a new Dynamixel + model in ``bam/dynamixel/``); or +- **create a new** ``bam//`` **package** if your manufacturer is + not supported yet. + +Each manufacturer package is made of three modules: + +- ``actuator.py`` — the only hardware-specific part. It handles the + communication with the motor: reading position, speed and load, and sending + position or current commands. Your actuator class subclasses the base + classes provided in :mod:`bam.actuator`. +- ``record.py`` — plays the trajectories on the motor and logs the resulting + data to JSON. It adapts the generic trajectories to the motor when needed + (for instance, scaling the velocity to stay within the actuator's limits). +- ``all_record.py`` — a convenience script that runs a full recording session + for a given mass/arm configuration, automatically sweeping over all the + P-gain values. + +When writing your own package, the existing ``bam/dynamixel/`` implementation +is a good reference to draw inspiration from. + +Once these modules are written, recording should be done using the following command: + +.. code-block:: text + + uv run python -m bam..all_record \ + --port /dev/ttyUSB0 \ + --motor motor_name \ + --mass 0.5 \ + --arm-mass 0.02 \ + --length 0.15 \ + --vin 7.5 \ + --logdir data_raw + +with each length/mass combination. Don't forget to update the mass, length, and arm mass +parameters for each recording. + +.. warning:: + + The P-gain values used in the ``/record.py`` file are + manufacturer-specific and must be adapted to your motor based on the + manufacturer's specifications: a gain that is meaningful for one firmware + may be far too high or too low for another. + + To determine the appropriate P-gain values for your actuator, you can + take the default P-gain value ``kp`` given by the manufacturer and test + [kp/6, kp/4, kp/2, kp]. + + +Raw data format +--------------- + +Each recording produces one JSON file: + +.. code-block:: json + + { + "mass": 0.5, + "arm_mass": 0.02, + "length": 0.15, + "kp": 50, + "vin": 7.5, + "motor": "motor_name", + "trajectory": "sin_time_square", + "entries": [ + { + "timestamp": 0.0077, + "position": 0.0015, + "speed": 0.024, + "load": 0.0, + "input_volts": 7.5, + "goal_position": 0.0, + "torque_enable": true + }, + ... + ] + } + +Entries are logged at the firmware's native rate, which is not necessarily +constant. The processing step resamples them to a fixed timestep. + +Checking jitter +--------------- + +You can use the following script: + +.. code-block:: bash + + uv run python -m bam.jitter --logdir data_raw/ + +To visualize the jitter histogram. + +Processing +---------- + +Resample raw logs to a constant timestep before fitting: + +.. code-block:: text + + uv run python -m bam.process \ + --raw data_raw \ + --logdir data_processed \ + --dt 0.005 + +``--dt`` is the target timestep in seconds. The +script linearly interpolates between consecutive entries and writes one +processed JSON per raw file into ``data_processed/``. + +Plotting +-------- + +You can plot the processed data using the following command: + +.. code-block:: bash + + uv run python -m bam.plot \ + --actuator xl330 \ + --logdir data_processed + +Example: Dynamixel XL-330 +------------------------- + +The Dynamixel XL-330 is supported through the ``bam/dynamixel/`` +package. The physical test bench used for this motor is shown in +:doc:`setup`. + +To record a full session for a 0.567 kg weight at the tip of a 0.17 m +arm weighing 0.016 kg, the following command is used: + +.. code-block:: text + + uv run python -m bam.dynamixel.all_record \ + --port /dev/ttyUSB0 \ + --motor xl330 \ + --mass 0.567 \ + --arm-mass 0.016 \ + --length 0.17 \ + --vin 7.5 \ + --logdir data_raw + +For the XL-330, ``all_record`` automatically sweeps the five P-gain values +``[50, 100, 200, 300, 400]`` over the four trajectories, producing 20 +recordings in ``data_raw/``. By repeating the command for each mass/length +combination presented in :doc:`setup`, a complete dataset of 240 6s recordings is obtained. + +The video below shows the four trajectories being played on the XL-330 for a +single P-gain value: + +.. raw:: html + +
+ +
+ +Once all sessions are recorded, the raw data are then processed into a fixed timestep as shown +above. diff --git a/docs/identification/actuator_modeling.rst b/docs/identification/actuator_modeling.rst new file mode 100644 index 0000000..fa4ceab --- /dev/null +++ b/docs/identification/actuator_modeling.rst @@ -0,0 +1,95 @@ +Modeling the actuator +===================== + +Before you start getting some data, you need to model your actuator in BAM software. Modeling an actuator consist +in subclassing BAM's :class:`~bam.actuator.Actuator` class and implementing the required methods. The following +questions need to be addressed: + +* How is the servomotor turning the state (encoder position, velocity) into **torque** ? + * What **control law** is in play ? + * What **limits** or **smoothing** is applied in the servo firmware ? +* What important **values** need to be identified to model the actuator ? + * In particular, you might want to have some initial guess ranges for those values + +Fortunately, the answers to those questions are often the same. That is why BAM already provides few +subclasses of :class:`~bam.actuator.Actuator`: + +* :class:`~bam.actuator.VoltageControlledActuator` for actuators that are controlled by a voltage command +* :class:`~bam.actuator.CurrentControlledActuator` for actuators that are controlled by a current command + +Voltage controlled actuator +--------------------------- + +If the command produced in firmware is a voltage, then you should subclass :class:`~bam.actuator.VoltageControlledActuator`. Find examples of such subclassing in `Dynamixel's actuator.py `_. + +Here are the constructor parameters: + +* ``vin``: the supply voltage for the actuator +* ``kp``: the proportional gain of the firmware control law, we recommend keeping it in *firmware unit*, meaning that you can use the exact same value as the one documented by the vendor +* ``error_gain``: this gain should be such that ``u = error_gain * kp * (target - position)`` is the voltage command sent to the motor. It is a constant that correct the firmware unit into a voltage unit +* ``max_pwm``: the maximum PWM value (between ``0`` and ``1``) that the firmware can set the PWM to. If you don't know, you can set it to ``1``. +* ``max_current``: if the firmware has a current limit, you can set it here. If you don't know, you can set it to ``None``. + +And here are the typical model parameters you'll want to identify: + +* :math:`k_t`: the torque constant of the motor, in :math:`N.m/A` +* :math:`R`: the resistance of the motor, in :math:`\Omega` +* :math:`I`: the apparent inertia of the motor, in :math:`kg.m^2` + +See below for initial guesses and ranges for those parameters. + +Current controlled actuator +--------------------------- + +If the command produced in firmware is a current, then you should subclass :class:`~bam.actuator.CurrentControlledActuator`. Find examples of such subclassing in `Dynamixel's actuator.py `_. + +Here are the constructor parameters: + +* ``vin``: the supply voltage for the actuator +* ``kp``: the proportional gain of the firmware control law, we recommend keeping it in *firmware unit*, meaning that you can use the exact same value as the one documented by the vendor +* ``error_gain``: this gain should be such that ``i = error_gain * kp * (target - position)`` is the current command sent to the motor. + +And here are the typical model parameters you'll want to identify: + +* :math:`k_t`: the torque constant of the motor, in :math:`N.m/A` +* :math:`R`: the resistance of the motor, in :math:`\Omega` +* :math:`I`: the apparent inertia of the motor, in :math:`kg.m^2` +* :math:`I_{max}`: the maximum current that the firmware can send to the motor, in :math:`A` + +Initial guesses and ranges +--------------------------- + +When declaring a model, you need to provide initial guesses and ranges for the parameters. For example: + +.. code-block:: python + + # Torque constant [Nm/A] or [V/(rad/s)] + self.model.kt = Parameter(0.7, 0.25, 1.5) + +This tells BAM that the initial guess for :math:`k_t` is ``0.7``, and that it should be searched in the range ``[0.25, 1.5]``. + +Torque constant :math:`k_t` +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Torque constant :math:`k_t` maps current in :math:`A` to torque in :math:`N.m`. It turns out that the same constant +also maps the motor velocity in :math:`rad/s` to back-EMF voltage in :math:`V`. + +Thus, to estimate :math:`k_t`, you can apply a known voltage to the motor and measure its free speed :math:`\omega`. The guess for :math:`k_t` is then given by :math:`k_t = \frac{V}{\omega}`. + +Don't hesitate to take multiple samples and use a method like least square to estimate the torque constant more accurately. + + +Resistance :math:`R` +~~~~~~~~~~~~~~~~~~~~ + +Motor resistance is usually small value, that you will not get accurately approximated with measuring tool like +multimeter. The best way to estimate it is to measure the current draw of the motor when applying a known voltage command, while keeping the motor blocked. Then, you can use Ohm's law to estimate the resistance :math:`R = \frac{V}{I}`. + +Apparent inertia :math:`I` +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Apparent inertia is the rotor inertia of the motor, which is multiplied by the square of the gear ratio. While the reduction ratio is often known, it is not easy to guess the value of the rotor inertia. + +This value can typically be adjusted across multiple BAM fits, monitor if the value saturates either up or low during +identification and ajust the range accordingly. + diff --git a/docs/identification/contributing.rst b/docs/identification/contributing.rst new file mode 100644 index 0000000..16e0a7a --- /dev/null +++ b/docs/identification/contributing.rst @@ -0,0 +1,73 @@ +Contributing a Model +==================== + +If you have identified parameters for a motor that is not yet in the BAM +library, sharing them benefits the whole community. This page explains how +to format and submit your contribution. + +What to include +--------------- + +A contribution consists of: + +- One JSON parameter file per model variant (at minimum M6; ideally M1–M6). +- Update in this documentation, adding your actuator to the :doc:`usage/actuators` page. +- Ideally, provide raw data so that the fitting is reproducible + +Parameter file format +--------------------- + +The JSON file produced by ``uv run python -m bam.fit`` is the expected format: + +.. code-block:: json + + { + "model": "m6", + "actuator": "my_motor", + "kt": 1.23, + "R": 4.56, + "armature": 0.01, + "friction_base": 0.05, + "friction_viscous": 0.02, + "friction_stribeck": 0.08, + "dtheta_stribeck": 1.5, + "alpha": 1.2, + "load_friction_motor": 0.15, + "load_friction_external": 0.03, + "load_friction_motor_stribeck": 0.10, + "load_friction_external_stribeck": 0.04, + "load_friction_motor_quad": 0.005, + "load_friction_external_quad": 0.001, + "q_offset": 0.0 + } + +The file is generated automatically at the end of a fitting run. No manual +editing is required. + +Quality checklist +----------------- + +Before submitting, verify: + +- The identified parameters reproduce recorded trajectories with a reasonable + MAE (use ``uv run python -m bam.plot --sim`` to inspect the fit visually). +- The fit was validated on a hold-out set (``--validation_kp``) to check for + overfitting. +- All six models (M1–M6) have been fitted so users can choose the complexity + level appropriate for their use case. + +Submitting +---------- + +Open a pull request on the `BAM GitHub repository `_ +and place your parameter files under: + +.. code-block:: text + + bam/params//m1.json + bam/params//m2.json + ... + bam/params//m6.json + +Include a short description of the motor and the identification conditions in +the pull request body. diff --git a/docs/identification/fitting.rst b/docs/identification/fitting.rst new file mode 100644 index 0000000..c047edd --- /dev/null +++ b/docs/identification/fitting.rst @@ -0,0 +1,167 @@ +Fitting +======= + +The fitting step optimizes the friction model parameters so that the BAM +simulator reproduces the recorded trajectories as closely as possible. The +objective is the mean absolute error (MAE) between simulated and measured +joint positions, averaged across all logs. + +Running the fit +--------------- + +.. code-block:: text + + uv run python -m bam.fit \ + --actuator xl330 \ + --model m6 \ + --logdir data_processed \ + --output params/xl330/m6.json + +``--actuator`` must match the motor name used during recording. +``--model`` selects the friction model variant (``m1`` through ``m6``). + +The optimizer writes ``params.json`` every few seconds as it runs, so +progress can be monitored by inspecting the output file or running +``uv run python -m bam.plot`` in parallel. + +Optimization options +-------------------- + +.. list-table:: + :header-rows: 1 + :widths: 25 15 60 + + * - Argument + - Default + - Description + * - ``--method`` + - ``cmaes`` + - Optimization algorithm: ``cmaes`` (CMA-ES with BIPOP restart), + ``random``, or ``nsgaii``. + * - ``--trials`` + - 100 000 + - Number of evaluations. Increase for better convergence on complex + models (M5, M6). + * - ``--workers`` + - 1 + - Number of parallel workers. Uses a shared SQLite study database when + greater than 1. + * - ``--load-study`` + - — + - Path to an existing Optuna study to resume optimization. + * - ``--reset_period`` + - — + - Re-synchronize the simulator state to the log at this interval + [seconds]. Useful when accumulated error destabilizes long rollouts. + +Validation split +---------------- + +To detect overfitting, hold out the logs recorded at one P-gain value and +use them as a validation set: + +.. code-block:: text + + uv run python -m bam.fit \ + --actuator xl330 \ + --model m6 \ + --logdir data_processed \ + --validation_kp 8 \ + --output params/xl330/m6.json + +Logs recorded with ``--kp 8`` are excluded from training and evaluated +separately. The best validation MAE is reported alongside the training score. + +Fitting all models +------------------ + +It is recommended to fit all six models and compare their validation error: + +.. code-block:: text + + for model in m1 m2 m3 m4 m5 m6; do + uv run python -m bam.fit \ + --actuator xl330 \ + --model $model \ + --logdir data_processed \ + --validation_kp 8 \ + --output params/xl330/$model.json \ + --trials 100000 + done + +Simpler models (M1, M2) train faster; richer models (M5, M6) may need more +trials to converge but can capture directional and load-dependent effects +that simpler models miss. The model with the best validation MAE and +acceptable parameter count is typically the right choice. + +Output file +----------- + +The optimizer writes a JSON file containing the identified parameters and +metadata: + +.. code-block:: json + + { + "model": "m6", + "actuator": "xl330", + "kt": 2.21, + "R": 2.03, + "armature": 0.026, + "friction_base": 1.0e-05, + "friction_viscous": 0.051, + "friction_stribeck": 0.122, + "dtheta_stribeck": 1.75, + "alpha": 1.14, + "load_friction_motor": 0.198, + "load_friction_external": 0.022, + "load_friction_motor_stribeck": 0.199, + "load_friction_external_stribeck": 0.087, + "load_friction_motor_quad": 0.010, + "load_friction_external_quad": 7.3e-05, + "q_offset": 0.0 + } + +The file can be passed directly to :func:`bam.model.load_model` or used as +``json_path`` in :class:`bam.mjlab.BamActuatorCfg`. + +Evaluating and visualizing results +----------------------------------- + +To evaluate a parameter file on the recorded logs: + +.. code-block:: text + + uv run python -m bam.fit \ + --actuator xl330 \ + --model m6 \ + --logdir data_processed \ + --eval \ + --output params/xl330/m6.json + +To plot measured versus simulated trajectories: + +.. code-block:: text + + uv run python -m bam.plot \ + --actuator xl330 \ + --logdir data_processed \ + --sim \ + --params params/xl330/m6.json + +Several ``--params`` files can be given to overlay multiple models on the +same plot, which is useful for comparing M1 through M6 side by side. + +Weights & Biases logging +------------------------ + +Pass ``--wandb`` to stream training and validation metrics to a W&B project: + +.. code-block:: text + + uv run python -m bam.fit \ + --actuator xl330 \ + --model m6 \ + --logdir data_processed \ + --wandb \ + --output params/xl330/m6.json diff --git a/docs/identification/index.rst b/docs/identification/index.rst new file mode 100644 index 0000000..31e623d --- /dev/null +++ b/docs/identification/index.rst @@ -0,0 +1,73 @@ +Identification +============== + +This section walks through the end-to-end identification pipeline: building +the test bench, recording trajectories, and fitting a friction model. + +.. grid:: 1 2 2 2 + :gutter: 3 + :class-container: bam-getting-started + + .. grid-item-card:: 🔩 Hardware setup + :link: setup + :link-type: doc + :class-card: bam-card + + Build the pendulum test bench with variable loads and lengths to collect + a rich set of trajectories. + + +++ + :doc:`Hardware setup → ` + + .. grid-item-card:: 🧩 Modeling the actuator + :link: actuator_modeling + :link-type: doc + :class-card: bam-card + + Subclass BAM's :class:`~bam.actuator.Actuator` and implement the methods + that describe your motor. + + +++ + :doc:`Actuator modeling → ` + + .. grid-item-card:: 🎬 Data acquisition + :link: acquisition + :link-type: doc + :class-card: bam-card + + Run the actuator through predefined trajectories while logging position, + velocity, and control signals. + + +++ + :doc:`Data acquisition → ` + + .. grid-item-card:: 📉 Fitting + :link: fitting + :link-type: doc + :class-card: bam-card + + Optimize the friction model parameters so the simulator reproduces the + recorded trajectories. + + +++ + :doc:`Fitting → ` + + .. grid-item-card:: 🤝 Contributing a model + :link: contributing + :link-type: doc + :class-card: bam-card + + Identified a new motor? Share your parameters with the community and add + them to the BAM library. + + +++ + :doc:`Contributing → ` + +.. toctree:: + :hidden: + + setup + actuator_modeling + acquisition + fitting + contributing diff --git a/docs/identification/setup.rst b/docs/identification/setup.rst new file mode 100644 index 0000000..fafa0b0 --- /dev/null +++ b/docs/identification/setup.rst @@ -0,0 +1,67 @@ +Hardware Setup +============== + +BAM's identification pipeline requires a pendulum test bench with variable +loads and lengths to increase the variety of the collected logs. The pendulum +is attached directly to the actuator's output shaft, optionally using a +counter-shaft when the actuator supports it. + + To present the identification process, the Dynamixel XL-330 motor is used as + a case study. The instructions are similar for other actuators, adjusting only + for the mass and length parameters. + +Requirements +------------ + +The hardware required for the identification process is as follows: + +- **A set of rigid arms of varying lengths**. + + The mass of the arms should be negligible compared to the attached loads. + Therefore, it is recommended to use 3D-printed or laser-cut wooden arms for + smaller motors handling light loads, and to reserve metal arms for more + powerful motors. + +- **A set of weights compatible with the arms**. + + The masses must be heavy enough to generate a wide range of load torques. + +- **A mounting bracket for the actuator**. + + The setup must ensure that the actuator remains firmly secured during data + logging, despite the fast movements of the weights at the end of the arm. The + arm-weight assemblies must have sufficient clearance to oscillate between + +/- 90° relative to the vertical position. + +- **A communication interface for the actuator**. + +Once the hardware is gathered, you must record the mass of the weights as well +as the length and mass of the arms. These parameters are mandatory for the identification process. + +Example: Dynamixel XL-330 +------------------------- + +Here is an example of a test bench for the Dynamixel XL-330 actuator. +The pendulum arms are 3D printed; you can refer to the 3D model for inspiration +`here `_. + +.. image:: /_static/hardware_1.png + :width: 80% + :align: center + :alt: 3D-printed pendulum test bench for the Dynamixel XL-330 + +The interface with the actuator is established using a U2D2 kit. + +.. image:: /_static/hardware_2.png + :width: 80% + :align: center + :alt: U2D2 communication interface + +The complete setup is shown below. The pendulum is attached to the +actuator's output shaft, and the masses are fixed to the end of the +pendulum arm. + +.. image:: /_static/hardware_3.png + :width: 80% + :align: center + :alt: Complete Dynamixel XL-330 test bench setup \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000..1e23982 --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,129 @@ +Better Actuator Models (BAM) +============================ + +.. grid:: 1 2 2 2 + :gutter: 2 + + .. grid-item:: + + .. video:: https://github.com/Rhoban/bam_media/raw/refs/heads/main/videos/m1_vs_m6.mp4 + :autoplay: + :muted: + :loop: + + .. grid-item:: + + .. video:: https://github.com/Rhoban/bam_media/raw/refs/heads/main/videos/sin_sin.mp4 + :autoplay: + :muted: + :loop: + +.. admonition:: What is it? + + + BAM (for Better Actuator Models) is a library to **identify** and **use** servo-actuator models in simulation + engines. In particular, it provides **extended friction models**, which are more accurate than the default friction + models used in most simulators (e.g., MuJoCo). + + The animation above is a face-to-face comparison of the :doc:`default MuJoCo friction model (M1) ` and an :doc:`extended friction model (M6) ` identified for Dynamixel MX-64 and MX-106 servo-actuators lifting a load. + +Features +-------- + +- :doc:`Pre-identified friction models ` for common servo-actuators (e.g., XL320, XL330, MX64, MX106...) +- **API for direct integration of friction models** into :doc:`MuJoCo CPU ` and :doc:`mjlab (MuJoCo Warp) ` +- :doc:`Extended friction models `, including Stribeck effect, load-dependance +- :doc:`Identification process ` for servo-actuator with friction models + +Getting started +--------------- + +Pick the path that matches where you are: + +.. grid:: 1 1 3 3 + :gutter: 3 + :class-container: bam-getting-started + + .. grid-item-card:: 🔌 Use a bundled model + :link: usage/index + :link-type: doc + :class-card: bam-card + + Your motor is already in the :doc:`library ` + (``xl320``, ``xl330``, ``mx64``, ``mx106``, ...). + + Load its parameters and plug them into your MuJoCo or mjlab simulation + right away. + + +++ + :doc:`Usage guide → ` + + .. grid-item-card:: 🔧 Identify a new motor + :link: identification/index + :link-type: doc + :class-card: bam-card + + Your motor is not in the library yet. + + Build a simple pendulum test bench, record trajectories under varying + load, and run the identification pipeline to fit a friction model. + + +++ + :doc:`Identification process → ` + + .. grid-item-card:: 📐 Understand the theory + :link: theory/index + :link-type: doc + :class-card: bam-card + + You want to understand the modeling approach. + + Dive into the pendulum dynamics, the friction-budget formulation, and + the six model variants M1–M6. + + +++ + :doc:`Theory → ` + +Reference paper +--------------- + +This library implements the modeling and identification approach introduced +in the paper `"Extended Friction Models for the Physics Simulation of Servo +Actuators" `_ by Marc Duclusaud, Gregoire Passault, Vincent Padois, and Olivier +Ly . + +If you use BAM or the friction models provided in this library for your +scientific work, please cite the publication: + +.. code-block:: bibtex + + @inproceedings{duclusaud2025extended, + title={Extended Friction Models for the Physics Simulation of Servo Actuators}, + author={Duclusaud, Marc and Passault, Gr{\'e}goire and Padois, Vincent and Ly, Olivier}, + booktitle={2025 IEEE International Conference on Robotics and Automation (ICRA)}, + pages={12091--12097}, + year={2025}, + organization={IEEE} + } + +If you also wish to cite the software directly, you may use: + +.. code-block:: bibtex + + @software{BAM, + title = {{BAM: Better Actuator Models}}, + author = {Duclusaud, Marc and Passault, Grégoire}, + license = {Apache-2.0}, + url = {https://github.com/Rhoban/bam}, + version = {0.0.1}, + year = {2024} + } + +.. toctree:: + :maxdepth: 2 + :caption: Contents + + usage/index + identification/index + theory/index + api/index diff --git a/docs/requirements.txt b/docs/requirements.txt new file mode 100644 index 0000000..f92f913 --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1,7 @@ +sphinx>=7.2,<9 +pydata-sphinx-theme>=0.15 +sphinx-copybutton>=0.5 +sphinx-autobuild>=2024.4.16 +sphinx-autoapi>=3.0 +sphinxcontrib-video +sphinx-design>=0.6 \ No newline at end of file diff --git a/docs/theory/dc_motor.rst b/docs/theory/dc_motor.rst new file mode 100644 index 0000000..30c477a --- /dev/null +++ b/docs/theory/dc_motor.rst @@ -0,0 +1,61 @@ +DC Motor +======== + +In BAM, servomotor sources are modeled using DC motors dynamics. A DC motor turns current into torque, according +to the following equation: + +.. math:: + + \tau = k_t i + +where :math:`\tau` is the torque, :math:`k_t` is the torque constant, and :math:`i` is the current. When a motor +rotates at a velocity of :math:`\omega`, it generates a back electromotive voltage of :math:`k_t \omega`. + +As a result, if you apply a voltage :math:`V` to the motor, the current is given by: + +.. math:: + + i = \frac{V - k_t \omega}{R} + +where :math:`R` is the motor resistance. Thus, the torque generated by a DC motor is given by: + +.. math:: + + \tau = k_t \frac{V}{R} - k_t^2 \frac{\omega}{R} + +Torque vs speed limit +--------------------- + +There are typically two limits in play in a DC motor: + +* The maximum **voltage** you can apply, this is typically limited because of your battery that, for example, +can only provide 12V +* The maximum **current** you can apply, this is typically enforced by firmwares to avoid overheating the motor. + +Let's first look at the voltage limit, using above equation, we get: + +.. image:: ../_static/velocity_vs_torque.svg + :alt: Torque vs speed limit + :width: 400 + :align: center + +The feasible green area here is depicting the limit caused by **voltage** limit. Note that the torque you can apply actually depends on the current speed of the motor. At a given velocity :math:`\omega_0`, you will not even be +able to apply positive torque (this is the no-load speed of the motor). Conversely, at zero speed, there is a +maximum torque :math:`\tau_{stall}` you can apply (this is the stall torque of the motor). + +It is in general a good idea to use voltages as high as possible. Firstly because it allows to use smaller wires, +but also because it can make the voltage-related limit less restrictive. However, many servo-actuators run with +low-voltage batteries and fall into that functioning regime. + +The second limit is the **current** limit, that is typically enforced by the firmware of the motor. Since the torque +is proportional to current, this limit can be seen as a maximum torque limit :math:`\tau_{max}`: + +.. image:: ../_static/velocity_vs_torque_custom_tau_max.svg + :alt: Torque vs speed limit + :width: 400 + :align: center + +Note that this is, however a *soft* limit. The firmware will *attempt* to limit the current, but it is not guaranteed that it will always be able to do so. + +For example, if the motor rotates at a very high speed, the best strategy to provide the less torque is to provide +as much volt as possible. However, your voltage supply is limited, and you might have no choice that to provide a voltage that will generate a current above the limit. diff --git a/docs/theory/index.rst b/docs/theory/index.rst new file mode 100644 index 0000000..59b7e4e --- /dev/null +++ b/docs/theory/index.rst @@ -0,0 +1,55 @@ +Theory +====== + +This section covers the theoretical foundations behind BAM: the pendulum +bench dynamics, the friction-budget formulation used in simulation, and the +six friction model variants M1–M6. + +It follows the modeling and identification approach introduced +in the paper `"Extended Friction Models for the Physics Simulation of Servo +Actuators" `_ by Marc Duclusaud, Gregoire Passault, Vincent Padois, and Olivier +Ly . + +.. grid:: 1 2 2 2 + :gutter: 3 + :class-container: bam-getting-started + + .. grid-item-card:: 📐 Theoretical framework + :link: theory + :link-type: doc + :class-card: bam-card + + The pendulum test bench dynamics and the friction-budget formulation + used to apply BAM models in simulation. + + +++ + :doc:`Theoretical framework → ` + + .. grid-item-card:: 🧮 Friction models (M1–M6) + :link: models + :link-type: doc + :class-card: bam-card + + The six friction models of increasing expressiveness, from Coulomb + friction to load-dependent and Stribeck effects. + + +++ + :doc:`Friction models → ` + + .. grid-item-card:: ⚙️ DC motor + :link: dc_motor + :link-type: doc + :class-card: bam-card + + How servomotor sources are modeled as DC motors, turning current into + torque. + + +++ + :doc:`DC motor → ` + +.. toctree:: + :hidden: + + theory + models + dc_motor diff --git a/docs/theory/models.rst b/docs/theory/models.rst new file mode 100644 index 0000000..f843116 --- /dev/null +++ b/docs/theory/models.rst @@ -0,0 +1,183 @@ +Friction Models (M1-M6) +======================= + +BAM supports six friction models of increasing expressiveness, denoted +:math:`\mathcal{M}_1` to :math:`\mathcal{M}_6`. + +They are ordered from the simplest Coulomb-Viscous approximation to a richer +directional and quadratic formulation that better captures gearbox behavior. + +- :math:`\mathcal{M}_1`: Coulomb-Viscous +- :math:`\mathcal{M}_2`: Stribeck +- :math:`\mathcal{M}_3`: Load-dependent +- :math:`\mathcal{M}_4`: Stribeck + load-dependent +- :math:`\mathcal{M}_5`: Directional load-dependent +- :math:`\mathcal{M}_6`: Quadratic directional variant + +Notation +-------- + +In the equations below: + +- :math:`\dot{\theta}` is joint velocity +- :math:`\tau_m` is motor torque +- :math:`\tau_e` is external/load torque +- :math:`\tau_{fm}` is the maximum resistive friction torque (friction budget) + +The simulation then applies friction by clipping the stopping torque in +:math:`[-\tau_{fm},\tau_{fm}]`. + +Drive/backdrive diagrams +------------------------ + +The drive/backdrive diagrams help to visualize the effect of friction. Let's for example consider the :math:`\mathcal{M}_6` model: + +.. image:: ../_static/drive_backdrive_m6.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +Here: + +- Below the blue line, the motor is driving the system +- Above the red line, the system is backdriving the motor +- On the black dashed line, motor torque and external torque exacttly cancel out +- In the middle, the friction budget is preventing motion in the system +- The lines with less opacity depicts what happens when the system is moving (1 rad/s per step). As you can notice, the faster the system is moving, the less friction appears here. + +They can be obtained using ``bam.drive_backdrive`` command from the repository: + +.. code-block:: bash + + uv run -m bam.drive_backdrive --params bam/params/erob80_100/m1.json --max_torque 100 + +Model :math:`\mathcal{M}_1`: Coulomb-Viscous +-------------------------------------------- + +.. image:: ../_static/drive_backdrive_m1.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +.. math:: + + \mathcal{M}_1:\quad + \tau_{fm} = K_v|\dot{\theta}| + K_c + +This is the baseline model used in most physics simulators. +It keeps only viscous damping and a constant Coulomb term. + +Model :math:`\mathcal{M}_2`: Stribeck +------------------------------------- + +.. image:: ../_static/drive_backdrive_m2.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +.. math:: + + \mathcal{M}_2:\quad + \tau_{fm} = K_v|\dot{\theta}| + K_c + + \exp\left(-\left|\frac{\dot{\theta}}{\dot{\theta}_s}\right|^{\alpha}\right)K_{cs} + +This adds higher friction near zero speed and smooth transition to sliding. +It models the fact that static friction is usually stronger than sliding friction. + +Model :math:`\mathcal{M}_3`: Load-dependent +------------------------------------------- + +.. image:: ../_static/drive_backdrive_m3.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +.. math:: + + \mathcal{M}_3:\quad + \tau_{fm} = K_v|\dot{\theta}| + K_c + K_l|\tau_m - \tau_e| + +This captures the increase of friction with transmitted gearbox load. +It is useful when the apparent resistance depends on how hard the transmission is loaded. + +Model :math:`\mathcal{M}_4`: Stribeck + load-dependent +------------------------------------------------------ + +.. image:: ../_static/drive_backdrive_m4.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +.. math:: + + \mathcal{M}_4:\quad + \tau_{fm} = K_v|\dot{\theta}| + K_c + K_l|\tau_m-\tau_e| + + \exp\left(-\left|\frac{\dot{\theta}}{\dot{\theta}_s}\right|^{\alpha}\right) + \left(K_{cs} + K_{ls}|\tau_m-\tau_e|\right) + +This combines presliding dynamics with load dependence. +It adds Stribeck smoothing on top of a load-sensitive friction budget. + +Model :math:`\mathcal{M}_5`: Directional load-dependent +------------------------------------------------------- + +.. image:: ../_static/drive_backdrive_m5.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +.. math:: + + \mathcal{M}_5:\quad + \tau_{fm} = K_v|\dot{\theta}| + K_c + |K_m\tau_m - K_e\tau_e| + + \exp\left(-\left|\frac{\dot{\theta}}{\dot{\theta}_s}\right|^{\alpha}\right) + \left(K_{cs} + |K_{ms}\tau_m - K_{es}\tau_e|\right) + +This separates motor-side and external-side contributions, which helps model +directional efficiency/backdrivability asymmetry. +It is appropriate when the gearbox behaves differently depending on the torque direction. + +Model :math:`\mathcal{M}_6`: Quadratic directional +-------------------------------------------------- + +.. image:: ../_static/drive_backdrive_m6.png + :alt: Drive/backdrive diagram + :width: 500 + :align: center + +.. math:: + + \mathcal{M}_6:\quad + \tau_{fm} = K_v|\dot{\theta}| + K_c + |K_m\tau_m - K_e\tau_e| + + \exp\left(-\left|\frac{\dot{\theta}}{\dot{\theta}_s}\right|^{\alpha}\right) + \left(K_{cs} + |K_{ms}\tau_m - K_{es}\tau_e| + Q\right) + +with piecewise quadratic contribution: + +.. math:: + + Q = + \begin{cases} + K_{eq}\tau_e^2 & \text{if } |\tau_m| > |\tau_e| \\ + K_{mq}\tau_m^2 & \text{otherwise} + \end{cases} + +This is useful for actuators where harmonic-drive-like effects create nonlinear +load-friction coupling. +It extends the directional model with a quadratic load term. + +Modeling hierarchy +------------------ + +The sequence :math:`\mathcal{M}_1 \rightarrow \mathcal{M}_6` reflects increasing +expressiveness and parameter count. In practice, BAM fits all candidates and +selects the best trade-off from validation error. + +Implementation in BAM +--------------------- + +Model behaviors are implemented in :mod:`bam.model`, notably: + +- :class:`bam.model.Model` +- :func:`bam.model.load_model` +- :func:`bam.model.load_model_from_dict` diff --git a/docs/theory/theory.rst b/docs/theory/theory.rst new file mode 100644 index 0000000..5779a92 --- /dev/null +++ b/docs/theory/theory.rst @@ -0,0 +1,66 @@ +Theoretical Framework +===================== + +Pendulum test bench dynamics +---------------------------- + +For a single-axis pendulum test bench: + +.. math:: + + \tau_m + \tau_e(\theta) + \tau_f = J\ddot{\theta} + +where: + +- :math:`\theta` is the joint position +- :math:`\tau_m` is the actuator torque +- :math:`\tau_e(\theta) = mgl\sin(\theta)` is the gravity torque +- :math:`\tau_f` is the friction torque +- :math:`J` is the apparent inertia (load + actuator side) + +Friction as a torque budget +--------------------------- + +The Coulomb-Viscous form is typically written as a force opposed to velocity. +In BAM, friction is handled through a maximum available resistive torque +:math:`\tau_{fm}`. + +For a discrete simulator with time step :math:`\Delta t`, stopping at next step +requires: + +.. math:: + + \dot{\theta}_{k+1} = 0 + +which yields the stopping torque: + +.. math:: + + \tau_{f,stop} = -\left(\frac{J}{\Delta t}\dot{\theta} + \tau_m + \tau_e\right) + +The applied friction is then this stopping torque clipped to the available friction budget: + +.. math:: + + \tau_f = \mathrm{clip}(\tau_{f,stop}, -\tau_{fm}, \tau_{fm}) + +Each model :math:`\mathcal{M}_i` defines :math:`\tau_{fm}` with a different level +of expressiveness. + +Servo model + friction model coupling +------------------------------------- + +In BAM, simulation combines: + +- a servo model :math:`\mathcal{S}` that maps state and target to motor torque +- a friction model :math:`\mathcal{M}` that computes the friction budget + +At each step: + +1. compute :math:`\tau_m` from control law and motor model +2. compute :math:`\tau_{fm}` from chosen friction model +3. compute :math:`\tau_f` by clipping :math:`\tau_{f,stop}` +4. integrate acceleration from bench dynamics + +This makes the framework both identifiable from logs and directly usable in +simulation. diff --git a/docs/usage/actuators.rst b/docs/usage/actuators.rst new file mode 100644 index 0000000..629f8da --- /dev/null +++ b/docs/usage/actuators.rst @@ -0,0 +1,90 @@ +Identified actuators +==================== + +Each motor below ships with pre-identified friction parameters. Click a motor +to browse its JSON parameters, or use the **Use in MuJoCo** / **Use in mjlab** +buttons to jump to the integration guide with the code examples pre-filled for +that motor. + +.. list-table:: + :header-rows: 1 + :widths: 14 14 18 28 26 + :class: actuators-table + + * - Actuator + - Image + - Name + - Description + - Use in simulation + + * - **MX-64** + - .. image:: ../_static/actuator_mx64.png + :width: 120px + + - ``mx64`` + - ROBOTIS Dynamixel MX-64 servo-actuator + (`Parameters `__, + `model `__, + `raw data `__) + - .. raw:: html + + Use in MuJoCo + Use in mjlab + + * - **MX-106** + - .. image:: ../_static/actuator_mx106.png + :width: 120px + + - ``mx106`` + - ROBOTIS Dynamixel MX-106 servo-actuator + (`Parameters `__, + `model `__, + `raw data `__) + - .. raw:: html + + Use in MuJoCo + Use in mjlab + + * - **XL-320** + - .. image:: ../_static/actuator_xl320.png + :width: 120px + + - ``xl320`` + - ROBOTIS Dynamixel XL-320 servo-actuator + (`Parameters `__, + `model `__) + - .. raw:: html + + Use in MuJoCo + Use in mjlab + + * - **XL-330** + - .. image:: ../_static/actuator_xl330.png + :width: 120px + + - ``xl330`` + - ROBOTIS Dynamixel XL-330 servo-actuator + (`Parameters `__, + `model `__) + - .. raw:: html + + Use in MuJoCo + Use in mjlab + + * - **STS3215 (7.4V)** + - .. image:: ../_static/actuator_feetech_sts3215_7_4V.png + :width: 120px + + - ``feetech_sts3215_7_4V`` + - Feetech STS3215 servo-actuator (7.4V version) + (`Parameters `__, + `model `__, + `raw data `__) + + .. raw:: html + +

⚠ Only the m1 model was identified for this actuator.

+ - .. raw:: html + + Use in MuJoCo + Use in mjlab diff --git a/docs/usage/examples.rst b/docs/usage/examples.rst new file mode 100644 index 0000000..0b1ccc4 --- /dev/null +++ b/docs/usage/examples.rst @@ -0,0 +1,15 @@ +Examples +======== + +BAM is used in real open-source robotics projects. If you are looking for +inspiration on how to wire it into a full pipeline, these are good places to +start: + +- **mjlab (GPU)** — `MjLab Microban `_ is an + open-source project, in which the fully open-source 30 cm humanoid Microban + learns to walk. It shows how to plug :class:`bam.mjlab.BamActuatorCfg` into + a vectorized MuJoCo Warp training pipeline. +- **MuJoCo (CPU)** — `Microban `_ is + the repository containing the CAD and codebase for the Microban humanoid. + It shows how to use :class:`bam.mujoco.MujocoController` in the MuJoCo + simulation of the robot run by ``make sim``. \ No newline at end of file diff --git a/docs/usage/index.rst b/docs/usage/index.rst new file mode 100644 index 0000000..f20b065 --- /dev/null +++ b/docs/usage/index.rst @@ -0,0 +1,60 @@ +Usage +===== + +This section explains how to use identified models in simulation stacks. + +.. grid:: 1 2 2 2 + :gutter: 3 + :class-container: bam-getting-started + + .. grid-item-card:: 📇 Identified actuators + :link: actuators + :link-type: doc + :class-card: bam-card + + Browse the bundled library of pre-identified servo-actuators and the + friction parameters shipped for each of them. + + +++ + :doc:`Browse actuators → ` + + .. grid-item-card:: 🖥️ MuJoCo (CPU) + :link: mujoco_cpu + :link-type: doc + :class-card: bam-card + + Plug BAM friction models into a standard MuJoCo simulation running on + CPU with :class:`bam.mujoco.MujocoController`. + + +++ + :doc:`MuJoCo CPU guide → ` + + .. grid-item-card:: ⚡ mjlab (MuJoCo GPU) + :link: mjlab_gpu + :link-type: doc + :class-card: bam-card + + Integrate BAM into an mjlab pipeline running on GPU with MuJoCo Warp + via :class:`bam.mjlab.BamActuatorCfg`. + + +++ + :doc:`mjlab GPU guide → ` + + .. grid-item-card:: 🤖 Examples + :link: examples + :link-type: doc + :class-card: bam-card + + See BAM wired into real open-source robotics projects for end-to-end + inspiration. + + +++ + :doc:`Browse examples → ` + +.. toctree:: + :hidden: + + actuators + mujoco_cpu + mjlab_gpu + examples diff --git a/docs/usage/mjlab_gpu.rst b/docs/usage/mjlab_gpu.rst new file mode 100644 index 0000000..4f4ddbe --- /dev/null +++ b/docs/usage/mjlab_gpu.rst @@ -0,0 +1,212 @@ +mjlab (MuJoCo GPU) +================== + +This page explains how to integrate BAM friction models into an mjlab +pipeline running on GPU with MuJoCo Warp. The entry point is +:class:`bam.mjlab.BamActuatorCfg`. + +Installation +------------ + +BAM is available on PyPI. Install it with the ``mjlab`` extra, which pulls in +mjlab together with mujoco, mujoco-warp and torch: + +.. code-block:: text + + pip install better-actuator-models[mjlab] + +Or, with `uv `_: + +.. code-block:: text + + uv add "better-actuator-models[mjlab]" + +BAM is compatible with mjlab 1.3. + +Registering the BAM init event +------------------------------ + +First, be sure to register the ``bam_init`` function as a startup event in your environment: + +.. code-block:: python + + from bam.mjlab import bam_init + + # cfg is your mjlab task configuration + cfg.events["bam_init"] = EventTermCfg(func=bam_init, mode="startup") + + + +Instantiating the config +------------------------ + +:class:`~bam.mjlab.BamActuatorCfg` is a dataclass that plugs into mjlab's +actuator system. When an ``Entity`` is built, it instantiates a +:class:`~bam.mjlab.BamActuator` that runs the full BAM pipeline — voltage +control law, DC motor torque, and BAM friction budget — fully vectorized +over all parallel environments via PyTorch tensors. + +Two approaches are available, mutually exclusive: + +**Bundled motor:** + +.. code-block:: python + + from bam.mjlab import BamActuatorCfg + + actuator_cfg = BamActuatorCfg( + motor_name="{actuator}", + model="m6", + target_names_expr=(r".*",), + ) + +**Custom JSON** (parameters produced by ``bam.fit``): + +.. code-block:: python + + actuator_cfg = BamActuatorCfg( + json_path="params/my_motor/m6.json", + target_names_expr=(r".*",), + ) + +The ``target_names_expr`` field is a tuple of regex patterns that select +which actuated joints this config controls. + +- Supported bundled motors: see the :doc:`list of identified actuators `. +- Supported model variants: ``"m1"`` through ``"m6"`` (see :doc:`../theory/models`). + +Voltage and P-gain overrides +----------------------------- + +By default, the supply voltage and firmware P-gain are read from the +parameter JSON. They can be overridden at config level: + +.. code-block:: python + + actuator_cfg = BamActuatorCfg( + motor_name="{actuator}", + model="m6", + target_names_expr=(r".*",), + vin=7.5, # supply voltage [V] + kp_fw=125.0, # firmware P-gain + ) + +Domain randomization +-------------------- + +:class:`~bam.mjlab.BamActuatorCfg` supports per-environment randomization +of two physical quantities that are naturally variable across hardware units +or charge states. + +**Battery voltage** — sample a different supply voltage for each environment +at startup: + +.. code-block:: python + + actuator_cfg = BamActuatorCfg( + motor_name="{actuator}", + model="m6", + target_names_expr=(r".*",), + vin_range=(7.0, 8.0), # sampled uniformly at startup [V] + ) + +``vin_range`` takes precedence over ``vin`` when both are set. + +**Voltage drop resistance** — model battery + cable resistance with a per-env +equivalent resistor between the battery and the motors: + +.. math:: + + V_\text{eff} = V_\text{in} - R_\text{drop} \, I, + \qquad + I = \frac{1}{K_t} \sum_i |\tau_i| + +where :math:`R_\text{drop}` (``vin_drop_resistance_range``) is the combined +battery + wire resistance in ohms, and the current :math:`I` is estimated from +the actuator torques using the torque constant :math:`K_t`. Randomizing this +resistance captures variability in cable length or connector quality across +units: + +.. code-block:: python + + actuator_cfg = BamActuatorCfg( + motor_name="{actuator}", + model="m6", + target_names_expr=(r".*",), + vin_range=(7.0, 8.0), + vin_drop_resistance_range=(0.05, 0.15), # [Ohm] ~100 mOhms of wire & battery resistance + vin_min=6.0, # hard lower bound [V] + ) + +Both ranges are sampled once at initialization and held constant across +episode resets. + +.. warning:: + + The voltage drop is computed independently by each + :class:`~bam.mjlab.BamActuator` from its own joints' current draw. If several + actuator configs share the same physical battery, their currents are **not** + summed together, so the modeled drop underestimates the real one. Group all + joints powered by the same battery under a single :class:`~bam.mjlab.BamActuatorCfg` + if you need the shared-supply behavior. In the current implementation, only one model + can be used per actuator config, voltage drop will not function properly if different motors + are mixed in the same config. + +Command delay +------------- + +BAM inherits mjlab's command delay system, which models the latency between +policy output and motor response (e.g. communication bus latency, firmware +scheduling). The lag is expressed in simulation steps and can be randomized +per environment: + +.. code-block:: python + + actuator_cfg = BamActuatorCfg( + motor_name="{actuator}", + model="m6", + target_names_expr=(r".*",), + delay_min_lag=1, # always at least 1 step of delay + # = 5ms with mjlab's default 200Hz control loop + delay_max_lag=3, # up to 3 steps, randomized per env + # = 15ms with mjlab's default 200Hz control loop + ) + +Setting ``delay_min_lag == delay_max_lag`` gives a fixed, deterministic delay. +Leave both at ``0`` (default) to disable delay entirely. + +Passing the config to an Entity +-------------------------------- + +Pass the config to the ``actuator_cfgs`` argument of an mjlab ``Entity``: + +.. code-block:: python + + import mjlab + + actuator_cfg = BamActuatorCfg( + motor_name="{actuator}", + model="m6", + target_names_expr=(r".*",), + kp_fw=125, + vin_range=(7.0, 8.0), + vin_drop_resistance_range=(0.05, 0.15), # [Ohm] + vin_min=6.0, + delay_min_lag=1, + delay_max_lag=3, + ) + + entity = mjlab.Entity( + xml_path="robot.xml", + actuator_cfgs=(actuator_cfg,), + ) + +mjlab calls :meth:`~bam.mjlab.BamActuatorCfg.build` internally to create the +:class:`~bam.mjlab.BamActuator` and wire it into the simulation graph. + +API reference +------------- + +- :class:`bam.mjlab.BamActuatorCfg` +- :class:`bam.mjlab.BamActuator` +- :func:`bam.model.load_model` diff --git a/docs/usage/mujoco_cpu.rst b/docs/usage/mujoco_cpu.rst new file mode 100644 index 0000000..2dd5e21 --- /dev/null +++ b/docs/usage/mujoco_cpu.rst @@ -0,0 +1,218 @@ +MuJoCo (CPU) +============ + +This page explains how to plug BAM friction models into a standard MuJoCo +simulation running on CPU. The entry point is :class:`bam.mujoco.MujocoController`. + +Installation +------------ + +BAM is available on PyPI. Install it with the ``mujoco`` extra to pull in the +MuJoCo dependency: + +.. code-block:: text + + pip install better-actuator-models[mujoco] + +Or, with `uv `_: + +.. code-block:: text + + uv add "better-actuator-models[mujoco]" + +Overview +-------- + +At each simulation step, :class:`~bam.mujoco.MujocoController` does three things: + +1. Optionally lowers the supply voltage by a drop proportional to the previous + step's load, to model battery + cable resistance. +2. Computes the motor torque from a firmware-like P-controller — optionally + clipping it to the firmware current limit — and applies it via + ``mj_data.ctrl``. +3. Evaluates the BAM friction model and writes the result into + ``mj_model.dof_frictionloss`` and ``mj_model.dof_damping``. + +Loading a model +--------------- + +Use :func:`bam.model.load_model` to obtain a :class:`~bam.model.Model` object. +Two approaches are available. + +**Bundled motor** — the library ships identified parameters for a set of common +servos: + +.. code-block:: python + + from bam.model import load_model + + model = load_model(motor_name="{actuator}", model="m6") + +- Supported motor names: see the :doc:`list of identified actuators `. +- Supported model variants: ``"m1"`` through ``"m6"`` (see :doc:`../theory/models`). + +**Custom JSON** — parameters produced by your own identification run: + +.. code-block:: python + + model = load_model("path/to/params.json") + +XML setup +--------- + +Each actuator must be declared as a ``motor`` in the MJCF file (not +``position`` or ``velocity``). BAM overwrites ``frictionloss``, ``damping``, +and ``armature`` at runtime, so any value set in the XML will be ignored. + +.. code-block:: xml + + + + ... + + + + +Instantiating the controller +----------------------------- + +.. code-block:: python + + import mujoco + from bam.mujoco import MujocoController + + mj_model = mujoco.MjModel.from_xml_file("robot.xml") + mj_data = mujoco.MjData(mj_model) + + controller = MujocoController( + model=model, + actuator=["joint_1", ..., "joint_n"], # must match the motor name in the XML + mujoco_model=mj_model, + mujoco_data=mj_data, + ) + +The ``actuator`` argument can take a single string or a list of strings, which +allows the same motor model to drive multiple joints. Each string must +match the ``name`` attribute of the ````. + +Simulation loop +--------------- + +Inside the loop, call :meth:`~bam.mujoco.MujocoController.set_q_target` to +provide the desired joint angle, then :meth:`~bam.mujoco.MujocoController.update` +before every ``mj_step``: + +.. code-block:: python + + mujoco.mj_resetData(mj_model, mj_data) + + joint_names = ["joint_1", ..., "joint_n"] + target_angles = [...] + + while True: + for joint_name, target_angle in zip(joint_names, target_angles): + controller.set_q_target(joint_name, target_angle) + controller.update() + mujoco.mj_step(mj_model, mj_data) + +Voltage drop (optional) +----------------------- + +Real batteries and cables have an internal resistance that causes a voltage +drop under load. BAM models this as an equivalent resistor placed between the +battery and the motors: + +.. math:: + + V_\text{eff} = V_\text{in} - R_\text{drop} \, I, + \qquad + I = \frac{1}{K_t} \sum_i |\tau_i| + +where ``vin_drop_resistance`` is :math:`R_\text{drop}` (the combined battery + +wire resistance, in ohms), and the current :math:`I` is estimated from the +actuator torques using the torque constant :math:`K_t`, summed over all +controlled joints. A hard lower bound ``vin_min`` can be set to prevent the +effective voltage from collapsing under heavy load: + +.. code-block:: python + + controller = MujocoController( + model=model, + actuator=["joint_1", ..., "joint_n"], + mujoco_model=mj_model, + mujoco_data=mj_data, + vin_drop_resistance=0.1, # 100 mOhms of wire & battery resistance + vin_min=6.0, # [V] + ) + +.. warning:: + + The voltage drop is computed independently by each + :class:`~bam.mujoco.MujocoController` from its own joints' current draw. + If several controllers share the same physical battery, their currents are + **not** summed together, so the modeled drop underestimates the real one. + Group all joints powered by the same battery under a single controller if you + need the shared-supply behavior. + +Multi-actuator config file +-------------------------- + +For robots with many joints, :func:`bam.mujoco.load_config` loads a JSON +configuration file that maps each group of joints to a model: + +.. code-block:: python + + from bam.mujoco import load_config + + controllers, dof_to_controller = load_config( + path="config.json", + mujoco_model=mj_model, + mujoco_data=mj_data, + kp=125.0, + vin=7.5, + ) + +The config file has the following structure: + +.. code-block:: json + + { + "arm": { + "dofs": ["shoulder", "elbow"], + "model": { + "kt": 1.6224667906987444, + "R": 3.949433673232461, + "armature": 0.011951238325312509, + "friction_base": 0.09038677246291783, + "friction_viscous": 0.011691602145974832, + "model": "m1", + "actuator": "mx64" + }, + "error_gain": 1.0, + "max_pwm": 885 + }, + "leg": { + "dofs": ["hip", "knee", "ankle"], + "model": { + "kt": 2.1913757006745245, + "R": 2.9649903987776804, + "armature": 0.026609234235148084, + "friction_base": 0.10352026623606064, + "friction_viscous": 0.03520238029013507, + "model": "m1", + "actuator": "mx106" + }, + "error_gain": 1.0, + "max_pwm": 885 + } + } + +``controllers`` is a dict keyed by group name; ``dof_to_controller`` maps each +DOF name back to its group. + +API reference +------------- + +- :class:`bam.mujoco.MujocoController` +- :func:`bam.mujoco.load_config` +- :func:`bam.model.load_model` diff --git a/docs/watch.sh b/docs/watch.sh new file mode 100644 index 0000000..a4eb1ae --- /dev/null +++ b/docs/watch.sh @@ -0,0 +1,28 @@ +#!/bin/bash +set -euo pipefail + +cd "$(dirname "$0")" + +if ! command -v inotifywait >/dev/null 2>&1; then + echo "inotifywait not found. Install inotify-tools (e.g. sudo apt install inotify-tools)." + exit 1 +fi + +if [[ ! -x ../.venv/bin/sphinx-build ]] && ! command -v sphinx-build >/dev/null 2>&1; then + echo "sphinx-build not found. Run: uv venv ../.venv && uv pip install --python ../.venv/bin/python -r requirements.txt" + exit 1 +fi + +make html +echo "Starting watching..." +pkill -f "python3 -m http.server 8080" >/dev/null 2>&1 || true +cd _build/html +python3 -m http.server 8080 --bind 127.0.0.1 & +cd ../.. + +while true +do + inotifywait *.rst */*.rst --recursive + make html + sleep 0.5 +done diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..91dd81e --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,77 @@ +[build-system] +requires = ["setuptools>=77.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "better-actuator-models" +version = "1.0.1" +description = "BAM: Better Actuator Models is a Python library dedicated to the identification and simulation of advanced actuator models." +readme = "README.md" +requires-python = ">=3.12,<3.13" +license = "Apache-2.0" +authors = [ + { name = "Marc Duclusaud" }, + { name = "Grégoire Passault" }, +] +keywords = ["robot", "robotics", "actuator", "simulation", "friction"] +classifiers = [ + "Programming Language :: Python :: 3", + "Operating System :: OS Independent", +] +dependencies = [ + "numpy", + "colorama", +] + +[project.optional-dependencies] +# Use identified models in a MuJoCo (CPU) simulation. +mujoco = [ + "mujoco", +] +# Use identified models in MuJoCo Warp via mjlab (GPU). +mjlab = [ + "mjlab>=1.3,<1.4", + # mjlab 1.3 targets the MuJoCo Warp 3.7 era: it uses `ls_parallel` (removed in + # mujoco-warp 3.9.1) and `wp.context` (removed in warp-lang 1.15). Pin the + # simulation stack to that compatible combo, otherwise Simulation() raises + # "ls_parallel was removed" / "module 'warp' has no attribute 'context'". + "mujoco-warp>=3.7,<3.8", + "mujoco>=3.7,<3.8", + "warp-lang>=1.12,<1.13", + "scipy" +] +# Identify your own motors: hardware drivers, optimization, logging, plotting. +identification = [ + "zmq", + "protobuf>=3.20,<4.0", + "dynamixel_sdk", + "rustypot>=1.5.0", + "optuna", + "cmaes", + "wandb", + "matplotlib", + "PyQt5" +] +# Everything at once. +all = [ + "better-actuator-models[mujoco,mjlab,identification]", +] + +[project.urls] +Homepage = "https://github.com/Rhoban/bam" +Repository = "https://github.com/Rhoban/bam" + +[tool.setuptools.packages.find] +include = ["bam*"] + +[tool.setuptools.package-data] +bam = ["params/**/*.json"] + +[tool.ruff] +line-length = 88 +target-version = "py312" +extend-exclude = ["docs/_build"] + +[tool.ruff.format] +quote-style = "double" +indent-style = "space"