feat: cartesian control

kennkhr · kennkhr · commit c7630e50020b · 2025-08-06T19:51:36.000+02:00
diff --git a/extensions/rcs_xarm7/pyproject.toml b/extensions/rcs_xarm7/pyproject.toml
@@ -13,10 +13,10 @@ dependencies = [
 readme = "README.md"
 maintainers = [
   { name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
+  { name = "Ken Nakahara", email = "knakahara@lasr.org" },
 ]
 authors = [
   { name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
-  { name = "Pierre Krack", email = "pierre.krack@utn.de" },
   { name = "Ken Nakahara", email = "knakahara@lasr.org" },
 ]
 requires-python = ">=3.10"
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py
@@ -33,17 +33,18 @@
 class RCSXArm7EnvCreator(RCSHardwareEnvCreator):
     def __call__(  # type: ignore
         self,
+        control_mode: ControlMode,
         ip: str,
-        urdf_path: str,
         calibration_dir: PathLike | str | None = None,
         camera_set: HardwareCameraSet | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
     ) -> gym.Env:
         if isinstance(calibration_dir, str):
             calibration_dir = Path(calibration_dir)
-        robot = XArm7(ip=ip, urdf_path=urdf_path)
-        env: gym.Env = RobotEnv(robot, ControlMode.JOINTS)
+        robot = XArm7(ip=ip)
+        # env: gym.Env = RobotEnv(robot, ControlMode.JOINTS, home_on_reset=True)
+        env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True)
 
         if camera_set is not None:
             camera_set.start()
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py
@@ -0,0 +1,85 @@
+import logging
+from time import sleep
+
+from rcs._core.common import RobotPlatform
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs_fr3.desk import ContextManager
+from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
+
+import rcs
+from rcs import sim
+import numpy as np
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+ROBOT_IP = "192.168.1.245"
+# ROBOT_INSTANCE = RobotPlatform.SIMULATION
+ROBOT_INSTANCE = RobotPlatform.HARDWARE
+
+
+def main():
+
+    if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+        env_rel = RCSXArm7EnvCreator()(
+            control_mode=ControlMode.CARTESIAN_TQuat,
+            ip=ROBOT_IP,
+            relative_to=RelativeTo.LAST_STEP,
+            max_relative_movement=np.deg2rad(3)
+        )
+        cm = env_rel.unwrapped.robot
+    else:
+        cm = ContextManager()
+        cfg = sim.SimRobotConfig()
+        cfg.robot_type = rcs.common.RobotType.XArm7
+        cfg.actuators = ["1", "2", "3", "4", "5"]
+        cfg.joints = ["1", "2", "3", "4", "5"]
+        cfg.arm_collision_geoms = []
+        cfg.attachment_site = "gripper"
+
+        grp_cfg = sim.SimGripperConfig()
+        grp_cfg.actuator = "6"
+        grp_cfg.joint = "6"
+        grp_cfg.collision_geoms = []
+        grp_cfg.collision_geoms_fingers = []
+
+        env_rel = XArm7SimEnvCreator()(
+            control_mode=ControlMode.JOINTS,
+            urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf",
+            robot_cfg=cfg,
+            collision_guard=False,
+            mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/XArm7/scene.xml",
+            gripper_cfg=grp_cfg,
+            # camera_set_cfg=default_mujoco_cameraset_cfg(),
+            max_relative_movement=None,
+            # max_relative_movement=10.0,
+            # max_relative_movement=0.5,
+            relative_to=RelativeTo.LAST_STEP,
+        )
+        env_rel.get_wrapper_attr("sim").open_gui()
+    env_rel.reset()
+    act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0}
+    obs, reward, terminated, truncated, info = env_rel.step(act)
+
+    with cm:
+        for _ in range(10):
+            for _ in range(10):
+                # move 1cm in x direction (forward) and close gripper
+                act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+                print(obs)
+                #sleep(2.0)
+            for _ in range(10):
+                # move 1cm in negative x direction (backward) and open gripper
+                act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1}
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+
+
+if __name__ == "__main__":
+    main()
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py
@@ -3,10 +3,12 @@
 
 from rcs._core.common import RobotPlatform
 from rcs.envs.base import ControlMode, RelativeTo
+from rcs_fr3.desk import ContextManager
 from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
 
 import rcs
 from rcs import sim
+import numpy as np
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -20,11 +22,14 @@ def main():
 
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
         env_rel = RCSXArm7EnvCreator()(
+            control_mode=ControlMode.JOINTS,
             ip=ROBOT_IP,
-            urdf_path="/home/ken/Downloads/xarm7.urdf",
             relative_to=RelativeTo.LAST_STEP,
+            max_relative_movement=np.deg2rad(3)
         )
+        cm = env_rel.unwrapped.robot
     else:
+        cm = ContextManager()
         cfg = sim.SimRobotConfig()
         cfg.robot_type = rcs.common.RobotType.XArm7
         cfg.actuators = ["1", "2", "3", "4", "5"]
@@ -53,20 +58,21 @@ def main():
         )
         env_rel.get_wrapper_attr("sim").open_gui()
 
-    for _ in range(10):
-        obs, info = env_rel.reset()
-        for _ in range(100):
-            # sample random relative action and execute it
-            act = env_rel.action_space.sample()
-            print(act)
-            # act["gripper"] = 1.0
-            obs, reward, terminated, truncated, info = env_rel.step(act)
-            print(obs)
-            if truncated or terminated:
-                logger.info("Truncated or terminated!")
-                return
-            logger.info(act["gripper"])
-            sleep(1.0)
+    with cm:
+        for _ in range(10):
+            obs, info = env_rel.reset()
+            for _ in range(3):
+                # sample random relative action and execute it
+                act = env_rel.action_space.sample()
+                print(act)
+                # act["gripper"] = 1.0
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                print(obs)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+                # logger.info(act["gripper"])
+                sleep(2.0)
 
 
 if __name__ == "__main__":
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py
@@ -1,32 +1,70 @@
+from dataclasses import dataclass, field
 import typing
