Skip to content

Commit be422ff

Browse files
juelgjuelg
committed
feat: add constructors to cpp config structs
1 parent 6c9c740 commit be422ff

18 files changed

Lines changed: 561 additions & 80 deletions

File tree

‎examples/fr3/fr3_direct_control.py‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -97,21 +97,19 @@ def main():
9797
simulation.open_gui()
9898

9999
else:
100-
fr3_cfg = default_fr3_hw_robot_cfg()
100+
fr3_cfg = default_fr3_hw_robot_cfg(ROBOT_IP)
101101
fr3_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
102-
fr3_cfg.ip = ROBOT_IP
103102
ik = rcs.common.Pin(
104103
fr3_cfg.kinematic_model_path,
105104
fr3_cfg.attachment_site,
106105
urdf=fr3_cfg.kinematic_model_path.endswith(".urdf"),
107106
)
108107
robot = hw.Franka(fr3_cfg, ik)
109108

110-
gripper_cfg_hw = hw.FHConfig()
109+
gripper_cfg_hw = hw.FHConfig(ip=ROBOT_IP)
111110
gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
112111
gripper_cfg_hw.speed = 0.1
113112
gripper_cfg_hw.force = 30
114-
gripper_cfg_hw.ip = ROBOT_IP
115113
gripper = hw.FrankaHand(gripper_cfg_hw)
116114
input("the robot is going to move, press enter whenever you are ready")
117115

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

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -32,9 +32,7 @@ enum IKSolver { franka_ik = 0, rcs_ik };
3232
// control
3333
enum Controller { none = 0, jsc, osc, ztc };
3434
struct FrankaConfig : common::RobotConfig {
35-
// TODO: max force and elbow?
36-
// TODO: we can either write specific bindings for each, or we use python
37-
// dictionaries with these objects
35+
std::string ip;
3836
common::RobotType robot_type = common::RobotType::FR3;
3937
common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE;
4038
IKSolver ik_solver = IKSolver::rcs_ik;
@@ -45,7 +43,6 @@ struct FrankaConfig : common::RobotConfig {
4543
bool async_control = false;
4644
bool tcp_offset_configured_in_desk = true;
4745
bool ignore_realtime = false;
48-
std::string ip;
4946
};
5047

5148
struct FR3Config : FrankaConfig {};

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

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,9 @@ int main() {
1919
try {
2020
auto ik =
2121
make_shared<rcs::common::Pin>(mjcf_path, "attachment_site_0", false);
22-
rcs::hw::Franka robot(ip, ik);
22+
rcs::hw::FrankaConfig cfg;
23+
cfg.ip = ip;
24+
rcs::hw::Franka robot(cfg, ik);
2325
robot.automatic_error_recovery();
2426
std::cout << "WARNING: This example will move the robot! "
2527
<< "Please make sure to have the user stop button at hand!"

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

Lines changed: 103 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -126,7 +126,6 @@ PYBIND11_MODULE(_core, m) {
126126
py::object robot_config =
127127
(py::object)py::module_::import("rcs").attr("common").attr("RobotConfig");
128128
py::class_<rcs::hw::FrankaConfig>(hw, "FrankaConfig", robot_config)
129-
.def(py::init<>())
130129
.def_readwrite("ik_solver", &rcs::hw::FrankaConfig::ik_solver)
131130
.def_readwrite("speed_factor", &rcs::hw::FrankaConfig::speed_factor)
132131
.def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters)
@@ -139,16 +138,116 @@ PYBIND11_MODULE(_core, m) {
139138
.def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime)
140139
.def_readwrite("ip", &rcs::hw::FrankaConfig::ip);
141140

141+
rcs::hw::FR3Config default_fr3_config;
142142
py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")
143-
.def(py::init<>());
143+
.def(py::init(
144+
[](const std::string& ip, rcs::hw::IKSolver ik_solver,
145+
double speed_factor,
146+
std::optional<rcs::hw::FrankaLoad> load_parameters,
147+
std::optional<rcs::common::Pose> nominal_end_effector_frame,
148+
std::optional<rcs::common::Pose> world_to_robot,
149+
bool async_control, bool tcp_offset_configured_in_desk,
150+
bool ignore_realtime, rcs::common::Pose tcp_offset,
151+
std::string attachment_site,
152+
std::string kinematic_model_path) {
153+
rcs::hw::FR3Config cfg;
154+
cfg.ik_solver = ik_solver;
155+
cfg.speed_factor = speed_factor;
156+
cfg.load_parameters = load_parameters;
157+
cfg.nominal_end_effector_frame = nominal_end_effector_frame;
158+
cfg.world_to_robot = world_to_robot;
159+
cfg.async_control = async_control;
160+
cfg.tcp_offset_configured_in_desk =
161+
tcp_offset_configured_in_desk;
162+
cfg.ignore_realtime = ignore_realtime;
163+
cfg.ip = ip;
164+
cfg.tcp_offset = tcp_offset;
165+
cfg.attachment_site = attachment_site;
166+
cfg.kinematic_model_path = kinematic_model_path;
167+
return cfg;
168+
}),
169+
py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver,
170+
py::arg("speed_factor") = default_fr3_config.speed_factor,
171+
py::arg("load_parameters") = default_fr3_config.load_parameters,
172+
py::arg("nominal_end_effector_frame") =
173+
default_fr3_config.nominal_end_effector_frame,
174+
py::arg("world_to_robot") = default_fr3_config.world_to_robot,
175+
py::arg("async_control") = default_fr3_config.async_control,
176+
py::arg("tcp_offset_configured_in_desk") =
177+
default_fr3_config.tcp_offset_configured_in_desk,
178+
py::arg("ignore_realtime") = default_fr3_config.ignore_realtime,
179+
py::arg("tcp_offset") = default_fr3_config.tcp_offset,
180+
py::arg("attachment_site") = default_fr3_config.attachment_site,
181+
py::arg("kinematic_model_path") =
182+
default_fr3_config.kinematic_model_path);
183+
rcs::hw::PandaConfig default_panda_config;
144184
py::class_<rcs::hw::PandaConfig, rcs::hw::FrankaConfig>(hw, "PandaConfig")
145-
.def(py::init<>());
185+
.def(py::init(
186+
[](const std::string& ip, rcs::hw::IKSolver ik_solver,
187+
double speed_factor,
188+
std::optional<rcs::hw::FrankaLoad> load_parameters,
189+
std::optional<rcs::common::Pose> nominal_end_effector_frame,
190+
std::optional<rcs::common::Pose> world_to_robot,
191+
bool async_control, bool tcp_offset_configured_in_desk,
192+
bool ignore_realtime, rcs::common::Pose tcp_offset,
193+
std::string attachment_site,
194+
std::string kinematic_model_path) {
195+
rcs::hw::PandaConfig cfg;
196+
cfg.ik_solver = ik_solver;
197+
cfg.speed_factor = speed_factor;
198+
cfg.load_parameters = load_parameters;
199+
cfg.nominal_end_effector_frame = nominal_end_effector_frame;
200+
cfg.world_to_robot = world_to_robot;
201+
cfg.async_control = async_control;
202+
cfg.tcp_offset_configured_in_desk =
203+
tcp_offset_configured_in_desk;
204+
cfg.ignore_realtime = ignore_realtime;
205+
cfg.ip = ip;
206+
cfg.tcp_offset = tcp_offset;
207+
cfg.attachment_site = attachment_site;
208+
cfg.kinematic_model_path = kinematic_model_path;
209+
return cfg;
210+
}),
211+
py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver,
212+
py::arg("speed_factor") = default_panda_config.speed_factor,
213+
py::arg("load_parameters") = default_panda_config.load_parameters,
214+
py::arg("nominal_end_effector_frame") =
215+
default_panda_config.nominal_end_effector_frame,
216+
py::arg("world_to_robot") = default_panda_config.world_to_robot,
217+
py::arg("async_control") = default_panda_config.async_control,
218+
py::arg("tcp_offset_configured_in_desk") =
219+
default_panda_config.tcp_offset_configured_in_desk,
220+
py::arg("ignore_realtime") = default_panda_config.ignore_realtime,
221+
py::arg("tcp_offset") = default_panda_config.tcp_offset,
222+
py::arg("attachment_site") = default_panda_config.attachment_site,
223+
py::arg("kinematic_model_path") =
224+
default_panda_config.kinematic_model_path);
146225

147226
py::object gripper_config =
148227
(py::object)py::module_::import("rcs").attr("common").attr(
149228
"GripperConfig");
229+
rcs::hw::FHConfig default_gripper_config;
150230
py::class_<rcs::hw::FHConfig>(hw, "FHConfig", gripper_config)
151-
.def(py::init<>())
231+
.def(py::init([](const std::string& ip, double grasping_width,
232+
double speed, double force, double epsilon_inner,
233+
double epsilon_outer, bool async_control) {
234+
rcs::hw::FHConfig cfg;
235+
cfg.ip = ip;
236+
cfg.grasping_width = grasping_width;
237+
cfg.speed = speed;
238+
cfg.force = force;
239+
cfg.epsilon_inner = epsilon_inner;
240+
cfg.epsilon_outer = epsilon_outer;
241+
cfg.async_control = async_control;
242+
return cfg;
243+
}),
244+
py::arg("ip"),
245+
py::arg("grasping_width") = default_gripper_config.grasping_width,
246+
py::arg("speed") = default_gripper_config.speed,
247+
py::arg("force") = default_gripper_config.force,
248+
py::arg("epsilon_inner") = default_gripper_config.epsilon_inner,
249+
py::arg("epsilon_outer") = default_gripper_config.epsilon_outer,
250+
py::arg("async_control") = default_gripper_config.async_control)
152251
.def_readwrite("ip", &rcs::hw::FHConfig::ip)
153252
.def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width)
154253
.def_readwrite("speed", &rcs::hw::FHConfig::speed)

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

Lines changed: 40 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,16 @@ class FHConfig(rcs._core.common.GripperConfig):
4545
grasping_width: float
4646
ip: str
4747
speed: float
48-
def __init__(self) -> None: ...
48+
def __init__(
49+
self,
50+
ip: str,
51+
grasping_width: float = 0.05,
52+
speed: float = 0.1,
53+
force: float = 5.0,
54+
epsilon_inner: float = 0.005,
55+
epsilon_outer: float = 0.005,
56+
async_control: bool = False,
57+
) -> None: ...
4958

5059
class FHState(rcs._core.common.GripperState):
5160
def __init__(self) -> None: ...
@@ -103,7 +112,6 @@ class FrankaConfig(rcs._core.common.RobotConfig):
103112
speed_factor: float
104113
tcp_offset_configured_in_desk: bool
105114
world_to_robot: rcs._core.common.Pose | None
106-
def __init__(self) -> None: ...
107115

108116
class FrankaHand(rcs._core.common.Gripper):
109117
def __init__(self, cfg: FHConfig) -> None: ...
@@ -285,10 +293,38 @@ class RobotState:
285293
def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
286294

287295
class FR3Config(FrankaConfig):
288-
def __init__(self) -> None: ...
296+
def __init__(
297+
self,
298+
ip: str,
299+
ik_solver: IKSolver = ...,
300+
speed_factor: float = 0.2,
301+
load_parameters: FrankaLoad | None = None,
302+
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
303+
world_to_robot: rcs._core.common.Pose | None = None,
304+
async_control: bool = False,
305+
tcp_offset_configured_in_desk: bool = True,
306+
ignore_realtime: bool = False,
307+
tcp_offset: rcs._core.common.Pose = ...,
308+
attachment_site: str = "attachment_site",
309+
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
310+
) -> None: ...
289311

290312
class PandaConfig(FrankaConfig):
291-
def __init__(self) -> None: ...
313+
def __init__(
314+
self,
315+
ip: str,
316+
ik_solver: IKSolver = ...,
317+
speed_factor: float = 0.2,
318+
load_parameters: FrankaLoad | None = None,
319+
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
320+
world_to_robot: rcs._core.common.Pose | None = None,
321+
async_control: bool = False,
322+
tcp_offset_configured_in_desk: bool = True,
323+
ignore_realtime: bool = False,
324+
tcp_offset: rcs._core.common.Pose = ...,
325+
attachment_site: str = "attachment_site",
326+
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
327+
) -> None: ...
292328

293329
class FrankaState(rcs._core.common.RobotState):
294330
def __init__(self) -> None: ...

‎extensions/rcs_fr3/src/rcs_fr3/creators.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -192,9 +192,9 @@ def __call__( # type: ignore
192192
ip=robot_ip,
193193
camera_set=camera_set,
194194
control_mode=control_mode,
195-
robot_cfg=default_fr3_hw_robot_cfg(),
195+
robot_cfg=default_fr3_hw_robot_cfg(robot_ip),
196196
collision_guard=None,
197-
gripper_cfg=default_fr3_hw_gripper_cfg() if gripper else None,
197+
gripper_cfg=default_fr3_hw_gripper_cfg(robot_ip) if gripper else None,
198198
max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None,
199199
relative_to=RelativeTo.LAST_STEP,
200200
)

‎extensions/rcs_fr3/src/rcs_fr3/desk.py‎

Lines changed: 4 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -47,12 +47,10 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
4747

4848
def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False):
4949
with Desk.fci(ip, username, password, unlock=unlock):
50-
robot_cfg = default_fr3_hw_robot_cfg()
50+
robot_cfg = default_fr3_hw_robot_cfg(ip)
5151
robot_cfg.speed_factor = 0.2
52-
robot_cfg.ip = ip
5352
f = rcs_fr3.hw.Franka(robot_cfg)
54-
config_hand = rcs_fr3.hw.FHConfig()
55-
config_hand.ip = ip
53+
config_hand = rcs_fr3.hw.FHConfig(ip=ip)
5654
g = rcs_fr3.hw.FrankaHand(config_hand)
5755
if shut:
5856
g.shut()
@@ -63,18 +61,16 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False
6361

6462
def info(ip: str, username: str, password: str, include_hand: bool = False):
6563
with Desk.fci(ip, username, password):
66-
robot_cfg = rcs_fr3.hw.FR3Config()
64+
robot_cfg = rcs_fr3.hw.FR3Config(ip=ip)
6765
robot_cfg.speed_factor = 0.2
68-
robot_cfg.ip = ip
6966
f = rcs_fr3.hw.Franka(robot_cfg)
7067
print("Robot info:")
7168
print("Current cartesian position:")
7269
print(f.get_cartesian_position())
7370
print("Current joint position:")
7471
print(f.get_joint_position())
7572
if include_hand:
76-
config_hand = default_fr3_hw_gripper_cfg()
77-
config_hand.ip = ip
73+
config_hand = default_fr3_hw_gripper_cfg(ip)
7874
g = rcs_fr3.hw.FrankaHand(config_hand)
7975
print("Gripper info:")
8076
print("Current normalized width:")

‎extensions/rcs_fr3/src/rcs_fr3/utils.py‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44
from rcs import common
55

66

7-
def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config:
8-
robot_cfg = hw.FR3Config()
7+
def default_fr3_hw_robot_cfg(ip: str = "", async_control: bool = False) -> hw.FR3Config:
8+
robot_cfg = hw.FR3Config(ip=ip)
99
robot_cfg.robot_type = rcs.scenes["fr3_empty_world"].robot_type
1010
robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot
1111
robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset())
@@ -16,8 +16,8 @@ def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config:
1616
return robot_cfg
1717

