feat: more flexible ik for 5 dof so101

extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.pyi

@@ -0,0 +1,7 @@
+from __future__ import annotations
+
+from rcs_robotics_library._core import rl
+
+from . import _core
+
+__all__: list = ["rl"]

extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi

@@ -1,4 +1,3 @@
-# ATTENTION: auto generated from C++ code, use `make stubgen` to update!
 """
 
         Robot Control Stack Python Bindings
@@ -11,9 +10,10 @@
 
 
 """
+
 from __future__ import annotations
 
 from . import rl
 
-__all__ = ["rl"]
+__all__: list[str] = ["rl"]
 __version__: str = "0.5.2"

extensions/rcs_robotics_library/src/rcs_robotics_library/_core/rl.pyi

@@ -1,12 +1,12 @@
-# ATTENTION: auto generated from C++ code, use `make stubgen` to update!
 """
 rcs robotics library module
 """
+
 from __future__ import annotations
 
 import rcs._core.common
 
-__all__ = ["RoboticsLibraryIK"]
+__all__: list[str] = ["RoboticsLibraryIK"]
 
 class RoboticsLibraryIK(rcs._core.common.Kinematics):
     def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...

extensions/rcs_so101/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.19)
+
+project(
+    rcs_so101
+    LANGUAGES C CXX
+    VERSION 0.5.2
+    DESCRIPTION "RCS so101 ik"
+)
+
+set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
+
+set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # Allow us to set options for subprojects
+
+cmake_policy(SET CMP0048 NEW) # Set version in project
+# Allow target properties affecting visibility during linking in static libraries
+set(CMAKE_POLICY_DEFAULT_CMP0063 NEW)
+cmake_policy(SET CMP0072 NEW) # Use GLVND instead of legacy libGL.so
+cmake_policy(SET CMP0135 NEW) # Use timestamp of file extraction not download
+cmake_policy(SET CMP0140 NEW) # Check return arguments
+
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+set(BUILD_SHARED_LIBS OFF)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+set(RL_BUILD_DEMOS OFF)
+set(RL_BUILD_RL_SG OFF)
+set(RL_BUILD_TESTS OFF)
+set(RL_BUILD_EXTRAS OFF)
+set(BUILD_PYTHON_INTERFACE OFF)
+set(BUILD_DOCUMENTATION OFF)
+
+include(FetchContent)
+
+find_package(Eigen3 REQUIRED)
+find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(rcs REQUIRED)
+
+FetchContent_Declare(pybind11
+    GIT_REPOSITORY https://github.com/pybind/pybind11.git
+    GIT_TAG v2.13.4
+    GIT_PROGRESS TRUE
+    EXCLUDE_FROM_ALL
+)
+FetchContent_MakeAvailable(pybind11)
+
+add_subdirectory(src)

extensions/rcs_so101/Makefile

@@ -0,0 +1,41 @@
+PYSRC = src
+CPPSRC = src
+COMPILE_MODE = Release
+
+# CPP
+cppcheckformat:
+	clang-format --dry-run -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h')
+
+cppformat:
+	clang-format -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h')
+
+cpplint: 
+	clang-tidy -p=build --warnings-as-errors='*' $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -name '*.h')
+
+# import errors
+# clang-tidy -p=build --warnings-as-errors='*' $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -name '*.h')
+
+gcccompile: 
+	pip install --upgrade --requirement requirements_dev.txt
+	cmake -DCMAKE_BUILD_TYPE=${COMPILE_MODE} -DCMAKE_C_COMPILER=gcc -DCMAKE_CXX_COMPILER=g++ -B build -G Ninja
+	cmake --build build --target _core
+
+clangcompile: 
+	pip install --upgrade --requirement requirements_dev.txt
+	cmake -DCMAKE_BUILD_TYPE=${COMPILE_MODE} -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++ -B build -G Ninja
+	cmake --build build --target _core
+
+# Auto generation of CPP binding stub files
+stubgen:
+	pybind11-stubgen -o src --numpy-array-use-type-var rcs_so101
+	find ./src -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/'
+	find ./src -not -path "./src/rcs_so101/_core/*" -name '*.pyi' -delete
+	find ./src/rcs_so101/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
+	find ./src/rcs_so101/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
+	ruff check --fix src/rcs_so101/_core
+	isort src/rcs_so101/_core
+	black src/rcs_so101/_core
+
+
+
+.PHONY: cppcheckformat cppformat cpplint gcccompile clangcompile stubgen

extensions/rcs_so101/cmake/Findpinocchio.cmake

