From a1a912427e13d5f1ab6c42463179214714e47ae0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 28 Nov 2025 21:59:31 +0100 Subject: [PATCH 1/4] feat: add franka state --- extensions/rcs_fr3/src/hw/Franka.cpp | 13 +- extensions/rcs_fr3/src/hw/Franka.h | 5 +- extensions/rcs_fr3/src/pybind/rcs.cpp | 69 ++++- .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 244 +++++++++++++++++- 4 files changed, 323 insertions(+), 8 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 2014ccaf..66433270 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -73,9 +73,16 @@ FrankaConfig* Franka::get_config() { return cfg; } -FrankaState* Franka::get_state() { - // dummy state until we define a prober state - FrankaState* state = new FrankaState(); +FrankaState *Franka::get_state() { + franka::RobotState current_robot_state; + if (this->running_controller == Controller::none) { + current_robot_state = this->robot.readOnce(); + } else { + std::lock_guard lock(this->interpolator_mutex); + current_robot_state = this->curr_state; + } + auto *state = new FrankaState(); + state->robot_state = current_robot_state; return state; } diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index fe4a5af4..6712b67b 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -2,6 +2,7 @@ #define RCS_FRANKA_H #include +#include #include #include @@ -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: diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index ea56afff..d812ca9b 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -41,10 +42,76 @@ PYBIND11_MODULE(_core, m) { // HARDWARE MODULE auto hw = m.def_submodule("hw", "rcs franka module"); + py::enum_(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_(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("robot_mode", &franka::RobotState::robot_mode) + .def_readonly("time", &franka::RobotState::time); + py::object robot_state = (py::object)py::module_::import("rcs").attr("common").attr("RobotState"); py::class_(hw, "FrankaState", robot_state) - .def(py::init<>()); + .def(py::init<>()) + .def_readonly("robot_state", &rcs::hw::FrankaState::robot_state); py::class_(hw, "FrankaLoad") .def(py::init<>()) .def_readwrite("load_mass", &rcs::hw::FrankaLoad::load_mass) 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 006e7e50..f217a4fa 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/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,242 @@ 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': , 'kIdle': , 'kMove': , 'kGuiding': , 'kReflex': , 'kUserStopped': , 'kAutomaticErrorRecovery': } + kAutomaticErrorRecovery: typing.ClassVar[RobotMode] # value = + kGuiding: typing.ClassVar[RobotMode] # value = + kIdle: typing.ClassVar[RobotMode] # value = + kMove: typing.ClassVar[RobotMode] # value = + kOther: typing.ClassVar[RobotMode] # value = + kReflex: typing.ClassVar[RobotMode] # value = + kUserStopped: typing.ClassVar[RobotMode] # value = + 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 current_errors(self) -> ...: ... + @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 last_motion_errors(self) -> ...: ... + @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)]: ... + @property + def time(self) -> ...: ... + 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 = +kAutomaticErrorRecovery: RobotMode # value = +kGuiding: RobotMode # value = +kIdle: RobotMode # value = +kMove: RobotMode # value = +kOther: RobotMode # value = +kReflex: RobotMode # value = +kUserStopped: RobotMode # value = rcs_ik: IKSolver # value = From 287365e7c23351f13990c3bb5d11341c684b252d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 3 Dec 2025 09:31:56 +0100 Subject: [PATCH 2/4] fix: remove libfranka unbound state errors --- extensions/rcs_fr3/src/pybind/rcs.cpp | 10 +- .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 14 +- .../src/rcs_panda/_core/hw/__init__.pyi | 135 ++++++++++++++++++ 3 files changed, 142 insertions(+), 17 deletions(-) diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index d812ca9b..be76609e 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -99,13 +99,13 @@ PYBIND11_MODULE(_core, m) { .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("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("robot_mode", &franka::RobotState::robot_mode) - .def_readonly("time", &franka::RobotState::time); + // .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"); 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 f217a4fa..76201173 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -278,11 +278,7 @@ class RobotState: @property def control_command_success_rate(self) -> float: ... @property - def current_errors(self) -> ...: ... - @property - def ddelbow_c( - self, - ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ... + def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ... @property def ddq_d( self, @@ -328,8 +324,6 @@ class RobotState: self, ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ... @property - def last_motion_errors(self) -> ...: ... - @property def m_ee(self) -> float: ... @property def m_load(self) -> float: ... @@ -358,11 +352,7 @@ class RobotState: 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)]: ... - @property - def time(self) -> ...: ... + def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ... class FR3Config(FrankaConfig): def __init__(self) -> None: ... 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 006e7e50..c4105501 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -138,6 +138,141 @@ 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': , 'kIdle': , 'kMove': , 'kGuiding': , 'kReflex': , 'kUserStopped': , 'kAutomaticErrorRecovery': } + kAutomaticErrorRecovery: typing.ClassVar[RobotMode] # value = + kGuiding: typing.ClassVar[RobotMode] # value = + kIdle: typing.ClassVar[RobotMode] # value = + kMove: typing.ClassVar[RobotMode] # value = + kOther: typing.ClassVar[RobotMode] # value = + kReflex: typing.ClassVar[RobotMode] # value = + kUserStopped: typing.ClassVar[RobotMode] # value = + 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: ... From ecf2add6f69278ce04f0e4de7a483659d7f2ad79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 12 Dec 2025 17:02:11 +0100 Subject: [PATCH 3/4] feat: added franka state to observation --- .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 148 +++++------------- extensions/rcs_fr3/src/rcs_fr3/envs.py | 13 +- .../src/rcs_panda/_core/hw/__init__.pyi | 25 ++- 3 files changed, 70 insertions(+), 116 deletions(-) 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 76201173..4713bfa1 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -192,137 +192,73 @@ class RobotMode: class RobotState: def __init__(self) -> None: ... @property - def EE_T_K( - self, - ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ... @property def m_ee(self) -> float: ... @property @@ -330,27 +266,17 @@ class RobotState: @property def m_total(self) -> float: ... @property - def q( - self, - ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... + 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)]: ... diff --git a/extensions/rcs_fr3/src/rcs_fr3/envs.py b/extensions/rcs_fr3/src/rcs_fr3/envs.py index b8f8a5a6..dddc9830 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/envs.py +++ b/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 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 c4105501..4713bfa1 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_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: @@ -279,5 +286,17 @@ class FR3Config(FrankaConfig): 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 = +kAutomaticErrorRecovery: RobotMode # value = +kGuiding: RobotMode # value = +kIdle: RobotMode # value = +kMove: RobotMode # value = +kOther: RobotMode # value = +kReflex: RobotMode # value = +kUserStopped: RobotMode # value = rcs_ik: IKSolver # value = From 2aabffd02be1780720fadff2ba12aa1b2523b637 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 12 Dec 2025 17:12:40 +0100 Subject: [PATCH 4/4] style: fix cpp formatting --- extensions/rcs_fr3/src/hw/Franka.cpp | 4 ++-- extensions/rcs_fr3/src/pybind/rcs.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 66433270..1c7165ff 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -73,7 +73,7 @@ FrankaConfig* Franka::get_config() { return cfg; } -FrankaState *Franka::get_state() { +FrankaState* Franka::get_state() { franka::RobotState current_robot_state; if (this->running_controller == Controller::none) { current_robot_state = this->robot.readOnce(); @@ -81,7 +81,7 @@ FrankaState *Franka::get_state() { std::lock_guard lock(this->interpolator_mutex); current_robot_state = this->curr_state; } - auto *state = new FrankaState(); + auto* state = new FrankaState(); state->robot_state = current_robot_state; return state; } diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index be76609e..453a224e 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -99,12 +99,12 @@ PYBIND11_MODULE(_core, m) { .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("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("time", &franka::RobotState::time) .def_readonly("robot_mode", &franka::RobotState::robot_mode); py::object robot_state =