Skip to content

Commit a9b15a3

Browse files
juelgjuelg
committed
refactor: introduce close method for cleanup
- added close method in robot, gripper and hand for cleanup (is called by the env's context manager) - fixed the sim example of xarm - removed a wrongly added cpp file in xarm
1 parent 151ccd9 commit a9b15a3

18 files changed

Lines changed: 133 additions & 100 deletions

File tree

‎examples/xarm7_env_cartesian_control.py‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
import logging
2-
from time import sleep
32

4-
import numpy as np
53
from rcs.envs.base import ControlMode, RelativeTo
64
from rcs.envs.creators import SimEnvCreator
75
from rcs.envs.utils import get_tcp_offset

‎examples/xarm7_env_joint_control.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11
import logging
2-
from time import sleep
32

43
import numpy as np
54
from rcs.envs.base import ControlMode, RelativeTo

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,7 @@ class FR3 : public common::Robot {
113113
common::Pose get_base_pose_in_world_coordinates() override;
114114

115115
void reset() override;
116+
void close() override{};
116117
};
117118
} // namespace hw
118119
} // namespace rcs

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,7 @@ class FrankaHand : public common::Gripper {
7676
void grasp() override;
7777
void open() override;
7878
void shut() override;
79+
void close() override{};
7980
};
8081
} // namespace hw
8182
} // namespace rcs

‎extensions/rcs_xarm7/src/rcs_xarm7/creators.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper
2020
from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig
2121
from rcs_xarm7.hw import XArm7
22-
from xarm.wrapper import XArmAPI
2322

2423
import rcs
2524
from rcs import common, sim

‎extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py‎

Lines changed: 38 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,14 @@
11
import logging
2-
from time import sleep
32

3+
import numpy as np
44
from rcs._core.common import RobotPlatform
55
from rcs.envs.base import ControlMode, RelativeTo
6-
from rcs_fr3.desk import ContextManager
6+
from rcs.envs.creators import SimEnvCreator
7+
from rcs.envs.utils import get_tcp_offset
78
from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
89

910
import rcs
1011
from rcs import sim
11-
import numpy as np
1212

1313
logger = logging.getLogger(__name__)
1414
logger.setLevel(logging.INFO)
@@ -25,43 +25,51 @@ def main():
2525
control_mode=ControlMode.CARTESIAN_TQuat,
2626
ip=ROBOT_IP,
2727
relative_to=RelativeTo.LAST_STEP,
28-
max_relative_movement=np.deg2rad(3)
28+
max_relative_movement=np.deg2rad(3),
2929
)
30-
cm = env_rel.unwrapped.robot
3130
else:
32-
cm = ContextManager()
33-
cfg = sim.SimRobotConfig()
34-
cfg.robot_type = rcs.common.RobotType.XArm7
35-
cfg.actuators = ["1", "2", "3", "4", "5"]
36-
cfg.joints = ["1", "2", "3", "4", "5"]
37-
cfg.arm_collision_geoms = []
38-
cfg.attachment_site = "gripper"
39-
40-
grp_cfg = sim.SimGripperConfig()
41-
grp_cfg.actuator = "6"
42-
grp_cfg.joint = "6"
43-
grp_cfg.collision_geoms = []
44-
grp_cfg.collision_geoms_fingers = []
45-
46-
env_rel = XArm7SimEnvCreator()(
47-
control_mode=ControlMode.JOINTS,
48-
urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf",
49-
robot_cfg=cfg,
31+
robot_cfg = sim.SimRobotConfig()
32+
robot_cfg.actuators = [
33+
"act1",
34+
"act2",
35+
"act3",
36+
"act4",
37+
"act5",
38+
"act6",
39+
"act7",
40+
]
41+
robot_cfg.joints = [
42+
"joint1",
43+
"joint2",
44+
"joint3",
45+
"joint4",
46+
"joint5",
47+
"joint6",
48+
"joint7",
49+
]
50+
robot_cfg.base = "base"
51+
robot_cfg.robot_type = rcs.common.RobotType.XArm7
52+
robot_cfg.attachment_site = "attachment_site"
53+
robot_cfg.arm_collision_geoms = []
54+
robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"]["mjcf_robot"])
55+
env_rel = SimEnvCreator()(
56+
control_mode=ControlMode.CARTESIAN_TQuat,
5057
collision_guard=False,
51-
mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/XArm7/scene.xml",
52-
gripper_cfg=grp_cfg,
53-
# camera_set_cfg=default_mujoco_cameraset_cfg(),
54-
max_relative_movement=None,
55-
# max_relative_movement=10.0,
56-
# max_relative_movement=0.5,
58+
robot_cfg=robot_cfg,
59+
gripper_cfg=None,
60+
# cameras=default_mujoco_cameraset_cfg(),
61+
max_relative_movement=0.5,
5762
relative_to=RelativeTo.LAST_STEP,
63+
mjcf=rcs.scenes["xarm7_empty_world"]["mjb"],
64+
robot_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"],
5865
)
5966
env_rel.get_wrapper_attr("sim").open_gui()
67+
6068
env_rel.reset()
6169
act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0}
6270
obs, reward, terminated, truncated, info = env_rel.step(act)
6371

