Feature/go bio ik integration

pixi.toml

@@ -160,7 +160,7 @@ ros-jazzy-soccer-vision-3d-rviz-markers = ">=1.0.0,<2"
 ros-jazzy-soccer-vision-3d-msgs = ">=1.0.0,<2"
 ros-jazzy-soccer-vision-attribute-msgs = ">=1.0.0,<2"
 ros-jazzy-rot-conv = ">=1.1.0,<2"
-ros-jazzy-bio-ik = ">=2.0.0,<3"
+# ros-jazzy-bio-ik = ">=2.0.0,<3"
 ros-jazzy-bio-ik-msgs = ">=0.0.0,<0.0.1"
 ros-jazzy-biped-interfaces = ">=0.0.0,<0.0.1"
 ros-jazzy-bitbots-tf-buffer = ">=1.0.0,<2"

src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt

@@ -18,9 +18,7 @@ if(ipo_supported)
 endif()
 
 # Build with release optimizations and debug symbols by default
-if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE RelWithDebug)
-endif()
+set(CMAKE_BUILD_TYPE RelWithDebug)
 
 set(PYBIND11_PYTHON_VERSION 3)
 set(PYBIND11_FINDPYTHON ON)

src/bitbots_robot/wolfgang_moveit_config/config/kinematics.yaml

@@ -2,36 +2,28 @@ LeftLeg:
   kinematics_solver: bio_ik/BioIKKinematicsPlugin # kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.00001
   kinematics_solver_timeout: 0.01
-  mode: gd_c
 RightLeg:
   kinematics_solver: bio_ik/BioIKKinematicsPlugin # kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.00001
   kinematics_solver_timeout: 0.01
-  mode: gd_c
 Legs:
   kinematics_solver_search_resolution: 0.0001
   kinematics_solver_timeout: 0.005
-  mode: gd_c
 RightArm:
   kinematics_solver: bio_ik/BioIKKinematicsPlugin
   kinematics_solver_search_resolution: 0.00001
   kinematics_solver_timeout: 0.01
-  mode: gd_c
 LeftArm:
   kinematics_solver: bio_ik/BioIKKinematicsPlugin
   kinematics_solver_search_resolution: 0.00001
   kinematics_solver_timeout: 0.01
-  mode: gd_c
 Arms:
   kinematics_solver_search_resolution: 0.00001
   kinematics_solver_timeout: 0.01
-  mode: gd_c
 Head:
   kinematics_solver: bio_ik/BioIKKinematicsPlugin
   kinematics_solver_search_resolution: 0.001
   kinematics_solver_timeout: 0.001
-  mode: gd_c
 All:
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
-  mode: gd_c

src/lib/bio_ik/.gitrepo

@@ -0,0 +1,12 @@
+; DO NOT EDIT (unless you know what you are doing)
+;
+; This subdirectory is a git "subrepo", and this file is maintained by the
+; git-subrepo command. See https://github.com/ingydotnet/git-subrepo#readme
+;
+[subrepo]
+	remote = git@github.com:bit-bots/bio_ik.git
+	branch = ros2
+	commit = ffec29f7aebb16a9ae67667db4a29d375046c23b
+	parent = af4b87fd8f3ddc0b65ed9f588c6647a344fdcff0
+	method = merge
+	cmdver = 0.4.9

src/lib/bio_ik/CMakeLists.txt

