Integrate kdl symmetric velocity profiles

rafaelrojasmiliani · Oct 9, 2024 · aed485d · aed485d
1 parent 2f6e166
commit aed485d
Show file tree

Hide file tree

Showing 9 changed files with 509 additions and 135 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,149 +8,166 @@ set(CMAKE_CXX_FLAGS_DEBUG "-g3 -pthread ")
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED True)
 set(CMAKE_INSTALL_PREFIX /usr)
-set(PYBIND11_FINDPYTHON
-    OFF
-    CACHE BOOL "")
+set(PYBIND11_FINDPYTHON OFF CACHE BOOL "")
 
-set(PYBIND11_PYTHON_VERSION
-    3.8
-    CACHE STRING "")
+set(PYBIND11_PYTHON_VERSION 3.8 CACHE STRING "")
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
+set(GSPLINES_USE_RUCKIG ON CACHE BOOL "" FORCE)
+set(GSPLINES_USE_KDL ON CACHE BOOL "" FORCE)
+
+set(Sources
+    ${PROJECT_SOURCE_DIR}/src/Basis/BasisLegendre.cpp
+    ${PROJECT_SOURCE_DIR}/src/Basis/BasisLagrange.cpp
+    ${PROJECT_SOURCE_DIR}/src/Basis/Basis0101.cpp
+    ${PROJECT_SOURCE_DIR}/src/Basis/Basis.cpp
+    ${PROJECT_SOURCE_DIR}/src/Interpolator.cpp
+    ${PROJECT_SOURCE_DIR}/src/GSpline.cpp
+    ${PROJECT_SOURCE_DIR}/src/Tools.cpp
+    ${PROJECT_SOURCE_DIR}/src/FunctionalAnalysis/Sobolev.cpp
+    ${PROJECT_SOURCE_DIR}/src/FunctionalAnalysis/Integral.cpp
+    ${PROJECT_SOURCE_DIR}/src/Optimization/ipopt_interface.cpp
+    ${PROJECT_SOURCE_DIR}/src/Optimization/ipopt_solver.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionSum.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionMul.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionsComp.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionsConcat.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/Functions.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionExpression.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionGenericOperations.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/FunctionBase.cpp
+    ${PROJECT_SOURCE_DIR}/src/Functions/ElementalFunctions.cpp
+    ${PROJECT_SOURCE_DIR}/src/Collocation/GaussLobattoPointsWeights.cpp
+    ${PROJECT_SOURCE_DIR}/src/Collocation/GaussLobattoLagrange.cpp
+    ${PROJECT_SOURCE_DIR}/src/Collocation/GaussLobattoLagrangeFunctionals.cpp
+)
+
 find_package(ifopt REQUIRED)
 find_package(Eigen3 REQUIRED)
-add_library(
-  gsplines SHARED
-  ${PROJECT_SOURCE_DIR}/src/Basis/BasisLegendre.cpp
-  ${PROJECT_SOURCE_DIR}/src/Basis/BasisLagrange.cpp
-  ${PROJECT_SOURCE_DIR}/src/Basis/Basis0101.cpp
-  ${PROJECT_SOURCE_DIR}/src/Basis/Basis.cpp
-  ${PROJECT_SOURCE_DIR}/src/Interpolator.cpp
-  ${PROJECT_SOURCE_DIR}/src/GSpline.cpp
-  ${PROJECT_SOURCE_DIR}/src/Tools.cpp
-  ${PROJECT_SOURCE_DIR}/src/FunctionalAnalysis/Sobolev.cpp
-  ${PROJECT_SOURCE_DIR}/src/FunctionalAnalysis/Integral.cpp
-  ${PROJECT_SOURCE_DIR}/src/Optimization/ipopt_interface.cpp
-  ${PROJECT_SOURCE_DIR}/src/Optimization/ipopt_solver.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionSum.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionMul.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionsComp.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionsConcat.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/Functions.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionExpression.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionGenericOperations.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/FunctionBase.cpp
-  ${PROJECT_SOURCE_DIR}/src/Functions/ElementalFunctions.cpp
-  ${PROJECT_SOURCE_DIR}/src/Collocation/GaussLobattoPointsWeights.cpp
-  ${PROJECT_SOURCE_DIR}/src/Collocation/GaussLobattoLagrange.cpp
-  ${PROJECT_SOURCE_DIR}/src/Collocation/GaussLobattoLagrangeFunctionals.cpp
-  # --
-  ${PROJECT_SOURCE_DIR}/src/Ruckig/Ruckig.cpp)
 
