diff --git a/.gitignore b/.gitignore index 3ee6fb06..3df36620 100644 --- a/.gitignore +++ b/.gitignore @@ -3,7 +3,6 @@ build plugin.h .session.vim .projections.json -pw.py __pycache__ .ruff_cache .mypy_cache @@ -18,3 +17,5 @@ wheels .env settings.json *.egg-info +video* +GEMINI.md diff --git a/Makefile b/Makefile index 126c0cc4..516c09c1 100644 --- a/Makefile +++ b/Makefile @@ -29,7 +29,7 @@ clangcompile: # Auto generation of CPP binding stub files stubgen: - pybind11-stubgen -o python --numpy-array-use-type-var rcs + pybind11-stubgen -o python --numpy-array-use-type-var --sort-by topological 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\]\]/tuple\[typing\.Literal[\1]\]/g' diff --git a/extensions/rcs_fr3/Makefile b/extensions/rcs_fr3/Makefile index 721e1b68..94f0d15a 100644 --- a/extensions/rcs_fr3/Makefile +++ b/extensions/rcs_fr3/Makefile @@ -22,7 +22,7 @@ clangcompile: # Auto generation of CPP binding stub files stubgen: - pybind11-stubgen -o src --numpy-array-use-type-var rcs_fr3 + pybind11-stubgen -o src --numpy-array-use-type-var --sort-by topological rcs_fr3 find ./${PYSRC} -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' find ./${PYSRC} -not -path "./${PYSRC}/_core/*" -name '*.pyi' -delete find ./${PYSRC}/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 53210249..9ee70f45 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -32,7 +32,13 @@ Franka::Franka(const std::string &ip, } // else default constructor } -Franka::~Franka() {} +Franka::~Franka() { + try { + this->stop_control_thread(); + } catch (const franka::Exception &e) { + std::cerr << "Exception in ~Franka(): " << e.what() << std::endl; + } +} /** * @brief Set the parameters for the robot diff --git a/extensions/rcs_fr3/src/hw/FrankaHand.cpp b/extensions/rcs_fr3/src/hw/FrankaHand.cpp index 501f55af..8adea316 100644 --- a/extensions/rcs_fr3/src/hw/FrankaHand.cpp +++ b/extensions/rcs_fr3/src/hw/FrankaHand.cpp @@ -18,7 +18,13 @@ FrankaHand::FrankaHand(const std::string &ip, const FHConfig &cfg) this->m_reset(); } -FrankaHand::~FrankaHand() {} +FrankaHand::~FrankaHand() { + try { + this->m_stop(); + } catch (const franka::Exception &e) { + std::cerr << "Exception in ~FrankaHand(): " << e.what() << std::endl; + } +} bool FrankaHand::set_config(const FHConfig &cfg) { franka::GripperState gripper_state = this->gripper.readOnce(); @@ -75,8 +81,8 @@ double FrankaHand::get_normalized_width() { void FrankaHand::m_stop() { try { this->gripper.stop(); - } catch (const franka::CommandException &e) { - std::cerr << "franka hand command exception ignored stop" << std::endl; + } catch (const franka::Exception &e) { + std::cerr << "FrankaHand::m_stop: " << e.what() << std::endl; } this->m_wait(); this->is_moving = false; @@ -181,5 +187,7 @@ void FrankaHand::shut() { this->is_moving = false; }); } + +void FrankaHand::close() { this->m_stop(); } } // namespace hw } // namespace rcs \ No newline at end of file diff --git a/extensions/rcs_fr3/src/hw/FrankaHand.h b/extensions/rcs_fr3/src/hw/FrankaHand.h index 6a9f3bc0..390cbc0c 100644 --- a/extensions/rcs_fr3/src/hw/FrankaHand.h +++ b/extensions/rcs_fr3/src/hw/FrankaHand.h @@ -76,7 +76,7 @@ class FrankaHand : public common::Gripper { void grasp() override; void open() override; void shut() override; - void close() override {}; + void close() override; }; } // namespace hw } // namespace rcs diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 3c26a10d..67e59c00 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -144,7 +144,8 @@ PYBIND11_MODULE(_core, m) { .def("get_state", &rcs::hw::FrankaHand::get_state) .def("set_config", &rcs::hw::FrankaHand::set_config, py::arg("cfg")) .def("is_grasped", &rcs::hw::FrankaHand::is_grasped) - .def("homing", &rcs::hw::FrankaHand::homing); + .def("homing", &rcs::hw::FrankaHand::homing) + .def("close", &rcs::hw::FrankaHand::close); auto hw_except = hw.def_submodule("exceptions", "exceptions from the hardware module"); 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 a5ac62c8..9f3b01f2 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -98,6 +98,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): class FrankaHand(rcs._core.common.Gripper): def __init__(self, ip: str, cfg: FHConfig) -> None: ... + def close(self) -> None: ... def get_config(self) -> FHConfig: ... def get_state(self) -> FHState: ... def homing(self) -> bool: ... diff --git a/extensions/rcs_panda/Makefile b/extensions/rcs_panda/Makefile index d6a979ac..94c1bce0 100644 --- a/extensions/rcs_panda/Makefile +++ b/extensions/rcs_panda/Makefile @@ -22,7 +22,7 @@ clangcompile: # Auto generation of CPP binding stub files stubgen: - pybind11-stubgen -o src --numpy-array-use-type-var rcs_panda + pybind11-stubgen -o src --numpy-array-use-type-var --sort-by topological rcs_panda find ./${PYSRC} -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' find ./${PYSRC} -not -path "./${PYSRC}/_core/*" -name '*.pyi' -delete find ./${PYSRC}/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index a5ac62c8..9f3b01f2 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -98,6 +98,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): class FrankaHand(rcs._core.common.Gripper): def __init__(self, ip: str, cfg: FHConfig) -> None: ... + def close(self) -> None: ... def get_config(self) -> FHConfig: ... def get_state(self) -> FHState: ... def homing(self) -> bool: ... diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 7c745e1e..b1b38409 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -700,6 +700,10 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True, open_on_re self._last_gripper_cmd = None self.open_on_reset = open_on_reset + def close(self): + self.gripper.close() + super().close() + def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: if self.open_on_reset: # resetting opens the gripper