From 293d72052a4adb8b082efdbcef8dbfb1b670d320 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 28 Mar 2025 13:32:10 +0100 Subject: [PATCH 1/9] refactor(ik): abstract ik class with rl subclass --- python/examples/fr3.py | 4 ++-- python/rcsss/_core/common.pyi | 5 ++++- python/rcsss/control/record.py | 2 +- python/rcsss/envs/factories.py | 4 ++-- python/rcsss/envs/sim.py | 2 +- src/common/IK.cpp | 33 ++++++++++++++++----------------- src/common/IK.h | 29 ++++++++++++++++++----------- src/hw/main.cpp | 2 +- src/pybind/rcsss.cpp | 7 +++++-- src/sim/test.cpp | 2 +- 10 files changed, 51 insertions(+), 39 deletions(-) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 5bf3b325..5b6aea02 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -69,7 +69,7 @@ def main(): simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) assert urdf_path is not None - ik = rcsss.common.IK(urdf_path) + ik = rcsss.common.RL(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) cfg = sim.FR3Config() cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) @@ -91,7 +91,7 @@ def main(): else: urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) assert urdf_path is not None - ik = rcsss.common.IK(urdf_path) + ik = rcsss.common.RL(urdf_path) robot = rcsss.hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) diff --git a/python/rcsss/_core/common.pyi b/python/rcsss/_core/common.pyi index f0e1f252..a9fda3bd 100644 --- a/python/rcsss/_core/common.pyi +++ b/python/rcsss/_core/common.pyi @@ -21,6 +21,7 @@ __all__ = [ "NRobotsWithGripper", "Pose", "RConfig", + "RL", "RPY", "RState", "Robot", @@ -45,7 +46,6 @@ class Gripper: def shut(self) -> None: ... class IK: - def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... def ik( self, pose: Pose, q0: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]], tcp_offset: Pose = ... ) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] | None: ... @@ -128,6 +128,9 @@ class Pose: class RConfig: pass +class RL(IK): + def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... + class RPY: pitch: float roll: float diff --git a/python/rcsss/control/record.py b/python/rcsss/control/record.py index b8be3410..cce582a3 100644 --- a/python/rcsss/control/record.py +++ b/python/rcsss/control/record.py @@ -194,7 +194,7 @@ def __init__( poses: list[Pose] | None = None, urdf_path: str | None = None, ): - self.ik = common.IK(urdf_path) if urdf_path else None + self.ik = common.RL(urdf_path) if urdf_path else None self.r: dict[str, hw.FR3] = {key: hw.FR3(ip, self.ik) for key, ip in name2ip.items()} # TODO: this config should be given to the constructor cfg = hw.FHConfig() diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index b5c85388..ebd2c084 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -113,7 +113,7 @@ def fr3_hw_env( gym.Env: The configured hardware environment for the FR3 robot. """ urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=collision_guard is not None) - ik = rcsss.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcsss.common.RL(str(urdf_path)) if urdf_path is not None else None robot = rcsss.hw.FR3(ip, ik) robot.set_parameters(robot_cfg) @@ -213,7 +213,7 @@ def fr3_sim_env( logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") simulation = sim.Sim(rcsss.scenes.get(str(mjcf), mjcf)) - ik = rcsss.common.IK(urdf_path) + ik = rcsss.common.RL(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) robot.set_parameters(robot_cfg) env: gym.Env = FR3Env(robot, control_mode) diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 08a9e6a4..73cc5b35 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -137,7 +137,7 @@ def env_from_xml_paths( ) -> "CollisionGuard": assert isinstance(env.unwrapped, FR3Env) simulation = sim.Sim(mjmld) - ik = rcsss.common.IK(urdf, max_duration_ms=300) + ik = rcsss.common.RL(urdf, max_duration_ms=300) robot = rcsss.sim.FR3(simulation, id, ik) cfg = sim.FR3Config() cfg.realtime = False diff --git a/src/common/IK.cpp b/src/common/IK.cpp index 0dd30d3d..50a03f97 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -15,32 +15,31 @@ namespace rcs { namespace common { -IK::IK(const std::string& urdf_path, size_t max_duration_ms) : rl() { - this->rl = RL(); - - this->rl.mdl = rl::mdl::UrdfFactory().create(urdf_path); - this->rl.kin = std::dynamic_pointer_cast(this->rl.mdl); - this->rl.ik = - std::make_shared(this->rl.kin.get()); - this->rl.ik->setRandomRestarts(0); - this->rl.ik->setEpsilon(1e-3); - this->rl.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); +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(0); + this->rl_data.ik->setEpsilon(1e-3); + this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); } -std::optional IK::ik(const Pose& pose, const Vector7d& q0, +std::optional RL::ik(const Pose& pose, const Vector7d& q0, const Pose& tcp_offset) { // pose is assumed to be in the robots coordinate frame - this->rl.kin->setPosition(q0); - this->rl.kin->forwardPosition(); + this->rl_data.kin->setPosition(q0); + this->rl_data.kin->forwardPosition(); rcs::common::Pose new_pose = pose * tcp_offset.inverse(); - this->rl.ik->addGoal(new_pose.affine_matrix(), 0); - bool success = this->rl.ik->solve(); + 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.kin->forwardPosition(); - return this->rl.kin->getPosition(); + this->rl_data.kin->forwardPosition(); + return this->rl_data.kin->getPosition(); } else { return std::nullopt; } diff --git a/src/common/IK.h b/src/common/IK.h index 740f709e..545b28f0 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -15,22 +15,29 @@ namespace rcs { namespace common { -struct RL { - std::shared_ptr mdl; - std::shared_ptr kin; - std::shared_ptr ik; +class IK { + public: + virtual ~IK(){}; + virtual std::optional ik( + const Pose& pose, const Vector7d& q0, + const Pose& tcp_offset = Pose::Identity()) = 0; }; -class IK { +class RL : public IK { private: - RL rl; + struct RLData { + std::shared_ptr mdl; + std::shared_ptr kin; + std::shared_ptr ik; + }; + RLData rl_data; public: - IK(const std::string& urdf_path, size_t max_duration_ms = 300); - std::optional ik(const Pose& pose, const Vector7d& q0, - const Pose& tcp_offset = Pose::Identity()); - - // TODO: set max time + RL(const std::string& urdf_path, size_t max_duration_ms = 300); + ~RL() override = default; + std::optional ik( + const Pose& pose, const Vector7d& q0, + const Pose& tcp_offset = Pose::Identity()) override; }; } // namespace common } // namespace rcs diff --git a/src/hw/main.cpp b/src/hw/main.cpp index 6c815142..2e2c89b5 100644 --- a/src/hw/main.cpp +++ b/src/hw/main.cpp @@ -17,7 +17,7 @@ const string urdf_path = "models/urdf/fr3.urdf"; int main() { try { - auto ik = make_shared(urdf_path); + auto ik = make_shared(urdf_path); rcs::hw::FR3 robot(ip, ik); robot.automatic_error_recovery(); std::cout << "WARNING: This example will move the robot! " diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index 3753f738..e61ba630 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -212,11 +212,14 @@ PYBIND11_MODULE(_core, m) { })); py::class_>(common, "IK") - .def(py::init(), py::arg("urdf_path"), - py::arg("max_duration_ms") = 300) .def("ik", &rcs::common::IK::ik, py::arg("pose"), 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, "RConfig"); py::class_(common, "RState"); py::class_(common, "GConfig"); diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 3c700473..5ee9cf95 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -126,7 +126,7 @@ int test_sim() { sim->set_config(cfg); std::string id = "0"; - auto ik = std::make_shared(urdf); + auto ik = std::make_shared(urdf); auto fr3 = rcs::sim::FR3(sim, id, ik); auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset()); rcs::sim::FR3Config fr3_config = *fr3.get_parameters(); From 29714dc0f4d66cd9d353067be2a20ee6880e1364 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 28 Mar 2025 16:02:46 +0100 Subject: [PATCH 2/9] feat(ik): added pinocchio ik --- python/rcsss/_core/common.pyi | 4 +++ src/common/CMakeLists.txt | 2 +- src/common/IK.cpp | 61 +++++++++++++++++++++++++++++++++-- src/common/IK.h | 30 ++++++++++++++--- src/pybind/CMakeLists.txt | 2 +- src/pybind/rcsss.cpp | 4 +++ 6 files changed, 95 insertions(+), 8 deletions(-) diff --git a/python/rcsss/_core/common.pyi b/python/rcsss/_core/common.pyi index a9fda3bd..2dfef0b8 100644 --- a/python/rcsss/_core/common.pyi +++ b/python/rcsss/_core/common.pyi @@ -19,6 +19,7 @@ __all__ = [ "IdentityRotQuartVec", "IdentityTranslation", "NRobotsWithGripper", + "Pin", "Pose", "RConfig", "RL", @@ -69,6 +70,9 @@ class NRobotsWithGripper: ) -> None: ... def shut(self, idxs: list[int]) -> None: ... +class Pin(IK): + def __init__(self, urdf_path: str) -> None: ... + class Pose: def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ... @typing.overload diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index f7aaae75..5dab4f4d 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,3 +1,3 @@ add_library(common) target_sources(common PRIVATE Pose.cpp NRobotsWithGripper.cpp Robot.cpp IK.cpp) -target_link_libraries(common PUBLIC Eigen3::Eigen mdl) +target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio) diff --git a/src/common/IK.cpp b/src/common/IK.cpp index 50a03f97..eabf3bbb 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -12,6 +12,12 @@ #include #include +#include + +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" + namespace rcs { namespace common { @@ -21,8 +27,8 @@ RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() { 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(0); - this->rl_data.ik->setEpsilon(1e-3); + 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)); } @@ -45,5 +51,56 @@ std::optional RL::ik(const Pose& pose, const Vector7d& q0, } } +Pin::Pin(const std::string& urdf_path) : model() { + pinocchio::urdf::buildModel(urdf_path, this->model); + this->data = pinocchio::Data(this->model); +} + +std::optional Pin::ik(const Pose& pose, const Vector7d& q0, + const Pose& tcp_offset) { + rcs::common::Pose new_pose = pose * tcp_offset.inverse(); + Vector7d q(q0); + const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); + pinocchio::Data::Matrix6x J(6, model.nv); + J.setZero(); + bool success = false; + Vector6d err; + Eigen::VectorXd v(model.nv); + for (int i = 0;; i++) { + pinocchio::forwardKinematics(model, data, q); + const pinocchio::SE3 iMd = data.oMi[this->JOINT_ID].actInv(oMdes); + err = pinocchio::log6(iMd).toVector(); // in joint frame + if (err.norm() < this->eps) { + success = true; + break; + } + if (i >= this->IT_MAX) { + success = false; + break; + } + pinocchio::computeJointJacobian(model, data, q, this->JOINT_ID, + J); // J in joint frame + pinocchio::Data::Matrix6 Jlog; + pinocchio::Jlog6(iMd.inverse(), Jlog); + J = -Jlog * J; + pinocchio::Data::Matrix6 JJt; + JJt.noalias() = J * J.transpose(); + JJt.diagonal().array() += this->damp; + v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + q = pinocchio::integrate(model, q, v * this->DT); + if (!(i % 10)) + std::cout << i << ": error = " << err.transpose() << std::endl; + } + if (success) { + // print amount of iterations + std::cout << "IK success after " << std::to_string(IT_MAX) << " iterations" + << std::endl; + return q; + } else { + std::cout << "IK failed" << std::endl; + return std::nullopt; + } +} + } // namespace common } // namespace rcs \ No newline at end of file diff --git a/src/common/IK.h b/src/common/IK.h index 545b28f0..db323390 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -8,8 +8,12 @@ #include #include #include +#include #include "Pose.h" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" #include "utils.h" namespace rcs { @@ -25,16 +29,34 @@ class IK { class RL : public IK { private: - struct RLData { + const int random_restarts = 0; + const double eps = 1e-3; + struct { std::shared_ptr mdl; std::shared_ptr kin; std::shared_ptr ik; - }; - RLData rl_data; + } rl_data; public: RL(const std::string& urdf_path, size_t max_duration_ms = 300); - ~RL() override = default; + std::optional ik( + const Pose& pose, const Vector7d& q0, + const Pose& tcp_offset = Pose::Identity()) override; +}; + +class Pin : public IK { + private: + const double eps = 1e-4; + const int IT_MAX = 1000; + const double DT = 1e-1; + const double damp = 1e-6; + const int JOINT_ID = 7; + + pinocchio::Model model; + pinocchio::Data data; + + public: + Pin(const std::string& urdf_path); std::optional ik( const Pose& pose, const Vector7d& q0, const Pose& tcp_offset = Pose::Identity()) override; diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index 0067d9da..da603c74 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -3,7 +3,7 @@ target_link_libraries(_core PRIVATE hw sim common) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(_core PROPERTIES - INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco" + INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco;$ORIGIN/../cmeel.prefix/lib" INTERPROCEDURAL_OPTIMIZATION TRUE ) set_target_properties(franka PROPERTIES diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index e61ba630..def3e3ba 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -220,6 +220,10 @@ PYBIND11_MODULE(_core, m) { .def(py::init(), py::arg("urdf_path"), py::arg("max_duration_ms") = 300); + py::class_>(common, "Pin") + .def(py::init(), py::arg("urdf_path")); + py::class_(common, "RConfig"); py::class_(common, "RState"); py::class_(common, "GConfig"); From b7995aef60b1fa08a8276f1c7b9770e56a2d802f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 31 Mar 2025 08:50:24 +0200 Subject: [PATCH 3/9] fix: pinocchio ik to use custom frame --- CMakeLists.txt | 2 +- python/rcsss/_core/common.pyi | 2 +- src/common/IK.cpp | 29 ++++++++++++++--------------- src/common/IK.h | 5 +++-- src/hw/CMakeLists.txt | 4 ++-- src/pybind/rcsss.cpp | 3 ++- src/sim/CMakeLists.txt | 4 ++-- src/sim/test.cpp | 4 ++-- 8 files changed, 27 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c8896ff..50c7bd3d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,7 @@ find_package(Eigen3 REQUIRED) find_package(glfw3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(MuJoCo REQUIRED) - +find_package(pinocchio REQUIRED) FetchContent_Declare( libfranka GIT_REPOSITORY https://github.com/frankaemika/libfranka.git diff --git a/python/rcsss/_core/common.pyi b/python/rcsss/_core/common.pyi index 2dfef0b8..89af2c87 100644 --- a/python/rcsss/_core/common.pyi +++ b/python/rcsss/_core/common.pyi @@ -71,7 +71,7 @@ class NRobotsWithGripper: def shut(self, idxs: list[int]) -> None: ... class Pin(IK): - def __init__(self, urdf_path: str) -> None: ... + def __init__(self, urdf_path: str, frame_id: str = "fr3_link8") -> None: ... class Pose: def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ... diff --git a/src/common/IK.cpp b/src/common/IK.cpp index eabf3bbb..0199c1ff 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -12,12 +12,12 @@ #include #include +#include +#include +#include +#include #include -#include "pinocchio/algorithm/jacobian.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/kinematics.hpp" - namespace rcs { namespace common { @@ -51,9 +51,14 @@ std::optional RL::ik(const Pose& pose, const Vector7d& q0, } } -Pin::Pin(const std::string& urdf_path) : model() { +Pin::Pin(const std::string& urdf_path, const std::string& frame_id) : model() { pinocchio::urdf::buildModel(urdf_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 Pin::ik(const Pose& pose, const Vector7d& q0, @@ -68,8 +73,9 @@ std::optional Pin::ik(const Pose& pose, const Vector7d& q0, Eigen::VectorXd v(model.nv); for (int i = 0;; i++) { pinocchio::forwardKinematics(model, data, q); - const pinocchio::SE3 iMd = data.oMi[this->JOINT_ID].actInv(oMdes); - err = pinocchio::log6(iMd).toVector(); // in joint frame + pinocchio::updateFramePlacements(model, data); + const pinocchio::SE3 iMd = data.oMf[this->FRAME_ID].actInv(oMdes); + err = pinocchio::log6(iMd).toVector(); if (err.norm() < this->eps) { success = true; break; @@ -78,8 +84,7 @@ std::optional Pin::ik(const Pose& pose, const Vector7d& q0, success = false; break; } - pinocchio::computeJointJacobian(model, data, q, this->JOINT_ID, - J); // J in joint frame + pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); pinocchio::Data::Matrix6 Jlog; pinocchio::Jlog6(iMd.inverse(), Jlog); J = -Jlog * J; @@ -88,16 +93,10 @@ std::optional Pin::ik(const Pose& pose, const Vector7d& q0, JJt.diagonal().array() += this->damp; v.noalias() = -J.transpose() * JJt.ldlt().solve(err); q = pinocchio::integrate(model, q, v * this->DT); - if (!(i % 10)) - std::cout << i << ": error = " << err.transpose() << std::endl; } if (success) { - // print amount of iterations - std::cout << "IK success after " << std::to_string(IT_MAX) << " iterations" - << std::endl; return q; } else { - std::cout << "IK failed" << std::endl; return std::nullopt; } } diff --git a/src/common/IK.h b/src/common/IK.h index db323390..689131a1 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -9,6 +9,7 @@ #include #include #include +#include #include "Pose.h" #include "pinocchio/algorithm/jacobian.hpp" @@ -50,13 +51,13 @@ class Pin : public IK { const int IT_MAX = 1000; const double DT = 1e-1; const double damp = 1e-6; - const int JOINT_ID = 7; + int FRAME_ID; pinocchio::Model model; pinocchio::Data data; public: - Pin(const std::string& urdf_path); + Pin(const std::string& urdf_path, const std::string& frame_id); std::optional ik( const Pose& pose, const Vector7d& q0, const Pose& tcp_offset = Pose::Identity()) override; diff --git a/src/hw/CMakeLists.txt b/src/hw/CMakeLists.txt index a06e75ae..e65d430f 100644 --- a/src/hw/CMakeLists.txt +++ b/src/hw/CMakeLists.txt @@ -6,5 +6,5 @@ target_link_libraries( PRIVATE math ) -add_executable(test_hw main.cpp) -target_link_libraries(test_hw PRIVATE hw) +# add_executable(test_hw main.cpp) +# target_link_libraries(test_hw PRIVATE hw) diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index def3e3ba..0e92409e 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -222,7 +222,8 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") - .def(py::init(), py::arg("urdf_path")); + .def(py::init(), + py::arg("urdf_path"), py::arg("frame_id") = "fr3_link8"); py::class_(common, "RConfig"); py::class_(common, "RState"); diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index 16ddf7ca..e031927f 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -4,5 +4,5 @@ target_sources(sim ) target_link_libraries(sim PUBLIC glfw common MuJoCo::MuJoCo mujoco_ui_lib) -add_executable(test_sim test.cpp sim.cpp) -target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common glfw) +# add_executable(test_sim test.cpp sim.cpp) +# target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common glfw pinocchio::pinocchio) diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 5ee9cf95..66ad864f 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -15,7 +15,7 @@ // 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 = "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf"; +const std::string urdf_path = "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf"; static const Eigen::Matrix iso_cube_center( 0.498, 0.0, 0.226); static const float iso_cube_size = 0.4; @@ -126,7 +126,7 @@ int test_sim() { sim->set_config(cfg); std::string id = "0"; - auto ik = std::make_shared(urdf); + auto ik = std::make_shared(urdf_path); auto fr3 = rcs::sim::FR3(sim, id, ik); auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset()); rcs::sim::FR3Config fr3_config = *fr3.get_parameters(); From 438fb97b45598524e9fca228079440f65a3fccc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 19:18:31 +0200 Subject: [PATCH 4/9] refactor: compatility with current master - fixed rl and pin iks - added forward function for iks - fixed stubfiles --- python/examples/fr3.py | 4 ++-- python/rcs/_core/common.pyi | 7 ------- python/rcs/envs/creators.py | 6 +++--- python/rcs_so101/creators.py | 2 +- python/rcs_so101/hw.py | 2 +- src/common/IK.cpp | 24 +++++++++++++++++------- src/common/IK.h | 7 +++++-- src/pybind/rcs.cpp | 4 ---- src/sim/test.cpp | 3 ++- 9 files changed, 31 insertions(+), 28 deletions(-) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 6652c95f..981a5c88 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -60,7 +60,7 @@ def main(): 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.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) cfg = sim.SimRobotConfig() cfg.add_id("0") cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) @@ -92,7 +92,7 @@ def main(): else: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) robot = rcs.hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 9058995e..07b872e7 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -21,12 +21,9 @@ __all__ = [ "IdentityRotMatrix", "IdentityRotQuatVec", "IdentityTranslation", - "NRobotsWithGripper", "Pin", "Pose", - "RConfig", "RL", - "Pose", "RPY", "Robot", "RobotConfig", @@ -67,7 +64,6 @@ class GripperState: pass class IK: - def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def ik( self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ... @@ -132,9 +128,6 @@ class Pose: def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ... -class RConfig: - pass - class RL(IK): def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index e1c984f4..ab48b788 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -84,7 +84,7 @@ def __call__( # type: ignore """ if urdf_path is None: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robot = rcs.hw.FR3(ip, ik) robot.set_parameters(robot_cfg) @@ -136,7 +136,7 @@ def __call__( # type: ignore ) -> gym.Env: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robots: dict[str, rcs.hw.FR3] = {} for ip in ips: robots[ip] = rcs.hw.FR3(ip, ik) @@ -232,7 +232,7 @@ def __call__( # type: ignore mjcf = rcs.scenes[mjcf]["mjb"] simulation = sim.Sim(mjcf) - ik = rcs.common.IK(str(urdf_path)) + 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) diff --git a/python/rcs_so101/creators.py b/python/rcs_so101/creators.py index 32d1132c..5d4b79cb 100644 --- a/python/rcs_so101/creators.py +++ b/python/rcs_so101/creators.py @@ -118,7 +118,7 @@ def __call__( # type: ignore """ simulation = sim.Sim(mjcf) - ik = rcs.common.IK(str(urdf_path)) + 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) diff --git a/python/rcs_so101/hw.py b/python/rcs_so101/hw.py index ee4e9ec7..e6bbb6f2 100644 --- a/python/rcs_so101/hw.py +++ b/python/rcs_so101/hw.py @@ -7,7 +7,7 @@ class SO101(common.Robot): def __init__(self, hf_robot: SO101Follower, urdf_path: str): - self.ik = common.IK(urdf_path=urdf_path) + self.ik = common.RL(urdf_path=urdf_path) self._hf_robot = hf_robot self._hf_robot.connect() diff --git a/src/common/IK.cpp b/src/common/IK.cpp index b29d7c63..bbbc4c72 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -32,7 +32,7 @@ RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() { this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); } -std::optional IK::ik(const Pose& pose, const VectorXd& q0, +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); @@ -50,6 +50,14 @@ std::optional IK::ik(const Pose& pose, const VectorXd& q0, 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& urdf_path, const std::string& frame_id) : model() { pinocchio::urdf::buildModel(urdf_path, this->model); @@ -61,10 +69,10 @@ Pin::Pin(const std::string& urdf_path, const std::string& frame_id) : model() { } } -std::optional Pin::ik(const Pose& pose, const Vector7d& q0, +std::optional Pin::ik(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { rcs::common::Pose new_pose = pose * tcp_offset.inverse(); - Vector7d q(q0); + VectorXd q(q0); const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); pinocchio::Data::Matrix6x J(6, model.nv); J.setZero(); @@ -101,11 +109,13 @@ std::optional Pin::ik(const Pose& pose, const Vector7d& q0, } } -Pose IK::forward(const VectorXd& q0, const Pose& tcp_offset) { +Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) { // pose is assumed to be in the robots coordinate frame - this->rl.kin->setPosition(q0); - this->rl.kin->forwardPosition(); - rcs::common::Pose pose = this->rl.kin->getOperationalPosition(0); + pinocchio::forwardKinematics(model, data, q0); + pinocchio::updateFramePlacements(model, data); + 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(); } diff --git a/src/common/IK.h b/src/common/IK.h index 46ac0677..ba50af7c 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -26,6 +26,7 @@ class IK { virtual std::optional ik( 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 { @@ -43,6 +44,7 @@ class RL : public IK { 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 { @@ -58,9 +60,10 @@ class Pin : public IK { public: Pin(const std::string& urdf_path, const std::string& frame_id); - std::optional ik( - const Pose& pose, const Vector7d& q0, + 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; }; } // namespace common } // namespace rcs diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 1c4c9d6c..6af76426 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -239,10 +239,6 @@ PYBIND11_MODULE(_core, m) { .def(py::init(), py::arg("urdf_path"), py::arg("frame_id") = "fr3_link8"); - py::class_(common, "RConfig"); - py::class_(common, "RState"); - py::class_(common, "GConfig"); - py::class_(common, "GState"); py::enum_(common, "RobotType") .value("FR3", rcs::common::RobotType::FR3) .value("UR5e", rcs::common::RobotType::UR5e) diff --git a/src/sim/test.cpp b/src/sim/test.cpp index e9e83af4..174e0d0e 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -15,7 +15,8 @@ // 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 urdf_path = + "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf"; static const Eigen::Matrix iso_cube_center( 0.498, 0.0, 0.226); static const float iso_cube_size = 0.4; From b96aaa37c0de823f53aee6d34dcbbe879fc26fa9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 19:40:07 +0200 Subject: [PATCH 5/9] bump: pinochhio version 2.7 to 3.7 --- cmake/Findpinocchio.cmake | 2 +- pyproject.toml | 2 +- requirements_dev.txt | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cmake/Findpinocchio.cmake b/cmake/Findpinocchio.cmake index d1da5ba9..90fb71ae 100644 --- a/cmake/Findpinocchio.cmake +++ b/cmake/Findpinocchio.cmake @@ -18,7 +18,7 @@ if (NOT pinocchio_FOUND) endif() # Check if the library file exists - cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path) + 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) diff --git a/pyproject.toml b/pyproject.toml index f41a8a55..d106907c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,7 +24,7 @@ dependencies = ["websockets>=11.0", # NOTE: when changing the mujoco version, also change it in requirements_dev.txt "mujoco==3.2.6", # NOTE: Same for pinocchio (pin) - "pin==2.7.0", + "pin==3.7.0", "tilburg-hand", "digit-interface", ] diff --git a/requirements_dev.txt b/requirements_dev.txt index b1ff0539..25a38953 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -11,5 +11,5 @@ scikit-build-core>=0.3.3 poetry-core>=2.1.3 pybind11 mujoco==3.2.6 -pin==2.7.0 +pin==3.7.0 wheel From 2b61347b666ed72e90ce710205bcc79f1306aef0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 19:47:53 +0200 Subject: [PATCH 6/9] feat: mjcf support in pinoccio ik --- src/common/IK.cpp | 9 +++++++-- src/common/IK.h | 4 ++-- src/pybind/rcs.cpp | 5 +++-- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/common/IK.cpp b/src/common/IK.cpp index bbbc4c72..63f4f96b 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -17,6 +17,7 @@ #include #include #include +#include namespace rcs { namespace common { @@ -59,8 +60,12 @@ Pose RL::forward(const VectorXd& q0, const Pose& tcp_offset) { return pose * tcp_offset.inverse(); } -Pin::Pin(const std::string& urdf_path, const std::string& frame_id) : model() { - pinocchio::urdf::buildModel(urdf_path, this->model); +Pin::Pin(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) { diff --git a/src/common/IK.h b/src/common/IK.h index ba50af7c..5c13529a 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -40,7 +40,7 @@ class RL : public IK { } rl_data; public: - RL(const std::string& urdf_path, size_t max_duration_ms = 300); + 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; @@ -59,7 +59,7 @@ class Pin : public IK { pinocchio::Data data; public: - Pin(const std::string& urdf_path, const std::string& frame_id); + Pin(const std::string& path, const std::string& frame_id, bool urdf); std::optional ik( const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()) override; diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 6af76426..c9763128 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -236,8 +236,9 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") - .def(py::init(), - py::arg("urdf_path"), py::arg("frame_id") = "fr3_link8"); + .def(py::init(), + py::arg("path"), py::arg("frame_id") = "fr3_link8", py::arg("urdf") = + true); py::enum_(common, "RobotType") .value("FR3", rcs::common::RobotType::FR3) From 686babe58cc5a859c8be396f5a39b11e55bf19e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 26 Jun 2025 08:49:07 +0200 Subject: [PATCH 7/9] fix(cmake): added pinocchio parsers --- cmake/Findpinocchio.cmake | 21 ++++++++++++++++++++- src/common/CMakeLists.txt | 2 +- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/cmake/Findpinocchio.cmake b/cmake/Findpinocchio.cmake index 90fb71ae..3cf7d3f1 100644 --- a/cmake/Findpinocchio.cmake +++ b/cmake/Findpinocchio.cmake @@ -27,6 +27,16 @@ if (NOT pinocchio_FOUND) 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) @@ -38,8 +48,17 @@ if (NOT pinocchio_FOUND) target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS}) set_target_properties( pinocchio::pinocchio - PROPERTIES + 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}" + ) set(pinocchio_FOUND TRUE) + endif() diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index 8a5e3942..abccdf63 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,3 +1,3 @@ add_library(common) target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio) +target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio pinocchio_parsers) From 25086374c8926b4c1b4fedb6057ced8b77206462 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 26 Jun 2025 08:53:49 +0200 Subject: [PATCH 8/9] style(cpp): formatting --- src/common/IK.cpp | 5 +++-- src/pybind/rcs.cpp | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/common/IK.cpp b/src/common/IK.cpp index 63f4f96b..a413e5f9 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -16,8 +16,8 @@ #include #include #include -#include #include +#include namespace rcs { namespace common { @@ -60,7 +60,8 @@ Pose RL::forward(const VectorXd& q0, const Pose& tcp_offset) { return pose * tcp_offset.inverse(); } -Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true) : model() { +Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true) + : model() { if (urdf) { pinocchio::urdf::buildModel(path, this->model); } else { diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index c9763128..a473dd59 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -237,8 +237,8 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") .def(py::init(), - py::arg("path"), py::arg("frame_id") = "fr3_link8", py::arg("urdf") = - true); + py::arg("path"), py::arg("frame_id") = "fr3_link8", + py::arg("urdf") = true); py::enum_(common, "RobotType") .value("FR3", rcs::common::RobotType::FR3) From 520b01e9611bca65fc679025a3912a0190d2fd13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 26 Jun 2025 13:12:01 +0200 Subject: [PATCH 9/9] fix: implicit numpy version bumping and stubfiles --- Makefile | 4 +-- python/rcs/_core/common.pyi | 48 +++++++++++++++++--------------- python/rcs/_core/hw/__init__.pyi | 4 +-- python/rcs/_core/sim.pyi | 10 +++---- python/rcs/envs/space_utils.py | 8 +++--- python/rcs_so101/hw.py | 6 ++-- 6 files changed, 41 insertions(+), 39 deletions(-) diff --git a/Makefile b/Makefile index b2f6c9a7..49fc50cc 100644 --- a/Makefile +++ b/Makefile @@ -27,8 +27,8 @@ stubgen: pybind11-stubgen -o python --numpy-array-use-type-var rcs find ./python -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete - find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' - find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' + 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 isort python/rcs/_core black python/rcs/_core diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 07b872e7..3238eb55 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -64,13 +64,13 @@ class GripperState: pass class IK: - def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... + def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def ik( - self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ... - ) -> numpy.ndarray[M, numpy.dtype[numpy.float64]] | None: ... + 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): - def __init__(self, urdf_path: str, frame_id: str = "fr3_link8") -> None: ... + def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = True) -> None: ... class Pose: def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ... @@ -84,26 +84,28 @@ class Pose: def __init__( self, rotation: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]], - translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], + translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], ) -> None: ... @typing.overload def __init__( self, - quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]], - translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], + quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]], + translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], ) -> None: ... @typing.overload - def __init__(self, rpy: RPY, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ... + def __init__( + self, rpy: RPY, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] + ) -> None: ... @typing.overload def __init__( self, - rpy_vector: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], - translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], + rpy_vector: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], + translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], ) -> None: ... @typing.overload - def __init__(self, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ... + def __init__(self, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ... @typing.overload - def __init__(self, quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]) -> None: ... + def __init__(self, quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]) -> None: ... @typing.overload def __init__(self, rpy: RPY) -> None: ... @typing.overload @@ -122,11 +124,11 @@ class Pose: def limit_translation_length(self, max_length: float) -> Pose: ... def pose_matrix(self) -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ... def rotation_m(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... - def rotation_q(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... + def rotation_q(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ... def rotation_rpy(self) -> RPY: ... def total_angle(self) -> float: ... - def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... - def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ... + 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: ... @@ -140,11 +142,11 @@ class RPY: @typing.overload def __init__(self, roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0) -> None: ... @typing.overload - def __init__(self, rpy: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ... + def __init__(self, rpy: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ... def __setstate__(self, arg0: tuple) -> None: ... def __str__(self) -> str: ... - def as_quaternion_vector(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... - def as_vector(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... + def as_quaternion_vector(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ... + def as_vector(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ... def is_close(self, other: RPY, eps: float = 1e-08) -> bool: ... def rotation_matrix( self, @@ -154,13 +156,13 @@ class Robot: def get_base_pose_in_world_coordinates(self) -> Pose: ... def get_cartesian_position(self) -> Pose: ... def get_ik(self) -> IK | None: ... - def get_joint_position(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... def get_parameters(self) -> RobotConfig: ... def get_state(self) -> RobotState: ... def move_home(self) -> None: ... def reset(self) -> None: ... def set_cartesian_position(self, pose: Pose) -> None: ... - def set_joint_position(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ... + def set_joint_position(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ... def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ... @@ -175,7 +177,7 @@ class RobotMetaConfig: @property def joint_limits(self) -> numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]: ... @property - def q_home(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def q_home(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... class RobotPlatform: """ @@ -243,8 +245,8 @@ class RobotType: def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ... def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... -def IdentityRotQuatVec() -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... -def IdentityTranslation() -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... +def IdentityRotQuatVec() -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ... +def IdentityTranslation() -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ... def _bootstrap_egl(fn_addr: int, display: int, context: int) -> None: ... def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ... diff --git a/python/rcs/_core/hw/__init__.pyi b/python/rcs/_core/hw/__init__.pyi index 995f33e4..19d7fea6 100644 --- a/python/rcs/_core/hw/__init__.pyi +++ b/python/rcs/_core/hw/__init__.pyi @@ -55,7 +55,7 @@ class FR3(rcs._core.common.Robot): def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ... def automatic_error_recovery(self) -> None: ... def controller_set_joint_position( - self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] + self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] ) -> None: ... def double_tap_robot_to_continue(self) -> None: ... def get_parameters(self) -> FR3Config: ... @@ -91,7 +91,7 @@ class FR3Config(rcs._core.common.RobotConfig): def __init__(self) -> None: ... class FR3Load: - f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None + f_x_cload: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] | None load_inertia: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]] | None load_mass: float def __init__(self) -> None: ... diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 6cb64111..6c6e34fe 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -67,9 +67,9 @@ class CameraType: class FrameSet: def __init__(self) -> None: ... @property - def color_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.uint8]]]: ... + def color_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.uint8]]]: ... @property - def depth_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.float32]]]: ... + def depth_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.float32]]]: ... @property def timestamp(self) -> float: ... @@ -142,7 +142,7 @@ class SimRobot(rcs._core.common.Robot): ) -> None: ... def get_parameters(self) -> SimRobotConfig: ... def get_state(self) -> SimRobotState: ... - def set_joints_hard(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ... + def set_joints_hard(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... def set_parameters(self, cfg: SimRobotConfig) -> bool: ... class SimRobotConfig(rcs._core.common.RobotConfig): @@ -173,9 +173,9 @@ class SimRobotState(rcs._core.common.RobotState): @property def is_moving(self) -> bool: ... @property - def previous_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def previous_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... @property - def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... default_free: CameraType # value = fixed: CameraType # value = diff --git a/python/rcs/envs/space_utils.py b/python/rcs/envs/space_utils.py index 86eb39ab..537bdf88 100644 --- a/python/rcs/envs/space_utils.py +++ b/python/rcs/envs/space_utils.py @@ -17,10 +17,10 @@ M = TypeVar("M", bound=int) VecType: TypeAlias = np.ndarray[M, np.dtype[np.float64]] -Vec7Type: TypeAlias = np.ndarray[Literal[7], np.dtype[np.float64]] -Vec3Type: TypeAlias = np.ndarray[Literal[3], np.dtype[np.float64]] -Vec6Type: TypeAlias = np.ndarray[Literal[6], np.dtype[np.float64]] -Vec18Type: TypeAlias = np.ndarray[Literal[18], np.dtype[np.float64]] +Vec7Type: TypeAlias = np.ndarray[tuple[Literal[7]], np.dtype[np.float64]] +Vec3Type: TypeAlias = np.ndarray[tuple[Literal[3]], np.dtype[np.float64]] +Vec6Type: TypeAlias = np.ndarray[tuple[Literal[6]], np.dtype[np.float64]] +Vec18Type: TypeAlias = np.ndarray[tuple[Literal[18]], np.dtype[np.float64]] class RCSpaceType(TypedDict): ... diff --git a/python/rcs_so101/hw.py b/python/rcs_so101/hw.py index e6bbb6f2..54367003 100644 --- a/python/rcs_so101/hw.py +++ b/python/rcs_so101/hw.py @@ -18,7 +18,7 @@ def get_cartesian_position(self) -> common.Pose: def get_ik(self) -> common.IK | None: return self.ik - def get_joint_position(self) -> np.ndarray[typing.Literal[5], np.dtype[np.float64]]: # type: ignore + def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore obs = self._hf_robot.get_observation() return np.array( [ @@ -43,7 +43,7 @@ def get_state(self) -> common.RobotState: def move_home(self) -> None: home = typing.cast( - np.ndarray[typing.Literal[5], np.dtype[np.float64]], + np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]], common.robots_meta_config(common.RobotType.SO101).q_home, ) self.set_joint_position(home) @@ -56,7 +56,7 @@ def set_cartesian_position(self, pose: common.Pose) -> None: if joints is not None: self.set_joint_position(joints) - def set_joint_position(self, q: np.ndarray[typing.Literal[5], np.dtype[np.float64]]) -> None: # type: ignore + def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore self._hf_robot.send_action( { "shoulder_pan.pos": q[0],