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 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..093beac --- /dev/null +++ b/.github/workflows/ci-colcon.yaml @@ -0,0 +1,135 @@ +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: [RelWithDebInfo] + 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: + 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 + 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 + run: | + set -e + set -x + cd ${GITHUB_WORKSPACE}/colcon_ws + 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 + 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: env.UPLOAD_DOCUMENTATION == 'true' + 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 diff --git a/.github/workflows/ci-standalone.yaml b/.github/workflows/ci-standalone.yaml index 6fac3dc..78ea64e 100644 --- a/.github/workflows/ci-standalone.yaml +++ b/.github/workflows/ci-standalone.yaml @@ -13,14 +13,17 @@ 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 }} 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: 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 }} 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/CMakeLists.txt b/CMakeLists.txt index 1685f47..3e5f72d 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) @@ -10,49 +10,83 @@ 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) +option(ENABLE_PYBULLET_TEST "Enable tests with pybullet simulation" OFF) -include(cmake/base.cmake) project(centroidal_control_collection LANGUAGES CXX) +include(GNUInstallDirs) # For CMAKE_INSTALL_LIBDIR -if(DEFINED CATKIN_DEVEL_PREFIX) - set(DOXYGEN_HTML_OUTPUT html) +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 - add_project_dependency(mc_rtc REQUIRED) + find_package(ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) +else() + option(BUILD_TESTING "Build test" ON) +endif() - find_package(catkin REQUIRED COMPONENTS - roscpp - qp_solver_collection - force_control_collection - nmpc_ddp +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}:: ) - catkin_package( - CATKIN_DEPENDS - roscpp - qp_solver_collection - force_control_collection - nmpc_ddp - DEPENDS EIGEN3 - INCLUDE_DIRS include - LIBRARIES CCC + include(CMakePackageConfigHelpers) + # generate the config file that is includes the exports + configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}" + NO_SET_AND_CHECK_MACRO + NO_CHECK_REQUIRED_COMPONENTS_MACRO ) -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) -endif() -add_subdirectory(src) + # 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 + ) -if(BUILD_TESTING OR CATKIN_ENABLE_TESTING) + # install config files + install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} + ) +endif() + +if(BUILD_TESTING) add_subdirectory(tests) endif() if(INSTALL_DOCUMENTATION) add_subdirectory(doc) endif() + +if(USE_ROS2) + ament_export_include_directories(include) + ament_export_dependencies( + rclcpp + qp_solver_collection + force_control_collection + nmpc_ddp + ) + ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + ament_package() +endif() \ No newline at end of file diff --git a/README.md b/README.md index f21ce17..09063c4 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,18 @@ +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 ### 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 +24,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 +43,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 +63,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 -DUSE_ROS2=ON +$ 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. +## Examples (not yet suppported on Humble) +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. diff --git a/cmake b/cmake deleted file mode 160000 index 277d1bd..0000000 --- a/cmake +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 277d1bd8a5491e6235413bd716756249c3922232 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 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} 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..d7f79ea 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + centroidal_control_collection 0.2.0 @@ -10,18 +10,21 @@ http://ros.org/wiki/centroidal_control_collection Masaki Murooka - catkin + ament_cmake - roscpp + rclcpp qp_solver_collection force_control_collection - nmpc_ddp + NMPC eigen - rosunit - rostest + ament_cmake_gtest doxygen graphviz + + + ament_cmake + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 14d885f..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 @@ -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 ${PROJECT_NAME} + 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) 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/CMakeLists.txt b/tests/CMakeLists.txt index d671d98..463fe0a 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 @@ -39,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) - - set(CCC_rostest_list - TestSimDdpSingleRigidBody - ) + 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 + ) - 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() + 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() +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 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])