@@ -0,0 +1,182 @@
+cmake_minimum_required(VERSION 3.15)
+project(bio_ik)
+
+set(CMAKE_BUILD_TYPE Release)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  Eigen3
+  moveit_core
+  moveit_ros_planning
+  pluginlib
+  rclcpp
+  tf2_eigen
+  tf2_ros
+  tf2_kdl
+  tf2_geometry_msgs
+)
+
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+find_package(OpenMP)
+# the specific flag is not yet present in cmake 2.8.12
+if(OpenMP_CXX_FOUND OR OPENMP_FOUND)
+  message(STATUS "OPENMP FOUND")
+  add_compile_options(${OpenMP_CXX_FLAGS})
+  if(NOT OpenMP_CXX_LIBRARIES)
+    # cmake 2.8.12 does not yet specify the library - assume we might need libgomp
+    set(OpenMP_LIBS gomp)
+  else()
+    set(OpenMP_LIBS ${OpenMP_CXX_LIBRARIES})
+  endif()
+else()
+    message(WARNING "OPENMP NOT FOUND. You will suffer performance loss.")
+    set(OpenMP_LIBS)
+endif()
+
+option(USE_FANN "build the neural-network-based IK solver (experimental)" OFF)
+if(USE_FANN)
+    find_library(FANN_LIBRARIES NAMES fann)
+    find_path(FANN_INCLUDE_DIRS NAMES fann.h)
+    if(NOT FANN_INCLUDE_DIRS OR NOT FANN_LIBRARIES)
+        message(FATAL_ERROR "Neural network solver requested, but libfann was not found.")
+    else()
+        message("Found libfann: ${FANN_LIBRARIES} / ${FANN_INCLUDE_DIRS}")
+    endif()
+else()
+    set(FANN_LIBRARIES)
+    set(FANN_INCLUDE_DIRS)
+endif()
+
+option(USE_CPPOPTLIB "Include gradient-based solvers from CppNumericalSolvers (bio_ik also provides its own solver)" OFF)
+if(USE_CPPOPTLIB)
+    find_path(CPPOPTLIB_INCLUDE_DIRS
+        NAMES cppoptlib/solver/bfgssolver.h
+        HINTS ../../CppNumericalSolvers/include)
+    if(NOT CPPOPTLIB_INCLUDE_DIRS)
+        message(FATAL_ERROR "cppoptlib support requested, but the headers could not be found.")
+    else()
+        message("Found cppoptlib: ${CPPOPTLIB_INCLUDE_DIRS}")
+    endif()
+    add_definitions(-DENABLE_CPP_OPTLIB)
+else()
+    set(CPPOPTLIB_INCLUDE_DIRS)
+endif()
+
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  add_compile_options(-frecord-gcc-switches)
+endif()
+
+add_compile_options($<$<CONFIG:Release>:-O3>)
+add_compile_options($<$<CONFIG:Release>:-ftree-vectorize>)
+add_compile_options($<$<CONFIG:Release>:-ffast-math>)
+
+include_directories(
+    include
+    ${FANN_INCLUDE_DIRS}
+    ${CPPOPTLIB_INCLUDE_DIRS}
+)
+
+set(SOURCES
+    src/goal_types.cpp
+    src/problem.cpp
+    src/ik_test.cpp
+    src/ik_gradient.cpp
+    src/ik_evolution_1.cpp
+    src/ik_evolution_2.cpp
+)
+
+if(USE_FANN)
+    list(APPEND SOURCES src/ik_neural.cpp)
+endif()
+
+if(USE_CPPOPTLIB)
+    list(APPEND SOURCES src/ik_cppoptlib.cpp)
+endif()
+
+add_library(${PROJECT_NAME} ${SOURCES})
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  PUBLIC
+    ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+target_link_libraries(
+  ${PROJECT_NAME}
+  PUBLIC
+  ${FANN_LIBRARIES}
+  ${OpenMP_LIBS}
+)
+
+add_library(${PROJECT_NAME}_plugin SHARED
+  ${SOURCES}
+  src/kinematics_plugin.cpp
+)
+
+target_include_directories(${PROJECT_NAME}_plugin PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(
+  ${PROJECT_NAME}_plugin
+  PRIVATE
+    ${PROJECT_NAME}
+)
+
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+install(
+  TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+
+pluginlib_export_plugin_description_file(
+  moveit_core
+  bio_ik_kinematics_description.xml
+)
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}_plugin
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(bio_ik_test test/utest.cpp)
+  target_link_libraries(bio_ik_test
+    bio_ik
+  )
+  target_include_directories(bio_ik_test
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    PUBLIC $<INSTALL_INTERFACE:include>)
+
+endif()
+
+ament_package()