64-
with cm:
72+
with env_rel:
6573
for _ in range(10):
6674
for _ in range(10):
6775
# move 1cm in x direction (forward) and close gripper

‎extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py‎

Lines changed: 36 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,15 @@
11
import logging
22
from time import sleep
33

4+
import numpy as np
45
from rcs._core.common import RobotPlatform
56
from rcs.envs.base import ControlMode, RelativeTo
6-
from rcs_fr3.desk import ContextManager
7+
from rcs.envs.creators import SimEnvCreator
8+
from rcs.envs.utils import get_tcp_offset
79
from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
810

911
import rcs
1012
from rcs import sim
11-
import numpy as np
1213

1314
logger = logging.getLogger(__name__)
1415
logger.setLevel(logging.INFO)
@@ -25,40 +26,47 @@ def main():
2526
control_mode=ControlMode.JOINTS,
2627
ip=ROBOT_IP,
2728
relative_to=RelativeTo.LAST_STEP,
28-
max_relative_movement=np.deg2rad(3)
29+
max_relative_movement=np.deg2rad(3),
2930
)
30-
cm = env_rel.unwrapped.robot
3131
else:
32-
cm = ContextManager()
33-
cfg = sim.SimRobotConfig()
34-
cfg.robot_type = rcs.common.RobotType.XArm7
35-
cfg.actuators = ["1", "2", "3", "4", "5"]
36-
cfg.joints = ["1", "2", "3", "4", "5"]
37-
cfg.arm_collision_geoms = []
38-
cfg.attachment_site = "gripper"
39-
40-
grp_cfg = sim.SimGripperConfig()
41-
grp_cfg.actuator = "6"
42-
grp_cfg.joint = "6"
43-
grp_cfg.collision_geoms = []
44-
grp_cfg.collision_geoms_fingers = []
45-
46-
env_rel = XArm7SimEnvCreator()(
32+
robot_cfg = sim.SimRobotConfig()
33+
robot_cfg.actuators = [
34+
"act1",
35+
"act2",
36+
"act3",
37+
"act4",
38+
"act5",
39+
"act6",
40+
"act7",
41+
]
42+
robot_cfg.joints = [
43+
"joint1",
44+
"joint2",
45+
"joint3",
46+
"joint4",
47+
"joint5",
48+
"joint6",
49+
"joint7",
50+
]
51+
robot_cfg.base = "base"
52+
robot_cfg.robot_type = rcs.common.RobotType.XArm7
53+
robot_cfg.attachment_site = "attachment_site"
54+
robot_cfg.arm_collision_geoms = []
55+
robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"]["mjcf_robot"])
56+
env_rel = SimEnvCreator()(
4757
control_mode=ControlMode.JOINTS,
48-
urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf",
49-
robot_cfg=cfg,
5058
collision_guard=False,
51-
mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/XArm7/scene.xml",
52-
gripper_cfg=grp_cfg,
53-
# camera_set_cfg=default_mujoco_cameraset_cfg(),
54-
max_relative_movement=None,
55-
# max_relative_movement=10.0,
56-
# max_relative_movement=0.5,
59+
robot_cfg=robot_cfg,
60+
gripper_cfg=None,
61+
# cameras=default_mujoco_cameraset_cfg(),
62+
max_relative_movement=np.deg2rad(5),
5763
relative_to=RelativeTo.LAST_STEP,
64+
mjcf=rcs.scenes["xarm7_empty_world"]["mjb"],
65+
robot_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"],
5866
)
5967
env_rel.get_wrapper_attr("sim").open_gui()
6068

61-
with cm:
69+
with env_rel:
6270
for _ in range(10):
6371
obs, info = env_rel.reset()
6472
for _ in range(3):

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

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1-
from dataclasses import dataclass, field
21
import typing
2+
from dataclasses import dataclass, field
33
from typing import List
44

55
import numpy as np
@@ -13,13 +13,14 @@ class XArm7Config(common.RobotConfig):
1313
# some_custom_config: str = "default_value"
1414
payload_weight: float = 0.624
1515
payload_tcp: List[float] = field(default_factory=lambda: [-4.15, 5.24, 76.38])
16+
1617
def __post_init__(self):
1718
super().__init__()
1819

1920

20-
class XArm7():
21+
class XArm7:
2122
def __init__(self, ip: str):
22-
self.ik = None #common.RL(urdf_path=urdf_path)
23+
self.ik = None # common.RL(urdf_path=urdf_path)
2324
self._config = XArm7Config()
2425
self._config.robot_platform = common.RobotPlatform.HARDWARE
2526
self._config.robot_type = common.RobotType.XArm7
@@ -35,11 +36,12 @@ def __init__(self, ip: str):
3536
center_of_gravity=self._config.payload_tcp,
3637
wait=True,
3738
)
38-
39+
3940
def get_cartesian_position(self) -> common.Pose:
4041
code, xyzrpy = self._xarm.get_position(is_radian=True)
4142
if code != 0:
42-
raise RuntimeError("couldn't get cartesian position from xarm")
43+
msg = "couldn't get cartesian position from xarm"
44+
raise RuntimeError(msg)
4345

