From dae6afdf4525545acbf1016542ba20a7b5ed4825 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 16:14:27 +0100 Subject: [PATCH 01/12] Add agimus-controller-doc Nix derivation and expose package in flake --- agimus_controller/package.xml | 1 + .../tests/resources/tiago_pro_atc.urdf | 2729 +++++++++++++++++ agimus_controller/tests/test_robot_models.py | 99 + agimus_controller_doc/README.md | 47 + .../agimus_controller_doc/__init__.py | 6 + .../agimus_controller_doc/cli.py | 32 + agimus_controller_doc/conf.py | 40 + agimus_controller_doc/default.nix | 37 + .../docs/api/ocp_croco_generic.md | 47 + agimus_controller_doc/docs/conf.py | 52 + agimus_controller_doc/docs/general/usage.md | 41 + agimus_controller_doc/docs/index.md | 15 + agimus_controller_doc/docs/requirements.txt | 4 + agimus_controller_doc/pyproject.toml | 20 + flake.nix | 1 + 15 files changed, 3171 insertions(+) create mode 100644 agimus_controller/tests/resources/tiago_pro_atc.urdf create mode 100644 agimus_controller_doc/README.md create mode 100644 agimus_controller_doc/agimus_controller_doc/__init__.py create mode 100644 agimus_controller_doc/agimus_controller_doc/cli.py create mode 100644 agimus_controller_doc/conf.py create mode 100644 agimus_controller_doc/default.nix create mode 100644 agimus_controller_doc/docs/api/ocp_croco_generic.md create mode 100644 agimus_controller_doc/docs/conf.py create mode 100644 agimus_controller_doc/docs/general/usage.md create mode 100644 agimus_controller_doc/docs/index.md create mode 100644 agimus_controller_doc/docs/requirements.txt create mode 100644 agimus_controller_doc/pyproject.toml diff --git a/agimus_controller/package.xml b/agimus_controller/package.xml index 8b7b0747..7f83c40e 100644 --- a/agimus_controller/package.xml +++ b/agimus_controller/package.xml @@ -32,6 +32,7 @@ ament_index_python franka_description + tiago_robot xacro python3-yaml diff --git a/agimus_controller/tests/resources/tiago_pro_atc.urdf b/agimus_controller/tests/resources/tiago_pro_atc.urdf new file mode 100644 index 00000000..a0a025f5 --- /dev/null +++ b/agimus_controller/tests/resources/tiago_pro_atc.urdf @@ -0,0 +1,2729 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 100.0 + base_imu + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + ~/out:=base_imu + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + 0 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1000000.0 + 1000.0 + 0.0 + 0.0 + 1 0 0 + 1.0 + 0.00 + 1 + Gazebo/Grey + + + + + + + + + + + + + + + + + + + + + + + 0 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1000000.0 + 1000.0 + 0.0 + 0.0 + 1 0 0 + 1.0 + 0.00 + 1 + Gazebo/Grey + + + + + + + + + + + + + + + + + + + + + + + 0 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1000000.0 + 1000.0 + 0.0 + 0.0 + 1 0 0 + 1.0 + 0.00 + 1 + Gazebo/Grey + + + + + + + + + + + + + + + + + + + + + + + 0 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1000000.0 + 1000.0 + 0.0 + 0.0 + 1 0 0 + 1.0 + 0.00 + 1 + Gazebo/Grey + + + Gazebo/White + + + 100000000.0 + 10.0 + 0.1 + 0.1 + 1 0 0 + 10.0 + 0.0005 + 0 + + + + + + cmd_vel:=mobile_base_controller/cmd_vel_unstamped + odom:=mobile_base_controller/odom + + 100 + 1000 + true + false + odom + base_footprint + 0.0001 + 0.0001 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ~/image_raw:=robot_face/image_raw + + 480 + 640 + + + + + + + + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/White + 0.9 + 0.9 + + + 1 + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1.4870205226991688 + 1.0122909661567112 + true + 30 + true + + B8G8R8 + 848 + 480 + + + 0.3 + 3.0 + + + gaussian + 0.0 + 0.100 + + + + head_front_camera_depth_optical_frame + 0.1 + 10.0 + + + head_front_camera/image_raw:=head_front_camera/color/image_raw + head_front_camera/camera_info:=head_front_camera/color/camera_info + + head_front_camera/image_raw/compressed:=head_front_camera/color/image_raw/compressed + + head_front_camera/image_raw/compressedDepth:=head_front_camera/color/image_raw/compressedDepth + head_front_camera/image_raw/theora:=head_front_camera/color/image_raw/theora + head_front_camera/points:=head_front_camera/depth/color/points + head_front_camera/depth/image_raw:=head_front_camera/depth/image_rect_raw + + head_front_camera/depth/image_raw/compressed:=head_front_camera/depth/image_rect_raw/compressed + + head_front_camera/depth/image_raw/compressedDepth:=head_front_camera/depth/image_rect_raw/compressedDepth + + head_front_camera/depth/image_raw/theora:=head_front_camera/depth/image_rect_raw/theora + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + best_effort + volatile + keep_last + automatic + + + + + + + + 1 + 200 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + head_front_camera_imu_optical_frame + + + ~/out:=head_front_camera/imu + + + + best_effort + volatile + keep_last + automatic + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/White + 0.9 + 0.9 + + + Gazebo/White + 0.9 + 0.9 + + + Gazebo/White + 0.9 + 0.9 + + + Gazebo/White + 0.9 + 0.9 + + + Gazebo/White + 0.9 + 0.9 + + + Gazebo/White + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + + gripper_finger_joint + gripper_finger_right_joint + 1.0 + 0.0 + + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + Gazebo/DarkGrey + 0.9 + 0.9 + + + 1 + + + + robot_control/RobotControl + can + + + + + + + transmission_interface/SimpleTransmission + + + 1.0 + + + + + + + + transmission_interface/SimpleTransmission + + + 1.0 + + + + + + + + transmission_interface/SimpleTransmission + + + 1.0 + + + + + + + + transmission_interface/SimpleTransmission + + + 1.0 + + + + + + robot_control/RobotControl + dynamixel + + + + + + + + + transmission_interface/SimpleTransmission + + + 0.0 + 1 + + + + + + + + + + transmission_interface/SimpleTransmission + + + 0.0 + 1 + + + + + + robot_control/RobotControl + ethercat + true + true + true + true + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 121.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 121.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + 0.07938 + + + + + + gripper_left_finger_joint + -8.28 + + + gripper_left_finger_joint + -8.28 + + + gripper_left_finger_joint + 8.28 + + + gripper_left_finger_joint + 0.22 + + + gripper_left_finger_joint + -8.28 + + + gripper_left_finger_joint + -8.28 + + + gripper_left_finger_joint + 8.28 + + + pal_pro_gripper_transmissions/PalGripperTransmission + + + 0.001343799 + + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 121.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 121.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + + + + + + + + + sea_transmissions/SeaSimpleTransmission + + + 0.0 + 101.0 + + sea_data + + + + + + + 0.15 + + + + + + transmission_interface/SimpleTransmission + + + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + + 1.0 + 0.0 + + + + diff --git a/agimus_controller/tests/test_robot_models.py b/agimus_controller/tests/test_robot_models.py index 1d39a444..6a497fef 100644 --- a/agimus_controller/tests/test_robot_models.py +++ b/agimus_controller/tests/test_robot_models.py @@ -453,5 +453,104 @@ def test_franka_description_collision_models(self): self.assertEqual(geom_obj_type, geom_obj_type_test) +class TestRobotModelsAgainstTiagoProDescription(unittest.TestCase): + @classmethod + def setUpClass(cls): + """ + This method sets up the shared environment for all test cases in the class. + """ + # Load the example robot model using example robot data to get the URDF path. + tiago_pro_description_path = Path( + get_package_share_directory("tiago_pro_description") + ) + # srdf_path = tiago_pro_description_path / "robots" / "tiago_pro" / "tiago_pro.srdf" + # with open(srdf_path, "r") as srdf_file: + # srdf_xml = srdf_file.read() + srdf_xml = "" + # robot_xacro_path = str( + # tiago_pro_description_path + # / "robots" + # / "tiago_pro" + # / "tiago_pro.urdf.xacro", + # ) + env_xacro_path = Path(__file__).parent / "resources" / "environment.xacro" + params_path = str( + Path(__file__).parent / "resources" / "agimus_controller_params.yaml" + ) + with open(params_path, "r") as file: + mpc_params = yaml.safe_load(file)["agimus_controller_node"][ + "ros__parameters" + ] + # robot_urdf_xml = xacro.process_file( + # robot_xacro_path, + # mappings={ + # # no-laser, sick-571 + # "laser_model": "sick-571", + # # tiago-pro, no-arm + # "arm_type_left": "tiago-pro", + # # tiago-pro, no-arm + # "arm_type_right": "tiago-pro", + # # straight-wrist, spherical-wrist + # "wrist_model_left": "spherical-wrist", + # # straight-wrist, spherical-wrist + # "wrist_model_right": "spherical-wrist", + # # False, True + # "tool_changer_right": "True", + # # False, True + # "tool_changer_left": "True", + # # pal-pro-gripper custom + # "end_effector_left": "pal-pro-gripper", + # # pal-pro-gripper, custom + # "end_effector_right": "pal-pro-gripper", + # # no-ft-sensor, rokubi + # "ft_sensor_left": "no-ft-sensor", + # # no-ft-sensor, rokubi + # "ft_sensor_right": "no-ft-sensor", + # # realsense-d435 + # "camera_model": "realsense-d435", + # }, + # ).toxml() + env_urdf_xml = xacro.process_file(env_xacro_path).toxml() + # Hack for the moving joint name + with open( + Path(__file__).parent / "resources" / "tiago_pro_atc.urdf", "r" + ) as urdf_file: + robot_urdf_xml = urdf_file.read() + model = pin.buildModelFromXML(robot_urdf_xml) + cls.locked_joint_names = [ + jn + for jn in model.names + if jn not in ["universe"] + mpc_params["moving_joint_names"] + ] + cls.params = RobotModelParameters( + q0=np.zeros(model.nq), + free_flyer=False, + moving_joint_names=mpc_params["moving_joint_names"], + robot_urdf=robot_urdf_xml, + env_urdf=env_urdf_xml, + srdf=srdf_xml, + collision_as_capsule=True, + self_collision=True, + urdf_meshes_dir=tiago_pro_description_path, + armature=np.array(mpc_params["ocp"]["armature"]), + ) + + def setUp(self): + """ + This method ensures that a fresh RobotModelParameters and RobotModels instance + are created for each test case. + """ + self.params = deepcopy(self.params) + self.robot_models = RobotModels(self.params) + + def test_rnea(self): + """Checking that the RNEA method works.""" + q = np.zeros(self.robot_models.robot_model.nq) + v = np.zeros(self.robot_models.robot_model.nv) + a = np.zeros(self.robot_models.robot_model.nv) + robot_data = self.robot_models.robot_model.createData() + pin.rnea(self.robot_models.robot_model, robot_data, q, v, a) + + if __name__ == "__main__": unittest.main() diff --git a/agimus_controller_doc/README.md b/agimus_controller_doc/README.md new file mode 100644 index 00000000..cabef64a --- /dev/null +++ b/agimus_controller_doc/README.md @@ -0,0 +1,47 @@ +## Building the docs + +1. Create a virtualenv and install requirements: + +```bash +python -m venv .venv +source .venv/bin/activate +pip install -r requirements.txt +``` + +2. Build HTML: + +```bash +make html +# Output is in _build/html +``` + +## Hosting + +### Push to ReadTheDocs + +By connecting this repository; RTD will run `sphinx-build` automatically. +This repository now packages the Sphinx sources under the `docs/` directory. + +To build the docs locally you have several options. + +### Using Nix (recommended for reproducible builds): + +```bash +# enter a reproducible shell with Sphinx and extensions available +nix build .#packages.x86_64-linux.agimus-controller-doc +``` + +Then using your favourite web-browser you can visualize the doc: +```bash +firefox result/share/doc/agimus-controller-doc/html +``` + +### Using Poetry (optional): + +```bash +poetry install +poetry run agimus-docs-build +# or use the Makefile inside docs/: cd docs && make html +``` + +If you don't use Poetry or Nix, install the requirements in `docs/requirements.txt` and run `sphinx-build` manually. diff --git a/agimus_controller_doc/agimus_controller_doc/__init__.py b/agimus_controller_doc/agimus_controller_doc/__init__.py new file mode 100644 index 00000000..fe57f3c8 --- /dev/null +++ b/agimus_controller_doc/agimus_controller_doc/__init__.py @@ -0,0 +1,6 @@ +"""agimus_controller_doc package + +Provides a tiny CLI helper to build the bundled Sphinx documentation. +""" + +__all__ = ["cli"] diff --git a/agimus_controller_doc/agimus_controller_doc/cli.py b/agimus_controller_doc/agimus_controller_doc/cli.py new file mode 100644 index 00000000..9359c073 --- /dev/null +++ b/agimus_controller_doc/agimus_controller_doc/cli.py @@ -0,0 +1,32 @@ +import sys +from pathlib import Path +from sphinx.cmd.build import build_main + + +def build(argv: list | None = None) -> int: + """Build the documentation. + + Usage (after `poetry install`): + poetry run agimus-docs-build + + This will run Sphinx to build HTML in `docs/_build/html`. + """ + project_root = Path(__file__).resolve().parents[1] + docs_src = project_root / "docs" + build_dir = docs_src / "_build" / "html" + + if argv is None: + argv = ["-b", "html", str(docs_src), str(build_dir)] + + # Ensure docs source exists + if not docs_src.exists(): + print(f"Docs source not found at {docs_src}", file=sys.stderr) + return 2 + + print(f"Building docs from {docs_src} into {build_dir}") + # Call sphinx build + return build_main(argv) + + +if __name__ == "__main__": + sys.exit(build()) diff --git a/agimus_controller_doc/conf.py b/agimus_controller_doc/conf.py new file mode 100644 index 00000000..94defbec --- /dev/null +++ b/agimus_controller_doc/conf.py @@ -0,0 +1,40 @@ +import os +import sys +from datetime import datetime + +# Add project root (package parent) to sys.path so autodoc can import package +sys.path.insert(0, os.path.abspath("..")) + +project = "agimus_controller" +author = "agimus_controller contributors" +copyright = f"{datetime.now().year}, {author}" + +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.napoleon", + "sphinx.ext.autosummary", + "myst_parser", + "sphinx_autodoc_typehints", + "sphinx.ext.intersphinx", + "sphinx.ext.viewcode", +] + +autosummary_generate = True +autodoc_typehints = "description" +autodoc_default_options = { + "members": True, + "undoc-members": False, + "show-inheritance": True, +} + +templates_path = ["_templates"] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +html_theme = "sphinx_rtd_theme" +html_static_path = ["_static"] + +# Intersphinx: link to Python and NumPy docs +intersphinx_mapping = { + "python": ("https://docs.python.org/3", None), + "numpy": ("https://numpy.org/doc/stable/", None), +} diff --git a/agimus_controller_doc/default.nix b/agimus_controller_doc/default.nix new file mode 100644 index 00000000..e2efdc9a --- /dev/null +++ b/agimus_controller_doc/default.nix @@ -0,0 +1,37 @@ +{ + stdenv, + python3Packages, + lib, + fetchFromGitHub ? null, + ... +}: + +stdenv.mkDerivation rec { + pname = "agimus-controller-doc"; + version = "0.1.0"; + + src = ./.; + + buildInputs = with python3Packages; [ + sphinx + sphinx-rtd-theme + myst-parser + sphinx-autodoc-typehints + ]; + + buildPhase = '' + mkdir -p _build/html + sphinx-build -b html ${src}/docs _build/html + ''; + + installPhase = '' + mkdir -p $out/share/doc/${pname} + cp -r _build/html $out/share/doc/${pname}/html + ''; + + meta = with lib; { + description = "Sphinx HTML documentation for agimus_controller"; + license = licenses.unfree; # docs only + platforms = platforms.unix; + }; +} diff --git a/agimus_controller_doc/docs/api/ocp_croco_generic.md b/agimus_controller_doc/docs/api/ocp_croco_generic.md new file mode 100644 index 00000000..e2b6aac1 --- /dev/null +++ b/agimus_controller_doc/docs/api/ocp_croco_generic.md @@ -0,0 +1,47 @@ +--- +title: OCP Crocoddyl Generic API +--- + +# `agimus_controller.ocp.ocp_croco_generic` + +This page exposes the API of the `ocp_croco_generic` module. + +```{automodule} agimus_controller.ocp.ocp_croco_generic +:members: +:undoc-members: +:show-inheritance: +``` + +## Costs — quick index + +Below are the main cost and residual classes implemented in this module. Click the class names above to open full API docs. + +```{autosummary} +ActivationModelWeightedQuad +ActivationModelExp +ActivationModelQuadExp +ResidualModelState +ResidualModelControl +ResidualModelControlGrav +ResidualModelFramePlacement +ResidualModelFrameTranslation +ResidualModelFrameRotation +ResidualModelFrameVelocity +ResidualDistanceCollision +ResidualDistanceCollision2 +CostModelResidual +CostModelSumItem +``` + +## Example usage + +```python +from agimus_controller.ocp.ocp_croco_generic import OCPCrocoGeneric, ResidualModelFramePlacement + +# Load an OCP from YAML and inspect available cost names +ocp = OCPCrocoGeneric(...) +print(ocp._data.running_model.differential.costs) + +# Programmatically create a Placement residual +# residual = ResidualModelFramePlacement(id="tool0") +``` diff --git a/agimus_controller_doc/docs/conf.py b/agimus_controller_doc/docs/conf.py new file mode 100644 index 00000000..968087f4 --- /dev/null +++ b/agimus_controller_doc/docs/conf.py @@ -0,0 +1,52 @@ +import os +import sys +from datetime import datetime + +# Compute and add repository root to sys.path so autodoc can import +# the `agimus_controller` package regardless of current working dir. +ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")) +sys.path.insert(0, ROOT) + +# When building docs in CI or on developer machines the environment may not +# have heavy C++-backed dependencies like `crocoddyl`, `pinocchio` or +# `colmpc`. Mock them so Sphinx can import the Python modules and +# autosummary/autodoc can generate API pages. +autodoc_mock_imports = [ + "crocoddyl", + "pinocchio", + "colmpc", +] + +project = "agimus_controller" +author = "agimus_controller contributors" +copyright = f"{datetime.now().year}, {author}" + +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.napoleon", + "sphinx.ext.autosummary", + "myst_parser", + "sphinx_autodoc_typehints", + "sphinx.ext.intersphinx", + "sphinx.ext.viewcode", +] + +autosummary_generate = True +autodoc_typehints = "description" +autodoc_default_options = { + "members": True, + "undoc-members": False, + "show-inheritance": True, +} + +templates_path = ["_templates"] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +html_theme = "sphinx_rtd_theme" +html_static_path = ["_static"] + +# Intersphinx: link to Python and NumPy docs +intersphinx_mapping = { + "python": ("https://docs.python.org/3", None), + "numpy": ("https://numpy.org/doc/stable/", None), +} diff --git a/agimus_controller_doc/docs/general/usage.md b/agimus_controller_doc/docs/general/usage.md new file mode 100644 index 00000000..1622f47e --- /dev/null +++ b/agimus_controller_doc/docs/general/usage.md @@ -0,0 +1,41 @@ +# Usage and General Docs + +This section provides a short overview of how to use `agimus_controller` and links to the API reference. + +## Goals + +- Explain available OCP building blocks +- Show example YAML usage for `OCPCrocoGeneric` + +## Costs overview + +The main costs and residual types are implemented in `agimus_controller.ocp.ocp_croco_generic` and include: + +- Activation models: `ActivationModelWeightedQuad`, `ActivationModelExp` (and legacy `ActivationModelQuadExp`) +- Residuals for state and control: `ResidualModelState`, `ResidualModelControl`, `ResidualModelControlGrav` +- Frame residuals: `ResidualModelFramePlacement`, `ResidualModelFrameTranslation`, `ResidualModelFrameRotation`, `ResidualModelFrameVelocity` +- Collision residuals: `ResidualDistanceCollision`, `ResidualDistanceCollision2` + +See the API for details and constructor arguments: :doc:`../api/ocp_croco_generic`. + +## Example (YAML snippet) + +```yaml +running_model: + differential: + class: DifferentialActionModelFreeFwdDynamics + costs: + - name: state + cost: + class: CostModelResidual + residual: + class: ResidualModelState + activation: + class: ActivationModelWeightedQuad + weights: 1.0 + +terminal_model: + differential: + class: DifferentialActionModelFreeFwdDynamics + costs: [] +``` diff --git a/agimus_controller_doc/docs/index.md b/agimus_controller_doc/docs/index.md new file mode 100644 index 00000000..bd2f5c7f --- /dev/null +++ b/agimus_controller_doc/docs/index.md @@ -0,0 +1,15 @@ +--- +title: agimus_controller documentation +--- + +# agimus_controller + +```{toctree} +:maxdepth: 2 + +general/usage +api/ocp_croco_generic + +``` + +This documentation contains usage guides and API reference for the `agimus_controller` package. diff --git a/agimus_controller_doc/docs/requirements.txt b/agimus_controller_doc/docs/requirements.txt new file mode 100644 index 00000000..c20d2ee6 --- /dev/null +++ b/agimus_controller_doc/docs/requirements.txt @@ -0,0 +1,4 @@ +Sphinx>=4.0 +sphinx-rtd-theme +myst-parser +sphinx-autodoc-typehints diff --git a/agimus_controller_doc/pyproject.toml b/agimus_controller_doc/pyproject.toml new file mode 100644 index 00000000..d7530590 --- /dev/null +++ b/agimus_controller_doc/pyproject.toml @@ -0,0 +1,20 @@ +[tool.poetry] +name = "agimus-controller-doc" +version = "0.1.0" +description = "Documentation package for agimus_controller (Sphinx sources and helpers)" +authors = ["agimus_controller contributors "] +readme = "README.md" + +[tool.poetry.dependencies] +python = ">=3.8,<4.0" +sphinx = "^6.0" +sphinx-rtd-theme = "*" +myst-parser = "*" +sphinx-autodoc-typehints = "*" + +[tool.poetry.scripts] +agimus-docs-build = "agimus_controller_doc.cli:build" + +[build-system] +requires = ["poetry-core>=1.0.0"] +build-backend = "poetry.core.masonry.api" diff --git a/flake.nix b/flake.nix index 5de9fb89..49f022c0 100644 --- a/flake.nix +++ b/flake.nix @@ -123,6 +123,7 @@ default = self'.packages.agimus-controller; agimus-controller = pkgs.python3Packages.agimus-controller; agimus-controller-examples = pkgs.python3Packages.agimus-controller-examples; + agimus-controller-doc = pkgs.callPackage ./agimus_controller_doc/default.nix { }; } // lib.optionalAttrs pkgs.stdenv.hostPlatform.isLinux { ros-humble-agimus-controller-ros = pkgs.rosPackages.humble.agimus-controller-ros; From 78412c640cf9fd8603ad03ca2247b8521186c726 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 16:15:52 +0100 Subject: [PATCH 02/12] Fix autosummary entries to use fully-qualified names --- .../docs/api/ocp_croco_generic.md | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/agimus_controller_doc/docs/api/ocp_croco_generic.md b/agimus_controller_doc/docs/api/ocp_croco_generic.md index e2b6aac1..06efd432 100644 --- a/agimus_controller_doc/docs/api/ocp_croco_generic.md +++ b/agimus_controller_doc/docs/api/ocp_croco_generic.md @@ -17,20 +17,20 @@ This page exposes the API of the `ocp_croco_generic` module. Below are the main cost and residual classes implemented in this module. Click the class names above to open full API docs. ```{autosummary} -ActivationModelWeightedQuad -ActivationModelExp -ActivationModelQuadExp -ResidualModelState -ResidualModelControl -ResidualModelControlGrav -ResidualModelFramePlacement -ResidualModelFrameTranslation -ResidualModelFrameRotation -ResidualModelFrameVelocity -ResidualDistanceCollision -ResidualDistanceCollision2 -CostModelResidual -CostModelSumItem +agimus_controller.ocp.ocp_croco_generic.ActivationModelWeightedQuad +agimus_controller.ocp.ocp_croco_generic.ActivationModelExp +agimus_controller.ocp.ocp_croco_generic.ActivationModelQuadExp +agimus_controller.ocp.ocp_croco_generic.ResidualModelState +agimus_controller.ocp.ocp_croco_generic.ResidualModelControl +agimus_controller.ocp.ocp_croco_generic.ResidualModelControlGrav +agimus_controller.ocp.ocp_croco_generic.ResidualModelFramePlacement +agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameTranslation +agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameRotation +agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameVelocity +agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision +agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision2 +agimus_controller.ocp.ocp_croco_generic.CostModelResidual +agimus_controller.ocp.ocp_croco_generic.CostModelSumItem ``` ## Example usage From 8711cfe8713966d99b5d1456fe2dc828c047cbda Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 16:23:56 +0100 Subject: [PATCH 03/12] Add per-class API pages to satisfy autosummary links --- agimus_controller_doc/docs/api/ActivationModelExp.md | 7 +++++++ agimus_controller_doc/docs/api/ActivationModelQuadExp.md | 7 +++++++ .../docs/api/ActivationModelWeightedQuad.md | 7 +++++++ agimus_controller_doc/docs/api/CostModelResidual.md | 7 +++++++ agimus_controller_doc/docs/api/CostModelSumItem.md | 7 +++++++ .../docs/api/ResidualDistanceCollision.md | 7 +++++++ .../docs/api/ResidualDistanceCollision2.md | 7 +++++++ agimus_controller_doc/docs/api/ResidualModelControl.md | 7 +++++++ agimus_controller_doc/docs/api/ResidualModelControlGrav.md | 7 +++++++ .../docs/api/ResidualModelFramePlacement.md | 7 +++++++ .../docs/api/ResidualModelFrameRotation.md | 7 +++++++ .../docs/api/ResidualModelFrameTranslation.md | 7 +++++++ .../docs/api/ResidualModelFrameVelocity.md | 7 +++++++ agimus_controller_doc/docs/api/ResidualModelState.md | 7 +++++++ 14 files changed, 98 insertions(+) create mode 100644 agimus_controller_doc/docs/api/ActivationModelExp.md create mode 100644 agimus_controller_doc/docs/api/ActivationModelQuadExp.md create mode 100644 agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md create mode 100644 agimus_controller_doc/docs/api/CostModelResidual.md create mode 100644 agimus_controller_doc/docs/api/CostModelSumItem.md create mode 100644 agimus_controller_doc/docs/api/ResidualDistanceCollision.md create mode 100644 agimus_controller_doc/docs/api/ResidualDistanceCollision2.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelControl.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelControlGrav.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelFramePlacement.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelFrameRotation.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md create mode 100644 agimus_controller_doc/docs/api/ResidualModelState.md diff --git a/agimus_controller_doc/docs/api/ActivationModelExp.md b/agimus_controller_doc/docs/api/ActivationModelExp.md new file mode 100644 index 00000000..f65e308e --- /dev/null +++ b/agimus_controller_doc/docs/api/ActivationModelExp.md @@ -0,0 +1,7 @@ +# ActivationModelExp + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ActivationModelExp +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ActivationModelQuadExp.md b/agimus_controller_doc/docs/api/ActivationModelQuadExp.md new file mode 100644 index 00000000..22cff4f3 --- /dev/null +++ b/agimus_controller_doc/docs/api/ActivationModelQuadExp.md @@ -0,0 +1,7 @@ +# ActivationModelQuadExp + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ActivationModelQuadExp +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md b/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md new file mode 100644 index 00000000..8ef43481 --- /dev/null +++ b/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md @@ -0,0 +1,7 @@ +# ActivationModelWeightedQuad + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ActivationModelWeightedQuad +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/CostModelResidual.md b/agimus_controller_doc/docs/api/CostModelResidual.md new file mode 100644 index 00000000..67b4bf31 --- /dev/null +++ b/agimus_controller_doc/docs/api/CostModelResidual.md @@ -0,0 +1,7 @@ +# CostModelResidual + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.CostModelResidual +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/CostModelSumItem.md b/agimus_controller_doc/docs/api/CostModelSumItem.md new file mode 100644 index 00000000..07a32b01 --- /dev/null +++ b/agimus_controller_doc/docs/api/CostModelSumItem.md @@ -0,0 +1,7 @@ +# CostModelSumItem + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.CostModelSumItem +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualDistanceCollision.md b/agimus_controller_doc/docs/api/ResidualDistanceCollision.md new file mode 100644 index 00000000..8f297c5a --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualDistanceCollision.md @@ -0,0 +1,7 @@ +# ResidualDistanceCollision + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md b/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md new file mode 100644 index 00000000..8d1c7b10 --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md @@ -0,0 +1,7 @@ +# ResidualDistanceCollision2 + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision2 +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelControl.md b/agimus_controller_doc/docs/api/ResidualModelControl.md new file mode 100644 index 00000000..30639bdd --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelControl.md @@ -0,0 +1,7 @@ +# ResidualModelControl + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelControl +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelControlGrav.md b/agimus_controller_doc/docs/api/ResidualModelControlGrav.md new file mode 100644 index 00000000..eabd6db7 --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelControlGrav.md @@ -0,0 +1,7 @@ +# ResidualModelControlGrav + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelControlGrav +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md b/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md new file mode 100644 index 00000000..5d45a00e --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md @@ -0,0 +1,7 @@ +# ResidualModelFramePlacement + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFramePlacement +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md b/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md new file mode 100644 index 00000000..296293d9 --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md @@ -0,0 +1,7 @@ +# ResidualModelFrameRotation + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameRotation +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md b/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md new file mode 100644 index 00000000..ff646614 --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md @@ -0,0 +1,7 @@ +# ResidualModelFrameTranslation + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameTranslation +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md b/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md new file mode 100644 index 00000000..d42a061f --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md @@ -0,0 +1,7 @@ +# ResidualModelFrameVelocity + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameVelocity +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/api/ResidualModelState.md b/agimus_controller_doc/docs/api/ResidualModelState.md new file mode 100644 index 00000000..572aad27 --- /dev/null +++ b/agimus_controller_doc/docs/api/ResidualModelState.md @@ -0,0 +1,7 @@ +# ResidualModelState + +```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelState +:members: +:undoc-members: +:show-inheritance: +``` From 9a68351e5bdab93a6c22ae3e56b9cb8a6a510292 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 16:27:45 +0100 Subject: [PATCH 04/12] Replace autosummary with explicit per-class links for ocp_croco_generic --- .../docs/api/ocp_croco_generic.md | 34 +++++++++---------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/agimus_controller_doc/docs/api/ocp_croco_generic.md b/agimus_controller_doc/docs/api/ocp_croco_generic.md index 06efd432..d4dea6a6 100644 --- a/agimus_controller_doc/docs/api/ocp_croco_generic.md +++ b/agimus_controller_doc/docs/api/ocp_croco_generic.md @@ -14,24 +14,22 @@ This page exposes the API of the `ocp_croco_generic` module. ## Costs — quick index -Below are the main cost and residual classes implemented in this module. Click the class names above to open full API docs. - -```{autosummary} -agimus_controller.ocp.ocp_croco_generic.ActivationModelWeightedQuad -agimus_controller.ocp.ocp_croco_generic.ActivationModelExp -agimus_controller.ocp.ocp_croco_generic.ActivationModelQuadExp -agimus_controller.ocp.ocp_croco_generic.ResidualModelState -agimus_controller.ocp.ocp_croco_generic.ResidualModelControl -agimus_controller.ocp.ocp_croco_generic.ResidualModelControlGrav -agimus_controller.ocp.ocp_croco_generic.ResidualModelFramePlacement -agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameTranslation -agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameRotation -agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameVelocity -agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision -agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision2 -agimus_controller.ocp.ocp_croco_generic.CostModelResidual -agimus_controller.ocp.ocp_croco_generic.CostModelSumItem -``` +Below are the main cost and residual classes implemented in this module. Click the class names to open full API docs. + +- [ActivationModelWeightedQuad](ActivationModelWeightedQuad.md) +- [ActivationModelExp](ActivationModelExp.md) +- [ActivationModelQuadExp](ActivationModelQuadExp.md) +- [ResidualModelState](ResidualModelState.md) +- [ResidualModelControl](ResidualModelControl.md) +- [ResidualModelControlGrav](ResidualModelControlGrav.md) +- [ResidualModelFramePlacement](ResidualModelFramePlacement.md) +- [ResidualModelFrameTranslation](ResidualModelFrameTranslation.md) +- [ResidualModelFrameRotation](ResidualModelFrameRotation.md) +- [ResidualModelFrameVelocity](ResidualModelFrameVelocity.md) +- [ResidualDistanceCollision](ResidualDistanceCollision.md) +- [ResidualDistanceCollision2](ResidualDistanceCollision2.md) +- [CostModelResidual](CostModelResidual.md) +- [CostModelSumItem](CostModelSumItem.md) ## Example usage From b621bda2efa8b3070a56b52b16c099be32a81258 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 16:38:50 +0100 Subject: [PATCH 05/12] Revert missed commits --- .../tests/resources/tiago_pro_atc.urdf | 2729 ----------------- agimus_controller/tests/test_robot_models.py | 99 - 2 files changed, 2828 deletions(-) delete mode 100644 agimus_controller/tests/resources/tiago_pro_atc.urdf diff --git a/agimus_controller/tests/resources/tiago_pro_atc.urdf b/agimus_controller/tests/resources/tiago_pro_atc.urdf deleted file mode 100644 index a0a025f5..00000000 --- a/agimus_controller/tests/resources/tiago_pro_atc.urdf +++ /dev/null @@ -1,2729 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - 100.0 - base_imu - - - gaussian - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - ~/out:=base_imu - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - 0 - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 1000.0 - 0.0 - 0.0 - 1 0 0 - 1.0 - 0.00 - 1 - Gazebo/Grey - - - - - - - - - - - - - - - - - - - - - - - 0 - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 1000.0 - 0.0 - 0.0 - 1 0 0 - 1.0 - 0.00 - 1 - Gazebo/Grey - - - - - - - - - - - - - - - - - - - - - - - 0 - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 1000.0 - 0.0 - 0.0 - 1 0 0 - 1.0 - 0.00 - 1 - Gazebo/Grey - - - - - - - - - - - - - - - - - - - - - - - 0 - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1000000.0 - 1000.0 - 0.0 - 0.0 - 1 0 0 - 1.0 - 0.00 - 1 - Gazebo/Grey - - - Gazebo/White - - - 100000000.0 - 10.0 - 0.1 - 0.1 - 1 0 0 - 10.0 - 0.0005 - 0 - - - - - - cmd_vel:=mobile_base_controller/cmd_vel_unstamped - odom:=mobile_base_controller/odom - - 100 - 1000 - true - false - odom - base_footprint - 0.0001 - 0.0001 - 0.01 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ~/image_raw:=robot_face/image_raw - - 480 - 640 - - - - - - - - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/White - 0.9 - 0.9 - - - 1 - - - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.4870205226991688 - 1.0122909661567112 - true - 30 - true - - B8G8R8 - 848 - 480 - - - 0.3 - 3.0 - - - gaussian - 0.0 - 0.100 - - - - head_front_camera_depth_optical_frame - 0.1 - 10.0 - - - head_front_camera/image_raw:=head_front_camera/color/image_raw - head_front_camera/camera_info:=head_front_camera/color/camera_info - - head_front_camera/image_raw/compressed:=head_front_camera/color/image_raw/compressed - - head_front_camera/image_raw/compressedDepth:=head_front_camera/color/image_raw/compressedDepth - head_front_camera/image_raw/theora:=head_front_camera/color/image_raw/theora - head_front_camera/points:=head_front_camera/depth/color/points - head_front_camera/depth/image_raw:=head_front_camera/depth/image_rect_raw - - head_front_camera/depth/image_raw/compressed:=head_front_camera/depth/image_rect_raw/compressed - - head_front_camera/depth/image_raw/compressedDepth:=head_front_camera/depth/image_rect_raw/compressedDepth - - head_front_camera/depth/image_raw/theora:=head_front_camera/depth/image_rect_raw/theora - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - best_effort - volatile - keep_last - automatic - - - - - - - - 1 - 200 - - - gaussian - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - head_front_camera_imu_optical_frame - - - ~/out:=head_front_camera/imu - - - - best_effort - volatile - keep_last - automatic - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - 1 - - - 1 - - - 1 - - - 1 - - - 1 - - - 1 - - - 1 - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/White - 0.9 - 0.9 - - - Gazebo/White - 0.9 - 0.9 - - - Gazebo/White - 0.9 - 0.9 - - - Gazebo/White - 0.9 - 0.9 - - - Gazebo/White - 0.9 - 0.9 - - - Gazebo/White - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - - gripper_finger_joint - gripper_finger_right_joint - 1.0 - 0.0 - - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - 1 - - - 1 - - - 1 - - - 1 - - - 1 - - - 1 - - - 1 - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - Gazebo/DarkGrey - 0.9 - 0.9 - - - 1 - - - - robot_control/RobotControl - can - - - - - - - transmission_interface/SimpleTransmission - - - 1.0 - - - - - - - - transmission_interface/SimpleTransmission - - - 1.0 - - - - - - - - transmission_interface/SimpleTransmission - - - 1.0 - - - - - - - - transmission_interface/SimpleTransmission - - - 1.0 - - - - - - robot_control/RobotControl - dynamixel - - - - - - - - - transmission_interface/SimpleTransmission - - - 0.0 - 1 - - - - - - - - - - transmission_interface/SimpleTransmission - - - 0.0 - 1 - - - - - - robot_control/RobotControl - ethercat - true - true - true - true - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 121.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 121.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - 0.07938 - - - - - - gripper_left_finger_joint - -8.28 - - - gripper_left_finger_joint - -8.28 - - - gripper_left_finger_joint - 8.28 - - - gripper_left_finger_joint - 0.22 - - - gripper_left_finger_joint - -8.28 - - - gripper_left_finger_joint - -8.28 - - - gripper_left_finger_joint - 8.28 - - - pal_pro_gripper_transmissions/PalGripperTransmission - - - 0.001343799 - - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 121.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 121.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - - - - - - - - - sea_transmissions/SeaSimpleTransmission - - - 0.0 - 101.0 - - sea_data - - - - - - - 0.15 - - - - - - transmission_interface/SimpleTransmission - - - 1 - - - - - - - - - - - transmission_interface/SimpleTransmission - - - 1.0 - 0.0 - - - - diff --git a/agimus_controller/tests/test_robot_models.py b/agimus_controller/tests/test_robot_models.py index 6a497fef..1d39a444 100644 --- a/agimus_controller/tests/test_robot_models.py +++ b/agimus_controller/tests/test_robot_models.py @@ -453,104 +453,5 @@ def test_franka_description_collision_models(self): self.assertEqual(geom_obj_type, geom_obj_type_test) -class TestRobotModelsAgainstTiagoProDescription(unittest.TestCase): - @classmethod - def setUpClass(cls): - """ - This method sets up the shared environment for all test cases in the class. - """ - # Load the example robot model using example robot data to get the URDF path. - tiago_pro_description_path = Path( - get_package_share_directory("tiago_pro_description") - ) - # srdf_path = tiago_pro_description_path / "robots" / "tiago_pro" / "tiago_pro.srdf" - # with open(srdf_path, "r") as srdf_file: - # srdf_xml = srdf_file.read() - srdf_xml = "" - # robot_xacro_path = str( - # tiago_pro_description_path - # / "robots" - # / "tiago_pro" - # / "tiago_pro.urdf.xacro", - # ) - env_xacro_path = Path(__file__).parent / "resources" / "environment.xacro" - params_path = str( - Path(__file__).parent / "resources" / "agimus_controller_params.yaml" - ) - with open(params_path, "r") as file: - mpc_params = yaml.safe_load(file)["agimus_controller_node"][ - "ros__parameters" - ] - # robot_urdf_xml = xacro.process_file( - # robot_xacro_path, - # mappings={ - # # no-laser, sick-571 - # "laser_model": "sick-571", - # # tiago-pro, no-arm - # "arm_type_left": "tiago-pro", - # # tiago-pro, no-arm - # "arm_type_right": "tiago-pro", - # # straight-wrist, spherical-wrist - # "wrist_model_left": "spherical-wrist", - # # straight-wrist, spherical-wrist - # "wrist_model_right": "spherical-wrist", - # # False, True - # "tool_changer_right": "True", - # # False, True - # "tool_changer_left": "True", - # # pal-pro-gripper custom - # "end_effector_left": "pal-pro-gripper", - # # pal-pro-gripper, custom - # "end_effector_right": "pal-pro-gripper", - # # no-ft-sensor, rokubi - # "ft_sensor_left": "no-ft-sensor", - # # no-ft-sensor, rokubi - # "ft_sensor_right": "no-ft-sensor", - # # realsense-d435 - # "camera_model": "realsense-d435", - # }, - # ).toxml() - env_urdf_xml = xacro.process_file(env_xacro_path).toxml() - # Hack for the moving joint name - with open( - Path(__file__).parent / "resources" / "tiago_pro_atc.urdf", "r" - ) as urdf_file: - robot_urdf_xml = urdf_file.read() - model = pin.buildModelFromXML(robot_urdf_xml) - cls.locked_joint_names = [ - jn - for jn in model.names - if jn not in ["universe"] + mpc_params["moving_joint_names"] - ] - cls.params = RobotModelParameters( - q0=np.zeros(model.nq), - free_flyer=False, - moving_joint_names=mpc_params["moving_joint_names"], - robot_urdf=robot_urdf_xml, - env_urdf=env_urdf_xml, - srdf=srdf_xml, - collision_as_capsule=True, - self_collision=True, - urdf_meshes_dir=tiago_pro_description_path, - armature=np.array(mpc_params["ocp"]["armature"]), - ) - - def setUp(self): - """ - This method ensures that a fresh RobotModelParameters and RobotModels instance - are created for each test case. - """ - self.params = deepcopy(self.params) - self.robot_models = RobotModels(self.params) - - def test_rnea(self): - """Checking that the RNEA method works.""" - q = np.zeros(self.robot_models.robot_model.nq) - v = np.zeros(self.robot_models.robot_model.nv) - a = np.zeros(self.robot_models.robot_model.nv) - robot_data = self.robot_models.robot_model.createData() - pin.rnea(self.robot_models.robot_model, robot_data, q, v, a) - - if __name__ == "__main__": unittest.main() From e56d3194ab94f59803254fc7ff01d324b90496b3 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 16:39:32 +0100 Subject: [PATCH 06/12] Remove extra config.py try to add agimus-controller as dependency of agimus-controller-doc --- agimus_controller_doc/conf.py | 40 ------------------------------ agimus_controller_doc/default.nix | 2 ++ agimus_controller_doc/docs/conf.py | 11 ++++---- flake.nix | 4 ++- 4 files changed, 11 insertions(+), 46 deletions(-) delete mode 100644 agimus_controller_doc/conf.py diff --git a/agimus_controller_doc/conf.py b/agimus_controller_doc/conf.py deleted file mode 100644 index 94defbec..00000000 --- a/agimus_controller_doc/conf.py +++ /dev/null @@ -1,40 +0,0 @@ -import os -import sys -from datetime import datetime - -# Add project root (package parent) to sys.path so autodoc can import package -sys.path.insert(0, os.path.abspath("..")) - -project = "agimus_controller" -author = "agimus_controller contributors" -copyright = f"{datetime.now().year}, {author}" - -extensions = [ - "sphinx.ext.autodoc", - "sphinx.ext.napoleon", - "sphinx.ext.autosummary", - "myst_parser", - "sphinx_autodoc_typehints", - "sphinx.ext.intersphinx", - "sphinx.ext.viewcode", -] - -autosummary_generate = True -autodoc_typehints = "description" -autodoc_default_options = { - "members": True, - "undoc-members": False, - "show-inheritance": True, -} - -templates_path = ["_templates"] -exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] - -html_theme = "sphinx_rtd_theme" -html_static_path = ["_static"] - -# Intersphinx: link to Python and NumPy docs -intersphinx_mapping = { - "python": ("https://docs.python.org/3", None), - "numpy": ("https://numpy.org/doc/stable/", None), -} diff --git a/agimus_controller_doc/default.nix b/agimus_controller_doc/default.nix index e2efdc9a..69f1fc1c 100644 --- a/agimus_controller_doc/default.nix +++ b/agimus_controller_doc/default.nix @@ -3,6 +3,7 @@ python3Packages, lib, fetchFromGitHub ? null, + agimus-controller, ... }: @@ -17,6 +18,7 @@ stdenv.mkDerivation rec { sphinx-rtd-theme myst-parser sphinx-autodoc-typehints + agimus-controller ]; buildPhase = '' diff --git a/agimus_controller_doc/docs/conf.py b/agimus_controller_doc/docs/conf.py index 968087f4..a7326fac 100644 --- a/agimus_controller_doc/docs/conf.py +++ b/agimus_controller_doc/docs/conf.py @@ -11,11 +11,12 @@ # have heavy C++-backed dependencies like `crocoddyl`, `pinocchio` or # `colmpc`. Mock them so Sphinx can import the Python modules and # autosummary/autodoc can generate API pages. -autodoc_mock_imports = [ - "crocoddyl", - "pinocchio", - "colmpc", -] +# Note: Now installed via nix, so no need to mock them anymore. +# autodoc_mock_imports = [ +# "crocoddyl", +# "pinocchio", +# "colmpc", +# ] project = "agimus_controller" author = "agimus_controller contributors" diff --git a/flake.nix b/flake.nix index 49f022c0..fcc1eb11 100644 --- a/flake.nix +++ b/flake.nix @@ -123,7 +123,9 @@ default = self'.packages.agimus-controller; agimus-controller = pkgs.python3Packages.agimus-controller; agimus-controller-examples = pkgs.python3Packages.agimus-controller-examples; - agimus-controller-doc = pkgs.callPackage ./agimus_controller_doc/default.nix { }; + agimus-controller-doc = pkgs.callPackage ./agimus_controller_doc/default.nix { + inherit (pkgs.python3Packages) agimus-controller; + }; } // lib.optionalAttrs pkgs.stdenv.hostPlatform.isLinux { ros-humble-agimus-controller-ros = pkgs.rosPackages.humble.agimus-controller-ros; From 6ad58c15b95f5428e2f1fd9994bab9f8c6785051 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Fri, 20 Feb 2026 17:31:27 +0100 Subject: [PATCH 07/12] Fix the build of the documentation --- .../ocp/ocp_croco_generic.py | 252 +++++++++++++++++- .../ocp/ocp_croco_generic_force_feedback.py | 45 ++++ agimus_controller_doc/docs/_static/.gitkeep | 1 + .../docs/api/ActivationModelExp.md | 9 +- .../docs/api/ActivationModelQuadExp.md | 9 +- .../docs/api/ActivationModelWeightedQuad.md | 9 +- .../docs/api/CostModelResidual.md | 9 +- .../docs/api/CostModelSumItem.md | 9 +- .../docs/api/ResidualDistanceCollision.md | 9 +- .../docs/api/ResidualDistanceCollision2.md | 9 +- .../docs/api/ResidualModelControl.md | 9 +- .../docs/api/ResidualModelControlGrav.md | 9 +- .../docs/api/ResidualModelFramePlacement.md | 9 +- .../docs/api/ResidualModelFrameRotation.md | 9 +- .../docs/api/ResidualModelFrameTranslation.md | 9 +- .../docs/api/ResidualModelFrameVelocity.md | 9 +- .../docs/api/ResidualModelState.md | 9 +- agimus_controller_doc/docs/api/index.md | 28 ++ .../docs/api/ocp_croco_generic.md | 55 ++-- agimus_controller_doc/docs/conf.py | 13 +- agimus_controller_doc/docs/index.md | 2 +- agimus_controller_doc/pyproject.toml | 1 + 22 files changed, 439 insertions(+), 84 deletions(-) create mode 100644 agimus_controller_doc/docs/_static/.gitkeep create mode 100644 agimus_controller_doc/docs/api/index.md diff --git a/agimus_controller/agimus_controller/ocp/ocp_croco_generic.py b/agimus_controller/agimus_controller/ocp/ocp_croco_generic.py index 90101136..7128bbbe 100644 --- a/agimus_controller/agimus_controller/ocp/ocp_croco_generic.py +++ b/agimus_controller/agimus_controller/ocp/ocp_croco_generic.py @@ -1,3 +1,28 @@ +"""ocp_croco_generic + +High-level builder for Crocoddyl/CoLMPC optimal control problems used in +agimus_controller. + +This module exposes dataclasses that describe residuals, activations, costs, +constraints and action models in a data-driven way (YAML -> dataclasses -> +Crocoddyl/CoLMPC objects). The resulting optimal control problem solved by +the controller has the usual form:: + + minimize_{u_0..u_{N-1}} sum_{k=0..N-1} L_k(x_k, u_k) + L_N(x_N) + subject to x_{k+1} = f(x_k, u_k) + +where each stage cost L_k is expressed using a residual r(x,u) and an +activation phi(r): + + L = phi(r(x,u)) + +Many small helper dataclasses below wrap Crocoddyl residuals and activations +and provide a `build()` method returning the corresponding Crocoddyl object. +The YAML-based factory `create_croco_dataclasses` instantiates these dataclasses +from a specification file and is used by `OCPCrocoGeneric` to assemble the +ShootingProblem. +""" + import pathlib import crocoddyl import numpy as np @@ -98,6 +123,14 @@ class ActivationModel: class ActivationModelWeightedQuad(ActivationModel): class_: T.ClassVar[str] = "ActivationModelWeightedQuad" weights: T.Union[None, float, npt.NDArray[np.float64]] = None + """Weighted quadratic activation. + + The activation energy is a weighted quadratic form on the residual r: + + phi(r) = 0.5 * r^T diag(weights) r + + If `weights` is a scalar it will be broadcast to the residual size. + """ def update(self, data, obj, weights): obj.weights = weights @@ -119,6 +152,13 @@ class ActivationModelExp(ActivationModel): class_: T.ClassVar[str] = "ActivationModelExp" alpha: float = 1.0 exponent: int = 1 + """Exponential-like activation. + + This activation uses the CoLMPC exponential activations. For `exponent` + equal to 1 it produces an activation similar to `exp(alpha * |r|)` and for + 2 it uses a quadratic-exponential variant. The parameter `alpha` scales the + activation intensity. + """ def build(self, data: BuildData, residual: crocoddyl.CostModelResidual): assert self.exponent in [1, 2] @@ -145,6 +185,15 @@ def __post_init__(self): @dataclasses.dataclass class ResidualModel: + """Base class for residual models. + + A residual maps a state (and optionally control) to a vector r(x,u) of + dimension `nr`. Implementations must provide `build(data)` returning a + Crocoddyl residual object and `update(...)` to update references from a + `WeightedTrajectoryPoint`. The helper `needs_colmpc_freefwd_dynamics` + indicates whether the residual requires the `colmpc` state/dynamics types. + """ + @staticmethod def needs_colmpc_freefwd_dynamics() -> bool: return False @@ -152,6 +201,19 @@ def needs_colmpc_freefwd_dynamics() -> bool: @dataclasses.dataclass class ResidualModelState(ResidualModel): + """State tracking residual. + + Computes the state error residual: + + r(x, u) = x - x_ref + + where x ∈ ℝⁿ is the current state and x_ref is the reference state. + The residual dimension is nr = state.nx. + + This residual is commonly used with a quadratic activation to penalize + deviations from a desired state trajectory. + """ + class_: T.ClassVar[str] = "ResidualModelState" xref: T.Optional[npt.NDArray[np.float64]] = None @@ -168,6 +230,19 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualModelControl(ResidualModel): + """Control regularization residual. + + Computes the control input error: + + r(x, u) = u - u_ref + + where u ∈ ℝᵐ is the control input and u_ref is the reference control. + The residual dimension is nr = actuation.nu. + + This residual is typically used with a quadratic activation to minimize + control effort and smooth control trajectories. + """ + class_: T.ClassVar[str] = "ResidualModelControl" uref: T.Optional[npt.NDArray[np.float64]] = None @@ -184,6 +259,19 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualModelControlGrav(ResidualModel): + """Gravity-compensated control residual. + + Computes the control residual with gravity compensation: + + r(x, u) = u - g(q) + + where g(q) ∈ ℝᵐ is the gravity torque vector computed from the robot + configuration q. The residual dimension is nr = actuation.nu. + + This residual penalizes control inputs that deviate from gravity compensation, + encouraging energy-efficient motions that naturally follow gravity. + """ + class_: T.ClassVar[str] = "ResidualModelControlGrav" def update(self, data, obj, pt: WeightedTrajectoryPoint): @@ -196,6 +284,20 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualModelFramePlacement(ResidualModel): + """6D frame placement residual (SE(3)). + + Computes the pose error between the current and reference frame placement: + + r(x) = log(M_ref⁻¹ · M_current) ∈ ℝ⁶ + + where M ∈ SE(3) is the frame placement (position + orientation), + and log: SE(3) → se(3) ≃ ℝ⁶ is the SE(3) logarithm map. + The residual has dimension nr = 6 (3 translation + 3 rotation). + + This residual is used for precise end-effector positioning tasks + that require both position and orientation control. + """ + class_: T.ClassVar[str] = "ResidualModelFramePlacement" id: T.Union[str, int] pref: T.Optional[npt.NDArray[np.float64]] = None @@ -250,6 +352,19 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualModelFrameTranslation(ResidualModel): + """3D frame translation residual. + + Computes the position error in Cartesian space: + + r(x) = p_current - p_ref ∈ ℝ³ + + where p ∈ ℝ³ is the frame position (translation component of SE(3)). + The residual dimension is nr = 3. + + This residual is used for position-only tracking tasks where orientation + is free or controlled separately. + """ + class_: T.ClassVar[str] = "ResidualModelFrameTranslation" id: T.Union[str, int] pref: T.Optional[npt.NDArray[np.float64]] = None @@ -304,6 +419,19 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualModelFrameRotation(ResidualModel): + """3D frame rotation residual (SO(3)). + + Computes the orientation error in rotation space: + + r(x) = log(R_ref^T · R_current) ∈ ℝ³ + + where R ∈ SO(3) is the rotation matrix, and log: SO(3) → so(3) ≃ ℝ³ + is the SO(3) logarithm map. The residual dimension is nr = 3. + + This residual is used for orientation-only tracking tasks where position + is free or controlled separately. + """ + class_: T.ClassVar[str] = "ResidualModelFrameRotation" id: T.Union[str, int] pref: T.Optional[npt.NDArray[np.float64]] = None @@ -358,6 +486,20 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualModelFrameVelocity(ResidualModel): + """6D frame velocity residual. + + Computes the velocity tracking error: + + r(x, v) = v_frame - v_ref ∈ ℝ⁶ + + where v_frame = (v_linear, ω_angular) is the spatial velocity of the frame, + expressed in the specified reference frame (WORLD, LOCAL, or LOCAL_WORLD_ALIGNED). + The residual dimension is nr = 6 (3 linear + 3 angular). + + This residual is used for tasks requiring velocity tracking, such as + following moving targets or imposing velocity constraints. + """ + class_: T.ClassVar[str] = "ResidualModelFrameVelocity" id: T.Union[str, int] pref: T.Optional[npt.NDArray[np.float64]] = None @@ -522,6 +664,20 @@ def _collision_pair_id(self, cmodel: pinocchio.GeometryModel) -> int: @dataclasses.dataclass class ResidualDistanceCollision(ResidualDistanceCollisionBase): + """Collision avoidance residual (basic). + + Computes the signed distance between a collision pair: + + r(x) = d(q) - d_safe + + where d(q) is the minimum distance between the two geometries at + configuration q, and d_safe is a safety margin. The residual is scalar (nr = 1). + + When d < d_safe, the residual is negative, indicating potential collision. + This residual should be used with an exponential activation to create + a smooth repulsive barrier. + """ + class_: T.ClassVar[str] = "ResidualDistanceCollision" def build(self, data: BuildData): @@ -535,6 +691,19 @@ def build(self, data: BuildData): @dataclasses.dataclass class ResidualDistanceCollision2(ResidualDistanceCollisionBase): + """Collision avoidance residual (advanced with derivatives). + + Enhanced collision residual that provides accurate derivatives: + + r(x) = d(q) - d_safe + + Similar to ResidualDistanceCollision but requires ColMPC's StateMultibody + for improved Jacobian computation. The residual is scalar (nr = 1). + + This variant provides more accurate gradient information, leading to + better optimization performance for collision avoidance. + """ + class_: T.ClassVar[str] = "ResidualDistanceCollision2" @staticmethod @@ -558,6 +727,23 @@ class CostModel: @dataclasses.dataclass class CostModelResidual(CostModel): + """Compose a residual and activation into a cost. + + The stage cost is computed as: + + L(x, u) = φ(r(x, u)) + + where r: ℝⁿ × ℝᵐ → ℝⁿʳ is the residual and φ: ℝⁿʳ → ℝ is the activation. + + Common activations: + - Quadratic: φ(r) = 0.5 * r^T W r (default if activation=None) + - Weighted Quadratic: φ(r) = 0.5 * r^T diag(w) r + - Exponential: φ(r) ≈ exp(α |r|) for smooth barrier functions + + The `build()` method creates the Crocoddyl cost, and `update()` sets + the reference and weights from trajectory points. + """ + class_: T.ClassVar[str] = "CostModelResidual" def build(self, data: BuildData): @@ -576,6 +762,23 @@ def update(self, data, obj, ref_w_pt: WeightedTrajectoryPoint): @dataclasses.dataclass class CostModelSumItem: + """Weighted cost term in the total stage cost. + + Represents a single term in the cost sum: + + L_total = Σᵢ weightᵢ * Lᵢ(x, u) + + where Lᵢ is the cost defined by `cost` and weightᵢ scales its contribution. + + Parameters: + name: Identifier for this cost term + cost: The cost model (residual + activation) + weight: Scalar multiplier for this cost term + active: Whether to include this cost in the optimization + update: Whether to update references from trajectory points + publish_residual: Whether to publish residual values for monitoring + """ + class_: T.ClassVar[str] = "CostModelSumItem" name: str cost: CostModel @@ -658,6 +861,15 @@ class DifferentialActionModelFreeFwdDynamics(DifferentialActionModel): costs: T.List[CostModelSumItem] constraints: T.List[ConstraintListItem] = dataclasses.field(default_factory=list) + """Differential action model using free forward dynamics. + + This dataclass aggregates stage `costs` and `constraints` and builds a + Crocoddyl (or CoLMPC when required) `DifferentialActionModelFreeFwdDynamics`. + The choice between `crocoddyl` and `colmpc` implementations is automatic + and depends on whether any residuals require the specialized `colmpc` + state/dynamics types (see `needs_colmpc_freefwd_dynamics`). + """ + @classmethod def from_dict(cls, kwargs: T.Dict[str, T.Any]): costs = [ @@ -750,6 +962,15 @@ class ShootingProblem: running_model: IntegratedActionModelAbstract terminal_model: IntegratedActionModelAbstract + """Container describing the shooting problem structure. + + - `running_model` describes the stage model (applied at k=0..N-1) + - `terminal_model` describes the terminal stage (k=N) + + Both fields are YAML-deserialized into the dataclass tree and later used + to construct Crocoddyl models through `build()`. + """ + def needs_colmpc_state(self) -> bool: return ( self.running_model.differential.needs_colmpc_freefwd_dynamics() @@ -762,6 +983,23 @@ def __post_init__(self): class OCPCrocoGeneric(OCPBaseCroco): + """Generic OCP builder that reads a YAML specification. + + `OCPCrocoGeneric` reads a YAML file describing the shooting problem + (residuals, activations, costs, and integration scheme) and constructs a + Crocoddyl `ShootingProblem` and solver via `OCPBaseCroco`. + + Key responsibilities / API: + - parse the YAML and assemble `ShootingProblem` dataclasses + - create running and terminal Crocoddyl models (`create_running_model_list`, + `create_terminal_model`) + - expose `input_transforms`: a dict of transforms (frame pairs) that + must be provided externally (e.g. via TF2) before updating visual-servo + residuals + - `set_reference_weighted_trajectory(...)`: update references and + activation weights from a list of `WeightedTrajectoryPoint` instances. + """ + def __init__( self, robot_models: RobotModels, @@ -855,7 +1093,19 @@ def fill_debug_data(self, res, ocp_results) -> None: def set_reference_weighted_trajectory( self, reference_weighted_trajectory: list[WeightedTrajectoryPoint] ): - """Set the reference trajectory for the OCP.""" + """Set the reference trajectory for the OCP. + + The method expects a list of `WeightedTrajectoryPoint` of length + `n_controls + 1` (running stages + terminal). For each stage it updates + the residual references and the activation weights. When + `expect_rolling_buffer` is True the function supports shifting the + running models in a circular buffer fashion used by MPC receding-horizon + updates: on the first call it fills the running models, on subsequent + calls it appends/rotates the models and updates only the last one. + + Args: + reference_weighted_trajectory: list of `WeightedTrajectoryPoint`. + """ assert len(reference_weighted_trajectory) == self.n_controls + 1 diff --git a/agimus_controller/agimus_controller/ocp/ocp_croco_generic_force_feedback.py b/agimus_controller/agimus_controller/ocp/ocp_croco_generic_force_feedback.py index 3a9dee08..95c4ce03 100644 --- a/agimus_controller/agimus_controller/ocp/ocp_croco_generic_force_feedback.py +++ b/agimus_controller/agimus_controller/ocp/ocp_croco_generic_force_feedback.py @@ -1,3 +1,21 @@ +"""Force-feedback OCP extensions for agimus_controller. + +This module provides differential/integrated action model wrappers that +augment Crocoddyl dynamics with soft-contact force variables used by the +force-feedback MPC implementation. Two main building blocks are provided: + +- `DAMSoftContactAugmentedFwdDynamics`: a differential action model that + exposes contact forces as additional variables and builds the underlying + force-feedback DAM from `force_feedback_mpc`. +- `IAMSoftContactAugmented`: an integrated action model wrapper that + integrates the augmented DAM over the timestep and adapts force bounds. + +The `OCPCrocoForceFeedbackGeneric` class assembles a full shooting problem +and solver (CSQP via `mim_solvers`) from a YAML specification similarly to +`OCPCrocoGeneric`, but adding the force variables to the state and exposing +helpers to update desired contact forces from `WeightedTrajectoryPoint`. +""" + import dataclasses import typing as T import yaml @@ -157,6 +175,17 @@ def update(self, data, dam, pt: WeightedTrajectoryPoint): if dam.with_gravity_torque_reg: dam.tau_grav_weight = pt.weights.w_robot_effort[0] + """Differential action model that augments forward dynamics with soft-contact forces. + + The model adds contact force variables to the state and exposes them to + costs/constraints. Internally it constructs the appropriate + `force_feedback_mpc.DAMSoftContact*AugmentedFwdDynamics` depending on the + configured dimension (1D or 3D). Update and build semantics mirror + Crocoddyl's action models: `build(data)` returns the DAM instance and + `update(data, dam, pt)` refreshes references and desired forces from the + provided `WeightedTrajectoryPoint`. + """ + @dataclasses.dataclass class IAMSoftContactAugmented(IntegratedActionModelAbstract): @@ -214,8 +243,24 @@ def build(self, data: BuildData): self.force_lb = np.array(self.force_lb) return iam + """Integrated action model that wraps a soft-contact augmented DAM. + + This wrapper integrates the augmented differential action model using the + chosen integrator (Euler by default). It also manages force bounds used + by the underlying solver: when no explicit bounds are provided it uses the + default ones produced by the underlying `force_feedback_mpc` object. + """ + class OCPCrocoForceFeedbackGeneric(OCPCrocoGeneric): + """Force-feedback variant of `OCPCrocoGeneric`. + + This class builds a shooting problem that augments the robot state with + contact forces and uses `mim_solvers.SolverCSQP` as a solver. It supports + the same YAML-driven dataclass specification as `OCPCrocoGeneric` but + expects differential/integrated action models that manage contact forces. + """ + def __init__( self, robot_models: RobotModels, diff --git a/agimus_controller_doc/docs/_static/.gitkeep b/agimus_controller_doc/docs/_static/.gitkeep new file mode 100644 index 00000000..d0f9e073 --- /dev/null +++ b/agimus_controller_doc/docs/_static/.gitkeep @@ -0,0 +1 @@ +# This file ensures the _static directory is tracked by git diff --git a/agimus_controller_doc/docs/api/ActivationModelExp.md b/agimus_controller_doc/docs/api/ActivationModelExp.md index f65e308e..1b7856e4 100644 --- a/agimus_controller_doc/docs/api/ActivationModelExp.md +++ b/agimus_controller_doc/docs/api/ActivationModelExp.md @@ -1,7 +1,8 @@ # ActivationModelExp -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ActivationModelExp -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ActivationModelExp + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ActivationModelQuadExp.md b/agimus_controller_doc/docs/api/ActivationModelQuadExp.md index 22cff4f3..8e84f95e 100644 --- a/agimus_controller_doc/docs/api/ActivationModelQuadExp.md +++ b/agimus_controller_doc/docs/api/ActivationModelQuadExp.md @@ -1,7 +1,8 @@ # ActivationModelQuadExp -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ActivationModelQuadExp -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ActivationModelQuadExp + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md b/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md index 8ef43481..bb08a736 100644 --- a/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md +++ b/agimus_controller_doc/docs/api/ActivationModelWeightedQuad.md @@ -1,7 +1,8 @@ # ActivationModelWeightedQuad -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ActivationModelWeightedQuad -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ActivationModelWeightedQuad + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/CostModelResidual.md b/agimus_controller_doc/docs/api/CostModelResidual.md index 67b4bf31..6a18a85e 100644 --- a/agimus_controller_doc/docs/api/CostModelResidual.md +++ b/agimus_controller_doc/docs/api/CostModelResidual.md @@ -1,7 +1,8 @@ # CostModelResidual -```{autoclass} agimus_controller.ocp.ocp_croco_generic.CostModelResidual -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.CostModelResidual + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/CostModelSumItem.md b/agimus_controller_doc/docs/api/CostModelSumItem.md index 07a32b01..9875416e 100644 --- a/agimus_controller_doc/docs/api/CostModelSumItem.md +++ b/agimus_controller_doc/docs/api/CostModelSumItem.md @@ -1,7 +1,8 @@ # CostModelSumItem -```{autoclass} agimus_controller.ocp.ocp_croco_generic.CostModelSumItem -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.CostModelSumItem + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualDistanceCollision.md b/agimus_controller_doc/docs/api/ResidualDistanceCollision.md index 8f297c5a..0f0e006d 100644 --- a/agimus_controller_doc/docs/api/ResidualDistanceCollision.md +++ b/agimus_controller_doc/docs/api/ResidualDistanceCollision.md @@ -1,7 +1,8 @@ # ResidualDistanceCollision -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md b/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md index 8d1c7b10..fb2b0e1b 100644 --- a/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md +++ b/agimus_controller_doc/docs/api/ResidualDistanceCollision2.md @@ -1,7 +1,8 @@ # ResidualDistanceCollision2 -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision2 -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualDistanceCollision2 + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelControl.md b/agimus_controller_doc/docs/api/ResidualModelControl.md index 30639bdd..0abdef55 100644 --- a/agimus_controller_doc/docs/api/ResidualModelControl.md +++ b/agimus_controller_doc/docs/api/ResidualModelControl.md @@ -1,7 +1,8 @@ # ResidualModelControl -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelControl -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelControl + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelControlGrav.md b/agimus_controller_doc/docs/api/ResidualModelControlGrav.md index eabd6db7..0948839e 100644 --- a/agimus_controller_doc/docs/api/ResidualModelControlGrav.md +++ b/agimus_controller_doc/docs/api/ResidualModelControlGrav.md @@ -1,7 +1,8 @@ # ResidualModelControlGrav -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelControlGrav -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelControlGrav + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md b/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md index 5d45a00e..a1b35dc8 100644 --- a/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md +++ b/agimus_controller_doc/docs/api/ResidualModelFramePlacement.md @@ -1,7 +1,8 @@ # ResidualModelFramePlacement -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFramePlacement -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelFramePlacement + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md b/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md index 296293d9..9a8249b9 100644 --- a/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md +++ b/agimus_controller_doc/docs/api/ResidualModelFrameRotation.md @@ -1,7 +1,8 @@ # ResidualModelFrameRotation -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameRotation -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameRotation + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md b/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md index ff646614..12bb6ba0 100644 --- a/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md +++ b/agimus_controller_doc/docs/api/ResidualModelFrameTranslation.md @@ -1,7 +1,8 @@ # ResidualModelFrameTranslation -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameTranslation -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameTranslation + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md b/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md index d42a061f..04e77b46 100644 --- a/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md +++ b/agimus_controller_doc/docs/api/ResidualModelFrameVelocity.md @@ -1,7 +1,8 @@ # ResidualModelFrameVelocity -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameVelocity -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelFrameVelocity + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/ResidualModelState.md b/agimus_controller_doc/docs/api/ResidualModelState.md index 572aad27..5025866f 100644 --- a/agimus_controller_doc/docs/api/ResidualModelState.md +++ b/agimus_controller_doc/docs/api/ResidualModelState.md @@ -1,7 +1,8 @@ # ResidualModelState -```{autoclass} agimus_controller.ocp.ocp_croco_generic.ResidualModelState -:members: -:undoc-members: -:show-inheritance: +```{eval-rst} +.. autoclass:: agimus_controller.ocp.ocp_croco_generic.ResidualModelState + :members: + :undoc-members: + :show-inheritance: ``` diff --git a/agimus_controller_doc/docs/api/index.md b/agimus_controller_doc/docs/api/index.md new file mode 100644 index 00000000..b5964493 --- /dev/null +++ b/agimus_controller_doc/docs/api/index.md @@ -0,0 +1,28 @@ +--- +title: API index +--- + +# API + +The pages below are generated by `autosummary` and provide detailed API +reference for the OCP module. + +```{toctree} +:maxdepth: 2 + +ActivationModelWeightedQuad +ActivationModelExp +ActivationModelQuadExp +ResidualModelState +ResidualModelControl +ResidualModelControlGrav +ResidualModelFramePlacement +ResidualModelFrameTranslation +ResidualModelFrameRotation +ResidualModelFrameVelocity +ResidualDistanceCollision +ResidualDistanceCollision2 +CostModelResidual +CostModelSumItem +ocp_croco_generic +``` diff --git a/agimus_controller_doc/docs/api/ocp_croco_generic.md b/agimus_controller_doc/docs/api/ocp_croco_generic.md index d4dea6a6..932de9f7 100644 --- a/agimus_controller_doc/docs/api/ocp_croco_generic.md +++ b/agimus_controller_doc/docs/api/ocp_croco_generic.md @@ -2,34 +2,35 @@ title: OCP Crocoddyl Generic API --- -# `agimus_controller.ocp.ocp_croco_generic` +# AGIMUS controller: generic croccodyl OCP -This page exposes the API of the `ocp_croco_generic` module. - -```{automodule} agimus_controller.ocp.ocp_croco_generic -:members: -:undoc-members: -:show-inheritance: -``` ## Costs — quick index Below are the main cost and residual classes implemented in this module. Click the class names to open full API docs. -- [ActivationModelWeightedQuad](ActivationModelWeightedQuad.md) -- [ActivationModelExp](ActivationModelExp.md) -- [ActivationModelQuadExp](ActivationModelQuadExp.md) -- [ResidualModelState](ResidualModelState.md) -- [ResidualModelControl](ResidualModelControl.md) -- [ResidualModelControlGrav](ResidualModelControlGrav.md) -- [ResidualModelFramePlacement](ResidualModelFramePlacement.md) -- [ResidualModelFrameTranslation](ResidualModelFrameTranslation.md) -- [ResidualModelFrameRotation](ResidualModelFrameRotation.md) -- [ResidualModelFrameVelocity](ResidualModelFrameVelocity.md) -- [ResidualDistanceCollision](ResidualDistanceCollision.md) -- [ResidualDistanceCollision2](ResidualDistanceCollision2.md) -- [CostModelResidual](CostModelResidual.md) -- [CostModelSumItem](CostModelSumItem.md) +### Activation Models + +- [ActivationModelWeightedQuad](ActivationModelWeightedQuad.md) — Weighted quadratic activation +- [ActivationModelExp](ActivationModelExp.md) — Exponential activation +- [ActivationModelQuadExp](ActivationModelQuadExp.md) — Quadratic-exponential activation + +### Residual Models + +- [ResidualModelState](ResidualModelState.md) — State tracking residual +- [ResidualModelControl](ResidualModelControl.md) — Control regularization +- [ResidualModelControlGrav](ResidualModelControlGrav.md) — Gravity-compensated control +- [ResidualModelFramePlacement](ResidualModelFramePlacement.md) — 6D frame placement +- [ResidualModelFrameTranslation](ResidualModelFrameTranslation.md) — 3D frame translation +- [ResidualModelFrameRotation](ResidualModelFrameRotation.md) — 3D frame rotation +- [ResidualModelFrameVelocity](ResidualModelFrameVelocity.md) — Frame velocity tracking +- [ResidualDistanceCollision](ResidualDistanceCollision.md) — Collision distance (basic) +- [ResidualDistanceCollision2](ResidualDistanceCollision2.md) — Collision distance (advanced) + +### Cost Models + +- [CostModelResidual](CostModelResidual.md) — Residual-based cost +- [CostModelSumItem](CostModelSumItem.md) — Weighted cost sum item ## Example usage @@ -43,3 +44,13 @@ print(ocp._data.running_model.differential.costs) # Programmatically create a Placement residual # residual = ResidualModelFramePlacement(id="tool0") ``` + +## Module API + +This page exposes the API of the `ocp_croco_generic` module. + +```{automodule} agimus_controller.ocp.ocp_croco_generic +:members: +:undoc-members: +:show-inheritance: +``` diff --git a/agimus_controller_doc/docs/conf.py b/agimus_controller_doc/docs/conf.py index a7326fac..86a8ecf5 100644 --- a/agimus_controller_doc/docs/conf.py +++ b/agimus_controller_doc/docs/conf.py @@ -40,6 +40,11 @@ "show-inheritance": True, } +# MyST-Parser configuration to enable Sphinx directives in markdown +myst_enable_extensions = [ + "colon_fence", # Enable ::: for directives +] + templates_path = ["_templates"] exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] @@ -47,7 +52,7 @@ html_static_path = ["_static"] # Intersphinx: link to Python and NumPy docs -intersphinx_mapping = { - "python": ("https://docs.python.org/3", None), - "numpy": ("https://numpy.org/doc/stable/", None), -} +# Disable remote intersphinx inventory fetches in offline/CI builds to avoid +# noisy network warnings. If you need cross-references to Python/NumPy docs +# re-enable the mapping below. +intersphinx_mapping = {} diff --git a/agimus_controller_doc/docs/index.md b/agimus_controller_doc/docs/index.md index bd2f5c7f..6872c4e2 100644 --- a/agimus_controller_doc/docs/index.md +++ b/agimus_controller_doc/docs/index.md @@ -8,7 +8,7 @@ title: agimus_controller documentation :maxdepth: 2 general/usage -api/ocp_croco_generic +api/index ``` diff --git a/agimus_controller_doc/pyproject.toml b/agimus_controller_doc/pyproject.toml index d7530590..0793a523 100644 --- a/agimus_controller_doc/pyproject.toml +++ b/agimus_controller_doc/pyproject.toml @@ -11,6 +11,7 @@ sphinx = "^6.0" sphinx-rtd-theme = "*" myst-parser = "*" sphinx-autodoc-typehints = "*" +agimus-controller = { path = "../agimus_controller" } [tool.poetry.scripts] agimus-docs-build = "agimus_controller_doc.cli:build" From da836dd14f06991772f781ca9a9c0eaecf7e730c Mon Sep 17 00:00:00 2001 From: Naveau Date: Fri, 20 Mar 2026 15:38:13 +0100 Subject: [PATCH 08/12] Remove the input fetchFromGithub in the local nix --- agimus_controller_doc/default.nix | 1 - 1 file changed, 1 deletion(-) diff --git a/agimus_controller_doc/default.nix b/agimus_controller_doc/default.nix index 69f1fc1c..5cecdab1 100644 --- a/agimus_controller_doc/default.nix +++ b/agimus_controller_doc/default.nix @@ -2,7 +2,6 @@ stdenv, python3Packages, lib, - fetchFromGitHub ? null, agimus-controller, ... }: From ba9e9594ed3bae3ae188c4dcadc5cad1bc91de9e Mon Sep 17 00:00:00 2001 From: Naveau Date: Fri, 20 Mar 2026 16:27:54 +0100 Subject: [PATCH 09/12] Update agimus_controller_doc/default.nix Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- agimus_controller_doc/default.nix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/agimus_controller_doc/default.nix b/agimus_controller_doc/default.nix index 5cecdab1..433efafa 100644 --- a/agimus_controller_doc/default.nix +++ b/agimus_controller_doc/default.nix @@ -32,7 +32,7 @@ stdenv.mkDerivation rec { meta = with lib; { description = "Sphinx HTML documentation for agimus_controller"; - license = licenses.unfree; # docs only + license = licenses.bsd3; platforms = platforms.unix; }; } From b784a0ec0d88488b8d4bea55deb9967b93d9157d Mon Sep 17 00:00:00 2001 From: Naveau Date: Fri, 20 Mar 2026 16:29:09 +0100 Subject: [PATCH 10/12] Update agimus_controller_doc/README.md Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- agimus_controller_doc/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/agimus_controller_doc/README.md b/agimus_controller_doc/README.md index cabef64a..c6be52fb 100644 --- a/agimus_controller_doc/README.md +++ b/agimus_controller_doc/README.md @@ -5,7 +5,7 @@ ```bash python -m venv .venv source .venv/bin/activate -pip install -r requirements.txt +pip install -r docs/requirements.txt ``` 2. Build HTML: From 8bf857e78dd03a79469dc547b9885f9bdb6ebc7a Mon Sep 17 00:00:00 2001 From: Naveau Date: Fri, 20 Mar 2026 16:30:00 +0100 Subject: [PATCH 11/12] Update agimus_controller_doc/agimus_controller_doc/cli.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- agimus_controller_doc/agimus_controller_doc/cli.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/agimus_controller_doc/agimus_controller_doc/cli.py b/agimus_controller_doc/agimus_controller_doc/cli.py index 9359c073..8dceb4f6 100644 --- a/agimus_controller_doc/agimus_controller_doc/cli.py +++ b/agimus_controller_doc/agimus_controller_doc/cli.py @@ -1,9 +1,10 @@ import sys from pathlib import Path from sphinx.cmd.build import build_main +from typing import List, Optional -def build(argv: list | None = None) -> int: +def build(argv: Optional[List[str]] = None) -> int: """Build the documentation. Usage (after `poetry install`): From d8697f31d95387ecf0267a3fffebf9987b61cb40 Mon Sep 17 00:00:00 2001 From: Maximilien Naveau Date: Thu, 23 Apr 2026 17:52:35 +0200 Subject: [PATCH 12/12] try to build the doc with nix --- flake.lock | 30 +++++++++++++++--------------- flake.nix | 16 +++++++++++++--- 2 files changed, 28 insertions(+), 18 deletions(-) diff --git a/flake.lock b/flake.lock index 54c3fa75..37876b51 100644 --- a/flake.lock +++ b/flake.lock @@ -58,11 +58,11 @@ "uv2nix": "uv2nix" }, "locked": { - "lastModified": 1772976349, - "narHash": "sha256-DH4c3vC/8liquLNrVRQoL/uf/NZSiLi7zItE6zxZSes=", + "lastModified": 1773524302, + "narHash": "sha256-0phiMh2h3tZoe3rJGWWKMMJhuLMMHa65hRtgkzbXBJQ=", "owner": "gepetto", "repo": "gazebros2nix", - "rev": "0a6e449fa1ab8e614ef1b1a6efe494148c80a866", + "rev": "0eb9151c41ec370e29d4d4ae1640aa4c6c4aa7a7", "type": "github" }, "original": { @@ -92,11 +92,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1772890336, - "narHash": "sha256-pjO4Od9QdCWDMmE/utZ2YVyxAL8+jo5oEpnQQ9dV0rc=", + "lastModified": 1773393994, + "narHash": "sha256-zm24obq1c4ielRR8KlUQ2M5XnNfUIcQjN++9s6CFvxc=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "8fe70ded2467c777c14bbd7087e57b0d3d88110e", + "rev": "a522c5a050a6d50471a29bbf6a060e2df16abf44", "type": "github" }, "original": { @@ -174,11 +174,11 @@ ] }, "locked": { - "lastModified": 1772865871, - "narHash": "sha256-/ZTSg97aouL0SlPHaokA4r3iuH9QzHVuWPACD2CUCFY=", + "lastModified": 1773190977, + "narHash": "sha256-fkJvOxz80cJViPz6GVaayC8BVCs5fclJ8qHDaNUpoEA=", "owner": "pyproject-nix", "repo": "pyproject.nix", - "rev": "e537db02e72d553cea470976b9733581bcf5b3ed", + "rev": "10ebca8a137bf26b7fbd3e94b339bf68cee18693", "type": "github" }, "original": { @@ -235,11 +235,11 @@ ] }, "locked": { - "lastModified": 1772660329, - "narHash": "sha256-IjU1FxYqm+VDe5qIOxoW+pISBlGvVApRjiw/Y/ttJzY=", + "lastModified": 1773297127, + "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "3710e0e1218041bbad640352a0440114b1e10428", + "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016", "type": "github" }, "original": { @@ -260,11 +260,11 @@ ] }, "locked": { - "lastModified": 1772545244, - "narHash": "sha256-Ys+5UMOqp2kRvnSjyBcvGnjOhkIXB88On1ZcAstz1vY=", + "lastModified": 1773359304, + "narHash": "sha256-knv2C6tIk5ysix+9TxWIenPvpB20kFjQ1CH6SJMBNsU=", "owner": "pyproject-nix", "repo": "uv2nix", - "rev": "482aba340ded40ef557d331315f227d5eba84ced", + "rev": "27b135ea72ab1637fc5845a61c101ea66d6636d6", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 9a218862..14bd507e 100644 --- a/flake.nix +++ b/flake.nix @@ -36,12 +36,22 @@ ]; }; }; - agimus-controller-doc = _final.callPackage ./agimus_controller_doc/default.nix { - inherit (_final.python3Packages) agimus-controller; - }; }; } ]; + perSystem = + { config, pkgs, ... }: + { + packages = { + agimus-controller-doc = pkgs.callPackage ./agimus_controller_doc/default.nix { + inherit (pkgs.rosPackages.humble) agimus-controller; + }; + }; + checks = { + # Build packages during nix flake check + inherit (config.packages) agimus-controller-doc; + }; + }; } ); }