From 56b496d0bb0783acc217fb7cfb97c46888db1775 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:32:38 +0900 Subject: [PATCH 01/27] [readme] update install/compile information --- README.md | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index f21ce17..d3b76b8 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ Collection of centroidal control for legged robots ### Requirements - Compiler supporting C++17 -- Tested on `Ubuntu 20.04 / ROS Noetic` and `Ubuntu 18.04 / ROS Melodic` +- Tested on `Ubuntu 22.04 / ROS Humble` ### Dependencies This package depends on @@ -21,11 +21,14 @@ This package also depends on the following packages. However, manual installatio - [NMPC](https://github.com/isri-aist/NMPC) ### Preparation -1. (Skip if ROS is already installed.) Install ROS. See [here](http://wiki.ros.org/ROS/Installation) for details. -```bash -$ export ROS_DISTRO=melodic -$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' -$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add - +1. (Skip if ROS is already installed.) Install ROS. See [here](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html) for details. +```bash +$ export ROS_DISTRO=humble +$ sudo apt install software-properties-common +$ sudo add-apt-repository universe +$ sudo apt update && sudo apt install curl -y +$ sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +$ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null $ sudo apt-get update $ sudo apt-get install ros-${ROS_DISTRO}-ros-base python-catkin-tools python-rosdep ``` @@ -37,7 +40,7 @@ $ sudo apt-get install libmc-rtc-dev mc-rtc-utils ros-${ROS_DISTRO}-mc-rtc-plugi ``` ### Installation procedure -1. Setup catkin workspace. +1. Setup colcon workspace. ```bash $ mkdir -p ~/ros/ws_ccc/src $ cd ~/ros/ws_ccc @@ -57,11 +60,12 @@ $ rosdep install -y -r --from-paths src --ignore-src 3. Build a package. ```bash -$ catkin build centroidal_control_collection -DCMAKE_BUILD_TYPE=RelWithDebInfo --catkin-make-args all tests +$ colcon build --packages-select centroidal_control_collection --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo +$ colcon test --merge-install --packages-select centroidal_control_collection # [optional] to compile and run tests ``` ## Examples -Make sure that it is built with `--catkin-make-args tests` option. +Make sure that it is built with `-DBUILD_TESTING=ON` option. ### Methods based on bipedal dynamics The CoM and ZMP trajectories are planned according to the ZMP reference trajectory and the ZMP region boundaries as inputs, which are determined from a given footstep sequence (i.e., the position and timing of the foot landings). The CoM velocity is jumped by emulating a disturbance during motion. From b89bc90b7e9034d8c05267447faab38426a2fb5b Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:33:35 +0900 Subject: [PATCH 02/27] [CI] update catkin to colcon --- .github/workflows/ci-catkin.yaml | 145 ------------------------------- .github/workflows/ci-colcon.yaml | 139 +++++++++++++++++++++++++++++ 2 files changed, 139 insertions(+), 145 deletions(-) delete mode 100644 .github/workflows/ci-catkin.yaml create mode 100644 .github/workflows/ci-colcon.yaml diff --git a/.github/workflows/ci-catkin.yaml b/.github/workflows/ci-catkin.yaml deleted file mode 100644 index 5df10f1..0000000 --- a/.github/workflows/ci-catkin.yaml +++ /dev/null @@ -1,145 +0,0 @@ -name: CI of CentroidalControlCollection (catkin) - -on: - push: - branches: - - '**' - pull_request: - branches: - - '**' - -jobs: - - clang-format: - runs-on: ubuntu-20.04 - steps: - - name: Checkout repository code - uses: actions/checkout@v2 - - name: Install clang-format-10 - run: | - sudo apt-get -qq update - sudo apt-get -qq install clang-format-10 - - name: Run clang-format-check - run: | - ./.clang-format-check.sh - - build-and-test: - strategy: - fail-fast: false - matrix: - os: [ubuntu-20.04] - build-type: [Debug, RelWithDebInfo] - mc-rtc-version: [head, stable] - exclude: - - build-type: Debug - mc-rtc-version: stable - runs-on: ${{ matrix.os }} - steps: - - name: Set ROS version - run: | - if [ "${{ matrix.os }}" == "ubuntu-20.04" ] - then - echo "ROS_DISTRO=noetic" >> $GITHUB_ENV - echo "PYTHON_PACKAGE_PREFIX=python3" >> $GITHUB_ENV - else # if [ "${{ matrix.os }}" == "ubuntu-18.04" ] - echo "ROS_DISTRO=melodic" >> $GITHUB_ENV - echo "PYTHON_PACKAGE_PREFIX=python" >> $GITHUB_ENV - fi - - name: Install ROS - run: | - set -e - set -x - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update -qq - sudo apt-get install -qq ros-${ROS_DISTRO}-ros-base ${PYTHON_PACKAGE_PREFIX}-catkin-tools ${PYTHON_PACKAGE_PREFIX}-rosdep doxygen graphviz - - name: Install mc_rtc - run: | - set -e - set -x - curl -1sLf 'https://dl.cloudsmith.io/public/mc-rtc/${{ matrix.mc-rtc-version }}/setup.deb.sh' | sudo -E bash - sudo apt-get install -qq libmc-rtc-dev libeigen-qld-dev - - name: Setup catkin workspace - run: | - mkdir -p ${GITHUB_WORKSPACE}/catkin_ws/src/ - cd ${GITHUB_WORKSPACE}/catkin_ws - set +x - . /opt/ros/${ROS_DISTRO}/setup.bash - set -x - catkin init - catkin build --limit-status-rate 0.1 - - name: Checkout repository code - uses: actions/checkout@v2 - with: - submodules: recursive - path: catkin_ws/src/CentroidalControlCollection - - name: Checkout QpSolverCollection - uses: actions/checkout@v2 - with: - repository: isri-aist/QpSolverCollection - submodules: recursive - path: catkin_ws/src/QpSolverCollection - - name: Checkout ForceControlCollection - uses: actions/checkout@v2 - with: - repository: isri-aist/ForceControlCollection - submodules: recursive - path: catkin_ws/src/ForceControlCollection - - name: Checkout NMPC - uses: actions/checkout@v2 - with: - repository: isri-aist/NMPC - submodules: recursive - path: catkin_ws/src/NMPC - - name: Rosdep install - run: | - set -e - set -x - cd ${GITHUB_WORKSPACE}/catkin_ws - set +x - . devel/setup.bash - set -x - sudo rosdep init - rosdep update - rosdep install -y -r --from-paths src --ignore-src - - name: Catkin build - run: | - set -e - set -x - cd ${GITHUB_WORKSPACE}/catkin_ws - set +x - . devel/setup.bash - set -x - catkin build --limit-status-rate 0.1 -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=ON - - name: Run tests - run: | - set -e - set -x - cd ${GITHUB_WORKSPACE}/catkin_ws - set +x - . devel/setup.bash - set -x - catkin build --limit-status-rate 0.1 --catkin-make-args run_tests -- centroidal_control_collection --no-deps - catkin_test_results --verbose --all build - - name: Upload documentation - # Only run for one configuration and on origin master branch - if: matrix.os == 'ubuntu-20.04' && matrix.build-type == 'RelWithDebInfo' && matrix.mc-rtc-version == 'head' && github.repository_owner == 'isri-aist' && github.ref == 'refs/heads/master' - run: | - set -e - set -x - cd ${GITHUB_WORKSPACE}/catkin_ws/src/CentroidalControlCollection - git config --global user.name "Masaki Murooka" - git config --global user.email "m-murooka@aist.go.jp" - git remote set-url origin "https://mmurooka:${{ secrets.CI_TOKEN }}@github.com/isri-aist/CentroidalControlCollection" - git fetch --depth=1 origin gh-pages:gh-pages - git checkout --quiet gh-pages - rm -rf doxygen/ cmake/ - cp -r ${GITHUB_WORKSPACE}/catkin_ws/build/centroidal_control_collection/doc/html/ doxygen - git add doxygen - git_status=`git status -s` - if test -n "$git_status"; then - git commit --quiet -m "Update Doxygen HTML files from commit ${{ github.sha }}" - git push origin gh-pages - else - echo "Github pages documentation is already up-to-date." - fi diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml new file mode 100644 index 0000000..a8d3eb2 --- /dev/null +++ b/.github/workflows/ci-colcon.yaml @@ -0,0 +1,139 @@ +name: CI of CentroidalControlCollection (colcon) + +on: + push: + branches: + - '**' + pull_request: + branches: + - '**' + +jobs: + + clang-format: + runs-on: ubuntu-22.04 + steps: + - name: Checkout repository code + uses: actions/checkout@v3 + - name: Install clang-format-14 + run: | + sudo apt-get -qq update + sudo apt-get -qq install clang-format-14 + - name: Run clang-format-check + run: | + ./.clang-format-check.sh + + build-and-test: + strategy: + fail-fast: false + matrix: + os: [ubuntu-22.04] + build-type: [Debug, RelWithDebInfo] + standalone: [standalone, colcon] + runs-on: ${{ matrix.os }} + steps: + - name: Set environment variables + run: | + set -e + set -x + echo "LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH" >> $GITHUB_ENV + if [ "${{ matrix.os }}" == "ubuntu-22.04" ] && \ + [ "${{ matrix.build-type }}" == "RelWithDebInfo" ] && \ + [ "${{ matrix.standalone }}" == "colcon" ] && \ + [ "${{ github.repository_owner }}" == "isri-aist" ] && \ + [ "${{ github.ref }}" == "refs/heads/master" ] + then + echo "UPLOAD_DOCUMENTATION=true" >> $GITHUB_ENV + sudo apt-get install -qq doxygen graphviz + else + echo "UPLOAD_DOCUMENTATION=false" >> $GITHUB_ENV + fi + - name: Install ROS2 + if: matrix.standalone == 'colcon' + uses: jrl-umi3218/github-actions/install-dependencies@master + with: + ros: | + apt: ros-base + - name: Install Dependencies + uses: jrl-umi3218/github-actions/install-dependencies@master + with: + build-type: ${{ matrix.build-type }} + ubuntu: | + apt-mirrors: + mc-rtc: + cloudsmith: mc-rtc/head + apt: libgtest-dev libeigen-qld-dev libmc-rtc-dev + + - name: Checkout repository code + uses: actions/checkout@v3 + with: + submodules: recursive + path: colcon_ws/src/CentroidalControlCollection + - name: Checkout QpSolverCollection + uses: actions/checkout@v3 + with: + repository: isri-aist/QpSolverCollection + path: colcon_ws/src/QpSolverCollection + - name: Checkout ForceControlCollection + uses: actions/checkout@v3 + with: + repository: isri-aist/ForceControlCollection + path: colcon_ws/src/ForceControlCollection + - name: Checkout NMPC + uses: actions/checkout@v3 + with: + repository: isri-aist/NMPC + submodules: recursive + path: colcon_ws/src/NMPC + - name: Rosdep install + run: | + set -e + set -x + cd ${GITHUB_WORKSPACE}/colcon_ws + set +x + . install/setup.bash + set -x + sudo rosdep init + rosdep update + rosdep install -y -r --from-paths src --ignore-src + - name: Colcon build + run: | + set -e + set -x + cd ${GITHUB_WORKSPACE}/colcon_ws + set +x + . install/setup.bash + set -x + colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=ON + - name: Run tests + run: | + set -e + set -x + cd ${GITHUB_WORKSPACE}/colcon_ws + set +x + . install/setup.bash + set -x + colcon test --merge-install --packages-select centroidal_control_collection + colcon test-result --all --verbose + - name: Upload documentation + # Only run for one configuration and on origin master branch + if: matrix.os == 'ubuntu-20.04' && matrix.build-type == 'RelWithDebInfo' && matrix.mc-rtc-version == 'head' && github.repository_owner == 'isri-aist' && github.ref == 'refs/heads/master' + run: | + set -e + set -x + cd ${GITHUB_WORKSPACE}/colcon_ws/src/${{ github.repository }} + git config --global user.name "Masaki Murooka" + git config --global user.email "m-murooka@aist.go.jp" + git remote set-url origin "https://mmurooka:${{ secrets.CI_TOKEN }}@github.com/isri-aist/CentroidalControlCollection" + git fetch --depth=1 origin gh-pages:gh-pages + git checkout --quiet gh-pages + rm -rf doxygen/ cmake/ + cp -r ${GITHUB_WORKSPACE}/colcon_ws/build/centroidal_control_collection/doc/html/ doxygen + git add doxygen + git_status=`git status -s` + if test -n "$git_status"; then + git commit --quiet -m "Update Doxygen HTML files from commit ${{ github.sha }}" + git push origin gh-pages + else + echo "Github pages documentation is already up-to-date." + fi From 06500dc2565b0b1074521787510db745f795cfa8 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:35:26 +0900 Subject: [PATCH 03/27] [CI] add packaging for Jammy --- .github/workflows/package.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/package.yaml b/.github/workflows/package.yaml index 7e05280..fc29623 100644 --- a/.github/workflows/package.yaml +++ b/.github/workflows/package.yaml @@ -22,6 +22,11 @@ jobs: uses: jrl-umi3218/github-actions/.github/workflows/package-project.yml@master with: main-repo: isri-aist/CentroidalControlCollection + matrix: | + { + "dist": ["jammy"], + "arch": ["amd64"] + } secrets: CLOUDSMITH_API_KEY: ${{ secrets.CLOUDSMITH_API_KEY }} GH_TOKEN: ${{ secrets.GH_PAGES_TOKEN }} From a0933faa9030a232ce8417158a521de5be631aa3 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:35:53 +0900 Subject: [PATCH 04/27] [CI] build standalone for Jammy --- .github/workflows/ci-standalone.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-standalone.yaml b/.github/workflows/ci-standalone.yaml index 6fac3dc..810c56f 100644 --- a/.github/workflows/ci-standalone.yaml +++ b/.github/workflows/ci-standalone.yaml @@ -13,7 +13,7 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-20.04] + os: [ubuntu-22.04] build-type: [Debug, RelWithDebInfo] compiler: [gcc, clang] runs-on: ${{ matrix.os }} From 5ba87d13290c1da17657d89a815c1b3ff31f755c Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:36:45 +0900 Subject: [PATCH 05/27] [submodules] remove cmake submodules --- .gitmodules | 3 --- cmake | 1 - 2 files changed, 4 deletions(-) delete mode 100644 .gitmodules delete mode 160000 cmake diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 85a3994..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "cmake"] - path = cmake - url = https://github.com/jrl-umi3218/jrl-cmakemodules diff --git a/cmake b/cmake deleted file mode 160000 index 277d1bd..0000000 --- a/cmake +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 277d1bd8a5491e6235413bd716756249c3922232 From 101e01c9ab700794bd409b04f33a285aaa8456d2 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:37:27 +0900 Subject: [PATCH 06/27] [clang] update clang version --- .clang-format-common.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.clang-format-common.sh b/.clang-format-common.sh index 767428a..3409527 100644 --- a/.clang-format-common.sh +++ b/.clang-format-common.sh @@ -1,12 +1,12 @@ # This script is meant to be sourced from other scripts -# Check for clang-format, prefer 10 if available -if [[ -x "$(command -v clang-format-10)" ]]; then - clang_format=clang-format-10 +# Check for clang-format, prefer 14 if available +if [[ -x "$(command -v clang-format-14)" ]]; then + clang_format=clang-format-14 elif [[ -x "$(command -v clang-format)" ]]; then clang_format=clang-format else - echo "clang-format or clang-format-10 must be installed" + echo "clang-format or clang-format-14 must be installed" exit 1 fi From ccc1c2a0322a299f78baebead7e239508c0c8098 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:45:10 +0900 Subject: [PATCH 07/27] [test] make test ros free --- tests/CMakeLists.txt | 52 ++++++++++++++++++++------------------------ 1 file changed, 23 insertions(+), 29 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d671d98..7f1d221 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,20 +1,14 @@ -if(NOT DEFINED CATKIN_DEVEL_PREFIX) - find_package(GTest REQUIRED) - include(GoogleTest) - # Prevents discovery failure before install - # No effect in CMake < 3.18 - set(CMAKE_GTEST_DISCOVER_TESTS_DISCOVERY_MODE PRE_TEST) - function(add_CCC_test NAME) - add_executable(${NAME} src/${NAME}.cpp) - target_link_libraries(${NAME} PUBLIC GTest::gtest CCC) - gtest_discover_tests(${NAME}) - endfunction() -else() - function(add_CCC_test NAME) - catkin_add_gtest(${NAME} src/${NAME}.cpp) - target_link_libraries(${NAME} CCC) - endfunction() -endif() +find_package(GTest REQUIRED) +include(GoogleTest) +# Prevents discovery failure before install +# No effect in CMake < 3.18 +set(CMAKE_GTEST_DISCOVER_TESTS_DISCOVERY_MODE PRE_TEST) + +function(add_CCC_test NAME) + add_executable(${NAME} src/${NAME}.cpp) + target_link_libraries(${NAME} PUBLIC GTest::gtest CCC) + gtest_discover_tests(${NAME}) +endfunction() set(CCC_gtest_list TestStateSpaceModel @@ -40,17 +34,17 @@ foreach(NAME IN LISTS CCC_gtest_list) endforeach() option(ENABLE_PYBULLET_TEST "Enable tests with pybullet simulation" OFF) -if(ENABLE_PYBULLET_TEST) - find_package(rostest REQUIRED) +# if(ENABLE_PYBULLET_TEST) +# find_package(rostest REQUIRED) - set(CCC_rostest_list - TestSimDdpSingleRigidBody - ) +# set(CCC_rostest_list +# TestSimDdpSingleRigidBody +# ) - if(DEFINED CATKIN_DEVEL_PREFIX) - foreach(NAME IN LISTS CCC_rostest_list) - add_rostest_gtest(${NAME} test/${NAME}.test src/${NAME}.cpp) - target_link_libraries(${NAME} CCC) - endforeach() - endif() -endif() +# if(USE_ROS2) +# foreach(NAME IN LISTS CCC_rostest_list) +# add_rostest_gtest(${NAME} test/${NAME}.test src/${NAME}.cpp) +# target_link_libraries(${NAME} CCC) +# endforeach() +# endif() +# endif() From 233230c231647ed0b716f564582b41cd2c99bcff Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:45:27 +0900 Subject: [PATCH 08/27] [doc] configure doxyfile --- doc/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 9951fec..ee53e0a 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -3,6 +3,8 @@ find_package(Doxygen REQUIRED) if(DOXYGEN_FOUND) set(DOXYFILE_PATH ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) + configure_file(Doxyfile.extra.in ${DOXYFILE_PATH}) + add_custom_target(CentroidalControlCollection_doc ALL ${DOXYGEN_EXECUTABLE} ${DOXYFILE_PATH} DEPENDS ${DOXYFILE_PATH} From 525a19ed076462a2aa53ab3e08fda0bf8e7ffe73 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:45:45 +0900 Subject: [PATCH 09/27] Mirgrate to ROS2 --- CMakeLists.txt | 56 +++++++++++++++++++++---------------------- include/CCC/console.h | 8 +++---- package.xml | 11 +++++---- src/CMakeLists.txt | 44 ++++++++++++++++++---------------- 4 files changed, 61 insertions(+), 58 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1685f47..a6ff50f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,49 +10,47 @@ set(CMAKE_CXX_STANDARD 17) set(PROJECT_USE_CMAKE_EXPORT TRUE) set(CXX_DISABLE_WERROR ON) set(CMAKE_COLOR_DIAGNOSTICS ON) + option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) +option(USE_ROS2 "Use ROS2" OFF) +option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) -include(cmake/base.cmake) project(centroidal_control_collection LANGUAGES CXX) -if(DEFINED CATKIN_DEVEL_PREFIX) - set(DOXYGEN_HTML_OUTPUT html) - +if(USE_ROS2) # mc_rtc - add_project_dependency(mc_rtc REQUIRED) + find_package(mc_rtc REQUIRED) - find_package(catkin REQUIRED COMPONENTS - roscpp - qp_solver_collection - force_control_collection - nmpc_ddp - ) - - catkin_package( - CATKIN_DEPENDS - roscpp - qp_solver_collection - force_control_collection - nmpc_ddp - DEPENDS EIGEN3 - INCLUDE_DIRS include - LIBRARIES CCC - ) + find_package(rclcpp REQUIRED) + find_package(qp_solver_collection REQUIRED) + find_package(force_control_collection REQUIRED) + find_package(nmpc_ddp REQUIRED) else() - set(DOXYGEN_HTML_OUTPUT doxygen-html) - set(CATKIN_ENABLE_TESTING OFF) - option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) - add_project_dependency(qp_solver_collection REQUIRED) - add_project_dependency(force_control_collection REQUIRED) - add_project_dependency(nmpc_ddp REQUIRED) + set(BUILD_TESTING OFF) + find_package(qp_solver_collection REQUIRED) + find_package(force_control_collection REQUIRED) + find_package(nmpc_ddp REQUIRED) endif() add_subdirectory(src) -if(BUILD_TESTING OR CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) add_subdirectory(tests) endif() if(INSTALL_DOCUMENTATION) add_subdirectory(doc) endif() + +if(USE_ROS2) + ament_export_include_directories(include) + ament_export_libraries(CCC) + ament_export_dependencies( + rclcpp + qp_solver_collection + force_control_collection + nmpc_ddp + ) + ament_export_targets(CCC) + ament_package() +endif() \ No newline at end of file diff --git a/include/CCC/console.h b/include/CCC/console.h index 247c210..439c9c4 100644 --- a/include/CCC/console.h +++ b/include/CCC/console.h @@ -6,8 +6,8 @@ # define CCC_WARN_STREAM(x) std::cerr << x << "\n" # define CCC_INFO_STREAM(x) std::cout << x << "\n" #else -# include -# define CCC_ERROR_STREAM ROS_ERROR_STREAM -# define CCC_WARN_STREAM ROS_WARN_STREAM -# define CCC_INFO_STREAM ROS_INFO_STREAM +# include +# define CCC_ERROR_STREAM(msg) RCLCPP_ERROR_STREAM(rclcpp::get_logger("CentroidalControlCollection"), msg) +# define CCC_WARN_STREAM(msg) RCLCPP_WARN_STREAM(rclcpp::get_logger("CentroidalControlCollection"), msg) +# define CCC_INFO_STREAM(msg) RCLCPP_INFO_STREAM(rclcpp::get_logger("CentroidalControlCollection"), msg) #endif diff --git a/package.xml b/package.xml index 35fbdd7..3ae7552 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + centroidal_control_collection 0.2.0 @@ -12,16 +12,19 @@ catkin - roscpp + rclcpp qp_solver_collection force_control_collection nmpc_ddp eigen - rosunit - rostest + ament_cmake_gtest doxygen graphviz + + + ament_cmake + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 14d885f..a6cd518 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,26 +13,27 @@ add_library(CCC PreviewControlCentroidal.cpp DdpCentroidal.cpp DdpSingleRigidBody.cpp - ) -if(DEFINED CATKIN_DEVEL_PREFIX) - target_include_directories(CCC PUBLIC - ${catkin_INCLUDE_DIRS} - ) - target_link_libraries(CCC PUBLIC - ${catkin_LIBRARIES} mc_rtc::mc_rtc_utils mc_rtc::mc_rtc_gui - ) -else() - target_link_libraries(CCC PUBLIC - qp_solver_collection::QpSolverCollection - force_control_collection::ForceColl - nmpc_ddp::nmpc_ddp - ) - target_compile_definitions(CCC PUBLIC CCC_STANDALONE) -endif() +) + target_include_directories(CCC PUBLIC $ $ ) + +if(USE_ROS2) + target_link_libraries(CCC PUBLIC rclcpp::rclcpp) +else() + target_compile_definitions(CCC PUBLIC CCC_STANDALONE) +endif() + +target_link_libraries(CCC PUBLIC + mc_rtc::mc_rtc_utils + mc_rtc::mc_rtc_gui + qp_solver_collection::QpSolverCollection + force_control_collection::ForceColl + nmpc_ddp::nmpc_ddp +) + target_compile_features(CCC PUBLIC cxx_std_17) if(BUILD_SHARED_LIBS) @@ -40,9 +41,10 @@ if(BUILD_SHARED_LIBS) endif() install(TARGETS CCC - EXPORT "${TARGETS_EXPORT_NAME}" - LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" - ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" - RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" + EXPORT CCC + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/CCC DESTINATION "${INCLUDE_INSTALL_DIR}") + +install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/CCC DESTINATION include) From 741345fda0491164775210f1875dcc554e3cf917 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:47:33 +0900 Subject: [PATCH 10/27] cland format --- src/DdpCentroidal.cpp | 16 +++++++++------- src/DdpSingleRigidBody.cpp | 16 +++++++++------- tests/src/TestDdpCentroidal.cpp | 12 ++++++++---- tests/src/TestDdpSingleRigidBody.cpp | 12 ++++++++---- tests/src/TestDdpZmp.cpp | 6 ++++-- tests/src/TestLinearMpcXY.cpp | 6 ++++-- tests/src/TestPreviewControlCentroidal.cpp | 6 ++++-- tests/src/TestSimDdpSingleRigidBody.cpp | 6 ++++-- 8 files changed, 50 insertions(+), 30 deletions(-) diff --git a/src/DdpCentroidal.cpp b/src/DdpCentroidal.cpp index a514496..e09b242 100644 --- a/src/DdpCentroidal.cpp +++ b/src/DdpCentroidal.cpp @@ -199,13 +199,15 @@ DdpCentroidal::DdpCentroidal(double mass, double horizon_dt, int horizon_steps, ddp_solver_->config().initial_lambda = 1e-6; ddp_solver_->config().lambda_min = 1e-8; ddp_solver_->config().lambda_thre = 1e-7; - ddp_solver_->setInputLimitsFunc([this](double t) -> std::array { - std::array limits; - int input_dim = ddp_problem_->inputDim(t); - limits[0].setConstant(input_dim, force_scale_limits_[0]); - limits[1].setConstant(input_dim, force_scale_limits_[1]); - return limits; - }); + ddp_solver_->setInputLimitsFunc( + [this](double t) -> std::array + { + std::array limits; + int input_dim = ddp_problem_->inputDim(t); + limits[0].setConstant(input_dim, force_scale_limits_[0]); + limits[1].setConstant(input_dim, force_scale_limits_[1]); + return limits; + }); } Eigen::VectorXd DdpCentroidal::planOnce(const std::function & motion_param_func, diff --git a/src/DdpSingleRigidBody.cpp b/src/DdpSingleRigidBody.cpp index e1e7143..c09a9c3 100644 --- a/src/DdpSingleRigidBody.cpp +++ b/src/DdpSingleRigidBody.cpp @@ -269,13 +269,15 @@ DdpSingleRigidBody::DdpSingleRigidBody(double mass, ddp_solver_->config().initial_lambda = 1e-6; ddp_solver_->config().lambda_min = 1e-8; ddp_solver_->config().lambda_thre = 1e-7; - ddp_solver_->setInputLimitsFunc([this](double t) -> std::array { - std::array limits; - int input_dim = ddp_problem_->inputDim(t); - limits[0].setConstant(input_dim, force_scale_limits_[0]); - limits[1].setConstant(input_dim, force_scale_limits_[1]); - return limits; - }); + ddp_solver_->setInputLimitsFunc( + [this](double t) -> std::array + { + std::array limits; + int input_dim = ddp_problem_->inputDim(t); + limits[0].setConstant(input_dim, force_scale_limits_[0]); + limits[1].setConstant(input_dim, force_scale_limits_[1]); + return limits; + }); } Eigen::VectorXd DdpSingleRigidBody::planOnce(const std::function & motion_param_func, diff --git a/tests/src/TestDdpCentroidal.cpp b/tests/src/TestDdpCentroidal.cpp index 5df24c7..5005685 100644 --- a/tests/src/TestDdpCentroidal.cpp +++ b/tests/src/TestDdpCentroidal.cpp @@ -32,7 +32,8 @@ TEST(TestDdpCentroidal, PlanOnce) CCC::DdpCentroidal ddp(mass, horizon_dt, horizon_steps, weight_param); // Setup contact - std::function motion_param_func = [](double t) { + std::function motion_param_func = [](double t) + { // Add small values to avoid numerical instability at inequality bounds constexpr double epsilon_t = 1e-6; t += epsilon_t; @@ -52,7 +53,8 @@ TEST(TestDdpCentroidal, PlanOnce) } return motion_param; }; - std::function ref_data_func = [](double t) { + std::function ref_data_func = [](double t) + { // Add small values to avoid numerical instability at inequality bounds constexpr double epsilon_t = 1e-6; t += epsilon_t; @@ -181,13 +183,15 @@ TEST(TestDdpCentroidal, CheckDerivatives) auto ddp_problem = std::make_shared(horizon_dt, mass, weight_param); std::function motion_param_func = [](double // t - ) { + ) + { CCC::DdpCentroidal::MotionParam motion_param; motion_param.contact_list.push_back(makeContactFromRect({Eigen::Vector2d(-0.1, -0.1), Eigen::Vector2d(0.1, 0.1)})); return motion_param; }; std::function ref_data_func = [](double // t - ) { + ) + { CCC::DdpCentroidal::RefData ref_data; ref_data.pos << 0.1, -0.2, 1.0; // [m] return ref_data; diff --git a/tests/src/TestDdpSingleRigidBody.cpp b/tests/src/TestDdpSingleRigidBody.cpp index be71b00..bcf2841 100644 --- a/tests/src/TestDdpSingleRigidBody.cpp +++ b/tests/src/TestDdpSingleRigidBody.cpp @@ -34,7 +34,8 @@ TEST(TestDdpSingleRigidBody, PlanOnce) CCC::DdpSingleRigidBody ddp(mass, horizon_dt, horizon_steps, weight_param); // Setup contact - std::function motion_param_func = [moment_of_inertia](double t) { + std::function motion_param_func = [moment_of_inertia](double t) + { // Add small values to avoid numerical instability at inequality bounds constexpr double epsilon_t = 1e-6; t += epsilon_t; @@ -55,7 +56,8 @@ TEST(TestDdpSingleRigidBody, PlanOnce) motion_param.inertia_mat.diagonal() = moment_of_inertia; return motion_param; }; - std::function ref_data_func = [](double t) { + std::function ref_data_func = [](double t) + { // Add small values to avoid numerical instability at inequality bounds constexpr double epsilon_t = 1e-6; t += epsilon_t; @@ -202,14 +204,16 @@ TEST(TestDdpSingleRigidBody, CheckDerivatives) auto ddp_problem = std::make_shared(horizon_dt, mass, weight_param); std::function motion_param_func = [](double // t - ) { + ) + { CCC::DdpSingleRigidBody::MotionParam motion_param; motion_param.contact_list.push_back(makeContactFromRect({Eigen::Vector2d(-0.1, -0.1), Eigen::Vector2d(0.1, 0.1)})); motion_param.inertia_mat.diagonal() << 15.0, 10.0, 5.0; return motion_param; }; std::function ref_data_func = [](double // t - ) { + ) + { CCC::DdpSingleRigidBody::RefData ref_data; ref_data.pos << 0.1, -0.2, 1.0; // [m] ref_data.ori << -0.1, 0.2, -0.3; // [rad] diff --git a/tests/src/TestDdpZmp.cpp b/tests/src/TestDdpZmp.cpp index 61fcced..5726b0a 100644 --- a/tests/src/TestDdpZmp.cpp +++ b/tests/src/TestDdpZmp.cpp @@ -44,7 +44,8 @@ TEST(TestDdpZmp, Test1) Footstep(Foot::Left, Eigen::Vector2d(0.6, 0.1), 6.0, transit_duration, swing_duration)); footstep_manager.appendFootstep( Footstep(Foot::Right, Eigen::Vector2d(0.6, -0.1), 7.0, transit_duration, swing_duration)); - std::function ref_data_func = [&](double t) { + std::function ref_data_func = [&](double t) + { CCC::DdpZmp::RefData ref_data; ref_data.zmp << footstep_manager.refZmp(t), 0.0; ref_data.com_z = ref_com_height; @@ -159,7 +160,8 @@ TEST(TestDdpZmp, CheckDerivatives) auto ddp_problem = std::make_shared(horizon_dt, mass, weight_param); std::function ref_data_func = [](double // t - ) { + ) + { CCC::DdpZmp::RefData ref_data; ref_data.zmp << 0.1, -0.2, 0.3; // [m] ref_data.com_z = 1.0; // [m] diff --git a/tests/src/TestLinearMpcXY.cpp b/tests/src/TestLinearMpcXY.cpp index fe98ff6..c1b2ecc 100644 --- a/tests/src/TestLinearMpcXY.cpp +++ b/tests/src/TestLinearMpcXY.cpp @@ -26,7 +26,8 @@ TEST(TestLinearMpcXY, Test1) CCC::LinearMpcXY mpc(mass, horizon_dt, horizon_steps); // Setup contact - std::function motion_param_func = [mass](double t) { + std::function motion_param_func = [mass](double t) + { CCC::LinearMpcXY::MotionParam motion_param; motion_param.com_z = 1.0; // [m] motion_param.total_force_z = mass * CCC::constants::g; // [N] @@ -54,7 +55,8 @@ TEST(TestLinearMpcXY, Test1) motion_param.contact_list.push_back(makeContactFromRect(rect_min_max)); return motion_param; }; - std::function ref_data_func = [](double t) { + std::function ref_data_func = [](double t) + { CCC::LinearMpcXY::RefData ref_data; if(t < 3.0) { diff --git a/tests/src/TestPreviewControlCentroidal.cpp b/tests/src/TestPreviewControlCentroidal.cpp index 1b42c86..b64b1d0 100644 --- a/tests/src/TestPreviewControlCentroidal.cpp +++ b/tests/src/TestPreviewControlCentroidal.cpp @@ -28,7 +28,8 @@ TEST(TestPreviewControlCentroidal, PlanOnce) CCC::PreviewControlCentroidal pc(mass, moment_of_inertia, horizon_duration, horizon_dt); // Setup contact - std::function motion_param_func = [](double t) { + std::function motion_param_func = [](double t) + { // Add small values to avoid numerical instability at inequality bounds constexpr double epsilon_t = 1e-6; t += epsilon_t; @@ -50,7 +51,8 @@ TEST(TestPreviewControlCentroidal, PlanOnce) } return motion_param; }; - std::function ref_data_func = [](double t) { + std::function ref_data_func = [](double t) + { // Add small values to avoid numerical instability at inequality bounds constexpr double epsilon_t = 1e-6; t += epsilon_t; diff --git a/tests/src/TestSimDdpSingleRigidBody.cpp b/tests/src/TestSimDdpSingleRigidBody.cpp index 834ab00..be17d1c 100644 --- a/tests/src/TestSimDdpSingleRigidBody.cpp +++ b/tests/src/TestSimDdpSingleRigidBody.cpp @@ -48,7 +48,8 @@ class TestSimDdpSingleRigidBody initial_param_.pos = Eigen::Vector3d(0.0, 0.0, 1.0); // Setup contact - std::function motion_param_func = [this](double t) { + std::function motion_param_func = [this](double t) + { CCC::DdpSingleRigidBody::MotionParam motion_param; Eigen::Vector2d contact_pos = Eigen::Vector2d::Zero(); if(forward_duration_ && (*forward_duration_)[0] <= t && t <= (*forward_duration_)[1]) @@ -63,7 +64,8 @@ class TestSimDdpSingleRigidBody motion_param.inertia_mat.diagonal() = moment_of_inertia_; return motion_param; }; - std::function ref_data_func = [this](double t) { + std::function ref_data_func = [this](double t) + { CCC::DdpSingleRigidBody::RefData ref_data; ref_data.pos << 0.0, 0.0, 1.0; if(forward_duration_ && (*forward_duration_)[0] <= t && t <= (*forward_duration_)[1]) From 2871b6ebaed81fa5d1f9e350b0e7f099c6b2c548 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:53:43 +0900 Subject: [PATCH 11/27] [CI] remove install source before colcon build --- .github/workflows/ci-colcon.yaml | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml index a8d3eb2..d791627 100644 --- a/.github/workflows/ci-colcon.yaml +++ b/.github/workflows/ci-colcon.yaml @@ -90,10 +90,10 @@ jobs: set -e set -x cd ${GITHUB_WORKSPACE}/colcon_ws - set +x - . install/setup.bash - set -x - sudo rosdep init + if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ] + then + sudo rosdep init + fi rosdep update rosdep install -y -r --from-paths src --ignore-src - name: Colcon build @@ -101,9 +101,6 @@ jobs: set -e set -x cd ${GITHUB_WORKSPACE}/colcon_ws - set +x - . install/setup.bash - set -x colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=ON - name: Run tests run: | From ad0d2bfa70d020b8eecd47fd820c9dcf0e215b67 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:54:09 +0900 Subject: [PATCH 12/27] [CI] update condition on doc upload --- .github/workflows/ci-colcon.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml index d791627..9096a1e 100644 --- a/.github/workflows/ci-colcon.yaml +++ b/.github/workflows/ci-colcon.yaml @@ -114,7 +114,7 @@ jobs: colcon test-result --all --verbose - name: Upload documentation # Only run for one configuration and on origin master branch - if: matrix.os == 'ubuntu-20.04' && matrix.build-type == 'RelWithDebInfo' && matrix.mc-rtc-version == 'head' && github.repository_owner == 'isri-aist' && github.ref == 'refs/heads/master' + if: env.UPLOAD_DOCUMENTATION == 'true' run: | set -e set -x From af4efcfdb32bea16fb1989c2e085f22419ad83e4 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 13 Mar 2025 19:59:10 +0900 Subject: [PATCH 13/27] [CI] remove standalone tests and update install documentation build option --- .github/workflows/ci-colcon.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml index 9096a1e..efd879e 100644 --- a/.github/workflows/ci-colcon.yaml +++ b/.github/workflows/ci-colcon.yaml @@ -29,7 +29,7 @@ jobs: matrix: os: [ubuntu-22.04] build-type: [Debug, RelWithDebInfo] - standalone: [standalone, colcon] + standalone: [colcon] runs-on: ${{ matrix.os }} steps: - name: Set environment variables @@ -101,7 +101,7 @@ jobs: set -e set -x cd ${GITHUB_WORKSPACE}/colcon_ws - colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=ON + colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} - name: Run tests run: | set -e From 377eec88f86554c9919682ea9514e85218686bce Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 10:32:42 +0900 Subject: [PATCH 14/27] [CI] add missing flag to build --- .github/workflows/ci-colcon.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml index efd879e..fc192b3 100644 --- a/.github/workflows/ci-colcon.yaml +++ b/.github/workflows/ci-colcon.yaml @@ -101,7 +101,7 @@ jobs: set -e set -x cd ${GITHUB_WORKSPACE}/colcon_ws - colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} + colcon build --merge-install --cmake-args -DUSE_ROS2=ON -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} -DENABLE_QLD=ON -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} - name: Run tests run: | set -e From c0bf152a6d6874c3588c2b32e53696d543e49547 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 10:48:23 +0900 Subject: [PATCH 15/27] [CI] remove recursive for CCC --- .github/workflows/ci-colcon.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml index fc192b3..2ad692a 100644 --- a/.github/workflows/ci-colcon.yaml +++ b/.github/workflows/ci-colcon.yaml @@ -67,7 +67,6 @@ jobs: - name: Checkout repository code uses: actions/checkout@v3 with: - submodules: recursive path: colcon_ws/src/CentroidalControlCollection - name: Checkout QpSolverCollection uses: actions/checkout@v3 From 7ad1aeaa644c2b2726ed97cffc3a967bffd39512 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 10:53:33 +0900 Subject: [PATCH 16/27] [CI] update LD_LIBRARY_PATH --- .github/workflows/ci-standalone.yaml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-standalone.yaml b/.github/workflows/ci-standalone.yaml index 810c56f..78ea64e 100644 --- a/.github/workflows/ci-standalone.yaml +++ b/.github/workflows/ci-standalone.yaml @@ -18,9 +18,12 @@ jobs: compiler: [gcc, clang] runs-on: ${{ matrix.os }} steps: + - name: Set environment variables + run: | + set -e + set -x + echo "LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH" >> $GITHUB_ENV - uses: actions/checkout@v3 - with: - submodules: recursive - name: Install dependencies uses: jrl-umi3218/github-actions/install-dependencies@master with: From c6932be196c31a0eab7401ce8da9b6a1c8f2bba3 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 11:00:00 +0900 Subject: [PATCH 17/27] [cmake] install cmake config --- CMakeLists.txt | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a6ff50f..f83a364 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,55 @@ endif() add_subdirectory(src) +if(NOT USE_ROS2) + install(EXPORT ${PROJECT_NAME} + FILE ${PROJECT_NAME}Targets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} + NAMESPACE ${PROJECT_NAME}:: + ) + + # Write initial config cmake file + file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/Config.cmake.in" +" +\@PACKAGE_INIT\@ + +include(\"\${CMAKE_CURRENT_LIST_DIR}/\@PROJECT_NAME\@Targets.cmake\") +include(\"\${CMAKE_CURRENT_LIST_DIR}/qp_solver_options.cmake\") + +set(\@PROJECT_NAME\@_LIBRARIES \@PROJECT_NAME\@::CCC) +") + + include(CMakePackageConfigHelpers) + # generate the config file that is includes the exports + configure_package_config_file(${CMAKE_CURRENT_BINARY_DIR}/Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION "lib/cmake/${PROJECT_NAME}" + NO_SET_AND_CHECK_MACRO + NO_CHECK_REQUIRED_COMPONENTS_MACRO + ) + + # generate the version file for the config file + # Extract version numbers from package.xml + file(READ package.xml PACKAGE_XML) + string(REGEX MATCH "[0-9]+\\.[0-9]+\\.[0-9]+" DIRTY_VERSION_STRING ${PACKAGE_XML}) + string(REGEX REPLACE "^([0-9]+)\\.([0-9]+)\\.([0-9]+)$" "\\1" CCC_MAJOR_VERSION "${DIRTY_VERSION_STRING}") + string(REGEX REPLACE "^([0-9]+)\\.([0-9]+)\\.([0-9]+)$" "\\2" CCC_MINOR_VERSION "${DIRTY_VERSION_STRING}") + string(REGEX REPLACE "^([0-9]+)\\.([0-9]+)\\.([0-9]+)$" "\\3" CCC_PATCH_VERSION "${DIRTY_VERSION_STRING}") + write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION "${CCC_MAJOR_VERSION}.${CCC_MINOR_VERSION}.${CCC_PATCH_VERSION}" + COMPATIBILITY AnyNewerVersion + ) + + # install config files + install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + "${QP_SOLVER_OPTIONS_CMAKE_FILE}" + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} + ) +endif() + if(BUILD_TESTING) add_subdirectory(tests) endif() From 0e81e8be199d60f82b8e58bf1cd0252002eee7a3 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 13:04:48 +0900 Subject: [PATCH 18/27] [cmake] remove installed files from config --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f83a364..48e852f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,6 @@ if(NOT USE_ROS2) \@PACKAGE_INIT\@ include(\"\${CMAKE_CURRENT_LIST_DIR}/\@PROJECT_NAME\@Targets.cmake\") -include(\"\${CMAKE_CURRENT_LIST_DIR}/qp_solver_options.cmake\") set(\@PROJECT_NAME\@_LIBRARIES \@PROJECT_NAME\@::CCC) ") @@ -78,7 +77,6 @@ set(\@PROJECT_NAME\@_LIBRARIES \@PROJECT_NAME\@::CCC) install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" - "${QP_SOLVER_OPTIONS_CMAKE_FILE}" DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} ) endif() From a8f8ae46e39fc11326ef86a553cecd27da55d675 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 13:06:18 +0900 Subject: [PATCH 19/27] [cmake] update export target name --- CMakeLists.txt | 2 +- src/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 48e852f..2e1e83a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,6 +98,6 @@ if(USE_ROS2) force_control_collection nmpc_ddp ) - ament_export_targets(CCC) + ament_export_targets(${PROJECT_NAME}) ament_package() endif() \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a6cd518..801c1cb 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -41,7 +41,7 @@ if(BUILD_SHARED_LIBS) endif() install(TARGETS CCC - EXPORT CCC + EXPORT ${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin From 33f850bc28b8eda803f351c293a7627cf183c08c Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 13:19:06 +0900 Subject: [PATCH 20/27] [cmake] make find_package global --- CMakeLists.txt | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e1e83a..e500bdb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,19 +17,17 @@ option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) project(centroidal_control_collection LANGUAGES CXX) +find_package(mc_rtc REQUIRED) +find_package(qp_solver_collection REQUIRED) +find_package(force_control_collection REQUIRED) +find_package(nmpc_ddp REQUIRED) + if(USE_ROS2) # mc_rtc - find_package(mc_rtc REQUIRED) - + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) - find_package(qp_solver_collection REQUIRED) - find_package(force_control_collection REQUIRED) - find_package(nmpc_ddp REQUIRED) else() - set(BUILD_TESTING OFF) - find_package(qp_solver_collection REQUIRED) - find_package(force_control_collection REQUIRED) - find_package(nmpc_ddp REQUIRED) + option(BUILD_TESTING "Build test" ON) endif() add_subdirectory(src) From 775b60dd06e4989dbe707ad3498b897016618d46 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Fri, 14 Mar 2025 13:30:08 +0900 Subject: [PATCH 21/27] [CI] remove debug test --- .github/workflows/ci-colcon.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-colcon.yaml b/.github/workflows/ci-colcon.yaml index 2ad692a..093beac 100644 --- a/.github/workflows/ci-colcon.yaml +++ b/.github/workflows/ci-colcon.yaml @@ -28,7 +28,7 @@ jobs: fail-fast: false matrix: os: [ubuntu-22.04] - build-type: [Debug, RelWithDebInfo] + build-type: [RelWithDebInfo] standalone: [colcon] runs-on: ${{ matrix.os }} steps: From 64b929a68ac0adfd5fc6a034b31b5e9f99666048 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Wed, 28 May 2025 20:36:43 +0900 Subject: [PATCH 22/27] feat(cmake): update min cmake version based on warning --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e500bdb..989f8d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) set(PROJECT_NAME centroidal_control_collection) set(PROJECT_GENERATED_HEADERS_SKIP_DEPRECATED ON) From 6a80b6d6fca1fa2d937f8388349d5992019a0295 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Wed, 28 May 2025 20:37:20 +0900 Subject: [PATCH 23/27] fix(package): add correct buildtool and depend on NMPC --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 3ae7552..d7f79ea 100644 --- a/package.xml +++ b/package.xml @@ -10,12 +10,12 @@ http://ros.org/wiki/centroidal_control_collection Masaki Murooka - catkin + ament_cmake rclcpp qp_solver_collection force_control_collection - nmpc_ddp + NMPC eigen From b49c236080aa6483995b32f5dc0d65e256e6fb82 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Wed, 2 Jul 2025 14:59:00 +0900 Subject: [PATCH 24/27] build(fix): export transitive dependencies --- CMakeLists.txt | 15 +++------------ cmake/Config.cmake.in | 14 ++++++++++++++ 2 files changed, 17 insertions(+), 12 deletions(-) create mode 100644 cmake/Config.cmake.in diff --git a/CMakeLists.txt b/CMakeLists.txt index 989f8d5..439f373 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ option(USE_ROS2 "Use ROS2" OFF) option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) project(centroidal_control_collection LANGUAGES CXX) +include(GNUInstallDirs) # For CMAKE_INSTALL_LIBDIR find_package(mc_rtc REQUIRED) find_package(qp_solver_collection REQUIRED) @@ -39,21 +40,11 @@ if(NOT USE_ROS2) NAMESPACE ${PROJECT_NAME}:: ) - # Write initial config cmake file - file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/Config.cmake.in" -" -\@PACKAGE_INIT\@ - -include(\"\${CMAKE_CURRENT_LIST_DIR}/\@PROJECT_NAME\@Targets.cmake\") - -set(\@PROJECT_NAME\@_LIBRARIES \@PROJECT_NAME\@::CCC) -") - include(CMakePackageConfigHelpers) # generate the config file that is includes the exports - configure_package_config_file(${CMAKE_CURRENT_BINARY_DIR}/Config.cmake.in + configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" - INSTALL_DESTINATION "lib/cmake/${PROJECT_NAME}" + INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}" NO_SET_AND_CHECK_MACRO NO_CHECK_REQUIRED_COMPONENTS_MACRO ) diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in new file mode 100644 index 0000000..547d964 --- /dev/null +++ b/cmake/Config.cmake.in @@ -0,0 +1,14 @@ + +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + + +find_dependency(Eigen3 REQUIRED) +find_dependency(mc_rtc REQUIRED) +find_dependency(qp_solver_collection REQUIRED) +find_dependency(force_control_collection REQUIRED) +find_dependency(nmpc_ddp REQUIRED) + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") +set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@::CCC) \ No newline at end of file From 24102269991fc1e7950ec1bdcb74737187d85dab Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 10 Jul 2025 13:04:19 +0900 Subject: [PATCH 25/27] build(cmake): CCC as shared library --- src/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 801c1cb..6a17b34 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(CCC +add_library(CCC SHARED CommonModels.cpp PreviewControlZmp.cpp DdpZmp.cpp From 458b38fb0f74ce9118106672bcc93b20bff85e64 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 10 Jul 2025 13:05:07 +0900 Subject: [PATCH 26/27] test(python): migrate to ros2 --- CMakeLists.txt | 4 +-- tests/CMakeLists.txt | 29 ++++++++-------- tests/scripts/simSingleRigidBody.py | 54 ++++++++++++++++++++++------- 3 files changed, 58 insertions(+), 29 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 439f373..3e5f72d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ set(CMAKE_COLOR_DIAGNOSTICS ON) option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) option(USE_ROS2 "Use ROS2" OFF) option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) +option(ENABLE_PYBULLET_TEST "Enable tests with pybullet simulation" OFF) project(centroidal_control_collection LANGUAGES CXX) include(GNUInstallDirs) # For CMAKE_INSTALL_LIBDIR @@ -80,13 +81,12 @@ endif() if(USE_ROS2) ament_export_include_directories(include) - ament_export_libraries(CCC) ament_export_dependencies( rclcpp qp_solver_collection force_control_collection nmpc_ddp ) - ament_export_targets(${PROJECT_NAME}) + ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_package() endif() \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7f1d221..463fe0a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -33,18 +33,19 @@ foreach(NAME IN LISTS CCC_gtest_list) add_CCC_test(${NAME}) endforeach() -option(ENABLE_PYBULLET_TEST "Enable tests with pybullet simulation" OFF) -# if(ENABLE_PYBULLET_TEST) -# find_package(rostest REQUIRED) +if(ENABLE_PYBULLET_TEST) + if(USE_ROS2) + find_package(launch_testing_ament_cmake) + ament_add_gtest(TestSimDdpSingleRigidBody src/TestSimDdpSingleRigidBody.cpp TIMEOUT 600) + ament_target_dependencies(TestSimDdpSingleRigidBody + rclcpp + std_msgs + std_srvs + ) -# set(CCC_rostest_list -# TestSimDdpSingleRigidBody -# ) - -# if(USE_ROS2) -# foreach(NAME IN LISTS CCC_rostest_list) -# add_rostest_gtest(${NAME} test/${NAME}.test src/${NAME}.cpp) -# target_link_libraries(${NAME} CCC) -# endforeach() -# endif() -# endif() + target_link_libraries(TestSimDdpSingleRigidBody CCC) + install(TARGETS TestSimDdpSingleRigidBody DESTINATION lib/${PROJECT_NAME}) + add_launch_test(scripts/simSingleRigidBody.py) + install(DIRECTORY scripts DESTINATION share/${PROJECT_NAME}/tests) + endif() +endif() \ No newline at end of file diff --git a/tests/scripts/simSingleRigidBody.py b/tests/scripts/simSingleRigidBody.py index af8fd8c..198f016 100755 --- a/tests/scripts/simSingleRigidBody.py +++ b/tests/scripts/simSingleRigidBody.py @@ -1,16 +1,41 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 import numpy as np import pybullet import pybullet_data +import unittest +import sys -import rospy -from tf import transformations +import rclpy +from tf_transformations import quaternion_from_euler, euler_from_quaternion from std_msgs.msg import Float64MultiArray - +import launch +import launch_testing +import pytest +from launch_ros.actions import Node + +@pytest.mark.launch_test +def generate_test_description(): + test_sim_ddp_srb = Node( + package='centroidal_control_collection', + executable='TestSimDdpSingleRigidBody', + name='test_sim_ddp_srb', + output='screen' + ) + + content = {} + + return ( + launch.LaunchDescription([ + test_sim_ddp_srb, + launch_testing.actions.ReadyToTest(), + ]), + content + ) class SimSingleRigidBody(object): def __init__(self, enable_gui=True): + self.node = rclpy.create_node("sim") # Instantiate simulator if enable_gui: pybullet.connect(pybullet.GUI) @@ -56,8 +81,8 @@ def __init__(self, enable_gui=True): self.force_line_uid_list = [] # Setup ROS - self.state_pub = rospy.Publisher("state", Float64MultiArray, queue_size=1) - self.control_sub = rospy.Subscriber("control", Float64MultiArray, self.callback, queue_size=1) + self.state_pub = self.node.create_publisher(Float64MultiArray, "state", 1) + self.control_sub = self.node.create_subscription(Float64MultiArray, "control", self.callback, 1) def runOnce(self): """"Run simulation step once.""" @@ -106,15 +131,15 @@ def runOnce(self): def getState(self): """"Get state [c, alpha, v, omega].""" c, quat = pybullet.getBasePositionAndOrientation(bodyUniqueId=self.body_uid) - alpha = transformations.euler_from_quaternion(quat, axes="rzyx") + alpha = euler_from_quaternion(quat, axes="rzyx") v, omega = pybullet.getBaseVelocity(bodyUniqueId=self.body_uid) - return np.array([c, alpha, v, omega]).flatten() + return np.array([c, alpha, v, omega]).flatten().tolist() def setState(self, state): """Set state [c, alpha, v, omega].""" pybullet.resetBasePositionAndOrientation(bodyUniqueId=self.body_uid, posObj=state[0:3], - ornObj=transformations.quaternion_from_euler(*state[3:6], axes="rzyx")) + ornObj=quaternion_from_euler(*state[3:6], axes="rzyx")) pybullet.resetBaseVelocity(objectUniqueId=self.body_uid, linearVelocity=state[6:9], angularVelocity=state[9:12]) @@ -129,7 +154,7 @@ def demo(): sim = SimSingleRigidBody(True) t = 0.0 # [sec] - rate = rospy.Rate(1.0 / sim.dt) + rate = sim.node.create_rate(1.0 / sim.dt) while pybullet.isConnected(): # Run simulation step sim.runOnce() @@ -138,7 +163,10 @@ def demo(): rate.sleep() t += sim.dt +class TestSimSingleRigidBody(unittest.TestCase): + def __init__(self, *args): + super().__init__(*args) -if __name__ == "__main__": - rospy.init_node("sim") - demo() + def test(self): + rclpy.init(args=sys.argv) + demo() \ No newline at end of file From 7d82464421e92218db5652f516ac2f62255a9df4 Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Thu, 23 Oct 2025 10:51:27 +0900 Subject: [PATCH 27/27] doc(readme): add ros2 comment, specify that examples are not yet support on humble --- README.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index d3b76b8..09063c4 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,11 @@ +This is the branch for ROS2; use the [ros1](https://github.com/isri-aist/CentroidalControlCollection/tree/ros1) branch for ROS1. + + # [CentroidalControlCollection](https://github.com/isri-aist/CentroidalControlCollection) Collection of centroidal control for legged robots [![CI-standalone](https://github.com/isri-aist/CentroidalControlCollection/actions/workflows/ci-standalone.yaml/badge.svg)](https://github.com/isri-aist/CentroidalControlCollection/actions/workflows/ci-standalone.yaml) -[![CI-catkin](https://github.com/isri-aist/CentroidalControlCollection/actions/workflows/ci-catkin.yaml/badge.svg)](https://github.com/isri-aist/CentroidalControlCollection/actions/workflows/ci-catkin.yaml) +[![CI-colcon](https://github.com/isri-aist/CentroidalControlCollection/actions/workflows/ci-catkin.yaml/badge.svg)](https://github.com/isri-aist/CentroidalControlCollection/actions/workflows/ci-colcon.yaml) [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/CentroidalControlCollection/) ## Install @@ -60,11 +63,11 @@ $ rosdep install -y -r --from-paths src --ignore-src 3. Build a package. ```bash -$ colcon build --packages-select centroidal_control_collection --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo +$ colcon build --packages-select centroidal_control_collection --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DUSE_ROS2=ON $ colcon test --merge-install --packages-select centroidal_control_collection # [optional] to compile and run tests ``` -## Examples +## Examples (not yet suppported on Humble) Make sure that it is built with `-DBUILD_TESTING=ON` option. ### Methods based on bipedal dynamics