feat: add viser example to crisp_py

crisp_py/config/robots/iiwa14.yaml

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

crisp_py/robot/robot_config.py

@@ -202,18 +202,18 @@ class IiwaConfig(RobotConfig):
 
     joint_names: list = field(
         default_factory=lambda: [
-            "joint_a1",
-            "joint_a2",
-            "joint_a3",
-            "joint_a4",
-            "joint_a5",
-            "joint_a6",
-            "joint_a7",
+            "lbr_A1",
+            "lbr_A2",
+            "lbr_A3",
+            "lbr_A4",
+            "lbr_A5",
+            "lbr_A6",
+            "lbr_A7",
         ]
     )
     home_config: list = field(
         default_factory=lambda: [
-            0,
+            np.pi / 2,
             -np.pi / 4,
             0,
             -3 * np.pi / 4,
@@ -222,8 +222,8 @@ class IiwaConfig(RobotConfig):
             np.pi / 4,
         ]
     )
-    base_frame: str = "iiwa_base"
-    target_frame: str = "tool0"
+    base_frame: str = "world"
+    target_frame: str = "lbr_link_ee"
 
 
 @dataclass

examples/01_figure_eight.py

@@ -6,18 +6,17 @@
 
 from crisp_py.robot import make_robot
 
-left_arm = make_robot("fr3_left")
-print(left_arm._current_joint)
-left_arm.wait_until_ready()
+robot = make_robot("fr3")
+robot.wait_until_ready()
 
 # %%
-print(left_arm.end_effector_pose)
-print(left_arm.joint_values)
+print(robot.end_effector_pose)
+print(robot.joint_values)
 
 # %%
 print("Going to home position...")
-left_arm.home()
-homing_pose = left_arm.end_effector_pose.copy()
+robot.home()
+homing_pose = robot.end_effector_pose.copy()
 
 
 # %%
@@ -30,8 +29,8 @@
 max_time = 8.0
 
 # %%
-left_arm.controller_switcher_client.switch_controller("cartesian_impedance_controller")
-left_arm.cartesian_controller_parameters_client.load_param_config(
+robot.controller_switcher_client.switch_controller("cartesian_impedance_controller")
+robot.cartesian_controller_parameters_client.load_param_config(
     # file_path="config/control/gravity_compensation.yaml"
     # file_path="config/control/default_operational_space_controller.yaml"
     # file_path="config/control/clipped_cartesian_impedance.yaml"
@@ -40,7 +39,7 @@
 
 # %%
 # The move_to function will publish a pose to /target_pose while interpolation linearly
-left_arm.move_to(position=center, speed=0.15)
+robot.move_to(position=center, speed=0.15)
 
 # %%
 # The set_target will directly publish the pose to /target_pose
@@ -50,21 +49,21 @@
 
 print("Starting to draw a circle...")
 t = 0.0
-target_pose = left_arm.end_effector_pose.copy()
-rate = left_arm.node.create_rate(ctrl_freq)
+target_pose = robot.end_effector_pose.copy()
+rate = robot.node.create_rate(ctrl_freq)
 
 while t < max_time:
     x = center[0]
     y = radius * np.sin(2 * np.pi * sin_freq_y * t) + center[1]
     z = radius * np.sin(2 * np.pi * sin_freq_z * t) + center[2]
     target_pose.position = np.array([x, y, z])
 
-    left_arm.set_target(pose=target_pose)
+    robot.set_target(pose=target_pose)
 
     rate.sleep()
 
-    ee_poses.append(left_arm.end_effector_pose.copy())
-    target_poses.append(left_arm._target_pose.copy())
+    ee_poses.append(robot.end_effector_pose.copy())
+    target_poses.append(robot._target_pose.copy())
     ts.append(t)
 
     t += 1.0 / ctrl_freq
@@ -74,8 +73,8 @@
 
     rate.sleep()
 
-    ee_poses.append(left_arm.end_effector_pose.copy())
-    target_poses.append(left_arm._target_pose.copy())
+    ee_poses.append(robot.end_effector_pose.copy())
+    target_poses.append(robot._target_pose.copy())
     ts.append(t)
 
     t += 1.0 / ctrl_freq
@@ -115,7 +114,7 @@
 # %%
 
 print("Going back home.")
-left_arm.home()
+robot.home()
 
 # %%
-left_arm.shutdown()
+robot.shutdown()

examples/06_franka_gripper.py

@@ -0,0 +1,15 @@
+"""Simple example to control the gripper."""
+import time
+
+from crisp_py.gripper.gripper import make_gripper
+
+gripper = make_gripper("gripper_franka")
+gripper.config.max_delta = 0.15
+gripper.wait_until_ready()
+
+# Open and close the gripper
+gripper.open()
+time.sleep(2.0)
+gripper.close()
+time.sleep(2.0)
+gripper.shutdown()

examples/07_camera_example_single_frame.py

@@ -0,0 +1,24 @@
+"""Simple example for a camera. It captures a single frame from a camera and plots it using matplotlib."""
+
+import matplotlib.pyplot as plt
+from crisp_py.camera import make_camera
+
+camera = make_camera("right_wrist_camera", namespace="right")
+
+# You could also use the Camera object directly:
+# from crisp_py.camera import CameraConfig, Camera
+# camera_config = CameraConfig(
+#     camera_name="primary",
+#     camera_frame="primary_link",
+#     resolution=[256, 256],
+#     camera_color_image_topic="/image_raw",
+#     camera_color_info_topic="/image_raw/camera_info",
+# )
+#
+# camera = Camera(config=camera_config, namespace="")
+
+camera.wait_until_ready()
+
+plt.imshow(camera.current_image)
+plt.axis("off")
+plt.show()

examples/13_camera_example_stream.py

@@ -0,0 +1,28 @@
+"""Simple toy example to visualize the camera image."""
+
+import time
+import matplotlib.pyplot as plt
+from crisp_py.camera import CameraConfig, Camera
+
+
+camera_config = CameraConfig(
+    camera_name="primary",
+    camera_frame="primary_link",
+    resolution=[256, 256],
+    camera_color_image_topic="/image_raw",
+    camera_color_info_topic="/image_raw/camera_info",
+)
+
+camera = Camera(config=camera_config, namespace="")
+camera.wait_until_ready()
+
+# Display camera feed
+plt.ion()
+fig, ax = plt.subplots()
+ax.axis("off")
+
+frame = camera.current_image
+im = ax.imshow(frame)
+while True:
+    im.set_data(camera.current_image)
+    plt.pause(1.0 / 30.0)