Skip to content

Commit 8db146a

Browse files
juelgjuelg
committed
feat: refactor robotics library ik into own extension
- moved robotics library to own extension, this means less dependencies for the main rcs install and ik is already covered by pinoccio - fixed automatic version numbering of extensions in commitizen - removed unused realtime flag in simrobot config - temporary fixed so101 to lint, this should be properly fixed once the so101 branch is merged in
1 parent 6c88b88 commit 8db146a

File tree

36 files changed

+484
-358
lines changed

36 files changed

+484
-358
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -39,20 +39,14 @@ find_package(MuJoCo REQUIRED)
3939
find_package(pinocchio REQUIRED)
4040

4141

42-
FetchContent_Declare(rl
43-
GIT_REPOSITORY https://github.com/roboticslibrary/rl.git
44-
GIT_TAG 0b3797215345a1d37903634095361233d190b2e6
45-
GIT_PROGRESS TRUE
46-
EXCLUDE_FROM_ALL
47-
)
4842
FetchContent_Declare(pybind11
4943
GIT_REPOSITORY https://github.com/pybind/pybind11.git
5044
GIT_TAG v2.13.4
5145
GIT_PROGRESS TRUE
5246
EXCLUDE_FROM_ALL
5347
)
5448

55-
FetchContent_MakeAvailable(rl pybind11)
49+
FetchContent_MakeAvailable(pybind11)
5650
include(compile_scenes)
5751

5852
add_subdirectory(src)

‎Makefile‎

Lines changed: 2 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -35,16 +35,8 @@ stubgen:
3535
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
3636
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
3737
ruff check --fix python/rcs/_core
38-
pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3
39-
find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete
40-
find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/'
41-
find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
42-
find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
43-
rm -r extensions/rcs_fr3/src/rcs_fr3/_core
44-
mv extensions/rcs_fr3/_core/ extensions/rcs_fr3/src/rcs_fr3/
45-
ruff check --fix extensions/rcs_fr3/src/rcs_fr3/_core
46-
isort python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core
47-
black python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core
38+
isort python/rcs/_core
39+
black python/rcs/_core
4840

4941
# Python
5042
pycheckformat:

‎examples/fr3/fr3_direct_control.py‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -60,8 +60,11 @@ def main():
6060
gripper: rcs.common.Gripper
6161
if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
6262
simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb)
63-
urdf_path = rcs.scenes["fr3_empty_world"].urdf
64-
ik = rcs.common.RL(str(urdf_path))
63+
mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot
64+
ik = rcs.common.Pin(
65+
mjcf_path,
66+
"attachment_site_0",
67+
)
6568
cfg = sim.SimRobotConfig()
6669
cfg.add_id("0")
6770
cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
@@ -92,8 +95,11 @@ def main():
9295
simulation.open_gui()
9396

9497
else:
95-
urdf_path = rcs.scenes["fr3_empty_world"].urdf
96-
ik = rcs.common.RL(str(urdf_path))
98+
mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot
99+
ik = rcs.common.Pin(
100+
mjcf_path,
101+
"attachment_site_0",
102+
)
97103
robot = hw.FR3(ROBOT_IP, ik)
98104
robot_cfg = hw.FR3Config()
99105
robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())

