feat(hw gripper): bool state and is moving

Author: juelg

python/rcsss/_core/hw/__init__.pyi

@@ -37,8 +37,12 @@ class FHConfig(rcsss._core.common.GConfig):
 class FHState(rcsss._core.common.GState):
     def __init__(self) -> None: ...
     @property
+    def bool_state(self) -> bool: ...
+    @property
     def is_grasped(self) -> bool: ...
     @property
+    def is_moving(self) -> bool: ...
+    @property
     def last_commanded_width(self) -> float: ...
     @property
     def max_unnormalized_width(self) -> float: ...

src/hw/FrankaHand.cpp

@@ -45,6 +45,8 @@ FHState *FrankaHand::get_state() {
   state->temperature = gripper_state.temperature;
   state->max_unnormalized_width = this->max_width;
   state->last_commanded_width = this->last_commanded_width;
+  state->bool_state = this->last_commanded_width > 0;
+  state->is_moving = this->is_moving;
   return state;
 }
 
@@ -74,7 +76,7 @@ void FrankaHand::m_stop(){
     this->control_thread->join();
     this->control_thread.reset();
   }
-
+  this->is_moving = false;
 }
 
 void FrankaHand::m_reset() {
@@ -83,7 +85,9 @@ void FrankaHand::m_reset() {
   franka::GripperState gripper_state = this->gripper.readOnce();
   this->max_width = gripper_state.max_width - 0.001;
   this->last_commanded_width = this->max_width;
+  this->is_moving = true;
   this->gripper.move(this->max_width, this->cfg.speed);
+  this->is_moving = false;
 }
 void FrankaHand::reset() { this->m_reset(); }
 
@@ -95,36 +99,57 @@ bool FrankaHand::is_grasped() {
 bool FrankaHand::homing() {
   // Do a homing in order to estimate the maximum
   // grasping width with the current fingers.
-  return this->gripper.homing();
+  this->is_moving = true;
+  bool success =  this->gripper.homing();
+  this->is_moving = false;
+  return success;
 }
 
 void FrankaHand::grasp() {
   this->last_commanded_width = 0;
   if (!this->cfg.async_control) {
+    this->is_moving = true;
     this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1);
+    this->is_moving = false;
     return;
   }
   this->m_stop();
-  this->control_thread = std::thread([&](){this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1);});
+  this->control_thread = std::thread([&](){
+    this->is_moving = true;
+    this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1);
+    this->is_moving = false;
+  });
 }
 
 void FrankaHand::open() {
   this->last_commanded_width = this->max_width;
   if (!this->cfg.async_control) {
+    this->is_moving = true;
     this->gripper.move(this->max_width, this->cfg.speed);
+    this->is_moving = false;
     return;
   }
   this->m_stop();
-  this->control_thread = std::thread([&](){this->gripper.move(this->max_width, this->cfg.speed);});
+  this->control_thread = std::thread([&](){
+    this->is_moving = true;
+    this->gripper.move(this->max_width, this->cfg.speed);
+    this->is_moving = false;
+  });
 }
 void FrankaHand::shut() {
   this->last_commanded_width = 0;
   if (!this->cfg.async_control) {
+    this->is_moving = true;
     this->gripper.move(0, this->cfg.speed);
+    this->is_moving = false;
     return;
   }
   this->m_stop();
-  this->control_thread = std::thread([&](){this->gripper.move(0, this->cfg.speed);});
+  this->control_thread = std::thread([&](){
+    this->is_moving = true;
+    this->gripper.move(0, this->cfg.speed);
+    this->is_moving = false;
+  });
 }
 }  // namespace hw
 }  // namespace rcs

src/hw/FrankaHand.h

@@ -30,10 +30,13 @@ struct FHConfig : common::GConfig {
 
 struct FHState : common::GState {
   double width;
+  // true is open
+  bool bool_state;
   bool is_grasped;
   uint16_t temperature;
   double last_commanded_width;
   double max_unnormalized_width;
+  bool is_moving;
 };
 
 class FrankaHand : public common::Gripper {
@@ -42,6 +45,7 @@ class FrankaHand : public common::Gripper {
   FHConfig cfg;
   double max_width;
   double last_commanded_width;
+  bool is_moving = false;
   std::optional<std::thread> control_thread = std::nullopt;
   void m_reset();
   void m_stop();

src/pybind/rcsss.cpp

@@ -343,6 +343,8 @@ PYBIND11_MODULE(_core, m) {
       .def(py::init<>())
       .def_readonly("width", &rcs::hw::FHState::width)
       .def_readonly("is_grasped", &rcs::hw::FHState::is_grasped)
+      .def_readonly("is_moving", &rcs::hw::FHState::is_moving)
+      .def_readonly("bool_state", &rcs::hw::FHState::bool_state)
       .def_readonly("last_commanded_width",
                     &rcs::hw::FHState::last_commanded_width)
       .def_readonly("max_unnormalized_width",