1818

19-
def default_fr3_hw_gripper_cfg(async_control: bool = False) -> hw.FHConfig:
20-
gripper_cfg = hw.FHConfig()
19+
def default_fr3_hw_gripper_cfg(ip: str = "", async_control: bool = False) -> hw.FHConfig:
20+
gripper_cfg = hw.FHConfig(ip=ip)
2121
gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
2222
gripper_cfg.speed = 0.1
2323
gripper_cfg.force = 30

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

Lines changed: 40 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,16 @@ class FHConfig(rcs._core.common.GripperConfig):
4545
grasping_width: float
4646
ip: str
4747
speed: float
48-
def __init__(self) -> None: ...
48+
def __init__(
49+
self,
50+
ip: str,
51+
grasping_width: float = 0.05,
52+
speed: float = 0.1,
53+
force: float = 5.0,
54+
epsilon_inner: float = 0.005,
55+
epsilon_outer: float = 0.005,
56+
async_control: bool = False,
57+
) -> None: ...
4958

5059
class FHState(rcs._core.common.GripperState):
5160
def __init__(self) -> None: ...
@@ -103,7 +112,6 @@ class FrankaConfig(rcs._core.common.RobotConfig):
103112
speed_factor: float
104113
tcp_offset_configured_in_desk: bool
105114
world_to_robot: rcs._core.common.Pose | None
106-
def __init__(self) -> None: ...
107115

