|
| 1 | +"""Example script to control the DynaArm robot using crisp_py.""" |
| 2 | +from pathlib import Path |
| 3 | +import time |
| 4 | +import numpy as np |
| 5 | + |
| 6 | +from crisp_py.robot import make_robot |
| 7 | + |
| 8 | +robot = make_robot("dynaarm") |
| 9 | +robot.wait_until_ready() # Wait until the robot is ready to receive commands |
| 10 | + |
| 11 | +#%% |
| 12 | + |
| 13 | +print(f"Starting pose: {robot.end_effector_pose}") |
| 14 | +print(f"Starting joint values: {robot.joint_values}") |
| 15 | + |
| 16 | +#%% |
| 17 | + |
| 18 | +print("Going to home position...") |
| 19 | +robot.home() # This requires the joint_trajectory_controller to be active |
| 20 | +homing_pose = robot.end_effector_pose.copy() |
| 21 | + |
| 22 | +print(f"Homing pose: {homing_pose}") |
| 23 | + |
| 24 | +#%% |
| 25 | + |
| 26 | +print("Switching to Cartesian Impedance Controller...") |
| 27 | +print("This will unload other controllers if necessary.") |
| 28 | + |
| 29 | +# path = Path("config/control/dynaarm_gravity.yaml") |
| 30 | +path = Path("config/control/dynaarm_cic_soft.yaml") |
| 31 | + |
| 32 | +robot.cartesian_controller_parameters_client.load_param_config(path) |
| 33 | +# robot.cartesian_controller_parameters_client.save_param_config("config/control/dynaarm_cic_soft.yaml") |
| 34 | + |
| 35 | +robot.controller_switcher_client.switch_controller( |
| 36 | + "crisp_cartesian_controller", |
| 37 | + controllers_that_should_be_active=["freedrive_controller", "safety_monitor_controller"] |
| 38 | +) |
| 39 | + |
| 40 | +robot.reset_targets() |
| 41 | +#%% |
| 42 | + |
| 43 | +target_pose = robot.end_effector_pose |
| 44 | +target_pose.position[2] -= 0.1 |
| 45 | + |
| 46 | +#%% |
| 47 | + |
| 48 | +# we apply a sine wave in z direction for 10 seconds from -0.15 to 0.15 m |
| 49 | + |
| 50 | +ee_poses = [] |
| 51 | +target_poses = [] |
| 52 | +ts = [] |
| 53 | + |
| 54 | +start_time = time.time() |
| 55 | +while True: |
| 56 | + t = time.time() |
| 57 | + position = target_pose.position.copy() |
| 58 | + # position[0] = target_pose.position[0] + 0.2 * np.sin(2 * np.pi * 0.6 * (t - start_time)) # 0.1 Hz sine wave |
| 59 | + position[2] = target_pose.position[2] + 0.15 * np.sin(2 * np.pi * 0.3 * (t - start_time)) # 0.05 Hz sine wave |
| 60 | + robot.set_target(position=position) |
| 61 | + if t - start_time > 10.0: |
| 62 | + break |
| 63 | + ee_poses.append(robot.end_effector_pose.copy()) |
| 64 | + target_poses.append(robot.target_pose.copy()) |
| 65 | + ts.append(t - start_time) |
| 66 | + time.sleep(0.01) # Sleep for a short time to avoid busy waiting |
| 67 | + |
| 68 | + |
| 69 | +#%% |
| 70 | + |
| 71 | +import matplotlib.pyplot as plt |
| 72 | +x_t = [target_pose_sample.position[0] for target_pose_sample in target_poses] |
| 73 | +y_t = [target_pose_sample.position[1] for target_pose_sample in target_poses] |
| 74 | +z_t = [target_pose_sample.position[2] for target_pose_sample in target_poses] |
| 75 | + |
| 76 | +x_ee = [ee_pose.position[0] for ee_pose in ee_poses] |
| 77 | +y_ee = [ee_pose.position[1] for ee_pose in ee_poses] |
| 78 | +z_ee = [ee_pose.position[2] for ee_pose in ee_poses] |
| 79 | + |
| 80 | +fig, ax = plt.subplots(1, 2, figsize=(10, 5)) |
| 81 | +ax[0].plot(ts, x_ee, label="current") |
| 82 | +ax[0].plot(ts, x_t, label="target", linestyle="--") |
| 83 | +ax[0].set_ylabel("$x$") |
| 84 | +ax[0].set_xlabel("$t$") |
| 85 | +ax[0].legend() |
| 86 | + |
| 87 | +ax[1].plot(ts, z_ee, label="current") |
| 88 | +ax[1].plot(ts, z_t, label="target", linestyle="--") |
| 89 | +ax[1].set_ylabel("$z$") |
| 90 | +ax[1].set_xlabel("$t$") |
| 91 | +ax[1].legend() |
| 92 | + |
| 93 | + |
| 94 | +for a in ax: |
| 95 | + a.grid() |
| 96 | + |
| 97 | +fig.tight_layout() |
| 98 | + |
| 99 | +plt.show() |
| 100 | +#%% |
| 101 | +print(f"End pose: {robot.end_effector_pose}") |
| 102 | +print(f"End joint values: {robot.joint_values}") |
| 103 | + |
| 104 | +#%% |
| 105 | + |
| 106 | +print(f"Shutting down connection in 2 seconds... (robot will stay in place).") |
| 107 | +time.sleep(2.0) |
| 108 | + |
| 109 | +robot.shutdown() |
| 110 | + |
| 111 | +# %% |
| 112 | + |
| 113 | +# robot.controller_switcher_client.switch_controller( |
| 114 | +# "gravity_compensation_controller", |
| 115 | +# controllers_that_should_be_active=["freedrive_controller", "safety_monitor_controller"] |
| 116 | +# ) |
0 commit comments