4446
translation_meter = np.array(xyzrpy[:3], dtype=np.float64) * 0.001
4547
rpy = xyzrpy[3:]
@@ -55,7 +57,7 @@ def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np
5557

5658
def get_parameters(self) -> XArm7Config:
5759
return self._config
58-
60+
5961
def set_parameters(self, robot_cfg: XArm7Config) -> None:
6062
self._config = robot_cfg
6163

@@ -80,8 +82,5 @@ def set_cartesian_position(self, pose: common.Pose) -> None:
8082
def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]) -> None: # type: ignore
8183
self._xarm.set_servo_angle(angle=q, is_radian=True, wait=True)
8284

83-
def __enter__(self):
84-
return self
85-
86-
def __exit__(self, exc_type, exc_value, traceback):
85+
def close(self):
8786
self._xarm.disconnect()

‎extensions/rcs_xarm7/src/rcs_xarm7/xArm7, RobotMetaConfig{.cpp‎

Lines changed: 0 additions & 11 deletions
This file was deleted.

‎include/rcs/Robot.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,8 @@ class Robot {
138138

139139
virtual void reset() = 0;
140140

141+
virtual void close() = 0;
142+
141143
virtual void set_cartesian_position(const Pose& pose) = 0;
142144

143145
virtual std::optional<std::shared_ptr<IK>> get_ik() = 0;
@@ -179,6 +181,9 @@ class Gripper {
179181

180182
// puts the gripper to max position
181183
virtual void reset() = 0;
184+
185+
// closes connection to gripper
186+
virtual void close() = 0;
182187
};
183188

184189
class Hand {
@@ -209,6 +214,9 @@ class Hand {
209214

210215
// puts the hand to max position
211216
virtual void reset() = 0;
217+
218+
// closes connection
219+
virtual void close() = 0;
212220
};
213221

214222
} // namespace common

0 commit comments

Comments
 (0)