Skip to content

Commit 312e9a9

Browse files
juelgjuelg
committed
feat(extensions): initial support for franka emika panda robot
1 parent 801800f commit 312e9a9

30 files changed

+3110
-2
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
cmake_minimum_required(VERSION 3.19)
1+
cmake_minimum_required(VERSION 3.5)
22

33
project(
44
rcs

‎extensions/rcs_panda/CMakeLists.txt‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(
4+
rcs_panda
5+
LANGUAGES C CXX
6+
VERSION 0.4.0
7+
DESCRIPTION "RCS Libfranka 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+
# turn off libfranka tests
35+
set(BUILD_TESTS OFF)
36+
set(BUILD_EXAMPLES OFF)
37+
set(RL_BUILD_DEMOS OFF)
38+
set(RL_BUILD_RL_SG OFF)
39+
set(RL_BUILD_TESTS OFF)
40+
set(RL_BUILD_EXTRAS OFF)
41+
set(BUILD_PYTHON_INTERFACE OFF)
42+
set(BUILD_DOCUMENTATION OFF)
43+
44+
include(FetchContent)
45+
46+
find_package(Eigen3 REQUIRED)
47+
find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
48+
find_package(pinocchio REQUIRED)
49+
find_package(rcs REQUIRED)
50+
51+
FetchContent_Declare(rl
52+
GIT_REPOSITORY https://github.com/roboticslibrary/rl.git
53+
GIT_TAG 0b3797215345a1d37903634095361233d190b2e6
54+
GIT_PROGRESS TRUE
55+
EXCLUDE_FROM_ALL
56+
)
57+
FetchContent_Declare(
58+
libfranka
59+
GIT_REPOSITORY https://github.com/frankaemika/libfranka.git
60+
GIT_TAG 0.9.2
61+
GIT_PROGRESS TRUE
62+
EXCLUDE_FROM_ALL
63+
)
64+
FetchContent_Declare(pybind11
65+
GIT_REPOSITORY https://github.com/pybind/pybind11.git
66+
GIT_TAG v2.13.4
67+
GIT_PROGRESS TRUE
68+
EXCLUDE_FROM_ALL
69+
)
70+
71+
FetchContent_MakeAvailable(rl libfranka pybind11)
72+
73+
add_subdirectory(src)

‎extensions/rcs_panda/README.md‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
# RCS Panda Hardware Extension
2+
Extension to control the panda with rcs.
3+
4+
## Installation
5+
```shell
6+
pip install -ve .
7+
```
8+
9+
Add your Panda credentials to a `.env` file like this:
10+
```env
11+
DESK_USERNAME=...
12+
DESK_PASSWORD=...
13+
```
14+
15+
## Usage
16+
```python
17+
import rcs_panda
18+
from rcs_panda._core import hw
19+
from rcs_panda.desk import FCI, ContextManager, Desk, load_creds_fr3_desk
20+
user, pw = load_creds_fr3_desk()
21+
with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False):
22+
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
23+
ik = rcs.common.RL(str(urdf_path))
24+
robot = hw.FR3(ROBOT_IP, ik)
25+
robot_cfg = FR3Config()
26+
robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
27+
robot_cfg.ik_solver = IKSolver.rcs_ik
28+
robot.set_parameters(robot_cfg)
29+
30+
gripper_cfg_hw = hw.FHConfig()
31+
gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
32+
gripper_cfg_hw.speed = 0.1
33+
gripper_cfg_hw.force = 30
34+
gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg_hw)
35+
robot.set_cartesian_position(
36+
robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0]))
37+
)
38+
gripper.grasp()
39+
```
40+
For more examples see the [examples](../../examples/) folder.
41+
You can switch to hardware by setting the following flag:
42+
```python
43+
ROBOT_INSTANCE = RobotPlatform.HARDWARE
44+
# ROBOT_INSTANCE = RobotPlatform.SIMULATION
45+
```
46+
47+
48+
## CLI
49+
Defines useful commands to handle the FR3 robot without the need to use the Desk Website.
50+
You can see the available subcommands as follows:
51+
```shell
52+
python -m rcs_panda --help
53+
```

‎extensions/rcs_panda/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()

‎extensions/rcs_panda/cmake/Findrcs.cmake‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
if (NOT rcs_FOUND)
2+
if (NOT Python3_FOUND)
3+
set(rcs_FOUND FALSE)
4+
if (rcs_FIND_REQUIRED)
5+
message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.")
6+
endif()
7+
return()
8+
endif()
9+
10+
# Check if the include directory exists
11+
cmake_path(APPEND Python3_SITELIB rcs include OUTPUT_VARIABLE rcs_INCLUDE_DIRS)
12+
if (NOT EXISTS ${rcs_INCLUDE_DIRS})
13+
set(rcs_FOUND FALSE)
14+
if (rcs_FIND_REQUIRED)
15+
message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.")
16+
endif()
17+
return()
18+
endif()
19+
20+
# Check if the library file exists
21+
cmake_path(APPEND Python3_SITELIB rcs OUTPUT_VARIABLE rcs_library_path)
22+
file(GLOB rcs_library_path "${rcs_library_path}/librcs.so")
23+
if (NOT EXISTS ${rcs_library_path})
24+
set(rcs_FOUND FALSE)
25+
if (rcs_FIND_REQUIRED)
26+
message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.")
27+
endif()
28+
return()
29+
endif()
30+
31+
# Extract version from the library filename
32+
# file(GLOB rcs_dist_info "${Python3_SITELIB}/rcs-*.dist-info")
33+
# cmake_path(GET rcs_dist_info FILENAME rcs_library_filename)
34+
# string(REPLACE "rcs-" "" rcs_VERSION "${rcs_library_filename}")
35+
# string(REPLACE ".dist-info" "" rcs_VERSION "${rcs_VERSION}")
36+
37+
# Create the imported target
38+
add_library(rcs SHARED IMPORTED)
39+
target_include_directories(rcs INTERFACE ${rcs_INCLUDE_DIRS})
40+
set_target_properties(
41+
rcs
42+
PROPERTIES
43+
IMPORTED_LOCATION "${rcs_library_path}"
44+
)
45+
set(rcs_FOUND TRUE)
46+
endif()

‎extensions/rcs_panda/pyproject.toml‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
[build-system]
2+
requires = []
3+
build-backend = "scikit_build_core.build"
4+
5+
[project]
6+
name = "rcs_panda"
7+
version = "0.4.0"
8+
description="RCS libfranka integration"
9+
dependencies = [
10+
# NOTE: when changing the pin version, also change it in requirements_dev.txt
11+
"pin==3.7.0",
12+
]
13+
readme = "../../README.md"
14+
maintainers = [
15+
{ name = "Tobias Jülg", email = "[email protected]" },
16+
]
17+
authors = [
18+
{ name = "Tobias Jülg", email = "[email protected]" },
19+
{ name = "Pierre Krack", email = "[email protected]" },
20+
]
21+
requires-python = ">=3.10"
22+
license = {file = "../../LICENSE"}
23+
classifiers = [
24+
"Development Status :: 3 - Alpha",
25+
"License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)",
26+
"Programming Language :: Python :: 3",
27+
]
28+
29+
30+
[tool.scikit-build]
31+
build.verbose = true
32+
build.targets = ["_core"]
33+
logging.level = "INFO"
34+
build-dir = "build"
35+
wheel.packages = ["src/rcs_panda"]
36+
install.components = ["python_package"]

‎extensions/rcs_panda/src/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
2+
add_subdirectory(hw)
3+
add_subdirectory(pybind)

‎extensions/rcs_panda/src/hw/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
add_library(hw)
2+
target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp FR3MotionGenerator.cpp)
3+
target_link_libraries(
4+
hw
5+
PUBLIC franka rcs pinocchio::all
6+
PRIVATE math
7+
)
8+
9+
# add_executable(test_hw main.cpp)
10+
# target_link_libraries(test_hw PRIVATE hw)

0 commit comments

Comments
 (0)