+from typing import List
 
 import numpy as np
 from xarm.wrapper import XArmAPI
 
 from rcs import common
 
+from scipy.spatial.transform import Rotation as R
 
-class XArm7(common.Robot):
-    def __init__(self, ip: str, urdf_path: str):
-        self.ik = common.RL(urdf_path=urdf_path)
-        self._xarm = XArmAPI(ip=ip)
+
+@dataclass(kw_only=True)
+class XArm7Config(common.RobotConfig):
+    # some_custom_config: str = "default_value"
+    payload_weight: float = 0.624
+    payload_tcp: List[float] = field(default_factory=lambda: [-4.15, 5.24, 76.38])
+    def __post_init__(self):
+        super().__init__()
+
+
+class XArm7():
+    def __init__(self, ip: str):
+        self.ik = None #common.RL(urdf_path=urdf_path)
+        self._config = XArm7Config()
+        self._config.robot_platform = common.RobotPlatform.HARDWARE
+        self._config.robot_type = common.RobotType.XArm7
+
+        self._xarm = XArmAPI(ip)
+        self._xarm.set_mode(0)
+        self._xarm.clean_error()
+        self._xarm.clean_warn()
+        self._xarm.motion_enable(enable=True)
+        self._xarm.set_state(state=0)
+        self._xarm.set_tcp_load(
+            weight=self._config.payload_weight,
+            center_of_gravity=self._config.payload_tcp,
+            wait=True,
+        )
 
     # def get_base_pose_in_world_coordinates(self) -> Pose: ...
+    
     def get_cartesian_position(self) -> common.Pose:
-        return self.ik.forward(self.get_joint_position())
+        code, xyzrpy = self._xarm.get_position(is_radian=True)
+        if code != 0:
+            raise RuntimeError("couldn't get cartesian position from xarm")
+
+        translation = np.array(xyzrpy[:3], dtype=np.float64).reshape((3, 1)) * 0.001
+        rpy = xyzrpy[3:]
+        quat = R.from_euler('xyz', rpy).as_quat()  # [x, y, z, w]
+        quat = np.array(quat, dtype=np.float64).reshape((4, 1))
+
+        # return common.Pose(quaternion=quat, translation=translation)
+        return common.Pose(rpy_vector=rpy, translation=translation)
 
     def get_ik(self) -> common.IK | None:
         return self.ik
 
     def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]:  # type: ignore
-        obs = self._hf_robot.get_servo_angle(is_radian=True)[1]
+        obs = self._xarm.get_servo_angle(is_radian=True)[1]
         return np.array(obs, dtype=np.float64)
 
-    def get_parameters(self):
-        a = common.RobotConfig()
-        a.robot_platform = common.RobotPlatform.HARDWARE
-        a.robot_type = common.RobotType.XArm7
-        return a
+    def get_parameters(self) -> XArm7Config:
+        return self._config
+    
+    def set_parameters(self, robot_cfg: XArm7Config) -> None:
+        self._config = robot_cfg
 
     def get_state(self) -> common.RobotState:
         return common.RobotState()
@@ -42,12 +80,19 @@ def reset(self) -> None:
         pass
 
     def set_cartesian_position(self, pose: common.Pose) -> None:
-        joints = self.ik.ik(pose, q0=self.get_joint_position())
-        if joints is not None:
-            self.set_joint_position(joints)
+        x, y, z, roll, pitch, yaw = pose.xyzrpy()
+        x_mm, y_mm, z_mm = 1000 * x, 1000 * y, 1000 * z
+        self._xarm.set_position(x_mm, y_mm, z_mm, roll, pitch, yaw, is_radian=True, wait=True)
 
     def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]) -> None:  # type: ignore
-        self._hf_robot.set_servo_angle(angle=q, is_radian=True, wait=True)
+        self._xarm.set_servo_angle(angle=q, is_radian=True, wait=True)
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_value, traceback):
+        self._xarm.disconnect()
+
 
     # def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ...
     # def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ...
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/test.py b/extensions/rcs_xarm7/src/rcs_xarm7/test.py
@@ -0,0 +1,12 @@
+from xarm.wrapper import XArmAPI
+import numpy as np
+import time
+xarm = XArmAPI(port="192.168.1.245")
+xarm.motion_enable(enable=True)
+xarm.clean_error()
+xarm.set_mode(0)
+xarm.set_state(state=0)
+# xarm.set_servo_angle(angle=np.zeros(7).tolist(), is_radian=True, wait=True)
+print(xarm.get_position())
+xarm.set_position(x=10, is_radian=True, relative=True, wait=True)
+print(xarm.get_position())

Original file line number	Diff line number	Diff line change
`@@ -13,10 +13,10 @@ dependencies = [`
`13`	`13`	`readme = "README.md"`
`14`	`14`	`maintainers = [`
`15`	`15`	`{ name = "Tobias Jülg", email = "[email protected]" },`
	`16`	`+ { name = "Ken Nakahara", email = "[email protected]" },`
`16`	`17`	`]`
`17`	`18`	`authors = [`
`18`	`19`	`{ name = "Tobias Jülg", email = "[email protected]" },`
`19`		`- { name = "Pierre Krack", email = "[email protected]" },`
`20`	`20`	`{ name = "Ken Nakahara", email = "[email protected]" },`
`21`	`21`	`]`
`22`	`22`	`requires-python = ">=3.10"`