feat(robots): Add UR config and example (#68)

* feat: add ur

* feat: add ur example

* chore: change home pose

---------

Co-authored-by: Daniel San José Pro <42489409+danielsanjosepro@users.noreply.github.com>

Author: lvjonok

crisp_py/config/robots/ur.yaml

@@ -0,0 +1 @@
+robot_type: "ur"

crisp_py/robot/robot_config.py

@@ -291,11 +291,43 @@ class DynaArmConfig(RobotConfig):
     cartesian_impedance_controller_name: str = "crisp_cartesian_controller"
 
 
+@dataclass
+class URConfig(RobotConfig):
+    """Configuration specific to Universal Robots (UR) arms.
+
+    Provides default values for frame names, joint names, and home configuration
+    specifically for UR robots.
+    """
+
+    joint_names: list = field(
+        default_factory=lambda: [
+            "shoulder_pan_joint",
+            "shoulder_lift_joint",
+            "elbow_joint",
+            "wrist_1_joint",
+            "wrist_2_joint",
+            "wrist_3_joint",
+        ]
+    )
+    home_config: list = field(
+        default_factory=lambda: [
+            0,
+            -np.pi / 2,
+            np.pi / 2,
+            -np.pi / 2,
+            -np.pi / 2,
+            0,
+        ],
+    )
+    base_frame: str = "base_link"
+    target_frame: str = "tool0"
+
+
 def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:  # noqa: ANN003
     """Factory function to create robot configuration objects.
 
     Args:
-        robot_type (str): Type of robot ('franka', 'kinova', 'iiwa', 'so101', 'dynaarm')
+        robot_type (str): Type of robot ('franka', 'kinova', 'iiwa', 'so101', 'dynaarm', 'ur')
         **kwargs: Additional keyword arguments to override default configuration
 
     Returns:
@@ -318,7 +350,9 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:  # noqa: ANN003
         return SO101Config(**kwargs)
     elif robot_type == "dynaarm":
         return DynaArmConfig(**kwargs)
+    elif robot_type == "ur":
+        return URConfig(**kwargs)
     else:
         raise ValueError(
-            f"Unsupported robot type: {robot_type}. Supported types: franka, panda, kinova, iiwa, so101, dynaarm"
+            f"Unsupported robot type: {robot_type}. Supported types: franka, panda, kinova, iiwa, so101, dynaarm, ur"
         )

examples/example_ur.py

@@ -0,0 +1,69 @@
+"""Example script for using crisp_py with a UR robot. This example assumes you have the `ur_robot_driver` ROS package running and properly configured to control your UR robot."""
+import time
+import numpy as np
+
+from crisp_py.robot import make_robot
+from crisp_py.utils.geometry import Pose
+
+robot = make_robot("ur")
+robot.wait_until_ready()
+
+#%%
+
+print(f"Starting pose: {robot.end_effector_pose}")
+print(f"Starting joint values: {robot.joint_values}")
+
+#%%
+
+print("Going to home position...")
+robot.home()  # This requires the joint_trajectory_controller to be active
+homing_pose = robot.end_effector_pose.copy()
+
+print(f"Homing pose: {homing_pose}")
+
+#%%
+
+print("Switching to Cartesian Impedance Controller...")
+print("This will unload other controllers if necessary.")
+
+# Change parameters now if needed
+# robot.cartesian_controller_parameters_client.set_parameters([
+#     ("task.k_pos_x", 600.0),
+#     ...
+# ])
+robot.controller_switcher_client.switch_controller("cartesian_impedance_controller")
+
+#%%
+
+# Figure eight
+input("Press Enter to start figure eight trajectory...")
+
+duration = 6.0
+start_time = time.time()
+rate = 20.0
+
+while time.time() - start_time < duration:
+    t = time.time() - start_time
+
+    # Create figure eight trajectory in the XZ plane
+    x = 0.1 * np.sin(2 * np.pi * t / duration)
+    y = 0.0
+    z = 0.1 * np.sin(4 * np.pi * t / duration)
+
+    target_pose = Pose(
+        position=homing_pose.position + np.array([x, y, z]),
+        orientation=homing_pose.orientation,
+    )
+
+    robot.set_target(pose=target_pose)
+
+    time.sleep(1.0 / rate)
+
+
+# %%
+
+print(f"Shutting down connection in 4 seconds... (robot will stay in place).")
+time.sleep(4.0)
+
+robot.shutdown()
+