-target_link_libraries(gsplines PUBLIC ${ifopt_LIBRARIES})
+if(GSPLINES_USE_RUCKIG)
+    list(APPEND Sources ${PROJECT_SOURCE_DIR}/src/Ruckig/Ruckig.cpp)
+    find_package(ruckig REQUIRED)
+    get_target_property(ruckig_INCLUDE_DIRS ruckig::ruckig INTERFACE_INCLUDE_DIRECTORIES)
+    set(ruckig_LIBRARIES "ruckig::ruckig")
+endif()
 
-find_package(ruckig REQUIRED)
-if(ruckig_FOUND)
-  target_compile_definitions(gsplines PUBLIC HAS_RUCKIG)
-  target_link_libraries(gsplines PUBLIC ruckig::ruckig)
+if(GSPLINES_USE_KDL)
+    list(APPEND Sources ${PROJECT_SOURCE_DIR}/src/kdl/kdl.cpp)
+    find_package(orocos_kdl REQUIRED)
 endif()
+
+add_library(gsplines SHARED ${Sources})
+
+if(GSPLINES_USE_RUCKIG)
+    target_compile_definitions(gsplines PUBLIC HAS_RUCKIG)
+    target_link_libraries(gsplines PUBLIC ${ruckig_LIBRARIES})
+    target_include_directories(gsplines PUBLIC ${ruckig_INCLUDE_DIRS})
+endif()
+
+if(GSPLINES_USE_KDL)
+    target_compile_definitions(gsplines PUBLIC HAS_KDL)
+    target_link_libraries(gsplines PUBLIC ${orocos_kdl_LIBRARIES})
+    target_include_directories(gsplines PUBLIC ${orocos_kdl_INCLUDE_DIRS})
+endif()
+
+target_link_libraries(gsplines PUBLIC ${ifopt_LIBRARIES})
+
 target_include_directories(
-  gsplines
-  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
-         $<INSTALL_INTERFACE:include> ${EIGEN3_INCLUDE_DIRS}
-         ${ifopt_INCLUDE_DIRS})
+    gsplines
+    PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>
+           ${EIGEN3_INCLUDE_DIRS}
+           ${ifopt_INCLUDE_DIRS}
+)
 
 set_target_properties(
-  gsplines
-  PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON
-             VERSION ${gsplines_VERSION}
-             SOVERSION ${gsplines_VERSION_MAJOR}.${gsplines_VERSION_MINOR}
-             EXPORT_NAME GSplines)
-
-link_directories(${PROJECT_SOURCE_DIR}/build)
+    gsplines PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON VERSION ${gsplines_VERSION}
+                        SOVERSION ${gsplines_VERSION_MAJOR}.${gsplines_VERSION_MINOR} EXPORT_NAME GSplines
+)
 
 if(NOT TARGET pybind11)
-  add_subdirectory(${PROJECT_SOURCE_DIR}/modules/pybind11)
+    add_subdirectory(${PROJECT_SOURCE_DIR}/modules/pybind11)
 endif()
 
 pybind11_add_module(pygsplines ${PROJECT_SOURCE_DIR}/bindings/bindings.cpp)
 
 target_include_directories(
-  pygsplines PUBLIC ${EIGEN3_INCLUDE_DIRS}
-                    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/modules/pybind11>)
-
-target_link_libraries(pygsplines PUBLIC ${EIGEN3_LIBRARIES} ${ifopt_LIBRARIES}
-                                        gsplines)
-if(ruckig_FOUND)
-  target_compile_definitions(pygsplines PUBLIC HAS_RUCKIG)
-  target_link_libraries(pygsplines PUBLIC ruckig::ruckig)
+    pygsplines PUBLIC ${EIGEN3_INCLUDE_DIRS} $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/modules/pybind11>
+)
+
+target_link_libraries(pygsplines PUBLIC ${EIGEN3_LIBRARIES} ${ifopt_LIBRARIES} gsplines)
+
+if(GSPLINES_USE_RUCKIG)
+    target_compile_definitions(pygsplines PUBLIC HAS_RUCKIG)
+    target_link_libraries(pygsplines PUBLIC ${ruckig_LIBRARIES})
+    # target_include_directories(pygsplines PUBLIC ${ruckig_INCLUDE_DIRS})
+endif()
+
+if(GSPLINES_USE_KDL)
+    target_compile_definitions(pygsplines PUBLIC HAS_KDL)
+    target_link_libraries(pygsplines PUBLIC ${orocos_kdl_LIBRARIES})
+    # target_include_directories(pygsplines PUBLIC ${orocos_kdl_INCLUDE_DIRS})
 endif()
 # Generate the environment setup script
 add_custom_command(
-  OUTPUT ${CMAKE_SOURCE_DIR}/setup_python_env.sh
-  COMMAND ${CMAKE_COMMAND} -E remove -f ${CMAKE_SOURCE_DIR}/setup_python_env.sh
-  COMMENT "Removing old setup_python_env.sh")
+    OUTPUT ${CMAKE_SOURCE_DIR}/setup_python_env.sh COMMAND ${CMAKE_COMMAND} -E remove -f
+                                                           ${CMAKE_SOURCE_DIR}/setup_python_env.sh
+    COMMENT "Removing old setup_python_env.sh"
+)
 add_custom_command(
-  OUTPUT ${CMAKE_SOURCE_DIR}/setup_python_env.py
-  COMMAND ${CMAKE_COMMAND} -E remove -f ${CMAKE_SOURCE_DIR}/setup_python_env.py
-  COMMENT "Removing old setup_python_env.py")
+    OUTPUT ${CMAKE_SOURCE_DIR}/setup_python_env.py COMMAND ${CMAKE_COMMAND} -E remove -f
+                                                           ${CMAKE_SOURCE_DIR}/setup_python_env.py
+    COMMENT "Removing old setup_python_env.py"
+)
 set(PYTHON_PACKAGE_DIR "${CMAKE_SOURCE_DIR}/python")
