ros2

.github/workflows/ros-build-test.yml

@@ -1,4 +1,4 @@
-name: Build and Test OCS2
+name: Build and Test OCS2 (ROS2 Jazzy)
 
 on:
   push:
@@ -8,71 +8,106 @@ on:
 
 jobs:
   build:
-
-    # build both Debug and Release mode
     strategy:
       fail-fast: false
       matrix:
-        build_type: [ Debug, Release ]
+        include:
+          - build_type: Release
+            build_testing: "ON"
+            run_tests: true
+            packages_up_to: ""
+          # Full Debug + tests is too slow for GitHub-hosted runners; still build Debug
+          # (without tests) to catch compilation issues. Also only build up to core ROS
+          # interfaces to keep disk usage below GitHub runner limits.
+          - build_type: Debug
+            build_testing: "OFF"
+            run_tests: false
+            packages_up_to: "ocs2_ros_interfaces"
 
-    # environment: regular Ubuntu with a vanilla ROS container
     runs-on: ubuntu-latest
     container: 
-      image: ros:noetic
+      image: ros:jazzy-ros-base
+
+    env:
+      DEBIAN_FRONTEND: noninteractive
 
     steps:
     - name: Environment Info
       run: |
         pwd
         uname -r
-        lsb_release -a
+        cat /etc/os-release
 
     - name: System deps
       run: |