‎extensions/rcs_fr3/src/hw/FR3.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,8 @@ struct FR3Config : common::RobotConfig {
3434
// TODO: max force and elbow?
3535
// TODO: we can either write specific bindings for each, or we use python
3636
// dictionaries with these objects
37+
common::RobotType robot_type = common::RobotType::FR3;
38+
common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE;
3739
IKSolver ik_solver = IKSolver::franka_ik;
3840
double speed_factor = DEFAULT_SPEED_FACTOR;
3941
std::optional<FR3Load> load_parameters = std::nullopt;

‎extensions/rcs_fr3/src/rcs_fr3/creators.py‎

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,6 @@ def __call__( # type: ignore
3838
camera_set: HardwareCameraSet | None = None,
3939
max_relative_movement: float | tuple[float, float] | None = None,
4040
relative_to: RelativeTo = RelativeTo.LAST_STEP,
41-
urdf_path: str | PathLike | None = None,
4241
) -> gym.Env:
4342
"""
4443
Creates a hardware environment for the FR3 robot.
@@ -55,14 +54,16 @@ def __call__( # type: ignore
5554
translational movement in meters. If tuple, it restricts both translational (in meters) and rotational
5655
(in radians) movements. If None, no restriction is applied.
5756
relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step.
58-
urdf_path (str | PathLike | None): Path to the URDF file. If None the included one is used. A URDF file is needed for collision guarding.
5957
6058
Returns:
6159
gym.Env: The configured hardware environment for the FR3 robot.
6260
"""
63-
if urdf_path is None:
64-
urdf_path = rcs.scenes["fr3_empty_world"].urdf
65-
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
61+
ik = rcs.common.Pin(
62+
robot_cfg.kinematic_model_path,
63+
robot_cfg.attachment_site,
64+
urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
65+
)
66+
# ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
6667
robot = hw.FR3(ip, ik)
6768
robot.set_config(robot_cfg)
6869

@@ -111,11 +112,15 @@ def __call__( # type: ignore
111112
camera_set: HardwareCameraSet | None = None,
112113
max_relative_movement: float | tuple[float, float] | None = None,
113114
relative_to: RelativeTo = RelativeTo.LAST_STEP,
114-
urdf_path: str | PathLike | None = None,
115115
) -> gym.Env:
116116

117-
urdf_path = rcs.scenes["fr3_empty_world"].urdf
118-
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
117+
ik = rcs.common.Pin(
118+
robot_cfg.kinematic_model_path,
119+
robot_cfg.attachment_site,
120+
urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
121+
)
122+
# ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
123+
119124
robots: dict[str, hw.FR3] = {}
120125
for ip in ips:
121126
robots[ip] = hw.FR3(ip, ik)

‎extensions/rcs_fr3/src/rcs_fr3/utils.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,15 @@
11
from rcs_fr3._core import hw
22

3+
import rcs
34
from rcs import common
45

56

67
def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config:
78
robot_cfg = hw.FR3Config()
9+
robot_cfg.robot_type = rcs.scenes["fr3_empty_world"].robot_type
10+
robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot
811
robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset())
12+
robot_cfg.attachment_site = "attachment_site_0"
913
robot_cfg.speed_factor = 0.1
1014
robot_cfg.ik_solver = hw.IKSolver.rcs_ik
1115
robot_cfg.async_control = async_control

‎extensions/rcs_robotics_library/CMakeLists.txt‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
cmake_minimum_required(VERSION 3.19)
2+
3+
project(
4+
rcs_fr3
5+
LANGUAGES C CXX
6+
VERSION 0.4.0
7+
DESCRIPTION "RCS robotics library integration"
8+
)
9+
10+
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
11+
12+
set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # Allow us to set options for subprojects
13+
14+
cmake_policy(SET CMP0048 NEW) # Set version in project
15+
# Allow target properties affecting visibility during linking in static libraries
16+
set(CMAKE_POLICY_DEFAULT_CMP0063 NEW)
17+
cmake_policy(SET CMP0072 NEW) # Use GLVND instead of legacy libGL.so
18+
cmake_policy(SET CMP0135 NEW) # Use timestamp of file extraction not download
19+
cmake_policy(SET CMP0140 NEW) # Check return arguments
20+
21+
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
22+
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
23+
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
24+
25+
set(CMAKE_CXX_STANDARD 20)
26+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
27+
set(CMAKE_CXX_EXTENSIONS OFF)
28+
29+
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
30+
31+
set(BUILD_SHARED_LIBS OFF)
32+
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
33+
34+
set(RL_BUILD_DEMOS OFF)
35+
set(RL_BUILD_RL_SG OFF)
36+
set(RL_BUILD_TESTS OFF)
37+
set(RL_BUILD_EXTRAS OFF)
38+
set(BUILD_PYTHON_INTERFACE OFF)
39+
set(BUILD_DOCUMENTATION OFF)
40+
41+
include(FetchContent)
42+
43+
find_package(Eigen3 REQUIRED)
44+
find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
45+
find_package(pinocchio REQUIRED)
46+
find_package(rcs REQUIRED)
47+
48+
FetchContent_Declare(rl
49+
GIT_REPOSITORY https://github.com/roboticslibrary/rl.git
50+
GIT_TAG 0b3797215345a1d37903634095361233d190b2e6
51+
GIT_PROGRESS TRUE
52+
EXCLUDE_FROM_ALL
53+
)
54+
FetchContent_Declare(pybind11
55+
GIT_REPOSITORY https://github.com/pybind/pybind11.git
56+
GIT_TAG v2.13.4
57+
GIT_PROGRESS TRUE
58+
EXCLUDE_FROM_ALL
59+
)
60+
61+
FetchContent_MakeAvailable(rl pybind11)
62+
63+
add_subdirectory(src)

