added new ik to ur5e

Author: Johannes Hechtl

examples/ur5e/ur5e_env_cartesian_control.py

@@ -16,7 +16,7 @@
 logger.setLevel(logging.INFO)
 
 ROBOT_IP = "192.168.1.15"
-ROBOT_INSTANCE = RobotPlatform.HARDWARE
+ROBOT_INSTANCE = RobotPlatform.SIMULATION  # Change to RobotPlatform.HARDWARE for real robot
 
 
 def main():
@@ -46,36 +46,35 @@ def main():
         robot_cfg.robot_type = rcs.common.RobotType.UR5e
         robot_cfg.attachment_site = "attachment_site"
         robot_cfg.arm_collision_geoms = []
+        robot_cfg.mjcf_scene_path = rcs.scenes["ur5e_empty_world"].mjb
+        robot_cfg.kinematic_model_path = rcs.scenes["ur5e_empty_world"].mjcf_robot
         env_rel = SimEnvCreator()(
-            control_mode=ControlMode.CARTESIAN_TQuat,
+            control_mode=ControlMode.CARTESIAN_TRPY,
             collision_guard=False,
             robot_cfg=robot_cfg,
             gripper_cfg=None,
-            # cameras=default_mujoco_cameraset_cfg(),
             max_relative_movement=0.5,
             relative_to=RelativeTo.LAST_STEP,
-            mjcf=rcs.scenes["ur5e_empty_world"].mjb,
-            urdf_path=rcs.scenes["ur5e_empty_world"].urdf,
         )
         env_rel.get_wrapper_attr("sim").open_gui()
 
     obs, info = env_rel.reset()
 
-    act = {"xyzrpy": [0.0, 0, 0.0, 0, 0, np.deg2rad(45)], "gripper": 0}
-    obs, reward, terminated, truncated, info = env_rel.step(act)
+    # act = {"xyzrpy": [0.0, 0, 0.0, 0, 0, np.deg2rad(45)], "gripper": 0}
+    # obs, reward, terminated, truncated, info = env_rel.step(act)
 
     for _ in range(100):
         for _ in range(10):
             # move 1cm in x direction (forward) and close gripper
-            act = {"tquat": [0.01, 0, 0, 0.0087265, 0, 0, 0.9999619], "gripper": 0}
+            act = {"xyzrpy": [-0.1, 0, 0, 0.0, 0, 0], "gripper": 0}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             if truncated or terminated:
                 logger.info("Truncated or terminated!")
                 return
             sleep(0.1)
         for _ in range(10):
             # move 1cm in negative x direction (backward) and open gripper
-            act = {"tquat": [-0.01, 0, 0, -0.0087265, 0, 0, 0.9999619], "gripper": 1}
+            act = {"xyzrpy": [-0.01, 0, 0, -0.0087265, 0, 0, 0.9999619], "gripper": 1}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             if truncated or terminated:
                 logger.info("Truncated or terminated!")

examples/ur5e/ur5e_env_joint_control.py

@@ -1,4 +1,5 @@
 import logging
+from time import sleep
 
 import numpy as np
 from rcs._core.common import RobotPlatform
@@ -14,7 +15,8 @@
 logger.setLevel(logging.INFO)
 
 ROBOT_IP = "192.168.25.201"
-ROBOT_INSTANCE = RobotPlatform.HARDWARE
+ROBOT_INSTANCE = RobotPlatform.SIMULATION  
+# ROBOT_INSTANCE = RobotPlatform.HARDWARE
 
 
 def main():
@@ -44,18 +46,15 @@ def main():
         robot_cfg.robot_type = rcs.common.RobotType.UR5e
         robot_cfg.attachment_site = "attachment_site"
         robot_cfg.arm_collision_geoms = []
