Skip to content

Commit 8db13d3

Browse files
juelgjuelg
committed
feat: add franka state
1 parent 7c4d48a commit 8db13d3

File tree

4 files changed

+322
-7
lines changed

4 files changed

+322
-7
lines changed

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

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

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

‎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>
@@ -55,7 +56,9 @@ struct PandaConfig : FrankaConfig {
5556
common::RobotType robot_type = common::RobotType::Panda;
5657
};
5758

58-
struct FrankaState : common::RobotState {};
59+
struct FrankaState : common::RobotState {
60+
franka::RobotState robot_state;
61+
};
5962

6063
class Franka : public common::Robot {
6164
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("robot_mode", &franka::RobotState::robot_mode)
108+
.def_readonly("time", &franka::RobotState::time);
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: 241 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

@@ -114,9 +124,6 @@ class FrankaLoad:
114124
load_mass: float
115125
def __init__(self) -> None: ...
116126

117-
class FrankaState(rcs._core.common.RobotState):
118-
def __init__(self) -> None: ...
119-
120127
class IKSolver:
121128
"""
122129
Members:
@@ -146,11 +153,242 @@ class IKSolver:
146153
@property
147154
def value(self) -> int: ...
148155

156+
class RobotMode:
157+
"""
158+
Members:
159+
160+
kOther
161+
162+
kIdle
163+
164+
kMove
165+
166+
kGuiding
167+
168+
kReflex
169+
170+
kUserStopped
171+
172+
kAutomaticErrorRecovery
173+
"""
174+
175+
__members__: typing.ClassVar[
176+
dict[str, RobotMode]
177+
] # 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>}
178+
kAutomaticErrorRecovery: typing.ClassVar[RobotMode] # value = <RobotMode.kAutomaticErrorRecovery: 6>
179+
kGuiding: typing.ClassVar[RobotMode] # value = <RobotMode.kGuiding: 3>
180+
kIdle: typing.ClassVar[RobotMode] # value = <RobotMode.kIdle: 1>
181+
kMove: typing.ClassVar[RobotMode] # value = <RobotMode.kMove: 2>
182+
kOther: typing.ClassVar[RobotMode] # value = <RobotMode.kOther: 0>
183+
kReflex: typing.ClassVar[RobotMode] # value = <RobotMode.kReflex: 4>
184+
kUserStopped: typing.ClassVar[RobotMode] # value = <RobotMode.kUserStopped: 5>
185+
def __eq__(self, other: typing.Any) -> bool: ...
186+
def __getstate__(self) -> int: ...
187+
def __hash__(self) -> int: ...
188+
def __index__(self) -> int: ...
189+
def __init__(self, value: int) -> None: ...
190+
def __int__(self) -> int: ...
191+
def __ne__(self, other: typing.Any) -> bool: ...
192+
def __repr__(self) -> str: ...
193+
def __setstate__(self, state: int) -> None: ...
194+
def __str__(self) -> str: ...
195+
@property
196+
def name(self) -> str: ...
197+
@property
198+
def value(self) -> int: ...
199+
200+
class RobotState:
201+
def __init__(self) -> None: ...
202+
@property
203+
def EE_T_K(
204+
self,
205+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
206+
@property
207+
def F_T_EE(
208+
self,
209+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
210+
@property
211+
def F_T_NE(
212+
self,
213+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
214+
@property
215+
def F_x_Cee(
216+
self,
217+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
218+
@property
219+
def F_x_Cload(
220+
self,
221+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
222+
@property
223+
def F_x_Ctotal(
224+
self,
225+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
226+
@property
227+
def I_ee(
228+
self,
229+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
230+
@property
231+
def I_load(
232+
self,
233+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
234+
@property
235+
def I_total(
236+
self,
237+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
238+
@property
239+
def K_F_ext_hat_K(
240+
self,
241+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
242+
@property
243+
def NE_T_EE(
244+
self,
245+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
246+
@property
247+
def O_F_ext_hat_K(
248+
self,
249+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
250+
@property
251+
def O_T_EE(
252+
self,
253+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
254+
@property
255+
def O_T_EE_c(
256+
self,
257+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
258+
@property
259+
def O_T_EE_d(
260+
self,
261+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
262+
@property
263+
def O_dP_EE_c(
264+
self,
265+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
266+
@property
267+
def O_dP_EE_d(
268+
self,
269+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
270+
@property
271+
def O_ddP_EE_c(
272+
self,
273+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
274+
@property
275+
def O_ddP_O(
276+
self,
277+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
278+
@property
279+
def cartesian_collision(
280+
self,
281+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
282+
@property
283+
def cartesian_contact(
284+
self,
285+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
286+
@property
287+
def control_command_success_rate(self) -> float: ...
288+
@property
289+
def current_errors(self) -> ...: ...
290+
@property
291+
def ddelbow_c(
292+
self,
293+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
294+
@property
295+
def ddq_d(
296+
self,
297+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
298+
@property
299+
def delbow_c(
300+
self,
301+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
302+
@property
303+
def dq(
304+
self,
305+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
306+
@property
307+
def dq_d(
308+
self,
309+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
310+
@property
311+
def dtau_J(
312+
self,
313+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
314+
@property
315+
def dtheta(
316+
self,
317+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
318+
@property
319+
def elbow(
320+
self,
321+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
322+
@property
323+
def elbow_c(
324+
self,
325+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
326+
@property
327+
def elbow_d(
328+
self,
329+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
330+
@property
331+
def joint_collision(
332+
self,
333+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
334+
@property
335+
def joint_contact(
336+
self,
337+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
338+
@property
339+
def last_motion_errors(self) -> ...: ...
340+
@property
341+
def m_ee(self) -> float: ...
342+
@property
343+
def m_load(self) -> float: ...
344+
@property
345+
def m_total(self) -> float: ...
346+
@property
347+
def q(
348+
self,
349+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
350+
@property
351+
def q_d(
352+
self,
353+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
354+
@property
355+
def robot_mode(self) -> RobotMode: ...
356+
@property
357+
def tau_J(
358+
self,
359+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
360+
@property
361+
def tau_J_d(
362+
self,
363+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
364+
@property
365+
def tau_ext_hat_filtered(
366+
self,
367+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
368+
@property
369+
def theta(
370+
self,
371+
) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
372+
@property
373+
def time(self) -> ...: ...
374+
149375
class FR3Config(FrankaConfig):
150376
def __init__(self) -> None: ...
151377

152378
class PandaConfig(FrankaConfig):
153379
def __init__(self) -> None: ...
154380

381+
class FrankaState(rcs._core.common.RobotState):
382+
def __init__(self) -> None: ...
383+
@property
384+
def robot_state(self) -> RobotState: ...
385+
155386
franka_ik: IKSolver # value = <IKSolver.franka_ik: 0>
387+
kAutomaticErrorRecovery: RobotMode # value = <RobotMode.kAutomaticErrorRecovery: 6>
388+
kGuiding: RobotMode # value = <RobotMode.kGuiding: 3>
389+
kIdle: RobotMode # value = <RobotMode.kIdle: 1>
390+
kMove: RobotMode # value = <RobotMode.kMove: 2>
391+
kOther: RobotMode # value = <RobotMode.kOther: 0>
392+
kReflex: RobotMode # value = <RobotMode.kReflex: 4>
393+
kUserStopped: RobotMode # value = <RobotMode.kUserStopped: 5>
156394
rcs_ik: IKSolver # value = <IKSolver.rcs_ik: 1>

0 commit comments

Comments
 (0)