feat: adds franka state bindings and and state observation

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -74,8 +74,15 @@ FrankaConfig* Franka::get_config() {
 }
 
 FrankaState* Franka::get_state() {
-  // dummy state until we define a prober state
-  FrankaState* state = new FrankaState();
+  franka::RobotState current_robot_state;
+  if (this->running_controller == Controller::none) {
+    current_robot_state = this->robot.readOnce();
+  } else {
+    std::lock_guard<std::mutex> lock(this->interpolator_mutex);
+    current_robot_state = this->curr_state;
+  }
+  auto* state = new FrankaState();
+  state->robot_state = current_robot_state;
   return state;
 }
 

extensions/rcs_fr3/src/hw/Franka.h

@@ -2,6 +2,7 @@
 #define RCS_FRANKA_H
 
 #include <franka/robot.h>
+#include <franka/robot_state.h>
 
 #include <cmath>
 #include <memory>
@@ -49,7 +50,9 @@ struct PandaConfig : FrankaConfig {
   common::RobotType robot_type = common::RobotType::Panda;
 };
 
-struct FrankaState : common::RobotState {};
+struct FrankaState : common::RobotState {
+  franka::RobotState robot_state;
+};
 
 class Franka : public common::Robot {
  private:

extensions/rcs_fr3/src/pybind/rcs.cpp

@@ -1,4 +1,5 @@
 #include <franka/exception.h>
+#include <franka/robot_state.h>
 #include <hw/Franka.h>
 #include <hw/FrankaHand.h>
 #include <pybind11/cast.h>
@@ -41,10 +42,76 @@ PYBIND11_MODULE(_core, m) {
   // HARDWARE MODULE
   auto hw = m.def_submodule("hw", "rcs franka module");
 
+  py::enum_<franka::RobotMode>(hw, "RobotMode")
+      .value("kOther", franka::RobotMode::kOther)
+      .value("kIdle", franka::RobotMode::kIdle)
+      .value("kMove", franka::RobotMode::kMove)
+      .value("kGuiding", franka::RobotMode::kGuiding)
+      .value("kReflex", franka::RobotMode::kReflex)
+      .value("kUserStopped", franka::RobotMode::kUserStopped)
+      .value("kAutomaticErrorRecovery",
+             franka::RobotMode::kAutomaticErrorRecovery)
+      .export_values();
+
+  py::class_<franka::RobotState>(hw, "RobotState")
+      .def(py::init<>())
+      .def_readonly("O_T_EE", &franka::RobotState::O_T_EE)
+      .def_readonly("O_T_EE_d", &franka::RobotState::O_T_EE_d)
+      .def_readonly("F_T_EE", &franka::RobotState::F_T_EE)
+      .def_readonly("F_T_NE", &franka::RobotState::F_T_NE)
+      .def_readonly("NE_T_EE", &franka::RobotState::NE_T_EE)
+      .def_readonly("EE_T_K", &franka::RobotState::EE_T_K)
+      .def_readonly("m_ee", &franka::RobotState::m_ee)
+      .def_readonly("I_ee", &franka::RobotState::I_ee)
+      .def_readonly("F_x_Cee", &franka::RobotState::F_x_Cee)
+      .def_readonly("m_load", &franka::RobotState::m_load)
+      .def_readonly("I_load", &franka::RobotState::I_load)
+      .def_readonly("F_x_Cload", &franka::RobotState::F_x_Cload)
+      .def_readonly("m_total", &franka::RobotState::m_total)
+      .def_readonly("I_total", &franka::RobotState::I_total)
+      .def_readonly("F_x_Ctotal", &franka::RobotState::F_x_Ctotal)
+      .def_readonly("elbow", &franka::RobotState::elbow)
+      .def_readonly("elbow_d", &franka::RobotState::elbow_d)
+      .def_readonly("elbow_c", &franka::RobotState::elbow_c)
+      .def_readonly("delbow_c", &franka::RobotState::delbow_c)
+      .def_readonly("ddelbow_c", &franka::RobotState::ddelbow_c)
+      .def_readonly("tau_J", &franka::RobotState::tau_J)
+      .def_readonly("tau_J_d", &franka::RobotState::tau_J_d)
+      .def_readonly("dtau_J", &franka::RobotState::dtau_J)
+      .def_readonly("q", &franka::RobotState::q)
+      .def_readonly("q_d", &franka::RobotState::q_d)
+      .def_readonly("dq", &franka::RobotState::dq)
+      .def_readonly("dq_d", &franka::RobotState::dq_d)
+      .def_readonly("ddq_d", &franka::RobotState::ddq_d)
+      .def_readonly("joint_contact", &franka::RobotState::joint_contact)
+      .def_readonly("cartesian_contact", &franka::RobotState::cartesian_contact)
+      .def_readonly("joint_collision", &franka::RobotState::joint_collision)
+      .def_readonly("cartesian_collision",
+                    &franka::RobotState::cartesian_collision)
+      .def_readonly("tau_ext_hat_filtered",
+                    &franka::RobotState::tau_ext_hat_filtered)
+      .def_readonly("O_F_ext_hat_K", &franka::RobotState::O_F_ext_hat_K)
+      .def_readonly("K_F_ext_hat_K", &franka::RobotState::K_F_ext_hat_K)
+      .def_readonly("O_dP_EE_d", &franka::RobotState::O_dP_EE_d)
+      .def_readonly("O_ddP_O", &franka::RobotState::O_ddP_O)
+      .def_readonly("O_T_EE_c", &franka::RobotState::O_T_EE_c)
+      .def_readonly("O_dP_EE_c", &franka::RobotState::O_dP_EE_c)
+      .def_readonly("O_ddP_EE_c", &franka::RobotState::O_ddP_EE_c)
+      .def_readonly("theta", &franka::RobotState::theta)
+      .def_readonly("dtheta", &franka::RobotState::dtheta)
+      //   .def_readonly("current_errors", &franka::RobotState::current_errors)
+      //   .def_readonly("last_motion_errors",
+      //                 &franka::RobotState::last_motion_errors)
+      .def_readonly("control_command_success_rate",
+                    &franka::RobotState::control_command_success_rate)
+      //   .def_readonly("time", &franka::RobotState::time)
+      .def_readonly("robot_mode", &franka::RobotState::robot_mode);
+
   py::object robot_state =
       (py::object)py::module_::import("rcs").attr("common").attr("RobotState");
   py::class_<rcs::hw::FrankaState>(hw, "FrankaState", robot_state)
-      .def(py::init<>());
+      .def(py::init<>())
+      .def_readonly("robot_state", &rcs::hw::FrankaState::robot_state);
   py::class_<rcs::hw::FrankaLoad>(hw, "FrankaLoad")
       .def(py::init<>())
       .def_readwrite("load_mass", &rcs::hw::FrankaLoad::load_mass)

extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi

@@ -7,6 +7,7 @@ from __future__ import annotations
 import typing
 
 import numpy
+import pybind11_stubgen.typing_ext
 import rcs._core.common
 
 from . import exceptions
@@ -22,8 +23,17 @@ __all__: list[str] = [
     "FrankaState",
     "IKSolver",
     "PandaConfig",
+    "RobotMode",
+    "RobotState",
     "exceptions",
     "franka_ik",
+    "kAutomaticErrorRecovery",
+    "kGuiding",
+    "kIdle",
+    "kMove",
+    "kOther",
+    "kReflex",
+    "kUserStopped",
     "rcs_ik",
 ]
 
@@ -106,9 +116,6 @@ class FrankaLoad:
     load_mass: float
     def __init__(self) -> None: ...
 
-class FrankaState(rcs._core.common.RobotState):
-    def __init__(self) -> None: ...
-
 class IKSolver:
     """
     Members:
@@ -138,11 +145,158 @@ class IKSolver:
     @property
     def value(self) -> int: ...
 
+class RobotMode:
+    """
+    Members:
+
+      kOther
+
+      kIdle
+
+      kMove
+
+      kGuiding
+
+      kReflex
+
+      kUserStopped
+
+      kAutomaticErrorRecovery
+    """
+
+    __members__: typing.ClassVar[
+        dict[str, RobotMode]
+    ]  # value = {'kOther': <RobotMode.kOther: 0>, 'kIdle': <RobotMode.kIdle: 1>, 'kMove': <RobotMode.kMove: 2>, 'kGuiding': <RobotMode.kGuiding: 3>, 'kReflex': <RobotMode.kReflex: 4>, 'kUserStopped': <RobotMode.kUserStopped: 5>, 'kAutomaticErrorRecovery': <RobotMode.kAutomaticErrorRecovery: 6>}
+    kAutomaticErrorRecovery: typing.ClassVar[RobotMode]  # value = <RobotMode.kAutomaticErrorRecovery: 6>
+    kGuiding: typing.ClassVar[RobotMode]  # value = <RobotMode.kGuiding: 3>
+    kIdle: typing.ClassVar[RobotMode]  # value = <RobotMode.kIdle: 1>
+    kMove: typing.ClassVar[RobotMode]  # value = <RobotMode.kMove: 2>
+    kOther: typing.ClassVar[RobotMode]  # value = <RobotMode.kOther: 0>
+    kReflex: typing.ClassVar[RobotMode]  # value = <RobotMode.kReflex: 4>
+    kUserStopped: typing.ClassVar[RobotMode]  # value = <RobotMode.kUserStopped: 5>
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
+    @property
+    def name(self) -> str: ...
+    @property
+    def value(self) -> int: ...
+
+class RobotState:
+    def __init__(self) -> None: ...
+    @property
+    def EE_T_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_T_NE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_x_Cee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def F_x_Cload(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def F_x_Ctotal(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def I_ee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def I_load(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def I_total(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def K_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def NE_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_T_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_T_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_dP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_dP_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_ddP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_ddP_O(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def cartesian_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def cartesian_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def control_command_success_rate(self) -> float: ...
+    @property
+    def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def ddq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def delbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def dq(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dtau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dtheta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def elbow(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def elbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def elbow_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def joint_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def m_ee(self) -> float: ...
+    @property
+    def m_load(self) -> float: ...
+    @property
+    def m_total(self) -> float: ...
+    @property
+    def q(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def q_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def robot_mode(self) -> RobotMode: ...
+    @property
+    def tau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def tau_J_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def tau_ext_hat_filtered(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+
 class FR3Config(FrankaConfig):
     def __init__(self) -> None: ...
 
 class PandaConfig(FrankaConfig):
     def __init__(self) -> None: ...
 
+class FrankaState(rcs._core.common.RobotState):
+    def __init__(self) -> None: ...
+    @property
+    def robot_state(self) -> RobotState: ...
+
 franka_ik: IKSolver  # value = <IKSolver.franka_ik: 0>
+kAutomaticErrorRecovery: RobotMode  # value = <RobotMode.kAutomaticErrorRecovery: 6>
+kGuiding: RobotMode  # value = <RobotMode.kGuiding: 3>
+kIdle: RobotMode  # value = <RobotMode.kIdle: 1>
+kMove: RobotMode  # value = <RobotMode.kMove: 2>
+kOther: RobotMode  # value = <RobotMode.kOther: 0>
+kReflex: RobotMode  # value = <RobotMode.kReflex: 4>
+kUserStopped: RobotMode  # value = <RobotMode.kUserStopped: 5>
 rcs_ik: IKSolver  # value = <IKSolver.rcs_ik: 1>

extensions/rcs_fr3/src/rcs_fr3/envs.py

@@ -17,13 +17,22 @@ def __init__(self, env):
 
     def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict]:
         try:
-            return super().step(action)
+            obs, reward, terminated, truncated, info = super().step(action)
+            obs = self.get_obs(obs)
+            return obs, reward, terminated, truncated, info
         except hw.exceptions.FrankaControlException as e:
             _logger.error("FrankaControlException: %s", e)
             self.hw_robot.automatic_error_recovery()
             # TODO: this does not work if some wrappers are in between
             # FR3HW and RobotEnv
-            return dict(self.unwrapped.get_obs()), 0, False, True, {}
+            return self.get_obs(), 0, False, True, {}
+
+    def get_obs(self, obs: dict | None = None) -> dict[str, Any]:
+        if obs is None:
+            obs = dict(self.unwrapped.get_obs())
+        robot_state = cast(hw.FrankaState, self.unwrapped.robot.get_state())
+        obs["robot_state"] = vars(robot_state.robot_state)
+        return obs
 
     def reset(
         self, seed: int | None = None, options: dict[str, Any] | None = None