+        robot_cfg.mjcf_scene_path = rcs.scenes["ur5e_empty_world"].mjb
+        robot_cfg.kinematic_model_path = rcs.scenes["ur5e_empty_world"].mjcf_robot
         env_rel = SimEnvCreator()(
             control_mode=ControlMode.JOINTS,
             collision_guard=False,
             robot_cfg=robot_cfg,
             gripper_cfg=None,
-            # cameras=default_mujoco_cameraset_cfg(),
             max_relative_movement=np.deg2rad(5),
             relative_to=RelativeTo.LAST_STEP,
-            # mjcf=rcs.scenes["ur5e_empty_world"]["mjb"],
-            mjcf="/home/johannes/repos/learning/robot-control-stack/assets/scenes/ur5e_empty_world/scene.xml",
-            # urdf_path=rcs.scenes["ur5e_empty_world"]["urdf"],
-            urdf_path="/home/johannes/repos/learning/robot-control-stack/assets/ur5e/urdf/ur5e.urdf",
         )
         env_rel.get_wrapper_attr("sim").open_gui()
 
@@ -68,6 +67,7 @@ def main():
             if truncated or terminated:
                 logger.info("Truncated or terminated!")
                 return
+            sleep(0.4)
 
 
 if __name__ == "__main__":

extensions/rcs_ur5e/src/rcs_ur5e/creators.py

@@ -1,6 +1,7 @@
 import logging
 
 import gymnasium as gym
+import rcs
 from rcs.camera.hw import HardwareCameraSet
 from rcs.envs.base import (
     CameraSetWrapper,
@@ -27,7 +28,12 @@ def __call__(  # type: ignore
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
     ) -> gym.Env:
-        robot = UR5e(ip)
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        robot = UR5e(ip, ik)
         robot.set_config(robot_cfg)
         env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True)
 

extensions/rcs_ur5e/src/rcs_ur5e/hw.py

@@ -12,6 +12,7 @@
 import rtde_control
 import rtde_receive
 
+import rcs
 from rcs_ur5e import robotiq_gripper
 from rcs import common
 
@@ -22,6 +23,10 @@
 
 @dataclass(kw_only=True)
 class UR5eConfig(common.RobotConfig):
+    # Kinematics Setup
+    kinematic_model_path = rcs.scenes["ur5e_empty_world"].mjcf_robot
+    attachment_site = "attachment_site_testo"
+
     # Robot movement parameters
     max_velocity: float = 1.0
     max_acceleration: float = 1.0
@@ -139,7 +144,7 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
                 # if np.max(np.abs(diff[0:3])) > robot_config.max_servo_cartesian_step:
                 #     diff = (robot_config.max_servo_cartesian_step / np.max(diff)) * diff
                 #     target_pose = (cartesian_state_view + diff).tolist()
-                # else:
+                # else: (j.hechtl)
                 # target_pose = cartesian_target_view.tolist()
                 ur_control.servoL(
                     target_pose,
@@ -166,8 +171,8 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
 
 
 class UR5e:  # (common.Robot): # should inherit and implement common.Robot, but currently there is a bug that needs to be fixed
-    def __init__(self, ip: str):
-        self.ik = common.RL(urdf_path="/home/johannes/repos/rcs/assets/ur5e/urdf/ur5e.urdf")
+    def __init__(self, ip: str, ik: common.Kinematics):
+        self.ik = ik
         self._config = UR5eConfig()
         self._config.robot_type = common.RobotType.UR5e
         self._ip = ip
@@ -233,7 +238,7 @@ def get_cartesian_position(self) -> common.Pose:
         pose = common.Pose(quaternion=rotvec.as_quaternion_vector(), translation=trans)
         return common.Pose(rpy_vector=[0, 0, np.deg2rad(180)], translation=[0, 0, 0]).inverse() * pose
 
-    def get_ik(self) -> common.IK | None:
+    def get_ik(self) -> common.Kinematics | None:
         return None
 
     def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]]:
@@ -312,8 +317,6 @@ def grasp(self) -> None:
         """
         self.set_normalized_width(0.0)
 
-    # def is_grasped(self) -> bool: ...
-
     def open(self) -> None:
         """
         Open the gripper to its maximum width.