Skip to content

Commit cdcba54

Browse files
kennkhrkennkhr
committed
feat: add xArm7 extension
1 parent c247142 commit cdcba54

File tree

9 files changed

+311
-2
lines changed

9 files changed

+311
-2
lines changed

‎extensions/rcs_xarm7/pyproject.toml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
[build-system]
2+
requires = ["setuptools"]
3+
build-backend = "setuptools.build_meta"
4+
5+
[project]
6+
name = "rcs_xarm7"
7+
version = "0.4.0"
8+
description="RCS xArm7 module"
9+
dependencies = [
10+
"rcs==0.4.0",
11+
"xarm-python-sdk==1.17.0",
12+
]
13+
readme = "README.md"
14+
maintainers = [
15+
{ name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
16+
]
17+
authors = [
18+
{ name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
19+
{ name = "Pierre Krack", email = "pierre.krack@utn.de" },
20+
{ name = "Ken Nakahara", email = "knakahara@lasr.org" },
21+
]
22+
requires-python = ">=3.10"
23+
classifiers = [
24+
"Development Status :: 3 - Alpha",
25+
"License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)",
26+
"Programming Language :: Python :: 3",
27+
]
28+

‎extensions/rcs_xarm7/src/rcs_xarm7/__init__.py‎

Whitespace-only changes.

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

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
import logging
2+
from os import PathLike
3+
from pathlib import Path
4+
from typing import Type
5+
6+
import gymnasium as gym
7+
from gymnasium.envs.registration import EnvCreator
8+
9+
from xarm.wrapper import XArmAPI
10+
11+
from rcs.camera.hw import HardwareCameraSet
12+
from rcs.camera.sim import SimCameraSet
13+
from rcs.envs.base import (
14+
CameraSetWrapper,
15+
ControlMode,
16+
GripperWrapper,
17+
RelativeActionSpace,
18+
RelativeTo,
19+
RobotEnv,
20+
)
21+
from rcs.envs.creators import RCSHardwareEnvCreator
22+
from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper
23+
from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig
24+
from rcs_xarm7.hw import XArm7
25+
26+
import rcs
27+
from rcs import common, sim
28+
29+
logger = logging.getLogger(__name__)
30+
logger.setLevel(logging.INFO)
31+
32+
33+
class RCSXArm7EnvCreator(RCSHardwareEnvCreator):
34+
def __call__( # type: ignore
35+
self,
36+
ip: str,
37+
urdf_path: str,
38+
calibration_dir: PathLike | str | None = None,
39+
camera_set: HardwareCameraSet | None = None,
40+
max_relative_movement: float | tuple[float, float] | None = None,
41+
relative_to: RelativeTo = RelativeTo.LAST_STEP,
42+
) -> gym.Env:
43+
if isinstance(calibration_dir, str):
44+
calibration_dir = Path(calibration_dir)
45+
robot = XArm7(ip=ip, urdf_path=urdf_path)
46+
env: gym.Env = RobotEnv(robot, ControlMode.JOINTS)
47+
48+
if camera_set is not None:
49+
camera_set.start()
50+
camera_set.wait_for_frames()
51+
logger.info("CameraSet started")
52+
env = CameraSetWrapper(env, camera_set)
53+
54+
if max_relative_movement is not None:
55+
env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
56+
57+
return env
58+
59+
60+
class XArm7SimEnvCreator(EnvCreator):
61+
def __call__( # type: ignore
62+
self,
63+
control_mode: ControlMode,
64+
robot_cfg: SimRobotConfig,
65+
urdf_path: str,
66+
mjcf: str,
67+
collision_guard: bool = False,
68+
gripper_cfg: SimGripperConfig | None = None,
69+
cameras: dict[str, SimCameraConfig] | None = None,
70+
max_relative_movement: float | tuple[float, float] | None = None,
71+
relative_to: RelativeTo = RelativeTo.LAST_STEP,
72+
sim_wrapper: Type[SimWrapper] | None = None,
73+
) -> gym.Env:
74+
"""
75+
Creates a simulation environment for the FR3 robot.
76+
Args:
77+
control_mode (ControlMode): Control mode for the robot.
78+
robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot.
79+
collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding.
80+
gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used.
81+
camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used.
82+
max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts
83+
translational movement in meters. If tuple, it restricts both translational (in meters) and rotational
84+
(in radians) movements. If None, no restriction is applied.
85+
relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step.
86+
urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced
87+
which requires a UTN compatible lab scene to be present.
88+
mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world".
89+
sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful
90+
for reset management e.g. resetting objects in the scene with correct observations. Defaults to None.
91+
Returns:
92+
gym.Env: The configured simulation environment for the FR3 robot.
93+
"""
94+
simulation = sim.Sim(mjcf)
95+
96+
ik = rcs.common.RL(str(urdf_path))
97+
robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
98+
env: gym.Env = RobotEnv(robot, control_mode)
99+
env = RobotSimWrapper(env, simulation, sim_wrapper)
100+
101+
if cameras is not None:
102+
camera_set = SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True)
103+
env = CameraSetWrapper(env, camera_set, include_depth=True)
104+
105+
if gripper_cfg is not None and isinstance(gripper_cfg, SimGripperConfig):
106+
gripper = sim.SimGripper(simulation, gripper_cfg)
107+
env = GripperWrapper(env, gripper, binary=False)
108+
env = GripperWrapperSim(env, gripper)
109+
110+
if collision_guard:
111+
env = CollisionGuard.env_from_xml_paths(
112+
env,
113+
str(rcs.scenes.get(str(mjcf), mjcf)),
114+
str(urdf_path),
115+
gripper=gripper_cfg is not None,
116+
check_home_collision=False,
117+
control_mode=control_mode,
118+
tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()),
119+
sim_gui=True,
120+
truncate_on_collision=True,
121+
)
122+
if max_relative_movement is not None:
123+
env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
124+
125+
return env

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

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
import logging
2+
from time import sleep
3+
4+
from rcs._core.common import RobotPlatform
5+
from rcs.envs.base import ControlMode, RelativeTo
6+
from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
7+
8+
import rcs
9+
from rcs import sim
10+
11+
logger = logging.getLogger(__name__)
12+
logger.setLevel(logging.INFO)
13+
14+
ROBOT_IP = "192.168.1.245"
15+
# ROBOT_INSTANCE = RobotPlatform.SIMULATION
16+
ROBOT_INSTANCE = RobotPlatform.HARDWARE
17+
18+
19+
def main():
20+
21+
if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
22+
env_rel = RCSXArm7EnvCreator()(
23+
ip=ROBOT_IP,
24+
urdf_path="/home/ken/Downloads/xarm7.urdf",
25+
relative_to=RelativeTo.LAST_STEP,
26+
)
27+
else:
28+
cfg = sim.SimRobotConfig()
29+
cfg.robot_type = rcs.common.RobotType.XArm7
30+
cfg.actuators = ["1", "2", "3", "4", "5"]
31+
cfg.joints = ["1", "2", "3", "4", "5"]
32+
cfg.arm_collision_geoms = []
33+
cfg.attachment_site = "gripper"
34+
35+
grp_cfg = sim.SimGripperConfig()
36+
grp_cfg.actuator = "6"
37+
grp_cfg.joint = "6"
38+
grp_cfg.collision_geoms = []
39+
grp_cfg.collision_geoms_fingers = []
40+
41+
env_rel = XArm7SimEnvCreator()(
42+
control_mode=ControlMode.JOINTS,
43+
urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf",
44+
robot_cfg=cfg,
45+
collision_guard=False,
46+
mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/XArm7/scene.xml",
47+
gripper_cfg=grp_cfg,
48+
# camera_set_cfg=default_mujoco_cameraset_cfg(),
49+
max_relative_movement=None,
50+
# max_relative_movement=10.0,
51+
# max_relative_movement=0.5,
52+
relative_to=RelativeTo.LAST_STEP,
53+
)
54+
env_rel.get_wrapper_attr("sim").open_gui()
55+
56+
for _ in range(10):
57+
obs, info = env_rel.reset()
58+
for _ in range(100):
59+
# sample random relative action and execute it
60+
act = env_rel.action_space.sample()
61+
print(act)
62+
# act["gripper"] = 1.0
63+
obs, reward, terminated, truncated, info = env_rel.step(act)
64+
print(obs)
65+
if truncated or terminated:
66+
logger.info("Truncated or terminated!")
67+
return
68+
logger.info(act["gripper"])
69+
sleep(1.0)
70+
71+
72+
if __name__ == "__main__":
73+
main()

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

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
import typing
2+
3+
import numpy as np
4+
from xarm.wrapper import XArmAPI
5+
6+
from rcs import common
7+
8+
9+
class XArm7(common.Robot):
10+
def __init__(self, ip: str, urdf_path: str):
11+
self.ik = common.RL(urdf_path=urdf_path)
12+
self._xarm = XArmAPI(ip=ip)
13+
14+
# def get_base_pose_in_world_coordinates(self) -> Pose: ...
15+
def get_cartesian_position(self) -> common.Pose:
16+
return self.ik.forward(self.get_joint_position())
17+
18+
def get_ik(self) -> common.IK | None:
19+
return self.ik
20+
21+
def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]: # type: ignore
22+
obs = self._hf_robot.get_servo_angle(is_radian=True)[1]
23+
return np.array(obs, dtype=np.float64)
24+
25+
def get_parameters(self):
26+
a = common.RobotConfig()
27+
a.robot_platform = common.RobotPlatform.HARDWARE
28+
a.robot_type = common.RobotType.XArm7
29+
return a
30+
31+
def get_state(self) -> common.RobotState:
32+
return common.RobotState()
33+
34+
def move_home(self) -> None:
35+
home = typing.cast(
36+
np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]],
37+
common.robots_meta_config(common.RobotType.XArm7).q_home,
38+
)
39+
self.set_joint_position(home)
40+
41+
def reset(self) -> None:
42+
pass
43+
44+
def set_cartesian_position(self, pose: common.Pose) -> None:
45+
joints = self.ik.ik(pose, q0=self.get_joint_position())
46+
if joints is not None:
47+
self.set_joint_position(joints)
48+
49+
def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]) -> None: # type: ignore
50+
self._hf_robot.set_servo_angle(angle=q, is_radian=True, wait=True)
51+
52+
# def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ...
53+
# def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ...
54+

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

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
xArm7, RobotMetaConfig{
2+
// q_home (7‐vector):
3+
(VectorXd(7) << 0, 0, 0, 0, 0, 0, 0).finished(),
4+
// dof:
5+
7,
6+
// joint_limits (2×7):
7+
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
8+
// low 7‐tuple
9+
-2 * M_PI, -2.094395, -2 * M_PI, -3.92699, -2 * M_PI, -M_PI, -2 * M_PI,
10+
// high 7‐tuple
11+
2 * M_PI, 2.059488, 2 * M_PI, 0.191986, 2 * M_PI, 1.692969, 2 * M_PI).finished()

