fix(fr3,panda): franka control and franka hand proper close (#229)

* fix(fr3,panda): franka control and franka hand proper close

- destructors properly close control threads
- franka hand has proper close method
- franka hand close method is used in gripper wrapper

* fix: topological sort pybind to keep stub order

Author: juelg

.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

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'

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'

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

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

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

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");

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: ...

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'

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: ...