108116
class FrankaHand(rcs._core.common.Gripper):
109117
def __init__(self, cfg: FHConfig) -> None: ...
@@ -285,10 +293,38 @@ class RobotState:
285293
def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
286294

287295
class FR3Config(FrankaConfig):
288-
def __init__(self) -> None: ...
296+
def __init__(
297+
self,
298+
ip: str,
299+
ik_solver: IKSolver = ...,
300+
speed_factor: float = 0.2,
301+
load_parameters: FrankaLoad | None = None,
302+
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
303+
world_to_robot: rcs._core.common.Pose | None = None,
304+
async_control: bool = False,
305+
tcp_offset_configured_in_desk: bool = True,
306+
ignore_realtime: bool = False,
307+
tcp_offset: rcs._core.common.Pose = ...,
308+
attachment_site: str = "attachment_site",
309+
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
310+
) -> None: ...
289311

290312
class PandaConfig(FrankaConfig):
291-
def __init__(self) -> None: ...
313+
def __init__(
314+
self,
315+
ip: str,
316+
ik_solver: IKSolver = ...,
317+
speed_factor: float = 0.2,
318+
load_parameters: FrankaLoad | None = None,
319+
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
320+
world_to_robot: rcs._core.common.Pose | None = None,
321+
async_control: bool = False,
322+
tcp_offset_configured_in_desk: bool = True,
323+
ignore_realtime: bool = False,
324+
tcp_offset: rcs._core.common.Pose = ...,
325+
attachment_site: str = "attachment_site",
326+
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
327+
) -> None: ...
292328

293329
class FrankaState(rcs._core.common.RobotState):
294330
def __init__(self) -> None: ...

0 commit comments

Comments
 (0)