Skip to content

Commit eac502b

Browse files
juelgjuelg
committed
refactor!: robot type now object based
- removed meta robot config due to limited extensibility - moved q_home and joint_limits to robot config - refactored robot type: now class based for python extensibility - readded wrapper configs: home_on_reset for robot wrapper and binary from gripper wrapper, they need to be extra configs for composition and not part of the robot/gripper config
1 parent 5ca3d84 commit eac502b

11 files changed

Lines changed: 165 additions & 240 deletions

File tree

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -632,9 +632,13 @@ void Franka::zero_torque_controller() {
632632
void Franka::move_home() {
633633
// sync
634634
this->stop_control_thread();
635+
if (!this->m_cfg.q_home.has_value()) {
636+
std::cerr << "Home position is not defined in the config." << std::endl;
637+
return;
638+
}
635639
FrankaMotionGenerator motion_generator(
636640
this->m_cfg.speed_factor,
637-
common::robots_meta_config.at(this->m_cfg.robot_type).q_home);
641+
this->m_cfg.q_home.value());
638642
this->robot.control(motion_generator);
639643
}
640644

‎extensions/rcs_so101/src/rcs_so101/hw.py‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -71,10 +71,10 @@ def _get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[n
7171
joints_in_rad = (
7272
joints_normalized
7373
* (
74-
common.robots_meta_config(common.RobotType.SO101).joint_limits[1]
75-
- common.robots_meta_config(common.RobotType.SO101).joint_limits[0]
74+
self._robot_config.joint_limits[1]
75+
- self._robot_config.joint_limits[0]
7676
)
77-
+ common.robots_meta_config(common.RobotType.SO101).joint_limits[0]
77+
+ self._robot_config.joint_limits[0]
7878
)
7979
self._last_joint = joints_in_rad
8080
return joints_in_rad
@@ -92,7 +92,7 @@ def get_state(self) -> common.RobotState:
9292
def move_home(self) -> None:
9393
home = typing.cast(
9494
np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]],
95-
common.robots_meta_config(common.RobotType.SO101).q_home,
95+
self._robot_config.q_home,
9696
)
9797
print("move home", home)
9898
self.set_joint_position(home)
@@ -108,9 +108,9 @@ def set_cartesian_position(self, pose: common.Pose) -> None:
108108

