feat(build): handle installation and linking of dependencies for pino…

…cchio collision support (#161)

CHANGELOG.md

@@ -13,6 +13,7 @@ Release Versions:
 
 ## Upcoming changes (in development)
 
+- feat(build): handle installation and linking of dependencies for pinocchio collision support (#161)
 - build: update dockerfiles (#153)
 - build: copy python packages into /usr instead of ~ros2 to avoid permission issues (#155)
 - feat: Add ParameterType conversion functions to go from enum to type label and the inverse (#154)

Dockerfile

@@ -76,42 +76,48 @@ xargs -a /tmp/new-packages.txt dpkg-query -L \
 # this root can then be copied to / to install everything globally or use LD_LIBRARY_PATH to use it locally
 HEREDOC
 
+FROM base as dep-base
+ARG TARGETPLATFORM
+ARG CACHEID
+COPY dependencies/base_dependencies.cmake CMakeLists.txt
+RUN --mount=type=cache,target=./build,id=cmake-osqp-${TARGETPLATFORM}-${CACHEID},uid=1000 \
+  cmake -B build -DCMAKE_BUILD_TYPE=Release && cmake --build build && cmake --install build --prefix /tmp/deps
+
 FROM base as dep-pinocchio
 COPY --from=apt-dependencies /tmp/apt /
+COPY --from=dep-base /tmp/deps /usr
 ARG TARGETPLATFORM
 ARG CACHEID
-ARG PINOCCHIO_TAG=v2.6.9
-ARG PINOCCHIO_TESTS=OFF
+ARG PINOCCHIO_TAG=v2.9.0
+ARG HPP_FCL_TAG=v1.8.1
 # FIXME: it would be nicer to have it all in the root CMakelists.txt but:
 #  * `pinocchio` doesn't provide an include directory we can easily plug into `target_include_directories` and thus needs to be installed first
 #  * `pinocchio` uses hacks relying on undocumented CMake quirks which break if you use `FetchContent`
 # FIXME: it needs `CMAKE_INSTALL_PREFIX` and `--prefix` because it doesn't install to the right place otherwise
+RUN --mount=type=cache,target=./hpp-fcl,id=cmake-hpp-fcl-src-${HPP_FCL_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
+  --mount=type=cache,target=./build,id=cmake-hpp-fcl-${HPP_FCL_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
+  if [ ! -f hpp-fcl/CMakeLists.txt ]; then rm -rf hpp-fcl/* && git clone --depth 1 -b ${HPP_FCL_TAG} --recursive https://github.com/humanoid-path-planner/hpp-fcl; fi \
+  && cmake -B build -S hpp-fcl -DBUILD_TESTING=OFF -DCMAKE_BUILD_TYPE=Release -DBUILD_PYTHON_INTERFACE=OFF -DCMAKE_INSTALL_PREFIX=/tmp/deps \
+  && cmake --build build --target all install
 RUN --mount=type=cache,target=./pinocchio,id=cmake-pinocchio-src-${PINOCCHIO_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   --mount=type=cache,target=./build,id=cmake-pinocchio-${PINOCCHIO_TAG}-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   if [ ! -f pinocchio/CMakeLists.txt ]; then rm -rf pinocchio/* && git clone --depth 1 -b ${PINOCCHIO_TAG} --recursive https://github.com/stack-of-tasks/pinocchio; fi \
-  && cmake -B build -S pinocchio -DBUILD_TESTING=${PINOCCHIO_TESTS} -DCMAKE_BUILD_TYPE=Release -DBUILD_PYTHON_INTERFACE=OFF -DCMAKE_INSTALL_PREFIX=/tmp/deps \
+  && cmake -B build -S pinocchio -DBUILD_TESTING=OFF -DCMAKE_BUILD_TYPE=Release -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_WITH_COLLISION_SUPPORT=ON -DCMAKE_INSTALL_PREFIX=/tmp/deps \
   && cmake --build build --target all install
 # FIXME: pinocchio produces non-portable paths
 RUN find /tmp/deps -type f -exec sed -i 's#/tmp/deps#/usr#g' '{}' \;
 
-FROM base as dep-osqp
-ARG TARGETPLATFORM
-ARG CACHEID
-COPY dependencies/osqp.cmake CMakeLists.txt
-RUN --mount=type=cache,target=./build,id=cmake-osqp-${TARGETPLATFORM}-${CACHEID},uid=1000 \
-  cmake -B build -DCMAKE_BUILD_TYPE=Release && cmake --build build && cmake --install build --prefix /tmp/deps
-
 FROM base as dependencies
 ARG TARGETPLATFORM
 ARG CACHEID
 # Needed to build `osqp-eigen`
-COPY --from=dep-osqp /tmp/deps /usr
+COPY --from=dep-base /tmp/deps /usr
 COPY dependencies/dependencies.cmake CMakeLists.txt
 RUN --mount=type=cache,target=./build,id=cmake-deps-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   cmake -B build -DCMAKE_BUILD_TYPE=Release \
   && cmake --build build \
   && cmake --install build --prefix /tmp/deps
-COPY --from=dep-osqp /tmp/deps /tmp/deps
+COPY --from=dep-base /tmp/deps /tmp/deps
 COPY --from=dep-pinocchio /tmp/deps /tmp/deps
 
 FROM base as code
@@ -158,7 +164,7 @@ ARG CACHEID
 RUN --mount=type=cache,target=./build,id=cmake-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   cmake -B build -DCMAKE_BUILD_TYPE=Release && cmake --build build
 
-FROM build as test
+FROM build as cpp-test
 ARG TARGETPLATFORM
 ARG CACHEID
 RUN --mount=type=cache,target=./build,id=cmake-${TARGETPLATFORM}-${CACHEID},uid=1000 \
@@ -176,11 +182,22 @@ ARG CACHEID
 COPY --from=apt-dependencies /tmp/apt /
 COPY --from=dependencies /tmp/deps /usr
 COPY --from=install /tmp/cl /usr
-COPY --chown=${USER}:${USER} ./python /python
+COPY --chown=${USER}:${USER} ./python/include /python/include
+COPY --chown=${USER}:${USER} ./python/source /python/source
+COPY --chown=${USER}:${USER} ./python/pyproject.toml ./python/setup.py /python/
 RUN --mount=type=cache,target=${HOME}/.cache,id=pip-${TARGETPLATFORM}-${CACHEID},uid=1000 \
   python3 -m pip install --prefix=/tmp/python /python
 RUN mv /tmp/python/local /tmp/python-usr
 
+FROM cpp-test as python-test
+ARG TARGETPLATFORM
+ARG CACHEID
+RUN pip install pytest
+COPY --from=install /tmp/cl /usr
+COPY --from=python /tmp/python-usr /usr
+COPY --chown=${USER}:${USER} ./python/test /test
+RUN pytest /test
+
 FROM base as python-stubs
 ARG TARGETPLATFORM
 ARG CACHEID

VERSION

@@ -1 +1 @@
-7.3.3
+7.3.4

apt-packages.txt

@@ -1,2 +1,3 @@
 libboost-all-dev
 liburdfdom-dev
+libassimp-dev

demos/CMakeLists.txt

@@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(control_libraries 7.3.3 CONFIG REQUIRED)
+find_package(control_libraries 7.3.4 CONFIG REQUIRED)
 
 set(DEMOS_SCRIPTS
   task_space_control_loop

dependencies/osqp.cmake → dependencies/base_dependencies.cmake

@@ -2,13 +2,20 @@
 #  * `OVERRIDE_FIND_PACKAGE` requires CMake 3.24
 #  * `osqp::osqp` is somehow not working in OSQP-Eigen when built together
 cmake_minimum_required(VERSION 3.15)
-project(control-libraries-osqp)
+project(control-libraries-pre-deps)
 
 include(FetchContent)
 FetchContent_Declare(
   osqp
   GIT_REPOSITORY https://github.com/oxfordcontrol/osqp
-  GIT_TAG        v0.6.2
+  GIT_TAG        v0.6.3
 )
 
-FetchContent_MakeAvailable(osqp)
+FetchContent_Declare(
+  Octomap
+  GIT_REPOSITORY https://github.com/OctoMap/octomap.git
+  GIT_TAG        devel
+  SOURCE_SUBDIR  octomap
+)
+
+FetchContent_MakeAvailable(osqp octomap)

dependencies/dependencies.cmake

@@ -8,7 +8,7 @@ include(FetchContent)
 FetchContent_Declare(
   OsqpEigen
   GIT_REPOSITORY https://github.com/robotology/osqp-eigen.git
-  GIT_TAG        v0.6.4
+  GIT_TAG        v0.8.1
 )
 
 FetchContent_Declare(

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 7.3.3
+PROJECT_NUMBER = 7.3.4
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

protocol/clproto_cpp/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(clproto VERSION 7.3.3)
+project(clproto VERSION 7.3.4)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

python/setup.py

@@ -11,7 +11,7 @@
 # names of the environment variables that define osqp and openrobots include directories
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
-__version__ = "7.3.3"
+__version__ = "7.3.4"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 
@@ -104,6 +104,7 @@
                           include_dirs=__include_dirs__,
                           libraries=['state_representation', 'robot_model'],
                           define_macros=[('MODULE_VERSION_INFO', __version__)],
+                          extra_compile_args=['-DPINOCCHIO_WITH_HPP_FCL']
                           )
     )
 

source/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 7.3.3)
+project(control_libraries VERSION 7.3.4)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

source/robot_model/CMakeLists.txt

@@ -1,10 +1,14 @@
 set(LIBRARY_NAME robot_model)
 
-set(PINOCCHIO_VERSION 2.6.9)
-set(OSQP_EIGEN_VERSION 0.6.4)
+set(PINOCCHIO_VERSION 2.9.0)
+set(HPP_FCL_VERSION 1.8.1)
+set(OSQP_EIGEN_VERSION 0.8.1)
 find_package(pinocchio ${PINOCCHIO_VERSION} REQUIRED)
+find_package(hpp-fcl ${HPP_FCL_VERSION} REQUIRED)
 find_package(OsqpEigen ${OSQP_EIGEN_VERSION} REQUIRED)
 
+add_definitions(-DPINOCCHIO_WITH_HPP_FCL)
+
 set(CORE_SOURCES
   src/Model.cpp
 )
@@ -22,8 +26,10 @@ target_include_directories(${LIBRARY_NAME}
 target_link_libraries(${LIBRARY_NAME}
   state_representation
   ${PINOCCHIO_LIBRARIES}
+  ${hpp-fcl_LIBRARIES}
   OsqpEigen::OsqpEigen
 )
+target_compile_definitions(${LIBRARY_NAME} PUBLIC -DPINOCCHIO_WITH_HPP_FCL)
 
 # install the target and create export-set
 install(TARGETS ${LIBRARY_NAME}