diff --git a/CMakeLists.txt b/CMakeLists.txt index 9850e896..00004a12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,12 +39,6 @@ find_package(MuJoCo REQUIRED) find_package(pinocchio REQUIRED) -FetchContent_Declare(rl - GIT_REPOSITORY https://github.com/roboticslibrary/rl.git - GIT_TAG 0b3797215345a1d37903634095361233d190b2e6 - GIT_PROGRESS TRUE - EXCLUDE_FROM_ALL -) FetchContent_Declare(pybind11 GIT_REPOSITORY https://github.com/pybind/pybind11.git GIT_TAG v2.13.4 @@ -52,7 +46,7 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) -FetchContent_MakeAvailable(rl pybind11) +FetchContent_MakeAvailable(pybind11) include(compile_scenes) add_subdirectory(src) diff --git a/Makefile b/Makefile index c6510394..f3764d69 100644 --- a/Makefile +++ b/Makefile @@ -35,16 +35,8 @@ stubgen: find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g' ruff check --fix python/rcs/_core - pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3 - find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete - find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' - find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' - find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g' - rm -r extensions/rcs_fr3/src/rcs_fr3/_core - mv extensions/rcs_fr3/_core/ extensions/rcs_fr3/src/rcs_fr3/ - ruff check --fix extensions/rcs_fr3/src/rcs_fr3/_core - isort python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core - black python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core + isort python/rcs/_core + black python/rcs/_core # Python pycheckformat: diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index c32c42ea..80e675a0 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/examples/fr3/fr3_direct_control.py @@ -60,8 +60,11 @@ def main(): gripper: rcs.common.Gripper if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb) - urdf_path = rcs.scenes["fr3_empty_world"].urdf - ik = rcs.common.RL(str(urdf_path)) + mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot + ik = rcs.common.Pin( + mjcf_path, + "attachment_site_0", + ) cfg = sim.SimRobotConfig() cfg.add_id("0") cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) @@ -92,8 +95,11 @@ def main(): simulation.open_gui() else: - urdf_path = rcs.scenes["fr3_empty_world"].urdf - ik = rcs.common.RL(str(urdf_path)) + mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot + ik = rcs.common.Pin( + mjcf_path, + "attachment_site_0", + ) robot = hw.FR3(ROBOT_IP, ik) robot_cfg = hw.FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) diff --git a/extensions/rcs_fr3/src/hw/FR3.cpp b/extensions/rcs_fr3/src/hw/FR3.cpp index 73222ba7..f0295afe 100644 --- a/extensions/rcs_fr3/src/hw/FR3.cpp +++ b/extensions/rcs_fr3/src/hw/FR3.cpp @@ -19,7 +19,8 @@ namespace rcs { namespace hw { -FR3::FR3(const std::string &ip, std::optional> ik, +FR3::FR3(const std::string &ip, + std::optional> ik, const std::optional &cfg) : robot(ip), m_ik(ik) { // set collision behavior and impedance @@ -650,7 +651,9 @@ double quintic_polynomial_speed_profile(double time, double start_time, // return (1 - std::cos(M_PI * progress)) / 2.0; } -std::optional> FR3::get_ik() { return this->m_ik; } +std::optional> FR3::get_ik() { + return this->m_ik; +} void FR3::set_cartesian_position(const common::Pose &x) { // pose is assumed to be in the robots coordinate frame @@ -686,8 +689,8 @@ void FR3::set_cartesian_position_ik(const common::Pose &pose) { "No inverse kinematics was provided. Cannot use IK to set cartesian " "position."); } - auto joints = this->m_ik.value()->ik(pose, this->get_joint_position(), - this->cfg.tcp_offset); + auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(), + this->cfg.tcp_offset); if (joints.has_value()) { this->set_joint_position(joints.value()); diff --git a/extensions/rcs_fr3/src/hw/FR3.h b/extensions/rcs_fr3/src/hw/FR3.h index 71d9091e..5e61fea5 100644 --- a/extensions/rcs_fr3/src/hw/FR3.h +++ b/extensions/rcs_fr3/src/hw/FR3.h @@ -10,7 +10,7 @@ #include #include -#include "rcs/IK.h" +#include "rcs/Kinematics.h" #include "rcs/LinearPoseTrajInterpolator.h" #include "rcs/Pose.h" #include "rcs/Robot.h" @@ -34,6 +34,8 @@ struct FR3Config : common::RobotConfig { // TODO: max force and elbow? // TODO: we can either write specific bindings for each, or we use python // dictionaries with these objects + common::RobotType robot_type = common::RobotType::FR3; + common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE; IKSolver ik_solver = IKSolver::franka_ik; double speed_factor = DEFAULT_SPEED_FACTOR; std::optional load_parameters = std::nullopt; @@ -48,7 +50,7 @@ class FR3 : public common::Robot { private: franka::Robot robot; FR3Config cfg; - std::optional> m_ik; + std::optional> m_ik; std::optional control_thread = std::nullopt; common::LinearPoseTrajInterpolator traj_interpolator; double controller_time = 0.0; @@ -62,7 +64,7 @@ class FR3 : public common::Robot { public: FR3(const std::string &ip, - std::optional> ik = std::nullopt, + std::optional> ik = std::nullopt, const std::optional &cfg = std::nullopt); ~FR3() override; @@ -100,7 +102,7 @@ class FR3 : public common::Robot { void set_cartesian_position(const common::Pose &pose) override; - std::optional> get_ik() override; + std::optional> get_ik() override; void set_cartesian_position_internal(const common::Pose &pose, double max_time, diff --git a/extensions/rcs_fr3/src/hw/main.cpp b/extensions/rcs_fr3/src/hw/main.cpp index df624717..e92a0a25 100644 --- a/extensions/rcs_fr3/src/hw/main.cpp +++ b/extensions/rcs_fr3/src/hw/main.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include #include diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index f626e082..f9dc98e5 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -9,7 +9,7 @@ #include -#include "rcs/IK.h" +#include "rcs/Kinematics.h" #include "rcs/Pose.h" #include "rcs/Robot.h" #include "rcs/utils.h" @@ -99,7 +99,7 @@ PYBIND11_MODULE(_core, m) { (py::object)py::module_::import("rcs").attr("common").attr("Robot"); py::class_>(hw, "FR3", robot) .def(py::init>>(), + std::optional>>(), py::arg("ip"), py::arg("ik") = std::nullopt) .def("set_config", &rcs::hw::FR3::set_config, py::arg("cfg")) .def("get_config", &rcs::hw::FR3::get_config) diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index b61f38bb..7252e32b 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -52,7 +52,7 @@ class FHState(rcs._core.common.GripperState): def width(self) -> float: ... class FR3(rcs._core.common.Robot): - def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ... + def __init__(self, ip: str, ik: rcs._core.common.Kinematics | None = None) -> None: ... def automatic_error_recovery(self) -> None: ... def controller_set_joint_position( self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 5dbbd35d..69e2f828 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -38,7 +38,6 @@ def __call__( # type: ignore camera_set: HardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, ) -> gym.Env: """ Creates a hardware environment for the FR3 robot. @@ -55,14 +54,16 @@ def __call__( # type: ignore translational movement in meters. If tuple, it restricts both translational (in meters) and rotational (in radians) movements. If None, no restriction is applied. relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - 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. Returns: gym.Env: The configured hardware environment for the FR3 robot. """ - if urdf_path is None: - urdf_path = rcs.scenes["fr3_empty_world"].urdf - ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.Pin( + robot_cfg.kinematic_model_path, + robot_cfg.attachment_site, + urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), + ) + # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) robot = hw.FR3(ip, ik) robot.set_config(robot_cfg) @@ -111,11 +112,15 @@ def __call__( # type: ignore camera_set: HardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, ) -> gym.Env: - urdf_path = rcs.scenes["fr3_empty_world"].urdf - ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.Pin( + robot_cfg.kinematic_model_path, + robot_cfg.attachment_site, + urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), + ) + # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) + robots: dict[str, hw.FR3] = {} for ip in ips: robots[ip] = hw.FR3(ip, ik) diff --git a/extensions/rcs_fr3/src/rcs_fr3/utils.py b/extensions/rcs_fr3/src/rcs_fr3/utils.py index 0c368bd7..0729a918 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/utils.py +++ b/extensions/rcs_fr3/src/rcs_fr3/utils.py @@ -1,11 +1,15 @@ from rcs_fr3._core import hw +import rcs from rcs import common def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config: robot_cfg = hw.FR3Config() + robot_cfg.robot_type = rcs.scenes["fr3_empty_world"].robot_type + robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) + robot_cfg.attachment_site = "attachment_site_0" robot_cfg.speed_factor = 0.1 robot_cfg.ik_solver = hw.IKSolver.rcs_ik robot_cfg.async_control = async_control diff --git a/extensions/rcs_robotics_library/CMakeLists.txt b/extensions/rcs_robotics_library/CMakeLists.txt new file mode 100644 index 00000000..7c5250ff --- /dev/null +++ b/extensions/rcs_robotics_library/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.19) + +project( + rcs_fr3 + LANGUAGES C CXX + VERSION 0.4.0 + DESCRIPTION "RCS robotics library integration" +) + +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(rl + GIT_REPOSITORY https://github.com/roboticslibrary/rl.git + GIT_TAG 0b3797215345a1d37903634095361233d190b2e6 + GIT_PROGRESS TRUE + EXCLUDE_FROM_ALL +) +FetchContent_Declare(pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + GIT_TAG v2.13.4 + GIT_PROGRESS TRUE + EXCLUDE_FROM_ALL +) + +FetchContent_MakeAvailable(rl pybind11) + +add_subdirectory(src) diff --git a/extensions/rcs_robotics_library/Makefile b/extensions/rcs_robotics_library/Makefile new file mode 100644 index 00000000..9320efb7 --- /dev/null +++ b/extensions/rcs_robotics_library/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_robotics_library + 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_robotics_library/_core/*" -name '*.pyi' -delete + 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' + find ./src/rcs_robotics_library/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g' + ruff check --fix src/rcs_robotics_library/_core + isort src/rcs_robotics_library/_core + black src/rcs_robotics_library/_core + + + +.PHONY: cppcheckformat cppformat cpplint gcccompile clangcompile stubgen diff --git a/extensions/rcs_robotics_library/README.md b/extensions/rcs_robotics_library/README.md new file mode 100644 index 00000000..54fd79e7 --- /dev/null +++ b/extensions/rcs_robotics_library/README.md @@ -0,0 +1,10 @@ +# RCS Extension for the Robotics Library +Implements access to the C++ library "Robotics Library". + +## Installation +```shell +pip install -ve . +``` + +## Usage + diff --git a/extensions/rcs_robotics_library/cmake/Findpinocchio.cmake b/extensions/rcs_robotics_library/cmake/Findpinocchio.cmake new file mode 100644 index 00000000..fcefcf5c --- /dev/null +++ b/extensions/rcs_robotics_library/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() diff --git a/extensions/rcs_robotics_library/cmake/Findrcs.cmake b/extensions/rcs_robotics_library/cmake/Findrcs.cmake new file mode 100644 index 00000000..08456ccc --- /dev/null +++ b/extensions/rcs_robotics_library/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() \ No newline at end of file diff --git a/extensions/rcs_robotics_library/pyproject.toml b/extensions/rcs_robotics_library/pyproject.toml new file mode 100644 index 00000000..63a181dd --- /dev/null +++ b/extensions/rcs_robotics_library/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = [] +build-backend = "scikit_build_core.build" + +[project] +name = "rcs_robotics_library" +version = "0.4.0" +description="RCS robotics library integration" +readme = "README.md" +maintainers = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, +] +authors = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Pierre Krack", email = "pierre.krack@utn.de" }, +] +requires-python = ">=3.10" +license = {file = "../../LICENSE"} + + +[tool.scikit-build] +build.verbose = true +build.targets = ["_core"] +logging.level = "INFO" +build-dir = "build" +wheel.packages = ["src/rcs_robotics_library"] +install.components = ["python_package"] \ No newline at end of file diff --git a/extensions/rcs_robotics_library/src/CMakeLists.txt b/extensions/rcs_robotics_library/src/CMakeLists.txt new file mode 100644 index 00000000..a458c164 --- /dev/null +++ b/extensions/rcs_robotics_library/src/CMakeLists.txt @@ -0,0 +1,2 @@ +target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(pybind) diff --git a/extensions/rcs_robotics_library/src/pybind/CMakeLists.txt b/extensions/rcs_robotics_library/src/pybind/CMakeLists.txt new file mode 100644 index 00000000..09d0acae --- /dev/null +++ b/extensions/rcs_robotics_library/src/pybind/CMakeLists.txt @@ -0,0 +1,11 @@ +pybind11_add_module(_core MODULE rcs.cpp) +target_link_libraries(_core PRIVATE rcs mdl 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_robotics_library COMPONENT python_package) diff --git a/extensions/rcs_robotics_library/src/pybind/RL.h b/extensions/rcs_robotics_library/src/pybind/RL.h new file mode 100644 index 00000000..8ad3c978 --- /dev/null +++ b/extensions/rcs_robotics_library/src/pybind/RL.h @@ -0,0 +1,75 @@ +#ifndef RCS_RL_H +#define RCS_RL_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace rcs { +namespace robotics_library { + +class RoboticsLibraryIK : public rcs::common::Kinematics { + private: + const int random_restarts = 0; + const double eps = 1e-3; + struct { + std::shared_ptr mdl; + std::shared_ptr kin; + std::shared_ptr ik; + } rl_data; + + public: + RoboticsLibraryIK(const std::string& urdf_path, size_t max_duration_ms = 300) + : rl_data() { + this->rl_data.mdl = rl::mdl::UrdfFactory().create(urdf_path); + this->rl_data.kin = + std::dynamic_pointer_cast(this->rl_data.mdl); + this->rl_data.ik = std::make_shared( + this->rl_data.kin.get()); + this->rl_data.ik->setRandomRestarts(this->random_restarts); + this->rl_data.ik->setEpsilon(this->eps); + this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); + } + std::optional inverse( + const rcs::common::Pose& pose, const rcs::common::VectorXd& q0, + const rcs::common::Pose& tcp_offset = + rcs::common::Pose::Identity()) override { + // pose is assumed to be in the robots coordinate frame + this->rl_data.kin->setPosition(q0); + this->rl_data.kin->forwardPosition(); + rcs::common::Pose new_pose = pose * tcp_offset.inverse(); + + this->rl_data.ik->addGoal(new_pose.affine_matrix(), 0); + bool success = this->rl_data.ik->solve(); + if (success) { + // is this forward needed and is it mabye possible to call + // this on the model? + this->rl_data.kin->forwardPosition(); + return this->rl_data.kin->getPosition(); + } else { + return std::nullopt; + } + } + rcs::common::Pose forward(const rcs::common::VectorXd& q0, + const rcs::common::Pose& tcp_offset) override { + // pose is assumed to be in the robots coordinate frame + this->rl_data.kin->setPosition(q0); + this->rl_data.kin->forwardPosition(); + rcs::common::Pose pose = this->rl_data.kin->getOperationalPosition(0); + // apply the tcp offset + return pose * tcp_offset.inverse(); + } +}; + +} // namespace robotics_library +} // namespace rcs + +#endif // RCS_RL_H diff --git a/extensions/rcs_robotics_library/src/pybind/rcs.cpp b/extensions/rcs_robotics_library/src/pybind/rcs.cpp new file mode 100644 index 00000000..8efaa951 --- /dev/null +++ b/extensions/rcs_robotics_library/src/pybind/rcs.cpp @@ -0,0 +1,47 @@ +#include +#include +#include +#include +#include + +#include + +#include "RL.h" +#include "rcs/Kinematics.h" +#include "rl/mdl/UrdfFactory.h" + +// TODO: define exceptions + +#define STRINGIFY(x) #x +#define MACRO_STRINGIFY(x) STRINGIFY(x) + +namespace py = pybind11; + +PYBIND11_MODULE(_core, m) { + m.doc() = R"pbdoc( + Robot Control Stack Python Bindings + ----------------------- + + .. currentmodule:: _core + + .. autosummary:: + :toctree: _generate + + )pbdoc"; +#ifdef VERSION_INFO + m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); +#else + m.attr("__version__") = "dev"; +#endif + + // HARDWARE MODULE + auto rl = m.def_submodule("rl", "rcs robotics library module"); + + py::object kinematics = + (py::object)py::module_::import("rcs").attr("common").attr("Kinematics"); + py::class_>( + rl, "RoboticsLibraryIK", kinematics) + .def(py::init(), py::arg("urdf_path"), + py::arg("max_duration_ms") = 300); +} diff --git a/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py b/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py new file mode 100644 index 00000000..684b6b35 --- /dev/null +++ b/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py @@ -0,0 +1,5 @@ +from rcs_robotics_library._core import rl + +__all__ = [ + "rl", +] diff --git a/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi b/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi new file mode 100644 index 00000000..63504020 --- /dev/null +++ b/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi @@ -0,0 +1,19 @@ +# ATTENTION: auto generated from C++ code, use `make stubgen` to update! +""" + + Robot Control Stack Python Bindings + ----------------------- + + .. currentmodule:: _core + + .. autosummary:: + :toctree: _generate + + +""" +from __future__ import annotations + +from . import rl + +__all__ = ["rl"] +__version__: str = "0.4.0" diff --git a/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/rl.pyi b/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/rl.pyi new file mode 100644 index 00000000..3565f4d1 --- /dev/null +++ b/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/rl.pyi @@ -0,0 +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"] + +class RoboticsLibraryIK(rcs._core.common.Kinematics): + def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 48947c68..44f3bafc 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -1,11 +1,8 @@ import logging -import typing from os import PathLike from pathlib import Path -from typing import Type import gymnasium as gym -from gymnasium.envs.registration import EnvCreator from lerobot.common.robots import make_robot_from_config from lerobot.common.robots.so101_follower.config_so101_follower import ( SO101FollowerConfig, @@ -16,8 +13,6 @@ from lerobot.common.teleoperators.so101_leader.so101_leader import SO101Leader from lerobot.common.teleoperators.utils import make_teleoperator_from_config from rcs.camera.hw import HardwareCameraSet -from rcs.camera.interface import BaseCameraSet -from rcs.camera.sim import SimCameraSet from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -27,13 +22,8 @@ RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper, SimWrapper -from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig from rcs_so101.hw import SO101, S0101Gripper -import rcs -from rcs import sim - logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -53,7 +43,7 @@ def __call__( # type: ignore calibration_dir = Path(calibration_dir) cfg = SO101FollowerConfig(id=id, calibration_dir=calibration_dir, port=port) hf_robot = make_robot_from_config(cfg) - robot = SO101(hf_robot, urdf_path=urdf_path) + robot = SO101(hf_robot) # TODO add path env: gym.Env = RobotEnv(robot, ControlMode.JOINTS) # env = FR3HW(env) @@ -83,73 +73,3 @@ def teleoperator( teleop = make_teleoperator_from_config(cfg) teleop.connect() return teleop - - -class SO101SimEnvCreator(EnvCreator): - def __call__( # type: ignore - self, - control_mode: ControlMode, - robot_cfg: SimRobotConfig, - urdf_path: str, - mjcf: str, - collision_guard: bool = False, - gripper_cfg: SimGripperConfig | None = None, - cameras: dict[str, SimCameraConfig] | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - sim_wrapper: Type[SimWrapper] | None = None, - ) -> gym.Env: - """ - Creates a simulation environment for the FR3 robot. - Args: - control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. - collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. - gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. - camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. - max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts - translational movement in meters. If tuple, it restricts both translational (in meters) and rotational - (in radians) movements. If None, no restriction is applied. - relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced - which requires a UTN compatible lab scene to be present. - mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". - sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful - for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. - Returns: - gym.Env: The configured simulation environment for the FR3 robot. - """ - simulation = sim.Sim(mjcf) - - ik = rcs.common.RL(str(urdf_path)) - robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, control_mode) - env = RobotSimWrapper(env, simulation, sim_wrapper) - - if cameras is not None: - camera_set = typing.cast( - BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) - ) - env = CameraSetWrapper(env, camera_set, include_depth=True) - - if gripper_cfg is not None and isinstance(gripper_cfg, SimGripperConfig): - gripper = sim.SimGripper(simulation, gripper_cfg) - env = GripperWrapper(env, gripper, binary=False) - env = GripperWrapperSim(env, gripper) - - # if collision_guard: - # env = CollisionGuard.env_from_xml_paths( - # env, - # str(rcs.scenes.get(str(mjcf), mjcf)), - # str(urdf_path), - # gripper=gripper_cfg is not None, - # check_home_collision=False, - # control_mode=control_mode, - # tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), - # sim_gui=True, - # truncate_on_collision=True, - # ) - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env diff --git a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py deleted file mode 100644 index a5e25f0d..00000000 --- a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py +++ /dev/null @@ -1,73 +0,0 @@ -import logging -from time import sleep - -from rcs._core.common import RobotPlatform -from rcs.envs.base import ControlMode, RelativeTo -from rcs_so101.creators import RCSSO101EnvCreator, SO101SimEnvCreator - -import rcs -from rcs import sim - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - -ROBOT_INSTANCE = RobotPlatform.SIMULATION - - -def main(): - - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - env_rel = RCSSO101EnvCreator()( - id="so101_follower", - urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", - port="/dev/ttyACM0", - calibration_dir="/home/tobi/coding/lerobot-container/calibration/robots/so101_follower/ninja_follower.json", - # max_relative_movement=(0.5, np.deg2rad(90)), - relative_to=RelativeTo.LAST_STEP, - ) - else: - cfg = sim.SimRobotConfig() - cfg.robot_type = rcs.common.RobotType.SO101 - cfg.actuators = ["1", "2", "3", "4", "5"] - cfg.joints = ["1", "2", "3", "4", "5"] - cfg.arm_collision_geoms = [] - cfg.attachment_site = "gripper" - - grp_cfg = sim.SimGripperConfig() - grp_cfg.actuator = "6" - grp_cfg.joint = "6" - grp_cfg.collision_geoms = [] - grp_cfg.collision_geoms_fingers = [] - - env_rel = SO101SimEnvCreator()( - control_mode=ControlMode.JOINTS, - urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", - robot_cfg=cfg, - mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/SO101/scene.xml", - gripper_cfg=grp_cfg, - # camera_set_cfg=default_mujoco_cameraset_cfg(), - max_relative_movement=None, - # max_relative_movement=10.0, - # max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - for _ in range(10): - obs, info = env_rel.reset() - for _ in range(100): - # sample random relative action and execute it - act = env_rel.action_space.sample() - print(act) - # act["gripper"] = 1.0 - obs, reward, terminated, truncated, info = env_rel.step(act) - print(obs) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - logger.info(act["gripper"]) - sleep(1.0) - - -if __name__ == "__main__": - main() diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py index 581f1555..29135dce 100644 --- a/extensions/rcs_so101/src/rcs_so101/hw.py +++ b/extensions/rcs_so101/src/rcs_so101/hw.py @@ -3,12 +3,17 @@ import numpy as np from lerobot.common.robots.so101_follower.so101_follower import SO101Follower -from rcs import common +from rcs import common, scenes class SO101(common.Robot): - def __init__(self, hf_robot: SO101Follower, urdf_path: str): - self.ik = common.RL(urdf_path=urdf_path) + def __init__(self, hf_robot: SO101Follower): + # TODO: fix the hardcoded path when merged with so101 branch + self.ik = common.Pin( + scenes["so101_empty_world"].mjcf_robot, + "attachment_site", + ) + # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) self._hf_robot = hf_robot self._hf_robot.connect() @@ -16,7 +21,7 @@ def __init__(self, hf_robot: SO101Follower, urdf_path: str): def get_cartesian_position(self) -> common.Pose: return self.ik.forward(self.get_joint_position()) - def get_ik(self) -> common.IK | None: + def get_ik(self) -> common.Kinematics | None: return self.ik def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore @@ -55,7 +60,7 @@ def reset(self) -> None: pass def set_cartesian_position(self, pose: common.Pose) -> None: - joints = self.ik.ik(pose, q0=self.get_joint_position()) + joints = self.ik.inverse(pose, q0=self.get_joint_position()) if joints is not None: self.set_joint_position(joints) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index 7c59e103..1a63d355 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -1,32 +1,21 @@ import logging -import typing from os import PathLike from pathlib import Path -from typing import Type import gymnasium as gym -from gymnasium.envs.registration import EnvCreator from rcs.camera.hw import HardwareCameraSet -from rcs.camera.interface import BaseCameraSet -from rcs.camera.sim import SimCameraSet from rcs.envs.base import ( CameraSetWrapper, ControlMode, - GripperWrapper, HandWrapper, RelativeActionSpace, RelativeTo, RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper, SimWrapper from rcs.hand.tilburg_hand import THConfig, TilburgHand -from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig from rcs_xarm7.hw import XArm7 -import rcs -from rcs import sim - logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -60,73 +49,3 @@ def __call__( # type: ignore env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) return env - - -class XArm7SimEnvCreator(EnvCreator): - def __call__( # type: ignore - self, - control_mode: ControlMode, - robot_cfg: SimRobotConfig, - urdf_path: str, - mjcf: str, - collision_guard: bool = False, - gripper_cfg: SimGripperConfig | None = None, - cameras: dict[str, SimCameraConfig] | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - sim_wrapper: Type[SimWrapper] | None = None, - ) -> gym.Env: - """ - Creates a simulation environment for the FR3 robot. - Args: - control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. - collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. - gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. - camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. - max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts - translational movement in meters. If tuple, it restricts both translational (in meters) and rotational - (in radians) movements. If None, no restriction is applied. - relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced - which requires a UTN compatible lab scene to be present. - mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". - sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful - for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. - Returns: - gym.Env: The configured simulation environment for the FR3 robot. - """ - simulation = sim.Sim(mjcf) - - ik = rcs.common.RL(str(urdf_path)) - robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, control_mode) - env = RobotSimWrapper(env, simulation, sim_wrapper) - - if cameras is not None: - camera_set = typing.cast( - BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) - ) - env = CameraSetWrapper(env, camera_set, include_depth=True) - - if gripper_cfg is not None and isinstance(gripper_cfg, SimGripperConfig): - gripper = sim.SimGripper(simulation, gripper_cfg) - env = GripperWrapper(env, gripper, binary=False) - env = GripperWrapperSim(env, gripper) - - # if collision_guard: - # env = CollisionGuard.env_from_xml_paths( - # env, - # str(rcs.scenes.get(str(mjcf), mjcf)), - # str(urdf_path), - # gripper=gripper_cfg is not None, - # check_home_collision=False, - # control_mode=control_mode, - # tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), - # sim_gui=True, - # truncate_on_collision=True, - # ) - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py index 49c1bf93..2fabff45 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py @@ -10,7 +10,6 @@ @dataclass(kw_only=True) class XArm7Config(common.RobotConfig): - # some_custom_config: str = "default_value" payload_weight: float = 0.624 payload_tcp: List[float] = field(default_factory=lambda: [-4.15, 5.24, 76.38]) async_control: bool = False @@ -23,7 +22,7 @@ class XArm7(common.Robot): def __init__(self, ip: str): super().__init__() - self.ik = None # common.RL(urdf_path=urdf_path) + self.ik = None self._config = XArm7Config() self._config.robot_platform = common.RobotPlatform.HARDWARE self._config.robot_type = common.RobotType.XArm7 @@ -52,7 +51,7 @@ def get_cartesian_position(self) -> common.Pose: return common.Pose(rpy_vector=rpy, translation=translation_meter) # type: ignore - def get_ik(self) -> common.IK | None: + def get_ik(self) -> common.Kinematics | None: return self.ik def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]: # type: ignore diff --git a/include/rcs/IK.h b/include/rcs/Kinematics.h similarity index 57% rename from include/rcs/IK.h rename to include/rcs/Kinematics.h index 5c13529a..57d7973a 100644 --- a/include/rcs/IK.h +++ b/include/rcs/Kinematics.h @@ -1,14 +1,10 @@ #ifndef RCS_IK_H #define RCS_IK_H -#include -#include - #include #include #include #include -#include #include #include "Pose.h" @@ -20,34 +16,18 @@ namespace rcs { namespace common { -class IK { +class Kinematics { public: - virtual ~IK(){}; - virtual std::optional ik( + virtual ~Kinematics(){}; + virtual std::optional inverse( const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()) = 0; virtual Pose forward(const VectorXd& q0, const Pose& tcp_offset) = 0; }; -class RL : public IK { - private: - const int random_restarts = 0; - const double eps = 1e-3; - struct { - std::shared_ptr mdl; - std::shared_ptr kin; - std::shared_ptr ik; - } rl_data; - public: - RL(const std::string& path, size_t max_duration_ms = 300); - std::optional ik( - const Pose& pose, const VectorXd& q0, - const Pose& tcp_offset = Pose::Identity()) override; - Pose forward(const VectorXd& q0, const Pose& tcp_offset) override; -}; -class Pin : public IK { +class Pin : public Kinematics { private: const double eps = 1e-4; const int IT_MAX = 1000; @@ -60,7 +40,7 @@ class Pin : public IK { public: Pin(const std::string& path, const std::string& frame_id, bool urdf); - std::optional ik( + std::optional inverse( const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()) override; Pose forward(const VectorXd& q0, const Pose& tcp_offset) override; diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index f8ab00c6..6971b38d 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -5,7 +5,7 @@ #include #include -#include "IK.h" +#include "Kinematics.h" #include "Pose.h" #include "utils.h" @@ -152,7 +152,7 @@ class Robot { virtual void set_cartesian_position(const Pose& pose) = 0; - virtual std::optional> get_ik() = 0; + virtual std::optional> get_ik() = 0; common::Pose to_pose_in_world_coordinates( const Pose& pose_in_robot_coordinates); diff --git a/pyproject.toml b/pyproject.toml index aceb5db2..2c29d2c6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -149,4 +149,16 @@ major_version_zero = true version_files = [ "CMakeLists.txt:VERSION", "python/rcs/_core/__init__.pyi:__version__", + + "extensions/rcs_fr3/CMakeLists.txt:VERSION", + "extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi:__version__", + "extensions/rcs_fr3/pyproject.toml:project.version", + + "extensions/rcs_robotics_library/CMakeLists.txt:VERSION", + "extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi:__version__", + "extensions/rcs_robotics_library/pyproject.toml:project.version", + + "extensions/rcs_realsense/pyproject.toml:project.version", + "extensions/rcs_so101/pyproject.toml:project.version", + "extensions/rcs_xarm7/pyproject.toml:project.version", ] diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 590b34a4..20575018 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -21,16 +21,15 @@ __all__ = [ "Hand", "HandConfig", "HandState", - "IK", "IdentityRotMatrix", "IdentityRotQuatVec", "IdentityTranslation", + "Kinematics", "LATERAL_GRASP", "POWER_GRASP", "PRECISION_GRASP", "Pin", "Pose", - "RL", "RPY", "Robot", "RobotConfig", @@ -128,13 +127,13 @@ class HandConfig: class HandState: def __init__(self) -> None: ... -class IK: +class Kinematics: def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... - def ik( + def inverse( self, pose: Pose, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ... ) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] | None: ... -class Pin(IK): +class Pin(Kinematics): def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = True) -> None: ... class Pose: @@ -195,9 +194,6 @@ class Pose: def translation(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ... def xyzrpy(self) -> numpy.ndarray[tuple[typing.Literal[6]], numpy.dtype[numpy.float64]]: ... -class RL(IK): - def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... - class RPY: pitch: float roll: float @@ -223,7 +219,7 @@ class Robot: def get_base_pose_in_world_coordinates(self) -> Pose: ... def get_cartesian_position(self) -> Pose: ... def get_config(self) -> RobotConfig: ... - def get_ik(self) -> IK | None: ... + def get_ik(self) -> Kinematics | None: ... def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... def get_state(self) -> RobotState: ... def move_home(self) -> None: ... diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 9ac55a03..d80af243 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -151,7 +151,7 @@ class SimGripperState(rcs._core.common.GripperState): class SimRobot(rcs._core.common.Robot): def __init__( - self, sim: Sim, ik: rcs._core.common.IK, cfg: SimRobotConfig, register_convergence_callback: bool = True + self, sim: Sim, ik: rcs._core.common.Kinematics, cfg: SimRobotConfig, register_convergence_callback: bool = True ) -> None: ... def get_config(self) -> SimRobotConfig: ... def get_state(self) -> SimRobotState: ... @@ -165,7 +165,6 @@ class SimRobotConfig(rcs._core.common.RobotConfig): joint_rotational_tolerance: float joints: list[str] mjcf_scene_path: str - realtime: bool seconds_between_callbacks: float trajectory_trace: bool def __init__(self) -> None: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index b3d0ca50..eaf7289d 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -83,7 +83,7 @@ def __call__( # type: ignore robot_cfg.attachment_site, urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), ) - # ik = rcs.common.RL(robot_cfg.kinematic_model_path) + # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) env: gym.Env = RobotEnv(robot, control_mode) @@ -258,7 +258,6 @@ def __call__( # type: ignore rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore ) robot_cfg.robot_type = rcs.common.RobotType.FR3 - robot_cfg.realtime = False robot_cfg.add_id("0") # only required for fr3 robot_cfg.mjcf_scene_path = mjcf_path robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot # .urdf (in case for urdf) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 80b0c4d9..30360d91 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -248,7 +248,6 @@ def env_from_xml_paths( simulation = sim.Sim(mjmld) cfg = default_sim_robot_cfg(mjmld, id) ik = rcs.common.Pin(cg_kinematics_path, cfg.attachment_site, False) - cfg.realtime = False if tcp_offset is not None: cfg.tcp_offset = tcp_offset robot = rcs.sim.SimRobot(simulation, ik, cfg) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index c0001f37..6f1e4460 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -16,7 +16,6 @@ def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: robot_cfg = rcs.sim.SimRobotConfig() - robot_cfg.realtime = False robot_cfg.robot_type = rcs.scenes[scene].robot_type robot_cfg.add_id(idx) robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb diff --git a/python/rcs/ompl/mj_ompl.py b/python/rcs/ompl/mj_ompl.py index 8ec5f233..cc8d840e 100644 --- a/python/rcs/ompl/mj_ompl.py +++ b/python/rcs/ompl/mj_ompl.py @@ -281,7 +281,7 @@ def ik(self, pose, q0=None, tcp_offset=None): numpy.ndarray: Joint positions that achieve the desired pose, or None if no solution is found. """ tcp_offset = tcp_offset if tcp_offset is not None else self.franka_hand_tcp - return self.env.unwrapped.robot.get_ik().ik(pose, q0, tcp_offset) # type: ignore[attr-defined] + return self.env.unwrapped.robot.get_ik().inverse(pose, q0, tcp_offset) # type: ignore[attr-defined] class MjOStateSpace(ob.RealVectorStateSpace): diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index e8f2521c..076df2c3 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -11,11 +11,10 @@ #include -#include "rcs/IK.h" +#include "rcs/Kinematics.h" #include "rcs/Pose.h" #include "rcs/Robot.h" #include "rcs/utils.h" -#include "rl/mdl/UrdfFactory.h" // TODO: define exceptions @@ -69,9 +68,10 @@ class PyRobot : public rcs::common::Robot { void close() override { PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, close, ); } - std::optional> get_ik() override { - PYBIND11_OVERRIDE_PURE(std::optional>, - rcs::common::Robot, get_ik, ); + std::optional> get_ik() override { + PYBIND11_OVERRIDE_PURE( + std::optional>, + rcs::common::Robot, get_ik, ); } rcs::common::Pose get_base_pose_in_world_coordinates() override { PYBIND11_OVERRIDE_PURE(rcs::common::Pose, rcs::common::Robot, @@ -286,18 +286,14 @@ PYBIND11_MODULE(_core, m) { return rcs::common::Pose(t); })); - py::class_>(common, "IK") - .def("ik", &rcs::common::IK::ik, py::arg("pose"), py::arg("q0"), - py::arg("tcp_offset") = rcs::common::Pose::Identity()) - .def("forward", &rcs::common::IK::forward, py::arg("q0"), + py::class_>( + common, "Kinematics") + .def("inverse", &rcs::common::Kinematics::inverse, py::arg("pose"), + py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()) + .def("forward", &rcs::common::Kinematics::forward, py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()); - py::class_>(common, "RL") - .def(py::init(), py::arg("urdf_path"), - py::arg("max_duration_ms") = 300); - - py::class_>(common, "Pin") .def(py::init(), py::arg("path"), py::arg("frame_id") = "fr3_link8", @@ -429,7 +425,6 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimRobotConfig::joint_rotational_tolerance) .def_readwrite("seconds_between_callbacks", &rcs::sim::SimRobotConfig::seconds_between_callbacks) - .def_readwrite("realtime", &rcs::sim::SimRobotConfig::realtime) .def_readwrite("mjcf_scene_path", &rcs::sim::SimRobotConfig::mjcf_scene_path) .def_readwrite("trajectory_trace", @@ -521,8 +516,8 @@ PYBIND11_MODULE(_core, m) { py::class_>(sim, "SimRobot") .def(py::init, - std::shared_ptr, rcs::sim::SimRobotConfig, - bool>(), + std::shared_ptr, + rcs::sim::SimRobotConfig, bool>(), py::arg("sim"), py::arg("ik"), py::arg("cfg"), py::arg("register_convergence_callback") = true) .def("get_config", &rcs::sim::SimRobot::get_config) diff --git a/src/rcs/CMakeLists.txt b/src/rcs/CMakeLists.txt index cd8b937c..141761d0 100644 --- a/src/rcs/CMakeLists.txt +++ b/src/rcs/CMakeLists.txt @@ -1,7 +1,7 @@ add_library(rcs SHARED) target_include_directories(rcs PUBLIC ${CMAKE_SOURCE_DIR}/include) -target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl pinocchio::all) +target_sources(rcs PRIVATE Pose.cpp Robot.cpp Kinematics.cpp utils.cpp) +target_link_libraries(rcs PUBLIC Eigen3::Eigen pinocchio::all) set_target_properties(rcs PROPERTIES INSTALL_RPATH "$ORIGIN;$ORIGIN/../cmeel.prefix/lib" ) diff --git a/src/rcs/IK.cpp b/src/rcs/Kinematics.cpp similarity index 53% rename from src/rcs/IK.cpp rename to src/rcs/Kinematics.cpp index 1643c4db..03916375 100644 --- a/src/rcs/IK.cpp +++ b/src/rcs/Kinematics.cpp @@ -1,16 +1,4 @@ -#include "rcs/IK.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "rcs/Kinematics.h" #include #include @@ -22,44 +10,6 @@ namespace rcs { namespace common { -RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() { - this->rl_data.mdl = rl::mdl::UrdfFactory().create(urdf_path); - this->rl_data.kin = - std::dynamic_pointer_cast(this->rl_data.mdl); - this->rl_data.ik = std::make_shared( - this->rl_data.kin.get()); - this->rl_data.ik->setRandomRestarts(this->random_restarts); - this->rl_data.ik->setEpsilon(this->eps); - this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); -} - -std::optional RL::ik(const Pose& pose, const VectorXd& q0, - const Pose& tcp_offset) { - // pose is assumed to be in the robots coordinate frame - this->rl_data.kin->setPosition(q0); - this->rl_data.kin->forwardPosition(); - rcs::common::Pose new_pose = pose * tcp_offset.inverse(); - - this->rl_data.ik->addGoal(new_pose.affine_matrix(), 0); - bool success = this->rl_data.ik->solve(); - if (success) { - // is this forward needed and is it mabye possible to call - // this on the model? - this->rl_data.kin->forwardPosition(); - return this->rl_data.kin->getPosition(); - } else { - return std::nullopt; - } -} -Pose RL::forward(const VectorXd& q0, const Pose& tcp_offset) { - // pose is assumed to be in the robots coordinate frame - this->rl_data.kin->setPosition(q0); - this->rl_data.kin->forwardPosition(); - rcs::common::Pose pose = this->rl_data.kin->getOperationalPosition(0); - // apply the tcp offset - return pose * tcp_offset.inverse(); -} - Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true) : model() { if (urdf) { @@ -75,8 +25,8 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true) } } -std::optional Pin::ik(const Pose& pose, const VectorXd& q0, - const Pose& tcp_offset) { +std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, + const Pose& tcp_offset) { rcs::common::Pose new_pose = pose * tcp_offset.inverse(); VectorXd q(model.nq); q.setZero(); diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index b94467d1..7b50a886 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -24,8 +24,9 @@ namespace sim { // TODO: check dof contraints // TODO: use C++11 feature to call one constructor from another -SimRobot::SimRobot(std::shared_ptr sim, std::shared_ptr ik, - SimRobotConfig cfg, bool register_convergence_callback) +SimRobot::SimRobot(std::shared_ptr sim, + std::shared_ptr ik, SimRobotConfig cfg, + bool register_convergence_callback) : sim{sim}, cfg{cfg}, state{}, m_ik(ik) { this->init_ids(); if (register_convergence_callback) { @@ -137,14 +138,14 @@ common::VectorXd SimRobot::get_joint_position() { return q; } -std::optional> SimRobot::get_ik() { +std::optional> SimRobot::get_ik() { return this->m_ik; } void SimRobot::set_cartesian_position(const common::Pose& pose) { // pose is assumed to be in the robots coordinate frame - auto joint_vals = - this->m_ik->ik(pose, this->get_joint_position(), this->cfg.tcp_offset); + auto joint_vals = this->m_ik->inverse(pose, this->get_joint_position(), + this->cfg.tcp_offset); if (joint_vals.has_value()) { this->state.ik_success = true; this->set_joint_position(joint_vals.value()); diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index abfc0d2d..ab917de6 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -1,7 +1,7 @@ #ifndef RCS_FR3SIM_H #define RCS_FR3SIM_H #include -#include +#include #include #include #include @@ -15,7 +15,6 @@ struct SimRobotConfig : common::RobotConfig { double joint_rotational_tolerance = .05 * (std::numbers::pi / 180.0); // 0.05 degree double seconds_between_callbacks = 0.1; // 10 Hz - bool realtime = false; bool trajectory_trace = false; std::vector arm_collision_geoms{ "fr3_link0_collision", "fr3_link1_collision", "fr3_link2_collision", @@ -59,8 +58,9 @@ struct SimRobotState : common::RobotState { class SimRobot : public common::Robot { public: - SimRobot(std::shared_ptr sim, std::shared_ptr ik, - SimRobotConfig cfg, bool register_convergence_callback = true); + SimRobot(std::shared_ptr sim, + std::shared_ptr ik, SimRobotConfig cfg, + bool register_convergence_callback = true); ~SimRobot() override; bool set_config(const SimRobotConfig &cfg); SimRobotConfig *get_config() override; @@ -71,7 +71,7 @@ class SimRobot : public common::Robot { void move_home() override; void set_cartesian_position(const common::Pose &pose) override; common::Pose get_base_pose_in_world_coordinates() override; - std::optional> get_ik() override; + std::optional> get_ik() override; void reset() override; void set_joints_hard(const common::VectorXd &q); void close() override {}; @@ -80,7 +80,7 @@ class SimRobot : public common::Robot { SimRobotConfig cfg; SimRobotState state; std::shared_ptr sim; - std::shared_ptr m_ik; + std::shared_ptr m_ik; struct { std::set cgeom; int attachment_site; diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 41cf2916..c9f3af2e 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -12,11 +12,9 @@ #include "sim.h" #include "sim/SimRobot.h" -// const std::string mjcf = MODEL_DIR "/mjcf/fr3_modular/scene.xml"; -// const std::string urdf = MODEL_DIR "/fr3/urdf/fr3_from_panda.urdf"; -const std::string mjcf = "build/_deps/scenes-src/scenes/lab/scene.xml"; -const std::string urdf_path = - "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf"; +const std::string mjcf = "assets/scenes/fr3_empty_world/scene.xml"; +const std::string mjcf_robot = "assets/scenes/fr3_empty_world/robot.xml"; +const std::string urdf_path = "assets/scenes/fr3_empty_world/assets/fr3.urdf"; static const Eigen::Matrix iso_cube_center( 0.498, 0.0, 0.226); static const float iso_cube_size = 0.4; @@ -127,7 +125,8 @@ int test_sim() { sim->set_config(cfg); std::string id = "0"; - auto ik = std::make_shared(urdf_path); + auto ik = std::make_shared(mjcf_robot, + "attachment_site_" + id, false); auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset()); rcs::sim::SimRobotConfig fr3_config; fr3_config.tcp_offset = tcp_offset;