Skip to content

Commit 18aae83

Browse files
juelgjuelg
authored
feat: adds franka state bindings and and state observation (#241)
* feat: add franka state * fix: remove libfranka unbound state errors * feat: added franka state to observation * style: fix cpp formatting
1 parent f2ad597 commit 18aae83

File tree

6 files changed

+406
-12
lines changed

6 files changed

+406
-12
lines changed

‎extensions/rcs_fr3/src/hw/Franka.cpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -74,8 +74,15 @@ FrankaConfig* Franka::get_config() {
7474
}
7575

7676
FrankaState* Franka::get_state() {
77-
// dummy state until we define a prober state
78-
FrankaState* state = new FrankaState();
77+
franka::RobotState current_robot_state;
78+
if (this->running_controller == Controller::none) {
79+
current_robot_state = this->robot.readOnce();
80+
} else {
81+
std::lock_guard<std::mutex> lock(this->interpolator_mutex);
82+
current_robot_state = this->curr_state;
83+
}
84+
auto* state = new FrankaState();
85+
state->robot_state = current_robot_state;
7986
return state;
8087
}
8188

‎extensions/rcs_fr3/src/hw/Franka.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
#define RCS_FRANKA_H
33

44
#include <franka/robot.h>
5+
#include <franka/robot_state.h>
56

67
#include <cmath>
78
#include <memory>
@@ -49,7 +50,9 @@ struct PandaConfig : FrankaConfig {
4950
common::RobotType robot_type = common::RobotType::Panda;
5051
};
5152

52-
struct FrankaState : common::RobotState {};
53+
struct FrankaState : common::RobotState {
54+
franka::RobotState robot_state;
55+
};
5356

5457
class Franka : public common::Robot {
5558
private:

‎extensions/rcs_fr3/src/pybind/rcs.cpp‎

Lines changed: 68 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include <franka/exception.h>
2+
#include <franka/robot_state.h>
23
#include <hw/Franka.h>
34
#include <hw/FrankaHand.h>
45
#include <pybind11/cast.h>
@@ -41,10 +42,76 @@ PYBIND11_MODULE(_core, m) {
4142
// HARDWARE MODULE
4243
auto hw = m.def_submodule("hw", "rcs franka module");
4344

45+
py::enum_<franka::RobotMode>(hw, "RobotMode")
46+
.value("kOther", franka::RobotMode::kOther)
47+
.value("kIdle", franka::RobotMode::kIdle)
48+
.value("kMove", franka::RobotMode::kMove)
49+
.value("kGuiding", franka::RobotMode::kGuiding)
50+
.value("kReflex", franka::RobotMode::kReflex)
51+
.value("kUserStopped", franka::RobotMode::kUserStopped)
52+
.value("kAutomaticErrorRecovery",
53+
franka::RobotMode::kAutomaticErrorRecovery)
54+
.export_values();
55+
56+
py::class_<franka::RobotState>(hw, "RobotState")
57+
.def(py::init<>())
58+
.def_readonly("O_T_EE", &franka::RobotState::O_T_EE)
59+
.def_readonly("O_T_EE_d", &franka::RobotState::O_T_EE_d)
60+
.def_readonly("F_T_EE", &franka::RobotState::F_T_EE)
61+
.def_readonly("F_T_NE", &franka::RobotState::F_T_NE)
62+
.def_readonly("NE_T_EE", &franka::RobotState::NE_T_EE)
63+
.def_readonly("EE_T_K", &franka::RobotState::EE_T_K)
64+
.def_readonly("m_ee", &franka::RobotState::m_ee)
65+
.def_readonly("I_ee", &franka::RobotState::I_ee)
66+
.def_readonly("F_x_Cee", &franka::RobotState::F_x_Cee)
67+
.def_readonly("m_load", &franka::RobotState::m_load)
68+
.def_readonly("I_load", &franka::RobotState::I_load)
69+
.def_readonly("F_x_Cload", &franka::RobotState::F_x_Cload)
70+
.def_readonly("m_total", &franka::RobotState::m_total)
71+
.def_readonly("I_total", &franka::RobotState::I_total)
72+
.def_readonly("F_x_Ctotal", &franka::RobotState::F_x_Ctotal)
73+
.def_readonly("elbow", &franka::RobotState::elbow)
74+
.def_readonly("elbow_d", &franka::RobotState::elbow_d)
75+
.def_readonly("elbow_c", &franka::RobotState::elbow_c)
76+
.def_readonly("delbow_c", &franka::RobotState::delbow_c)
77+
.def_readonly("ddelbow_c", &franka::RobotState::ddelbow_c)
78+
.def_readonly("tau_J", &franka::RobotState::tau_J)
79+
.def_readonly("tau_J_d", &franka::RobotState::tau_J_d)
80+
.def_readonly("dtau_J", &franka::RobotState::dtau_J)
81+
.def_readonly("q", &franka::RobotState::q)
82+
.def_readonly("q_d", &franka::RobotState::q_d)
83+
.def_readonly("dq", &franka::RobotState::dq)
84+
.def_readonly("dq_d", &franka::RobotState::dq_d)
85+
.def_readonly("ddq_d", &franka::RobotState::ddq_d)
86+
.def_readonly("joint_contact", &franka::RobotState::joint_contact)
87+
.def_readonly("cartesian_contact", &franka::RobotState::cartesian_contact)
88+
.def_readonly("joint_collision", &franka::RobotState::joint_collision)
89+
.def_readonly("cartesian_collision",
90+
&franka::RobotState::cartesian_collision)
91+
.def_readonly("tau_ext_hat_filtered",
92+
&franka::RobotState::tau_ext_hat_filtered)
93+
.def_readonly("O_F_ext_hat_K", &franka::RobotState::O_F_ext_hat_K)
94+
.def_readonly("K_F_ext_hat_K", &franka::RobotState::K_F_ext_hat_K)
95+
.def_readonly("O_dP_EE_d", &franka::RobotState::O_dP_EE_d)
96+
.def_readonly("O_ddP_O", &franka::RobotState::O_ddP_O)
97+
.def_readonly("O_T_EE_c", &franka::RobotState::O_T_EE_c)
98+
.def_readonly("O_dP_EE_c", &franka::RobotState::O_dP_EE_c)
99+
.def_readonly("O_ddP_EE_c", &franka::RobotState::O_ddP_EE_c)
100+
.def_readonly("theta", &franka::RobotState::theta)
101+
.def_readonly("dtheta", &franka::RobotState::dtheta)
102+
// .def_readonly("current_errors", &franka::RobotState::current_errors)
103+
// .def_readonly("last_motion_errors",
104+
// &franka::RobotState::last_motion_errors)
105+
.def_readonly("control_command_success_rate",
106+
&franka::RobotState::control_command_success_rate)
107+
// .def_readonly("time", &franka::RobotState::time)
108+
.def_readonly("robot_mode", &franka::RobotState::robot_mode);
109+
44110
py::object robot_state =
45111
(py::object)py::module_::import("rcs").attr("common").attr("RobotState");
46112
py::class_<rcs::hw::FrankaState>(hw, "FrankaState", robot_state)
47-
.def(py::init<>());
113+
.def(py::init<>())
114+
.def_readonly("robot_state", &rcs::hw::FrankaState::robot_state);
48115
py::class_<rcs::hw::FrankaLoad>(hw, "FrankaLoad")
49116
.def(py::init<>())
50117
.def_readwrite("load_mass", &rcs::hw::FrankaLoad::load_mass)

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

Lines changed: 157 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ from __future__ import annotations
77
import typing
88

99
import numpy
10+
import pybind11_stubgen.typing_ext
1011
import rcs._core.common
1112

1213
from . import exceptions
@@ -22,8 +23,17 @@ __all__: list[str] = [
2223
"FrankaState",
2324
"IKSolver",
2425
"PandaConfig",
26+
"RobotMode",
27+
"RobotState",
2528
"exceptions",
2629
"franka_ik",
30+
"kAutomaticErrorRecovery",
31+
"kGuiding",
32+
"kIdle",
33+
"kMove",
34+
"kOther",
35+
"kReflex",
36+
"kUserStopped",
2737
"rcs_ik",
2838
]
2939

@@ -106,9 +116,6 @@ class FrankaLoad:
106116
load_mass: float
107117
def __init__(self) -> None: ...
108118

109-
class FrankaState(rcs._core.common.RobotState):
110-
def __init__(self) -> None: ...
111-
112119
class IKSolver:
113120
"""
114121
Members:
@@ -138,11 +145,158 @@ class IKSolver:
138145
@property
139146
def value(self) -> int: ...
140147

148+
class RobotMode:
149+
"""
150+
Members:
151+
152+
kOther
153+
154+
kIdle
155+
156+
kMove
157+
158+
kGuiding
159+
160+
kReflex
161+
162+
kUserStopped
163+
164+
kAutomaticErrorRecovery
165+
"""
166+
167+
__members__: typing.ClassVar[
168+
dict[str, RobotMode]
169+
] # 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>}
170+
kAutomaticErrorRecovery: typing.ClassVar[RobotMode] # value = <RobotMode.kAutomaticErrorRecovery: 6>
171+
kGuiding: typing.ClassVar[RobotMode] # value = <RobotMode.kGuiding: 3>
172+
kIdle: typing.ClassVar[RobotMode] # value = <RobotMode.kIdle: 1>
173+
kMove: typing.ClassVar[RobotMode] # value = <RobotMode.kMove: 2>
174+
kOther: typing.ClassVar[RobotMode] # value = <RobotMode.kOther: 0>
175+
kReflex: typing.ClassVar[RobotMode] # value = <RobotMode.kReflex: 4>
176+
kUserStopped: typing.ClassVar[RobotMode] # value = <RobotMode.kUserStopped: 5>
177+
def __eq__(self, other: typing.Any) -> bool: ...
178+
def __getstate__(self) -> int: ...
179+
def __hash__(self) -> int: ...
180+
def __index__(self) -> int: ...
181+
def __init__(self, value: int) -> None: ...
182+
def __int__(self) -> int: ...
183+
def __ne__(self, other: typing.Any) -> bool: ...
184+
def __repr__(self) -> str: ...
185+
def __setstate__(self, state: int) -> None: ...
186+
def __str__(self) -> str: ...
187+
@property
188+
def name(self) -> str: ...
189+
@property
190+
def value(self) -> int: ...
191+
192+
class RobotState:
193+
def __init__(self) -> None: ...
194+
@property
195+
def EE_T_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
196+
@property
197+
def F_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
198+
@property
199+
def F_T_NE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
200+
@property
201+
def F_x_Cee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
202+
@property
203+
def F_x_Cload(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
204+
@property
205+
def F_x_Ctotal(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
206+
@property
207+
def I_ee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
208+
@property
209+
def I_load(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
210+
@property
211+
def I_total(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
212+
@property
213+
def K_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
214+
@property
215+
def NE_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
216+
@property
217+
def O_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
218+
@property
219+
def O_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
220+
@property
221+
def O_T_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
222+
@property
223+
def O_T_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
224+
@property
225+
def O_dP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
226+
@property
227+
def O_dP_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
228+
@property
229+
def O_ddP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
230+
@property
231+
def O_ddP_O(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
232+
@property
233+
def cartesian_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
234+
@property
235+
def cartesian_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
236+
@property
237+
def control_command_success_rate(self) -> float: ...
238+
@property
239+
def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
240+
@property
241+
def ddq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
242+
@property
243+
def delbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
244+
@property
245+
def dq(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
246+
@property
247+
def dq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
248+
@property
249+
def dtau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
250+
@property
251+
def dtheta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
252+
@property
253+
def elbow(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
254+
@property
255+
def elbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
256+
@property
257+
def elbow_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
258+
@property
259+
def joint_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
260+
@property
261+
def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
262+
@property
263+
def m_ee(self) -> float: ...
264+
@property
265+
def m_load(self) -> float: ...
266+
@property
267+
def m_total(self) -> float: ...
268+
@property
269+
def q(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
270+
@property
271+
def q_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
272+
@property
273+
def robot_mode(self) -> RobotMode: ...
274+
@property
275+
def tau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
276+
@property
277+
def tau_J_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
278+
@property
279+
def tau_ext_hat_filtered(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
280+
@property
281+
def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
282+
141283
class FR3Config(FrankaConfig):
142284
def __init__(self) -> None: ...
143285

144286
class PandaConfig(FrankaConfig):
145287
def __init__(self) -> None: ...
146288

289+
class FrankaState(rcs._core.common.RobotState):
290+
def __init__(self) -> None: ...
291+
@property
292+
def robot_state(self) -> RobotState: ...
293+
147294
franka_ik: IKSolver # value = <IKSolver.franka_ik: 0>
295+
kAutomaticErrorRecovery: RobotMode # value = <RobotMode.kAutomaticErrorRecovery: 6>
296+
kGuiding: RobotMode # value = <RobotMode.kGuiding: 3>
297+
kIdle: RobotMode # value = <RobotMode.kIdle: 1>
298+
kMove: RobotMode # value = <RobotMode.kMove: 2>
299+
kOther: RobotMode # value = <RobotMode.kOther: 0>
300+
kReflex: RobotMode # value = <RobotMode.kReflex: 4>
301+
kUserStopped: RobotMode # value = <RobotMode.kUserStopped: 5>
148302
rcs_ik: IKSolver # value = <IKSolver.rcs_ik: 1>

‎extensions/rcs_fr3/src/rcs_fr3/envs.py‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,13 +17,22 @@ def __init__(self, env):
1717

1818
def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict]:
1919
try:
20-
return super().step(action)
20+
obs, reward, terminated, truncated, info = super().step(action)
21+
obs = self.get_obs(obs)
22+
return obs, reward, terminated, truncated, info
2123
except hw.exceptions.FrankaControlException as e:
2224
_logger.error("FrankaControlException: %s", e)
2325
self.hw_robot.automatic_error_recovery()
2426
# TODO: this does not work if some wrappers are in between
2527
# FR3HW and RobotEnv
26-
return dict(self.unwrapped.get_obs()), 0, False, True, {}
28+
return self.get_obs(), 0, False, True, {}
29+
30+
def get_obs(self, obs: dict | None = None) -> dict[str, Any]:
31+
if obs is None:
32+
obs = dict(self.unwrapped.get_obs())
33+
robot_state = cast(hw.FrankaState, self.unwrapped.robot.get_state())
34+
obs["robot_state"] = vars(robot_state.robot_state)
35+
return obs
2736

2837
def reset(
2938
self, seed: int | None = None, options: dict[str, Any] | None = None

0 commit comments

Comments
 (0)