‎extensions/rcs_robotics_library/Makefile‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
PYSRC = src
2+
CPPSRC = src
3+
COMPILE_MODE = Release
4+
5+
# CPP
6+
cppcheckformat:
7+
clang-format --dry-run -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h')
8+
9+
cppformat:
10+
clang-format -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h')
11+
12+
cpplint:
13+
clang-tidy -p=build --warnings-as-errors='*' $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -name '*.h')
14+
15+
# import errors
16+
# clang-tidy -p=build --warnings-as-errors='*' $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -name '*.h')
17+
18+
gcccompile:
19+
pip install --upgrade --requirement requirements_dev.txt
20+
cmake -DCMAKE_BUILD_TYPE=${COMPILE_MODE} -DCMAKE_C_COMPILER=gcc -DCMAKE_CXX_COMPILER=g++ -B build -G Ninja
21+
cmake --build build --target _core
22+
23+
clangcompile:
24+
pip install --upgrade --requirement requirements_dev.txt
25+
cmake -DCMAKE_BUILD_TYPE=${COMPILE_MODE} -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++ -B build -G Ninja
26+
cmake --build build --target _core
27+
28+
# Auto generation of CPP binding stub files
29+
stubgen:
30+
pybind11-stubgen -o src --numpy-array-use-type-var rcs_robotics_library
31+
find ./src -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/'
32+
find ./src -not -path "./src/rcs_robotics_library/_core/*" -name '*.pyi' -delete
33+
find ./src/rcs_robotics_library/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
34+
find ./src/rcs_robotics_library/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
35+
ruff check --fix src/rcs_robotics_library/_core
36+
isort src/rcs_robotics_library/_core
37+
black src/rcs_robotics_library/_core
38+
39+
40+
41+
.PHONY: cppcheckformat cppformat cpplint gcccompile clangcompile stubgen

‎extensions/rcs_robotics_library/README.md‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
# RCS Extension for the Robotics Library
2+
Implements access to the C++ library "Robotics Library".
3+
4+
## Installation
5+
```shell
6+
pip install -ve .
7+
```
8+
9+
## Usage
10+

‎extensions/rcs_robotics_library/cmake/Findpinocchio.cmake‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
if (NOT pinocchio_FOUND)
2+
if (NOT Python3_FOUND)
3+
set(pinocchio_FOUND FALSE)
4+
if (pinocchio_FIND_REQUIRED)
5+
message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
6+
endif()
7+
return()
8+
endif()
9+
10+
# Check if the include directory exists
11+
cmake_path(APPEND Python3_SITELIB cmeel.prefix include OUTPUT_VARIABLE pinocchio_INCLUDE_DIRS)
12+
if (NOT EXISTS ${pinocchio_INCLUDE_DIRS})
13+
set(pinocchio_FOUND FALSE)
14+
if (pinocchio_FIND_REQUIRED)
15+
message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
16+
endif()
17+
return()
18+
endif()
19+
20+
# Check if the library file exists
21+
cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_default.so OUTPUT_VARIABLE pinocchio_library_path)
22+
if (NOT EXISTS ${pinocchio_library_path})
23+
set(pinocchio_FOUND FALSE)
24+
if (pinocchio_FIND_REQUIRED)
25+
message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
26+
endif()
27+
return()
28+
endif()
29+
30+
# Check if the library file exists
31+
cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_parsers.so OUTPUT_VARIABLE pinocchio_parsers_path)
32+
if (NOT EXISTS ${pinocchio_parsers_path})
33+
set(pinocchio_FOUND FALSE)
34+
if (pinocchio_FIND_REQUIRED)
35+
message(FATAL_ERROR "Could not find pinocchio parsers path. Please install pinocchio using pip.")
36+
endif()
37+
return()
38+
endif()
39+
40+
# Extract version from the library filename
41+
file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info")
42+
cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename)
43+
string(REPLACE "pin-" "" pinocchio_VERSION "${pinocchio_library_filename}")
44+
string(REPLACE ".dist-info" "" pinocchio_VERSION "${pinocchio_VERSION}")
45+
46+
# Create the imported target
47+
add_library(pinocchio::pinocchio SHARED IMPORTED)
48+
target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS})
49+
set_target_properties(pinocchio::pinocchio
50+
PROPERTIES
51+
IMPORTED_LOCATION "${pinocchio_library_path}"
52+
)
53+
54+
add_library(pinocchio::parsers SHARED IMPORTED)
55+
target_include_directories(pinocchio::parsers INTERFACE ${pinocchio_INCLUDE_DIRS})
56+
set_target_properties(pinocchio::parsers
57+
PROPERTIES
58+
IMPORTED_LOCATION "${pinocchio_parsers_path}"
59+
)
60+
61+
add_library(pinocchio::all INTERFACE IMPORTED)
62+
set_target_properties(pinocchio::all
63+
PROPERTIES
64+
INTERFACE_LINK_LIBRARIES "pinocchio::pinocchio;pinocchio::parsers"
65+
)
66+
set(pinocchio_FOUND TRUE)
67+
68+
endif()

0 commit comments

Comments
 (0)