refactor(hw): guiding mode configure DOFs

Author: juelg

python/rcsss/_core/hw/__init__.pyi

@@ -62,14 +62,22 @@ class FR3(rcsss._core.common.Robot):
     ) -> None: ...
     def set_cartesian_position_internal(self, pose: rcsss._core.common.Pose) -> None: ...
     def set_default_robot_behavior(self) -> None: ...
-    def set_guiding_mode(self, enabled: bool) -> None: ...
+    def set_guiding_mode(
+        self,
+        x: bool = True,
+        y: bool = True,
+        z: bool = True,
+        roll: bool = True,
+        pitch: bool = True,
+        yaw: bool = True,
+        elbow: bool = True,
+    ) -> None: ...
     def set_parameters(self, cfg: FR3Config) -> bool: ...
     def stop_control_thread(self) -> None: ...
     def zero_torque(self) -> None: ...
 
 class FR3Config(rcsss._core.common.RConfig):
     async_control: bool
-    guiding_mode_enabled: bool
     ik_solver: IKSolver
     load_parameters: FR3Load | None
     nominal_end_effector_frame: rcsss._core.common.Pose | None

src/hw/FR3.cpp

@@ -24,7 +24,7 @@ FR3::FR3(const std::string &ip, std::optional<std::shared_ptr<common::IK>> ik,
     : robot(ip), m_ik(ik) {
   // set collision behavior and impedance
   this->set_default_robot_behavior();
-  this->set_guiding_mode(true);
+  this->set_guiding_mode(true, true, true, true, true, true, true);
 
   if (cfg.has_value()) {
     this->cfg = cfg.value();
@@ -121,11 +121,11 @@ common::Vector7d FR3::get_joint_position() {
   return joints;
 }
 
-void FR3::set_guiding_mode(bool enabled) {
-  std::array<bool, 6> activated;
-  activated.fill(enabled);
-  this->robot.setGuidingMode(activated, enabled);
-  this->cfg.guiding_mode_enabled = enabled;
+void FR3::set_guiding_mode(bool x, bool y, bool z,
+                           bool roll, bool pitch, bool yaw,
+                           bool elbow) {
+  std::array<bool, 6> activated = {x, y, z, roll, pitch, yaw};
+  this->robot.setGuidingMode(activated, elbow);
 }
 
 void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,

src/hw/FR3.h

@@ -35,7 +35,6 @@ struct FR3Config : common::RConfig {
   // TODO: we can either write specific bindings for each, or we use python
   // dictionaries with these objects
   IKSolver ik_solver = IKSolver::franka;
-  bool guiding_mode_enabled = true;
   double speed_factor = DEFAULT_SPEED_FACTOR;
   std::optional<FR3Load> load_parameters = std::nullopt;
   std::optional<common::Pose> nominal_end_effector_frame = std::nullopt;
@@ -79,7 +78,9 @@ class FR3 : public common::Robot {
 
   common::Vector7d get_joint_position() override;
 
-  void set_guiding_mode(bool enabled);
+  void set_guiding_mode(bool x, bool y, bool z,
+                           bool roll, bool pitch, bool yaw,
+                           bool elbow);
 
   void controller_set_joint_position(const common::Vector7d &desired_q);
   void osc_set_cartesian_position(

src/pybind/rcsss.cpp

@@ -323,8 +323,6 @@ PYBIND11_MODULE(_core, m) {
       .def(py::init<>())
       .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver)
       .def_readwrite("speed_factor", &rcs::hw::FR3Config::speed_factor)
-      .def_readwrite("guiding_mode_enabled",
-                     &rcs::hw::FR3Config::guiding_mode_enabled)
       .def_readwrite("load_parameters", &rcs::hw::FR3Config::load_parameters)
       .def_readwrite("nominal_end_effector_frame",
                      &rcs::hw::FR3Config::nominal_end_effector_frame)
@@ -362,7 +360,7 @@ PYBIND11_MODULE(_core, m) {
       .def("set_default_robot_behavior",
            &rcs::hw::FR3::set_default_robot_behavior)
       .def("set_guiding_mode", &rcs::hw::FR3::set_guiding_mode,
-           py::arg("enabled"))
+           py::arg("x") = true, py::arg("y") = true, py::arg("z") = true, py::arg("roll") = true, py::arg("pitch") = true, py::arg("yaw") = true, py::arg("elbow") = true)
       .def("zero_torque", &rcs::hw::FR3::zero_torque)
       .def("osc_set_cartesian_position",
            &rcs::hw::FR3::osc_set_cartesian_position,