Skip to content

Commit 0297dee

Browse files
juelgjuelg
committed
feat: add franka state
1 parent 0ea2572 commit 0297dee

4 files changed

Lines changed: 323 additions & 8 deletions

File tree

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

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -73,9 +73,16 @@ FrankaConfig* Franka::get_config() {
7373
return cfg;
7474
}
7575

76-
FrankaState* Franka::get_state() {
77-
// dummy state until we define a prober state
78-
FrankaState* state = new FrankaState();
76+
FrankaState *Franka::get_state() {
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>
@@ -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)

0 commit comments

Comments
 (0)