109109
def _set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore
110110
self._last_joint = q
111-
q_normalized = (q - common.robots_meta_config(common.RobotType.SO101).joint_limits[0]) / (
112-
common.robots_meta_config(common.RobotType.SO101).joint_limits[1]
113-
- common.robots_meta_config(common.RobotType.SO101).joint_limits[0]
111+
q_normalized = (q - self._robot_config.joint_limits[0]) / (
112+
self._robot_config.joint_limits[1]
113+
- self._robot_config.joint_limits[0]
114114
)
115115
q_hf = (q_normalized * 200) - 100
116116
self._hf_robot.send_action(

‎extensions/rcs_ur5e/src/rcs_ur5e/hw.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -306,7 +306,7 @@ def set_joint_position(self, q: np.ndarray[tuple[typing.Any], np.dtype[np.float6
306306
def move_home(self) -> None:
307307
home = typing.cast(
308308
np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]],
309-
common.robots_meta_config(common.RobotType.UR5e).q_home,
309+
self._config.q_home,
310310
)
311311
if np.any((home < -2 * np.pi) | (home > 2 * np.pi)):
312312
msg = f"Home position {home} is out of bounds."

‎extensions/rcs_xarm7/src/rcs_xarm7/hw.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ def get_state(self) -> common.RobotState:
8181
def move_home(self) -> None:
8282
home = typing.cast(
8383
np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]],
84-
common.robots_meta_config(common.RobotType.XArm7).q_home,
84+
self._config.q_home,
8585
)
8686
# self.set_joint_position(home)
8787
self._xarm.set_mode(0)

‎include/rcs/Robot.h‎

Lines changed: 53 additions & 101 deletions
Original file line numberDiff line numberDiff line change
@@ -12,125 +12,77 @@
1212
namespace rcs {
1313
namespace common {
1414

15-
struct RobotMetaConfig {
16-
VectorXd q_home;
17-
int dof;
18-
Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor> joint_limits;
19-
};
2015

21-
enum RobotType { FR3 = 0, UR5e, SO101, XArm7, Panda };
2216
enum RobotPlatform { SIMULATION = 0, HARDWARE };
2317

24-
static const std::unordered_map<RobotType, RobotMetaConfig> robots_meta_config =
25-
{{// -------------- FR3 --------------
26-
{FR3,
27-
RobotMetaConfig{
28-
// q_home:
29-
(VectorXd(7) << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2,
30-
M_PI_4)
31-
.finished(),
32-
// dof:
33-
7,
34-
// joint_limits:
35-
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
36-
// low 7‐tuple
37-
-2.3093,
38-
-1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895,
39-
// high 7‐tuple
40-
2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895)
41-
.finished()}},
42-
{Panda,
43-
RobotMetaConfig{
44-
// q_home:
45-
(VectorXd(7) << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2,
46-
M_PI_4)
47-
.finished(),
48-
// dof:
49-
7,
50-
// joint_limits:
51-
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
52-
// low 7‐tuple
53-
-166. / 180. * M_PI, -101. / 180. * M_PI, -166. / 180. * M_PI, -176. / 180. * M_PI, -166. / 180. * M_PI, -1. / 180. * M_PI, -166. / 180. * M_PI,
54-
// high 7‐tuple
55-
166. / 180. * M_PI, 101. / 180. * M_PI, 166. / 180. * M_PI, -4. / 180. * M_PI, 166. / 180. * M_PI, 215. / 180. * M_PI, 166. / 180. * M_PI
56-
)
57-
.finished()}},
58-
59-
// -------------- UR5e --------------
60-
{UR5e,
61-
RobotMetaConfig{
62-
// q_home (6‐vector):
63-
(VectorXd(6) << 0.0, -2.02711196, 1.64630026, -1.18999615,
64-
-1.57079762, 0.0)
65-
.finished(),
66-
// dof:
67-
6,
68-
// joint_limits (2×6):
69-
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 6) <<
70-
// low 6‐tuple
71-
-2 * M_PI,
72-
-2 * M_PI, -1 * M_PI, -2 * M_PI, -2 * M_PI, -2 * M_PI,
73-
// high 6‐tuple
74-
2 * M_PI, 2 * M_PI, 1 * M_PI, 2 * M_PI, 2 * M_PI, 2 * M_PI)
75-
.finished()}},
76-
// -------------- XArm7 --------------
77-
{XArm7, RobotMetaConfig{
78-
// q_home (7‐vector):
79-
(VectorXd(7) << 0,
80-
-45. / 180. * M_PI,
81-
0,
82-
15. / 180. * M_PI,
83-
0,
84-
-25. / 180. * M_PI,
85-
0
86-
).finished(),
87-
// dof:
88-
7,
89-
// joint_limits (2×7):
90-
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
91-
// low 7‐tuple
92-
-2 * M_PI, -2.094395, -2 * M_PI, -3.92699, -2 * M_PI, -M_PI, -2 * M_PI,
93-
// high 7‐tuple
94-
2 * M_PI, 2.059488, 2 * M_PI, 0.191986, 2 * M_PI, 1.692969, 2 * M_PI).finished()}},
95-
// -------------- SO101 --------------
96-
{SO101,
97-
RobotMetaConfig{
98-
// q_home (5‐vector):
99-
// (VectorXd(5) << -9.40612320177057, -99.66130397967824,
100-
// 99.9124726477024, 69.96996996996998, -9.095744680851055)
101-
// .finished(),
102-
(VectorXd(5) << -0.01914898, -1.90521916, 1.56476701, 1.04783839, -1.40323926)
103-
.finished(),
104-
// dof:
105-
5,
106-
// joint_limits (2×5):
107-
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 5) <<
108-
// low 5‐tuple
109-
-1.9198621771937616,
110-
-1.9198621771937634, -1.7453292519943295, -1.6580627969561903,
111-
-2.7925268969992407,
112-
113-
// high 5‐tuple
114-
1.9198621771937616, 1.9198621771937634, 1.5707963267948966,
115-
1.6580627969561903, 2.7925268969992407)
116-
.finished()}}}};
18+
template <typename Derived>
19+
struct TypeBase {
20+
std::string id;
21+
22+
// Each subclass gets its own unique static map
23+
static std::map<std::string, const Derived*>& registry() {
24+
static std::map<std::string, const Derived*> _registry;
25+
return _registry;
26+
}
27+
28+
// Constructor automatically registers the instance
29+
TypeBase(std::string id_) : id(std::move(id_)) {
30+
auto& reg = registry();
31+
if (reg.find(id) == reg.end()) {
32+
reg[id] = static_cast<const Derived*>(this);
33+
}
34+
}
35+
36+
// Returns all registered instances of this specific type
37+
static std::vector<Derived> get_all() {
38+
std::vector<Derived> all;
39+
for (const auto& [key, ptr] : registry()) {
40+
all.push_back(*ptr);
41+
}
42+
return all;
43+
}
44+
45+
bool operator==(const TypeBase& other) const { return id == other.id; }
46+
};
47+
struct RobotType : public TypeBase<RobotType> {
48+
using TypeBase::TypeBase; // Inherit the constructor
49+
50+
static const RobotType FR3;
51+
};
52+
inline const RobotType RobotType::FR3{"FR3"};
11753

11854
struct RobotConfig {
11955
RobotType robot_type = RobotType::FR3;
12056
RobotPlatform robot_platform = RobotPlatform::SIMULATION;
12157
rcs::common::Pose tcp_offset = rcs::common::Pose::Identity();
12258
std::string attachment_site = "attachment_site";
12359
std::string kinematic_model_path = "assets/scenes/fr3_empty_world/robot.xml";
124-
bool home_on_reset = true;
12560
std::optional<VectorXd> q_home = std::nullopt;
61+
size_t dof = 7;
62+
Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor> joint_limits =
63+
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
64+
// low 7‐tuple
65+
-2.3093,
66+
-1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895,
67+
// high 7‐tuple
68+
2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895)
69+
.finished();
12670
virtual ~RobotConfig(){};
12771
};
12872
struct RobotState {
12973
virtual ~RobotState(){};
13074
};
13175

76+
77+
struct GripperType : public TypeBase<GripperType> {
78+
using TypeBase::TypeBase;
79+
80+
static const GripperType FrankaHand;
81+
};
82+
inline const GripperType GripperType::FrankaHand{"FrankaHand"};
83+
13284
struct GripperConfig {
133-
bool binary = true;
85+
GripperType gripper_type = GripperType::FrankaHand;
13486
virtual ~GripperConfig(){};
13587
};
13688
struct GripperState {

0 commit comments

Comments
 (0)