-        apt-get update
-        apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync
+        apt-get update && apt-get install -y --no-install-recommends \
+          build-essential cmake git ninja-build \
+          curl ca-certificates gnupg lsb-release \
+          python3-colcon-common-extensions python3-rosdep python3-pip \
+          python3-dev pybind11-dev \
+          libeigen3-dev libboost-all-dev libglpk-dev \
+          libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \
+          liburdfdom-dev \
+          ros-jazzy-eigen3-cmake-module \
+          ros-jazzy-hpp-fcl \
+          ros-jazzy-grid-map \
+          ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \
+        && rm -rf /var/lib/apt/lists/*
 
-    - uses: actions/checkout@v2
-      with: 
-        path: src/ocs2 
+    - name: Install Pinocchio (robotpkg)
+      run: |
+        install -d -m 0755 /etc/apt/keyrings
+        curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null
+        echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list
+        apt-get update && apt-get install -y --no-install-recommends \
+          robotpkg-pinocchio robotpkg-coal \
+        && rm -rf /var/lib/apt/lists/*
 
-    - name: Rosdep
+    - name: Set OpenRobots env
       run: |
-        rosdep update
-        rosdep install --from-paths src --ignore-src -r -y
+        echo "CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH}" >> $GITHUB_ENV
+        echo "LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH}" >> $GITHUB_ENV
 
-    - name: Checkout dependencies
+    - name: Pin setuptools < 80
       run: |
-        git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio
-        git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl
-        git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets
-
-    - name: Install RaiSim
+        pip3 install --no-cache-dir --break-system-packages 'setuptools<80'
+
+    - name: Checkout
+      uses: actions/checkout@v4
+      with: 
+        path: src/ocs2 
+
+    - name: Checkout dependencies
       run: |
-        git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech
-        cd src/raisim_tech
-        mkdir build
-        cd build
-        cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
-        make -j4 && checkinstall
-
-    - name: Install ONNX Runtime
+        git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets
+        git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy
+        cd src/elevation_mapping_cupy
+        git sparse-checkout set plane_segmentation
+
+    - name: Rosdep
+      shell: bash
       run: |
-        wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft
-        tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft
-        rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime
-        rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib
-        rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime
+        if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi
+        rosdep update
+        source /opt/ros/jazzy/setup.bash
+        rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio
 
     - name: Build (${{ matrix.build_type }})
       shell: bash
       run: |
-        source /opt/ros/noetic/setup.bash
-        catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} 
+        source /opt/ros/jazzy/setup.bash
+        if [ -n "${{ matrix.packages_up_to }}" ]; then
+          colcon build --packages-up-to ${{ matrix.packages_up_to }} --event-handlers console_direct+ \
+            --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }}
+        else
+          colcon build --event-handlers console_direct+ \
+            --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }}
+        fi
 
     - name: Test
+      if: ${{ matrix.run_tests }}
       shell: bash
       run: |
-        source devel_isolated/setup.bash
-        catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests
-        catkin_test_results
+        source install/setup.bash
+        colcon test --event-handlers console_direct+
+        colcon test-result --verbose

README.md

@@ -1,15 +1,66 @@
-# OCS2 Toolbox
+# OCS2 Toolbox (ROS 2)
 
-## Summary
-OCS2 is a C++ toolbox tailored for Optimal Control for Switched Systems (OCS2). The toolbox provides an efficient implementation of the following algorith
-
-* SLQ: Continuous-time domin DDP
-* iLQR: Discrete-time domain DDP
-* SQP: Multiple-shooting algorithm based on HPIPM
-* PISOC: Path integral stochatic optimal control
+This branch (`ros2`) ports OCS2 from ROS 1/catkin to **ROS 2 (colcon/ament)** and currently targets **Ubuntu 24.04 + ROS 2 Jazzy**.
+For the ROS 1 version, see branch `main`.
 
 ![legged-robot](https://leggedrobotics.github.io/ocs2/_static/gif/legged_robot.gif)
 
-OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides the user with additional tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate its deployment on robotic platforms, the OCS2 provides tools for ROS interfaces. The toolbox’s efficient and numerically stable implementations in conjunction with its user-friendly interface have paved the way for employing it on numerous robotic applications with limited onboard computation power.
+OCS2 (**O**ptimal **C**ontrol for **S**witched **S**ystems) is a C++ toolbox for formulating and solving nonlinear optimal control problems, with an emphasis on real-time **Model Predictive Control (MPC)** for robotics.
+
+OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate deployment on robotic platforms, OCS2 provides tools for ROS interfaces.
+
+## What’s included
+
+- **Optimal control solvers**
+  - **SLQ**: continuous-time constrained DDP
+  - **iLQR**: discrete-time constrained DDP
+  - **SQP**: multiple-shooting SQP (QP subproblems via HPIPM/BLASFEO)
+  - **SLP**: sequential linear programming (PIPG)
+  - **IPM**: multiple-shooting nonlinear interior-point method
+- **Robotics tooling**: URDF/Pinocchio integration, kinematics, centroidal models, and self-collision constraints (HPP-FCL).
+- **ROS integration**: messages, nodes, and visualization/plotting tools for deploying MPC on robots.
+- **Robotic examples**: end-to-end MPC examples (and ROS wrappers) for double integrator, cartpole, ballbot, quadrotor, mobile manipulator, legged robot, and more.
+
+## Port status (ROS 2 Jazzy)
+
+- This branch replaces catkin with ament/colcon across the workspace.
+- `blasfeo_catkin` / `hpipm_catkin` keep their names but are ROS 2 `ament_cmake` packages.
+- `ocs2_mpcnet` and `ocs2_raisim` are currently not ported and are ignored via `COLCON_IGNORE`.
+- `rqt_multiplot` is not released for Jazzy on Ubuntu 24.04; multiplot launch files are optional.
+
+## Installation
+
+Follow the branch-specific instructions in `installation.md` (dependencies, Pinocchio on Jazzy, and colcon build):
+- [`installation.md`](installation.md)
+
+Optional Docker environment:
+- [`docker/README.md`](docker/README.md)
+
+## Run an example
+
+After building and sourcing your workspace:
+
+```bash
+ros2 launch ocs2_ballbot_ros ballbot_mpc_mrt.launch.py
+```
+
+## Documentation
+
+Project documentation is hosted at:
+- https://leggedrobotics.github.io/ocs2/
+
+Note: the online docs still describe the ROS 1/catkin workflow; the solver concepts and APIs are largely identical, but build/launch instructions differ on this branch.
+
+## Citing OCS2
+
+```latex
+@misc{OCS2,
+  title  = {{OCS2}: An open source library for Optimal Control of Switched Systems},
+  note   = {[Online]. Available: \url{https://github.com/leggedrobotics/ocs2}},
+  author = {Farbod Farshidian and others}
+}
+```
+
+## License
 
-For more information refer to the project's [Documentation Page](https://leggedrobotics.github.io/ocs2/) 
+BSD 3-Clause, see `LICENCE.txt`.

docker/Dockerfile.jazzy

@@ -0,0 +1,61 @@
+FROM ros:jazzy-ros-base
+
+SHELL ["/bin/bash", "-c"]
+ENV DEBIAN_FRONTEND=noninteractive
+
+# System deps needed by OCS2 (core + pinocchio + plane segmentation + examples).
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    build-essential cmake git \
+    curl ca-certificates gnupg lsb-release \
+    python3-colcon-common-extensions python3-rosdep \
+    python3-dev python3-pip pybind11-dev \
+    libeigen3-dev libboost-all-dev libglpk-dev \
+    libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \
+    liburdfdom-dev \
+    ros-jazzy-eigen3-cmake-module \
+    ros-jazzy-hpp-fcl \
+    ros-jazzy-grid-map \
+    ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \
+  && rm -rf /var/lib/apt/lists/*
+
+# Pinocchio on Jazzy: install from OpenRobots robotpkg (rosdep maps it to a non-existent ros-jazzy-pinocchio).
+RUN install -d -m 0755 /etc/apt/keyrings && \
+    curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null && \
+    echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list && \
+    apt-get update && apt-get install -y --no-install-recommends \
+      robotpkg-pinocchio robotpkg-coal \
+    && rm -rf /var/lib/apt/lists/*
+
+ENV CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH}
+ENV LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH}
+
+# Pin setuptools < 80 for colcon --symlink-install compatibility
+# See: https://github.com/colcon/colcon-core/issues/696
+RUN pip3 install --no-cache-dir --break-system-packages 'setuptools<80'
+
+# rosdep (optional but convenient).
+RUN if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi && rosdep update
+
+WORKDIR /ws
+RUN mkdir -p /ws/src
+
+# Copy OCS2 repo (build context should be the root of this repository).
+COPY . /ws/src/ocs2
+
+# Assets used by many examples/tests.
+RUN git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets
+
+# Plane segmentation packages used by perceptive examples (convex_plane_decomposition*, grid_map_filters_rsl).
+RUN git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git /ws/src/elevation_mapping_cupy && \
+    cd /ws/src/elevation_mapping_cupy && \
+    git sparse-checkout set plane_segmentation
+
+# Install ROS deps from package.xml, then build.
+RUN source /opt/ros/jazzy/setup.bash && \
+    rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio && \
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+
+RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \
+    echo "source /ws/install/setup.bash" >> /root/.bashrc
+
+CMD ["bash"]

docker/README.md

@@ -0,0 +1,16 @@
+## OCS2 Jazzy Docker
+
+Build:
+
+```bash
+docker build -f docker/Dockerfile.jazzy -t ocs2:jazzy .
+```
+
+Run:
+
+```bash
+docker run --rm -it --net=host ocs2:jazzy
+```
+
+Note: the Dockerfile clones `ocs2_robotic_assets` from its `ros2` branch.
+It also sparse-checkouts the `plane_segmentation/` packages from `elevation_mapping_cupy` (branch `ros2`) for the perceptive examples.