-configure_file(${CMAKE_SOURCE_DIR}/setup_python_env.in
-               ${CMAKE_SOURCE_DIR}/setup_python_env.sh @ONLY)
-configure_file(${CMAKE_SOURCE_DIR}/setup_python_env.in.py
-               ${CMAKE_SOURCE_DIR}/setup_python_env.py @ONLY)
-
-if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}
-   AND ${BUILD_TESTING})
-  enable_testing()
-  add_subdirectory(tests)
+configure_file(${CMAKE_SOURCE_DIR}/setup_python_env.in ${CMAKE_SOURCE_DIR}/setup_python_env.sh @ONLY)
+configure_file(${CMAKE_SOURCE_DIR}/setup_python_env.in.py ${CMAKE_SOURCE_DIR}/setup_python_env.py @ONLY)
+
+if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR} AND ${BUILD_TESTING})
+    enable_testing()
+    add_subdirectory(tests)
 endif()
 
 if(EXISTS "${CMAKE_CURRENT_BINARY_DIR}/compile_commands.json")
-  execute_process(
-    COMMAND
-      ${CMAKE_COMMAND} -E copy_if_different
-      ${CMAKE_CURRENT_BINARY_DIR}/compile_commands.json
-      ${CMAKE_CURRENT_SOURCE_DIR}/compile_commands.json)
+    execute_process(
+        COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/compile_commands.json
+                ${CMAKE_CURRENT_SOURCE_DIR}/compile_commands.json
+    )
 endif()
 
 include(GNUInstallDirs)
 set(INSTALL_CMAKE_CONFIG_DIR ${CMAKE_INSTALL_LIBDIR}/cmake/GSplines)
 
-install(
-  TARGETS gsplines
-  EXPORT GSplinesTargets
-  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
-  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
+install(TARGETS gsplines EXPORT GSplinesTargets LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+)
 
 execute_process(
-  COMMAND "${PYTHON_EXECUTABLE}" -c
-          "import site; print(site.getsitepackages()[0])"
-  OUTPUT_VARIABLE PYTHON_INSTALL_PATH
-  OUTPUT_STRIP_TRAILING_WHITESPACE)
+    COMMAND "${PYTHON_EXECUTABLE}" -c "import site; print(site.getsitepackages()[0])"
+    OUTPUT_VARIABLE PYTHON_INSTALL_PATH OUTPUT_STRIP_TRAILING_WHITESPACE
+)
 
 install(DIRECTORY python/ DESTINATION ${PYTHON_INSTALL_PATH})
 install(TARGETS pygsplines DESTINATION ${PYTHON_INSTALL_PATH}/gsplines)
 
 install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
 
-export(EXPORT GSplinesTargets
-       FILE ${CMAKE_CURRENT_BINARY_DIR}/GSplinesTargets.cmake)
+export(EXPORT GSplinesTargets FILE ${CMAKE_CURRENT_BINARY_DIR}/GSplinesTargets.cmake)
 
 include(CMakePackageConfigHelpers)
 
