diff --git a/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.pyi b/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.pyi new file mode 100644 index 00000000..b563fe04 --- /dev/null +++ b/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"] 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 index c0ac500f..173d1555 100644 --- a/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi +++ b/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" 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 index 3565f4d1..4c4b0afb 100644 --- a/extensions/rcs_robotics_library/src/rcs_robotics_library/_core/rl.pyi +++ b/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: ... diff --git a/extensions/rcs_so101/CMakeLists.txt b/extensions/rcs_so101/CMakeLists.txt new file mode 100644 index 00000000..eb53b6b3 --- /dev/null +++ b/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) diff --git a/extensions/rcs_so101/Makefile b/extensions/rcs_so101/Makefile new file mode 100644 index 00000000..5793f066 --- /dev/null +++ b/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 diff --git a/extensions/rcs_so101/README_IMPORTANT.md b/extensions/rcs_so101/README.md similarity index 100% rename from extensions/rcs_so101/README_IMPORTANT.md rename to extensions/rcs_so101/README.md diff --git a/extensions/rcs_so101/cmake/Findpinocchio.cmake b/extensions/rcs_so101/cmake/Findpinocchio.cmake new file mode 100644 index 00000000..fcefcf5c --- /dev/null +++ b/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() diff --git a/extensions/rcs_so101/cmake/Findrcs.cmake b/extensions/rcs_so101/cmake/Findrcs.cmake new file mode 100644 index 00000000..08456ccc --- /dev/null +++ b/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() \ No newline at end of file diff --git a/extensions/rcs_so101/pyproject.toml b/extensions/rcs_so101/pyproject.toml index 04454930..c390d01a 100644 --- a/extensions/rcs_so101/pyproject.toml +++ b/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" \ No newline at end of file +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"] \ No newline at end of file diff --git a/extensions/rcs_so101/src/CMakeLists.txt b/extensions/rcs_so101/src/CMakeLists.txt new file mode 100644 index 00000000..a458c164 --- /dev/null +++ b/extensions/rcs_so101/src/CMakeLists.txt @@ -0,0 +1,2 @@ +target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(pybind) diff --git a/extensions/rcs_so101/src/pybind/CMakeLists.txt b/extensions/rcs_so101/src/pybind/CMakeLists.txt new file mode 100644 index 00000000..de44bb17 --- /dev/null +++ b/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) diff --git a/extensions/rcs_so101/src/pybind/SO101.h b/extensions/rcs_so101/src/pybind/SO101.h new file mode 100644 index 00000000..ed25318f --- /dev/null +++ b/extensions/rcs_so101/src/pybind/SO101.h @@ -0,0 +1,212 @@ +#ifndef RCS_SO101_H +#define RCS_SO101_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace rcs { +namespace so101 { + +class SO101IK : public rcs::common::Kinematics { +private: + const double eps = 1e-4; + const int IT_MAX = 1000; + const double DT = 1e-1; + const double damp = 1e-6; + const double eps_pos = 0.01; + const double eps_ori = 10 * (M_PI / 180.0); + int FRAME_ID; + + pinocchio::Model model; + pinocchio::Data data; + + public: + SO101IK(const std::string& path, const std::string& frame_id, bool urdf = true) + : model() { + if (urdf) { + pinocchio::urdf::buildModel(path, this->model); + } else { + pinocchio::mjcf::buildModel(path, this->model); + } + this->data = pinocchio::Data(this->model); + this->FRAME_ID = model.getFrameId(frame_id); + if (FRAME_ID == -1) { + throw std::runtime_error( + frame_id + " frame id could not be found in the provided URDF"); + } + } + std::optional inverse( + const rcs::common::Pose& pose, const rcs::common::VectorXd& q0, + const rcs::common::Pose& tcp_offset = + rcs::common::Pose::Identity()) override { + // --- Tunables (could be class members) ------------------------------------ + const double WP = 1.0; // position weight + const double WO = 0.12; // orientation (pitch+yaw) weight + const double WO_ROLL = 0.03; // orientation roll weight (let it "float" more) + const double ORI_TOL = 5.0 * M_PI / 180.0; // 5 deg tolerance (dead-zone) + const double ORI_CAP = + 0.6; // cap on |eo| scaling (rad/s-equivalent per iter) + const double STEP_CAP = + 0.15; // cap on ||J * v|| (meters/rad) per iter to avoid jumps + // If you already adapt damping elsewhere, keep using this->damp. Otherwise: + const double DAMP_BASE = this->damp; // your existing scalar + // -------------------------------------------------------------------------- + + rcs::common::Pose new_pose = pose * tcp_offset.inverse(); + rcs::common::VectorXd q(model.nq); + q.setZero(); + q.head(q0.size()) = q0; + + const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); + + pinocchio::Data::Matrix6x J(6, model.nv); + J.setZero(); + bool success = false; + + // Pre-allocations + rcs::common::Vector6d err, err_w; // 6x1 + Eigen::Vector3d ep, eo; // position/orientation parts + Eigen::VectorXd v(model.nv); // nv x 1 + pinocchio::Data::Matrix6 JJt; // 6x6 + Eigen::Matrix Wsqrt; // sqrt of weights + + // Build constant sqrt-weights (diag): [WP, WP, WP, WO, WO, WO_ROLL] + Wsqrt.setZero(); + Wsqrt(0, 0) = std::sqrt(WP); + Wsqrt(1, 1) = std::sqrt(WP); + Wsqrt(2, 2) = std::sqrt(WP); + Wsqrt(3, 3) = std::sqrt(WO); + Wsqrt(4, 4) = std::sqrt(WO); + Wsqrt(5, 5) = std::sqrt(WO_ROLL); + + for (int i = 0;; ++i) { + // FK + current frame pose + pinocchio::forwardKinematics(model, data, q); + pinocchio::updateFramePlacements(model, data); + + // Error in frame tangent space (twist): [trans; rot] + const pinocchio::SE3 iMd = data.oMf[this->FRAME_ID].actInv(oMdes); + err = pinocchio::log6(iMd).toVector(); + + // Split and shape errors + ep = err.head<3>(); + eo = err.tail<3>(); + + // ---- Orientation tolerance (dead-zone + soft cap) ---------------------- + // If |eo| is small, ignore it completely (prevents twitch). + const double eo_norm = eo.norm(); + if (eo_norm < ORI_TOL) { + eo.setZero(); + } else { + // shrink just the "excess" beyond tolerance (preserve direction) + const double scaled = std::min(eo_norm - ORI_TOL, ORI_CAP); + eo *= (scaled / eo_norm); + } + // ------------------------------------------------------------------------ + + // Re-pack the shaped error + err.head<3>() = ep; + err.tail<3>() = eo; + + // Convergence test emphasizes position because eo may be zeroed + if (ep.norm() < this->eps_pos && eo.norm() < this->eps_ori) { + success = true; + break; + } + if (i >= this->IT_MAX) { + success = false; + break; + } + + // 6xnv body Jacobian for the frame + pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); + + // Map to log space (same as your original) + pinocchio::Data::Matrix6 Jlog; + pinocchio::Jlog6(iMd.inverse(), Jlog); + J = -Jlog * J; // tangent-space Jacobian for the error you’re minimizing + + // ---- Weighted damped least-squares ------------------------------------- + // Apply sqrt-weights on rows: this prioritizes translation > orientation, + // and de-weights tool roll further. + const auto Jw = Wsqrt * J; // 6xnv + err_w = Wsqrt * err; // 6x1 + + // Optional: adapt damping using position manipulability (safer near + // singularities) + double damp = DAMP_BASE; + { + // Use position block for manipulability (3xnv) + Eigen::Matrix Jp = J.topRows<3>(); + Eigen::JacobiSVD svd( + Jp, Eigen::ComputeThinU | Eigen::ComputeThinV); + const double sigma_min = + std::max(1e-9, svd.singularValues().tail<1>()(0)); + // Increase damping when near singular (simple schedule) + damp = DAMP_BASE + 0.01 / sigma_min; + } + + // Normal equations on weighted system + JJt.noalias() = Jw * Jw.transpose(); + JJt.diagonal().array() += damp; + + // Solve for v (joint velocity increment) + v.noalias() = -Jw.transpose() * JJt.ldlt().solve(err_w); + // ------------------------------------------------------------------------ + + // ---- Step-size limiter in task space to avoid sudden jumps ------------- + // Predict task step and scale if too large + Eigen::Matrix task_step = J * v * this->DT; + double step_norm = task_step.head<3>().norm() + task_step.tail<3>().norm(); + if (step_norm > STEP_CAP && step_norm > 1e-9) { + const double scale = STEP_CAP / step_norm; + v *= scale; + } + // Also respect per-joint velocity limits if you have them (not shown) + // ------------------------------------------------------------------------ + + // Integrate + q = pinocchio::integrate(model, q, v * this->DT); + } + + if (success) { + return q.head(q0.size()); + } + // std::cout << "IK failed after " << this->IT_MAX << " iterations." << + // std::endl; + std::cout << "ep: " << ep.norm() << " eo: " << eo.norm() << std::endl; + 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 + rcs::common::VectorXd q(model.nq); + q.setZero(); + q.head(q0.size()) = q0; + pinocchio::framesForwardKinematics(model, data, q); + rcs::common::Pose pose(data.oMf[this->FRAME_ID].rotation(), + data.oMf[this->FRAME_ID].translation()); + + // apply the tcp offset + return pose * tcp_offset.inverse(); + } +}; + +} // namespace so101 +} // namespace rcs + +#endif // RCS_SO101_H diff --git a/extensions/rcs_so101/src/pybind/rcs.cpp b/extensions/rcs_so101/src/pybind/rcs.cpp new file mode 100644 index 00000000..bc359f70 --- /dev/null +++ b/extensions/rcs_so101/src/pybind/rcs.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include +#include + +#include + +#include "SO101.h" +#include "rcs/Kinematics.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 ik = m.def_submodule("so101_ik", "rcs relaxed ik for so101"); + + py::object kinematics = + (py::object)py::module_::import("rcs").attr("common").attr("Kinematics"); + py::class_>( + ik, "SO101IK", kinematics) + .def(py::init(), py::arg("path"), + py::arg("frame_id"), py::arg("urdf") = true); +} diff --git a/extensions/rcs_so101/src/rcs_so101/__init__.py b/extensions/rcs_so101/src/rcs_so101/__init__.py index e69de29b..f710b730 100644 --- a/extensions/rcs_so101/src/rcs_so101/__init__.py +++ b/extensions/rcs_so101/src/rcs_so101/__init__.py @@ -0,0 +1,6 @@ +from rcs_so101._core.so101_ik import SO101IK + +from .creators import RCSSO101EnvCreator +from .hw import SO101, SO101Config, SO101Gripper + +__all__ = ["SO101IK", "RCSSO101EnvCreator", "SO101", "SO101Config", "SO101Gripper"] diff --git a/extensions/rcs_so101/src/rcs_so101/_core/__init__.pyi b/extensions/rcs_so101/src/rcs_so101/_core/__init__.pyi new file mode 100644 index 00000000..5449de25 --- /dev/null +++ b/extensions/rcs_so101/src/rcs_so101/_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 so101_ik + +__all__: list[str] = ["so101_ik"] +__version__: str = "0.5.2" diff --git a/extensions/rcs_so101/src/rcs_so101/_core/so101_ik.pyi b/extensions/rcs_so101/src/rcs_so101/_core/so101_ik.pyi new file mode 100644 index 00000000..7f0fb1db --- /dev/null +++ b/extensions/rcs_so101/src/rcs_so101/_core/so101_ik.pyi @@ -0,0 +1,12 @@ +# ATTENTION: auto generated from C++ code, use `make stubgen` to update! +""" +rcs relaxed ik for so101 +""" +from __future__ import annotations + +import rcs._core.common + +__all__: list[str] = ["SO101IK"] + +class SO101IK(rcs._core.common.Kinematics): + def __init__(self, path: str, frame_id: str, urdf: bool = True) -> None: ... diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 07224b44..eaf32465 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -11,6 +11,7 @@ RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator +from rcs_so101 import SO101IK from rcs_so101.hw import SO101, SO101Config, SO101Gripper import rcs @@ -28,7 +29,7 @@ def __call__( # type: ignore max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, ) -> gym.Env: - ik = rcs.common.Pin( + ik = SO101IK( robot_cfg.kinematic_model_path, robot_cfg.attachment_site, urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),