‎include/rcs/Robot.h‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ struct RobotMetaConfig {
1818
Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor> joint_limits;
1919
};
2020

21-
enum RobotType { FR3 = 0, UR5e, SO101 };
21+
enum RobotType { FR3 = 0, UR5e, SO101, XArm7 };
2222
enum RobotPlatform { SIMULATION = 0, HARDWARE };
2323

2424
static const std::unordered_map<RobotType, RobotMetaConfig> robots_meta_config =
@@ -57,6 +57,18 @@ static const std::unordered_map<RobotType, RobotMetaConfig> robots_meta_config =
5757
// high 6‐tuple
5858
2 * M_PI, 2 * M_PI, 1 * M_PI, 2 * M_PI, 2 * M_PI, 2 * M_PI)
5959
.finished()}},
60+
// -------------- XArm7 --------------
61+
{XArm7, RobotMetaConfig{
62+
// q_home (7‐vector):
63+
(VectorXd(7) << 0, 0, 0, 0, 0, 0, 0).finished(),
64+
// dof:
65+
7,
66+
// joint_limits (2×7):
67+
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
68+
// low 7‐tuple
69+
-2 * M_PI, -2.094395, -2 * M_PI, -3.92699, -2 * M_PI, -M_PI, -2 * M_PI,
70+
// high 7‐tuple
71+
2 * M_PI, 2.059488, 2 * M_PI, 0.191986, 2 * M_PI, 1.692969, 2 * M_PI).finished()}},
6072
// -------------- SO101 --------------
6173
{SO101,
6274
RobotMetaConfig{

‎python/rcs/_core/common.pyi‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ __all__ = [
3434
"SIMULATION",
3535
"SO101",
3636
"UR5e",
37+
"XArm7",
3738
"robots_meta_config",
3839
]
3940
M = typing.TypeVar("M", bound=int)
@@ -220,14 +221,17 @@ class RobotType:
220221
UR5e
221222
222223
SO101
224+
225+
XArm7
223226
"""
224227

225228
FR3: typing.ClassVar[RobotType] # value = <RobotType.FR3: 0>
226229
SO101: typing.ClassVar[RobotType] # value = <RobotType.SO101: 2>
227230
UR5e: typing.ClassVar[RobotType] # value = <RobotType.UR5e: 1>
231+
XArm7: typing.ClassVar[RobotType] # value = <RobotType.XArm7: 3>
228232
__members__: typing.ClassVar[
229233
dict[str, RobotType]
230-
] # value = {'FR3': <RobotType.FR3: 0>, 'UR5e': <RobotType.UR5e: 1>, 'SO101': <RobotType.SO101: 2>}
234+
] # value = {'FR3': <RobotType.FR3: 0>, 'UR5e': <RobotType.UR5e: 1>, 'SO101': <RobotType.SO101: 2>, 'XArm7': <RobotType.XArm7: 3>}
231235
def __eq__(self, other: typing.Any) -> bool: ...
232236
def __getstate__(self) -> int: ...
233237
def __hash__(self) -> int: ...
@@ -255,3 +259,4 @@ HARDWARE: RobotPlatform # value = <RobotPlatform.HARDWARE: 1>
255259
SIMULATION: RobotPlatform # value = <RobotPlatform.SIMULATION: 0>
256260
SO101: RobotType # value = <RobotType.SO101: 2>
257261
UR5e: RobotType # value = <RobotType.UR5e: 1>
262+
XArm7: RobotType # value = <RobotType.XArm7: 3>

‎src/pybind/rcs.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -241,6 +241,7 @@ PYBIND11_MODULE(_core, m) {
241241
.value("FR3", rcs::common::RobotType::FR3)
242242
.value("UR5e", rcs::common::RobotType::UR5e)
243243
.value("SO101", rcs::common::RobotType::SO101)
244+
.value("XArm7", rcs::common::RobotType::XArm7)
244245
.export_values();
245246

246247
py::enum_<rcs::common::RobotPlatform>(common, "RobotPlatform")

0 commit comments

Comments
 (0)