-write_basic_package_version_file(
-  ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfigVersion.cmake
-  COMPATIBILITY SameMajorVersion)
+write_basic_package_version_file(${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfigVersion.cmake COMPATIBILITY SameMajorVersion)
 
 configure_package_config_file(
-  cmake/GSplinesConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfig.cmake
-  INSTALL_DESTINATION ${INSTALL_CMAKE_CONFIG_DIR})
+    cmake/GSplinesConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfig.cmake
+    INSTALL_DESTINATION ${INSTALL_CMAKE_CONFIG_DIR}
+)
 
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfig.cmake
-              ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfigVersion.cmake
-        DESTINATION ${INSTALL_CMAKE_CONFIG_DIR})
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/GSplinesConfigVersion.cmake
+        DESTINATION ${INSTALL_CMAKE_CONFIG_DIR}
+)
 
 install(EXPORT GSplinesTargets DESTINATION ${INSTALL_CMAKE_CONFIG_DIR})
 
@@ -163,17 +180,16 @@ set(CPACK_SYSTEM_NAME ${CMAKE_HOST_SYSTEM_PROCESSOR})
 # Debian
 find_program(DPKG_PROG dpkg DOC "'dpkg' executable")
 if(DPKG_PROG)
-  execute_process(
-    COMMAND ${DPKG_PROG} --print-architecture
-    OUTPUT_VARIABLE CPACK_DEBIAN_PACKAGE_ARCHITECTURE
-    OUTPUT_STRIP_TRAILING_WHITESPACE)
-  # Change system name to use the correct architecture in file name
-  set(CPACK_SYSTEM_NAME ${CPACK_DEBIAN_PACKAGE_ARCHITECTURE})
+    execute_process(
+        COMMAND ${DPKG_PROG} --print-architecture OUTPUT_VARIABLE CPACK_DEBIAN_PACKAGE_ARCHITECTURE
+        OUTPUT_STRIP_TRAILING_WHITESPACE
+    )
+    # Change system name to use the correct architecture in file name
+    set(CPACK_SYSTEM_NAME ${CPACK_DEBIAN_PACKAGE_ARCHITECTURE})
 endif()
 # Debian versions require a dash
 set(CPACK_DEBIAN_PACKAGE_VERSION ${CPACK_PACKAGE_VERSION}-1)
 set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Rafael A. Rojas")
-set(CPACK_DEBIAN_PACKAGE_DEPENDS
-    "coinor-libipopt-dev, ros-noetic-ifopt, libeigen3-dev")
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "coinor-libipopt-dev, ros-noetic-ifopt, libeigen3-dev")
 
 include(CPack)
diff --git a/bindings/bindings.cpp b/bindings/bindings.cpp
@@ -12,6 +12,9 @@
 #ifdef HAS_RUCKIG
 #include <gsplines/Ruckig/Ruckig.hpp>
 #endif
+#ifdef HAS_KDL
+#include <gsplines/kdl/kdl.hpp>
+#endif
 
 PYBIND11_MODULE(pygsplines, gsplines_module) {
   gsplines_module.doc() = "Generalized Splines Library with Optimization tools";
@@ -25,8 +28,6 @@ PYBIND11_MODULE(pygsplines, gsplines_module) {
   py::module functional_analysis_submodule =
       gsplines_module.def_submodule("functional_analysis");
 
-  py::module ruckig_submodule = gsplines_module.def_submodule("ruckig");
-
   // ----------------
   // Basis Submodule
   // ----------------
@@ -431,6 +432,7 @@ PYBIND11_MODULE(pygsplines, gsplines_module) {
   // ------------------------------
   // Ruckig submodule
   // ------------------------------
+  py::module ruckig_submodule = gsplines_module.def_submodule("ruckig");
   py::class_<gsplines::ruckig::RuckigCurve, gsplines::functions::FunctionBase>(
       ruckig_submodule, "Ruckig")
       .def(py::init([](const Eigen::Ref<const Eigen::MatrixXd>& _waypoints,
@@ -445,4 +447,22 @@ PYBIND11_MODULE(pygsplines, gsplines_module) {
            py::arg("_deg") = 1);
   ruckig_submodule.def("interpolator", &gsplines::ruckig::interpolator);
 #endif
+#ifdef HAS_KDL
+  // ------------------------------
+  // KDL submodule
+  // ------------------------------
+  py::module kdl_submodule = gsplines_module.def_submodule("kdl");
+  py::class_<gsplines::kdl::KdlTrap, gsplines::functions::FunctionBase>(
+      kdl_submodule, "KdlTrap")
+      .def(py::init([](const Eigen::Ref<const Eigen::MatrixXd>& _waypoints,
+                       const std::vector<double>& _max_abs_vel,
+                       const std::vector<double>& _max_abs_acc) {
+        return gsplines::kdl::interpolator(_waypoints, _max_abs_vel,
+                                           _max_abs_acc)
+            .value();
+      }))
+      .def("deriv", &gsplines::kdl::KdlTrap::derivate, py::arg("_deg") = 1);
+
+  kdl_submodule.def("interpolator", &gsplines::kdl::interpolator);
+#endif
 }