@@ -0,0 +1,68 @@
+if (NOT pinocchio_FOUND)
+    if (NOT Python3_FOUND)
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the include directory exists
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix include OUTPUT_VARIABLE pinocchio_INCLUDE_DIRS)
+    if (NOT EXISTS ${pinocchio_INCLUDE_DIRS})
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the library file exists
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_default.so OUTPUT_VARIABLE pinocchio_library_path)
+    if (NOT EXISTS ${pinocchio_library_path})
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the library file exists
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_parsers.so OUTPUT_VARIABLE pinocchio_parsers_path)
+    if (NOT EXISTS ${pinocchio_parsers_path})
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio parsers path. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Extract version from the library filename
+    file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info")
+    cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename)
+    string(REPLACE "pin-" "" pinocchio_VERSION "${pinocchio_library_filename}")
+    string(REPLACE ".dist-info" "" pinocchio_VERSION "${pinocchio_VERSION}")
+
+    # Create the imported target
+    add_library(pinocchio::pinocchio SHARED IMPORTED)
+    target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS})
+    set_target_properties(pinocchio::pinocchio
+        PROPERTIES
+        IMPORTED_LOCATION "${pinocchio_library_path}"
+    )
+
+    add_library(pinocchio::parsers SHARED IMPORTED)
+    target_include_directories(pinocchio::parsers INTERFACE ${pinocchio_INCLUDE_DIRS})
+    set_target_properties(pinocchio::parsers
+        PROPERTIES
+        IMPORTED_LOCATION "${pinocchio_parsers_path}"
+    )
+
+    add_library(pinocchio::all INTERFACE IMPORTED)
+    set_target_properties(pinocchio::all
+        PROPERTIES
+        INTERFACE_LINK_LIBRARIES "pinocchio::pinocchio;pinocchio::parsers"
+    )
+    set(pinocchio_FOUND TRUE)
+
+endif()

extensions/rcs_so101/cmake/Findrcs.cmake

@@ -0,0 +1,46 @@
+if (NOT rcs_FOUND)
+    if (NOT Python3_FOUND)
+        set(rcs_FOUND FALSE)
+        if (rcs_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the include directory exists
+    cmake_path(APPEND Python3_SITELIB rcs include OUTPUT_VARIABLE rcs_INCLUDE_DIRS)
+    if (NOT EXISTS ${rcs_INCLUDE_DIRS})
+        set(rcs_FOUND FALSE)
+        if (rcs_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the library file exists
+    cmake_path(APPEND Python3_SITELIB rcs OUTPUT_VARIABLE rcs_library_path)
+    file(GLOB rcs_library_path "${rcs_library_path}/librcs.so")
+    if (NOT EXISTS ${rcs_library_path})
+        set(rcs_FOUND FALSE)
+        if (rcs_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.")
+        endif()
+        return()
+    endif()
+
+    # Extract version from the library filename
+    # file(GLOB rcs_dist_info "${Python3_SITELIB}/rcs-*.dist-info")
+    # cmake_path(GET rcs_dist_info FILENAME rcs_library_filename)
+    # string(REPLACE "rcs-" "" rcs_VERSION "${rcs_library_filename}")
+    # string(REPLACE ".dist-info" "" rcs_VERSION "${rcs_VERSION}")
+
+    # Create the imported target
+    add_library(rcs SHARED IMPORTED)
+    target_include_directories(rcs INTERFACE ${rcs_INCLUDE_DIRS})
+    set_target_properties(
+        rcs
+        PROPERTIES
+        IMPORTED_LOCATION "${rcs_library_path}"
+    )
+    set(rcs_FOUND TRUE)
+endif()

extensions/rcs_so101/pyproject.toml

@@ -1,6 +1,6 @@
 [build-system]
-requires = ["setuptools"]
-build-backend = "setuptools.build_meta"
+requires = []
+build-backend = "scikit_build_core.build"
 
 [project]
 name = "rcs_so101"
@@ -19,4 +19,12 @@ authors = [
   { name = "Pierre Krack", email = "pierre.krack@utn.de" },
 ]
 requires-python = ">=3.10"
-license = "AGPL-3.0-or-later"
+license = "AGPL-3.0-or-later"
+
+[tool.scikit-build]
+build.verbose = true
+build.targets = ["_core"]
+logging.level = "INFO"
+build-dir = "build"
+wheel.packages = ["src/rcs_so101"]
+install.components = ["python_package"]

extensions/rcs_so101/src/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
+add_subdirectory(pybind)

extensions/rcs_so101/src/pybind/CMakeLists.txt

@@ -0,0 +1,11 @@
+pybind11_add_module(_core MODULE rcs.cpp)
+target_link_libraries(_core PRIVATE rcs Eigen3::Eigen pinocchio::all)
+target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION})
+
+set_target_properties(_core PROPERTIES
+    INSTALL_RPATH "$ORIGIN;$ORIGIN/../rcs;$ORIGIN/../cmeel.prefix/lib"
+    INTERPROCEDURAL_OPTIMIZATION TRUE
+)
+
+# in pip
+install(TARGETS _core DESTINATION rcs_so101 COMPONENT python_package)