| Author | SHA1 | Message | Date |
|---|---|---|---|
|
|
88f3f37b8f | Add IOS lidar example for reachy human shadowing | 9 months ago |
|
|
a31a6914ac | Add ios dev | 9 months ago |
|
|
cd25e9b422 | Add helper function for dora-rerun | 9 months ago |
|
|
e3462bfa93 | Add readme | 9 months ago |
|
|
d29a9d12a1 | Adding initial example | 9 months ago |
|
|
d1943119c5 | Fix linting | 9 months ago |
|
|
0ca722ac12 | Adding dae ignore | 9 months ago |
|
|
ac7834a9b5 | Fixing small typo in mediapipe demo | 9 months ago |
|
|
1c7866b463 | Adding mediapipe with dora-rerun visualization | 9 months ago |
|
|
bc6f4e36bf | fix pytorch kinematics | 9 months ago |
|
|
f5e954d8cd | Add rustypot | 9 months ago |
|
|
c8e89518b6 | add so-100 remote example | 9 months ago |
| @@ -11,6 +11,7 @@ examples/**/*.txt | |||
| # Remove hdf and stl files | |||
| *.stl | |||
| *.dae | |||
| *.STL | |||
| *.hdf5 | |||
| @@ -853,7 +853,7 @@ dependencies = [ | |||
| "enumflags2", | |||
| "futures-channel", | |||
| "futures-util", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "raw-window-handle 0.6.2", | |||
| "serde", | |||
| "serde_repr", | |||
| @@ -1827,7 +1827,7 @@ dependencies = [ | |||
| "metal 0.27.0", | |||
| "num-traits", | |||
| "num_cpus", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "rand_distr", | |||
| "rayon", | |||
| "safetensors", | |||
| @@ -3480,6 +3480,7 @@ dependencies = [ | |||
| "k", | |||
| "ndarray 0.15.6", | |||
| "pyo3", | |||
| "rand 0.9.1", | |||
| "rerun", | |||
| "tokio", | |||
| ] | |||
| @@ -4343,7 +4344,7 @@ dependencies = [ | |||
| "cudarc", | |||
| "half", | |||
| "num-traits", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "rand_distr", | |||
| ] | |||
| @@ -5121,7 +5122,7 @@ dependencies = [ | |||
| "cfg-if 1.0.0", | |||
| "crunchy", | |||
| "num-traits", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "rand_distr", | |||
| ] | |||
| @@ -6895,7 +6896,7 @@ dependencies = [ | |||
| "image", | |||
| "indexmap 2.8.0", | |||
| "mistralrs-core", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "reqwest 0.12.15", | |||
| "serde", | |||
| "serde_json", | |||
| @@ -6947,7 +6948,7 @@ dependencies = [ | |||
| "objc", | |||
| "once_cell", | |||
| "radix_trie", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "rand_isaac", | |||
| "rayon", | |||
| "regex", | |||
| @@ -9194,7 +9195,7 @@ checksum = "b820744eb4dc9b57a3398183639c511b5a26d2ed702cedd3febaa1393caa22cc" | |||
| dependencies = [ | |||
| "bytes", | |||
| "getrandom 0.3.2", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "ring 0.17.14", | |||
| "rustc-hash 2.1.1", | |||
| "rustls 0.23.25", | |||
| @@ -9272,13 +9273,12 @@ dependencies = [ | |||
| [[package]] | |||
| name = "rand" | |||
| version = "0.9.0" | |||
| version = "0.9.1" | |||
| source = "registry+https://github.com/rust-lang/crates.io-index" | |||
| checksum = "3779b94aeb87e8bd4e834cee3650289ee9e0d5677f976ecdb6d219e5f4f6cd94" | |||
| checksum = "9fbfd9d094a40bf3ae768db9361049ace4c0e04a4fd6b359518bd7b73a73dd97" | |||
| dependencies = [ | |||
| "rand_chacha 0.9.0", | |||
| "rand_core 0.9.3", | |||
| "zerocopy 0.8.24", | |||
| ] | |||
| [[package]] | |||
| @@ -9326,7 +9326,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" | |||
| checksum = "6a8615d50dcf34fa31f7ab52692afec947c4dd0ab803cc87cb3b0b4570ff7463" | |||
| dependencies = [ | |||
| "num-traits", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| ] | |||
| [[package]] | |||
| @@ -11606,7 +11606,7 @@ dependencies = [ | |||
| "num-derive", | |||
| "num-traits", | |||
| "paste", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "serde", | |||
| "serde_repr", | |||
| "socket2 0.5.8", | |||
| @@ -14253,7 +14253,7 @@ checksum = "458f7a779bf54acc9f347480ac654f68407d3aab21269a6e3c9f922acd9e2da9" | |||
| dependencies = [ | |||
| "getrandom 0.3.2", | |||
| "js-sys", | |||
| "rand 0.9.0", | |||
| "rand 0.9.1", | |||
| "serde", | |||
| "uuid-macro-internal", | |||
| "wasm-bindgen", | |||
| @@ -0,0 +1,26 @@ | |||
| nodes: | |||
| - id: camera | |||
| build: pip install -e ../../node-hub/dora-pyrealsense | |||
| path: dora-pyrealsense | |||
| inputs: | |||
| tick: dora/timer/millis/100 | |||
| outputs: | |||
| - image | |||
| - depth | |||
| - id: dora-mediapipe | |||
| build: pip install -e ../../node-hub/dora-mediapipe | |||
| path: dora-mediapipe | |||
| inputs: | |||
| image: camera/image | |||
| depth: camera/depth | |||
| outputs: | |||
| - points3d | |||
| - id: plot | |||
| build: pip install -e ../../node-hub/dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| realsense/image: camera/image | |||
| realsense/depth: camera/depth | |||
| realsense/points3d: dora-mediapipe/points3d | |||
| @@ -0,0 +1,25 @@ | |||
| nodes: | |||
| - id: camera | |||
| build: pip install opencv-video-capture | |||
| path: opencv-video-capture | |||
| inputs: | |||
| tick: dora/timer/millis/100 | |||
| outputs: | |||
| - image | |||
| env: | |||
| CAPTURE_PATH: 0 | |||
| - id: dora-mediapipe | |||
| build: pip install -e ../../node-hub/dora-mediapipe | |||
| path: dora-mediapipe | |||
| inputs: | |||
| image: camera/image | |||
| outputs: | |||
| - points2d | |||
| - id: plot | |||
| build: pip install dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| image: camera/image | |||
| series_mediapipe: dora-mediapipe/points2d | |||
| @@ -0,0 +1,25 @@ | |||
| ## Get started | |||
| ```bash | |||
| ## Deactivate any virtual environment | |||
| uv venv --seed | |||
| # This is because the current urdf fix is not yet merged in main | |||
| uv pip install git+https://github.com/dora-rs/rerun-loader-python-urdf.git@fix-urdf | |||
| # Git Clone somewhere: | |||
| git clone https://github.com/haixuanTao/reachy_description --depth 1 | |||
| dora build realsense-dev.yml --uv | |||
| dora run realsense-dev.yml --uv | |||
| ``` | |||
| For macos version | |||
| ``` | |||
| dora build ios-dev.yml --uv | |||
| dora run ios-dev.yml --uv | |||
| ``` | |||
| @@ -0,0 +1,542 @@ | |||
| import os | |||
| from typing import Dict, Literal, Tuple | |||
| import mediapipe as mp | |||
| import numpy as np | |||
| import pyarrow as pa | |||
| from dora import Node | |||
| from scipy.optimize import minimize | |||
| REACHY_R_SHOULDER_COORDINATES = np.array([0, -0.19, 0]) | |||
| REACHY_L_SHOULDER_COORDINATES = np.array([0, 0.19, 0]) | |||
| ELBOW_WEIGHT = 0.1 | |||
| MAX_CHANGE = 5.0 | |||
| MAX_ITER = 20 | |||
| SMOOTHING_BUFFER_SIZE = 10 | |||
| POSITION_ALPHA = 0.3 | |||
| MAX_CHANGE = 5.0 | |||
| TARGET_POS_TOLERANCE = 0.03 | |||
| MOVEMENT_MIN_TOLERANCE = 0.005 | |||
| HAND_SCALE_FACTOR = 0.94 | |||
| ELBOW_SCALE_FACTOR = 0.93 | |||
| mp_pose = mp.solutions.pose | |||
| initial_pose = [ | |||
| -0.11903145498601328, | |||
| 0.11292280260403312, | |||
| 0.48048914307403895, | |||
| -1.4491468779308918, | |||
| 0.1895427567665842, | |||
| 0.009599310885968814, | |||
| -0.20141099568014562, | |||
| 2.2656896114349365, | |||
| -0.13212142437597074, | |||
| -0.07731808586334879, | |||
| -0.5141739976375295, | |||
| -1.512502329778286, | |||
| 0.00034906585039886593, | |||
| 0.3193952531149623, | |||
| 0.40474185353748504, | |||
| 2.2610876560211, | |||
| ] | |||
| def mattransfo(alpha, d, theta, r): | |||
| ct = np.cos(theta) | |||
| st = np.sin(theta) | |||
| ca = np.cos(alpha) | |||
| sa = np.sin(alpha) | |||
| return np.array( | |||
| [ | |||
| [ct, -st, 0, d], | |||
| [ca * st, ca * ct, -sa, -r * sa], | |||
| [sa * st, sa * ct, ca, r * ca], | |||
| [0, 0, 0, 1], | |||
| ], | |||
| dtype=np.float64, | |||
| ) | |||
| def compute_transformation_matrices(joint_angles, length, side): | |||
| """ | |||
| Compute the transformation matrices for the robotic arm. | |||
| """ | |||
| pi = np.pi | |||
| alpha = [0, -pi / 2, -pi / 2, -pi / 2, +pi / 2, -pi / 2, -pi / 2, -pi / 2] | |||
| r = np.array([0, 0, -length[0], 0, -length[1], 0, 0, -length[2]]) | |||
| th = [ | |||
| joint_angles[0], | |||
| joint_angles[1] - pi / 2, | |||
| joint_angles[2] - pi / 2, | |||
| joint_angles[3], | |||
| joint_angles[4], | |||
| joint_angles[5] - pi / 2, | |||
| joint_angles[6] - pi / 2, | |||
| -pi / 2, | |||
| ] | |||
| if side == "right": | |||
| d = [0, 0, 0, 0, 0, 0, -0.325, -0.01] | |||
| Tbase0 = mattransfo(-pi / 2, 0, -pi / 2, -0.19) | |||
| if side == "left": | |||
| d = [0, 0, 0, 0, 0, 0, -0.325, 0.01] | |||
| Tbase0 = mattransfo(-pi / 2, 0, -pi / 2, 0.19) | |||
| T01 = Tbase0 @ mattransfo(alpha[0], d[0], th[0], r[0]) | |||
| T12 = mattransfo(alpha[1], d[1], th[1], r[1]) | |||
| T23 = mattransfo(alpha[2], d[2], th[2], r[2]) | |||
| T34 = mattransfo(alpha[3], d[3], th[3], r[3]) | |||
| T45 = mattransfo(alpha[4], d[4], th[4], r[4]) | |||
| T56 = mattransfo(alpha[5], d[5], th[5], r[5]) | |||
| T67 = mattransfo(alpha[6], d[6], th[6], r[6]) | |||
| T78 = mattransfo(alpha[7], d[7], th[7], r[7]) | |||
| T02 = T01 @ T12 | |||
| T03 = T02 @ T23 | |||
| T04 = T03 @ T34 | |||
| T05 = T04 @ T45 | |||
| T06 = T05 @ T56 | |||
| T07 = T06 @ T67 | |||
| T08 = T07 @ T78 | |||
| return T04, T08 | |||
| def forward_kinematics(joint_angles, length=[0.28, 0.25, 0.075], side="right"): | |||
| """ | |||
| Calculate the hand-effector position using forward kinematics. | |||
| """ | |||
| T04, T08 = compute_transformation_matrices(joint_angles, length, side) | |||
| position_e = np.array(T04[0:3, 3], dtype=np.float64).flatten() | |||
| position = np.array(T08[0:3, 3], dtype=np.float64).flatten() | |||
| return position_e, position | |||
| def cost_function( | |||
| joint_angles, target_ee_coords, target_elbow_coords, length, side | |||
| ) -> float: | |||
| """ | |||
| Compute the cost function that includes end-effector and elbow position errors. | |||
| """ | |||
| # Compute the end-effector and elbow coordinates + errors | |||
| elbow_coords, ee_coords = forward_kinematics(joint_angles, length, side) | |||
| ee_error = np.linalg.norm(target_ee_coords - ee_coords) | |||
| elbow_error = np.linalg.norm(target_elbow_coords - elbow_coords) | |||
| # Compute the total cost | |||
| total_cost = ee_error + ELBOW_WEIGHT * elbow_error | |||
| return total_cost | |||
| def inverse_kinematics_fixed_wrist( | |||
| hand_position, | |||
| elbow_position, | |||
| initial_guess, | |||
| length=[0.28, 0.25, 0.075], | |||
| side="right", | |||
| ): | |||
| """ | |||
| Implement the inverse kinematics with a fixed wrist. | |||
| Input initial_guess is in degrees. Output is in degrees. | |||
| """ | |||
| pi = np.pi | |||
| # Convert initial guess from degrees to radians | |||
| initial_guess_rad = np.deg2rad(initial_guess) | |||
| joint_limits = [ | |||
| (-1.0 * pi, 0.5 * pi), | |||
| (-1.0 * pi, 10 / 180 * pi), | |||
| (-0.5 * pi, 0.5 * pi), | |||
| (-125 / 180 * pi, 0), | |||
| (0, 0), | |||
| (0, 0), | |||
| (0, 0), | |||
| ] | |||
| result = minimize( | |||
| cost_function, | |||
| initial_guess_rad, # Use radian value for optimization | |||
| args=(hand_position, elbow_position, length, side), | |||
| # method="SLSQP", | |||
| method="L-BFGS-B", | |||
| bounds=joint_limits, | |||
| # NOTE: these are new | |||
| tol=1e-3, # Higher tolerance = faster but less accurate | |||
| options={"maxiter": 20}, | |||
| ) | |||
| # Convert result from radians to degrees | |||
| return np.rad2deg(result.x) | |||
| # Helper function to convert a landmark to 3D coordinates from the camera's perspective using the human coordinate system | |||
| def get_3D_coordinates(landmark, depth_frame, w, h, intrinsics): | |||
| """Convert a landmark to 3D coordinates using depth information. | |||
| Note that this function converts the camera frame to the human frame whereas the origin remains the same. | |||
| Transforms from camera coordinates to robot coordinates: | |||
| Human x = -Camera depth | |||
| Human y = -Camera x | |||
| Human z = -Camera y | |||
| Args: | |||
| landmark: Either a landmark object with x, y, z attributes or | |||
| a numpy array/list with [x, y, z] coordinates (normalized) | |||
| depth_frame: The depth frame from the camera | |||
| w: Image width | |||
| h: Image height | |||
| intrinsics: Camera intrinsic parameters | |||
| """ | |||
| # Handle both landmark objects and numpy arrays/lists | |||
| if hasattr(landmark, "x") and hasattr(landmark, "y"): | |||
| cx, cy = int(landmark.x * w), int(landmark.y * h) | |||
| else: | |||
| # Assume it's a numpy array or list with [x, y, z] | |||
| cx, cy = int(landmark[0] * w), int(landmark[1] * h) | |||
| # Check if pixel coordinates are within image bounds | |||
| if 0 <= cx < w and 0 <= cy < h: | |||
| depth = depth_frame.get_distance(cx, cy) | |||
| if depth > 0: # Ensure depth is valid | |||
| # Get camera intrinsic parameters | |||
| fx, fy = intrinsics.fx, intrinsics.fy | |||
| ppx, ppy = intrinsics.ppx, intrinsics.ppy | |||
| # Convert to camera 3D coordinates | |||
| x = (cx - ppx) * depth / fx | |||
| y = (cy - ppy) * depth / fy | |||
| # Transform to robot coordinate system | |||
| # TODO: based on the camera's system, it should really be -z, -x, -y | |||
| return np.array([-depth, x, -y]) | |||
| # Default return if coordinates are invalid | |||
| return np.array([0, 0, 0]) | |||
| def get_3D_coordinates_of_hand( | |||
| index_landmark, pinky_landmark, depth_frame, w, h, intrinsics | |||
| ): | |||
| # Calculate average coordinates | |||
| x_pixels = (index_landmark.x + pinky_landmark.x) / 2 | |||
| y_pixels = (index_landmark.y + pinky_landmark.y) / 2 | |||
| z_zoom_factor = (index_landmark.z + pinky_landmark.z) / 2 | |||
| # Create a numpy array with the averaged values | |||
| avg_coords = np.array([x_pixels, y_pixels, z_zoom_factor]) | |||
| # Call get_3D_coordinates with all required parameters | |||
| return get_3D_coordinates(avg_coords, depth_frame, w, h, intrinsics) | |||
| def get_landmark_indices(side: Literal["right", "left"], mp_pose): | |||
| """Return MediaPipe landmark indices for the specified arm side | |||
| Args: | |||
| side: Either "right" or "left" | |||
| mp_pose: MediaPipe pose module | |||
| Returns: | |||
| Dictionary mapping landmark names to MediaPipe indices | |||
| """ | |||
| if side == "right": | |||
| return { | |||
| "shoulder": mp_pose.PoseLandmark.RIGHT_SHOULDER, | |||
| "elbow": mp_pose.PoseLandmark.RIGHT_ELBOW, | |||
| "index": mp_pose.PoseLandmark.RIGHT_INDEX, | |||
| "pinky": mp_pose.PoseLandmark.RIGHT_PINKY, | |||
| } | |||
| else: | |||
| return { | |||
| "shoulder": mp_pose.PoseLandmark.LEFT_SHOULDER, | |||
| "elbow": mp_pose.PoseLandmark.LEFT_ELBOW, | |||
| "index": mp_pose.PoseLandmark.LEFT_INDEX, | |||
| "pinky": mp_pose.PoseLandmark.LEFT_PINKY, | |||
| } | |||
| def get_arm_coordinates( | |||
| landmarks_data, landmark_indices, depth_frame, w, h, intrinsics | |||
| ): | |||
| """Get 3D coordinates for the arm | |||
| Args: | |||
| landmarks_data: MediaPipe pose landmarks | |||
| landmark_indices: Dictionary mapping landmark names to MediaPipe indices | |||
| depth_frame: RealSense depth frame | |||
| w: Image width | |||
| h: Image height | |||
| intrinsics: Camera intrinsic parameters | |||
| Returns: | |||
| Tuple of (shoulder, elbow, hand) 3D coordinates | |||
| """ | |||
| shoulder = get_3D_coordinates( | |||
| landmarks_data[landmark_indices["shoulder"]], | |||
| depth_frame, | |||
| w, | |||
| h, | |||
| intrinsics, | |||
| ) | |||
| elbow = get_3D_coordinates( | |||
| landmarks_data[landmark_indices["elbow"]], | |||
| depth_frame, | |||
| w, | |||
| h, | |||
| intrinsics, | |||
| ) | |||
| hand = get_3D_coordinates_of_hand( | |||
| landmarks_data[landmark_indices["index"]], | |||
| landmarks_data[landmark_indices["pinky"]], | |||
| depth_frame, | |||
| w, | |||
| h, | |||
| intrinsics, | |||
| ) | |||
| return shoulder, elbow, hand | |||
| def process_new_position( | |||
| target_ee_coords: np.ndarray, | |||
| current_ee_coords: np.ndarray, | |||
| position_history: list | None, | |||
| ) -> Tuple[bool, np.ndarray, np.ndarray]: | |||
| """Process a new target end effector position and determine if movement is needed | |||
| Args: | |||
| target_ee_coord: The target hand position in Reachy's coordinate system | |||
| prev_hand_pos: Previous hand position | |||
| position_history: List of previous positions for smoothing | |||
| current_ee_pos: Current end effector position | |||
| Returns: | |||
| Tuple containing: | |||
| bool: True if the position has changed enough to require an update | |||
| np.ndarray: The smoothed target position | |||
| """ | |||
| # ! We're not actually using the position history for smoothing here | |||
| # if position_history is None: | |||
| # position_history = [] | |||
| # # Apply position smoothing | |||
| # position_history.append(target_ee_coords) | |||
| # if len(position_history) > SMOOTHING_BUFFER_SIZE: | |||
| # position_history.pop(0) | |||
| # Compute EMA for smoother position | |||
| smoothed_position = ( | |||
| POSITION_ALPHA * target_ee_coords + (1 - POSITION_ALPHA) * current_ee_coords | |||
| ) | |||
| # Check if the desired position is different from current position | |||
| already_at_position = np.allclose( | |||
| current_ee_coords, smoothed_position, atol=TARGET_POS_TOLERANCE | |||
| ) | |||
| # Check if the position has changed significantly from the previous position | |||
| should_update_position = ( | |||
| not np.allclose( | |||
| current_ee_coords, smoothed_position, atol=MOVEMENT_MIN_TOLERANCE | |||
| ) | |||
| and not already_at_position | |||
| ) | |||
| return should_update_position, smoothed_position | |||
| def calculate_joint_positions_custom_ik( | |||
| target_ee_position: np.ndarray, | |||
| target_elbow_position: np.ndarray, | |||
| joint_array: np.ndarray, | |||
| side: Literal["right", "left"], | |||
| ) -> Tuple[bool, Dict[str, float]]: | |||
| """Calculate new joint positions using inverse kinematics | |||
| Args: | |||
| target_ee_position: The target hand position | |||
| target_elbow_position: The target elbow position | |||
| joint_array: Current joint positions | |||
| side: Arm side (right or left) | |||
| Returns: | |||
| Tuple containing: | |||
| bool: True if calculation was successful | |||
| Dict[str, float]: Updated joint positions dictionary | |||
| """ | |||
| prefix = f"{side[0]}_" # "r_" or "l_" | |||
| joint_dict = {} | |||
| try: | |||
| # Compute IK | |||
| joint_pos = inverse_kinematics_fixed_wrist( | |||
| hand_position=target_ee_position, | |||
| elbow_position=target_elbow_position, | |||
| initial_guess=joint_array, | |||
| length=[0.28, 0.25, 0.075], | |||
| side=side, | |||
| ) | |||
| # Get joint names for this arm | |||
| joint_names = [ | |||
| f"{prefix}shoulder_pitch", | |||
| f"{prefix}shoulder_roll", | |||
| f"{prefix}arm_yaw", | |||
| f"{prefix}elbow_pitch", | |||
| f"{prefix}forearm_yaw", | |||
| f"{prefix}wrist_pitch", | |||
| f"{prefix}wrist_roll", | |||
| ] | |||
| # Apply rate limiting and update joint dictionary | |||
| updated_joint_array = joint_array.copy() | |||
| for i, (name, value) in enumerate(zip(joint_names, joint_pos)): | |||
| # Apply rate limiting | |||
| limited_change = np.clip(value - joint_array[i], -MAX_CHANGE, MAX_CHANGE) | |||
| updated_joint_array[i] += limited_change | |||
| joint_dict[name] = updated_joint_array[i] | |||
| # Handle gripper separately - maintain closed | |||
| joint_dict[f"{prefix}gripper"] = 0 | |||
| return True, joint_dict | |||
| except Exception as e: | |||
| print(f"{side.capitalize()} arm IK calculation error: {e}") | |||
| return False, {} | |||
| def get_reachy_coordinates(point, shoulder, sf, side="right"): | |||
| """ | |||
| Args: | |||
| point: The point in camera-relative coordinates and human frame | |||
| shoulder: The shoulder (on same side as the point) in camera-relative coordinates and human frame | |||
| sf: The scaling factor | |||
| arm: Either "right" or "left" to specify which shoulder | |||
| Returns: | |||
| The point, now relative to Reachy's origin and scaled | |||
| """ | |||
| if side.lower() == "left": | |||
| return (point - shoulder) * sf + REACHY_L_SHOULDER_COORDINATES | |||
| else: | |||
| return (point - shoulder) * sf + REACHY_R_SHOULDER_COORDINATES | |||
| def entry_point( | |||
| human_shoulder_coords: np.ndarray, | |||
| human_elbow_coords: np.ndarray, | |||
| human_hand_coords: np.ndarray, | |||
| current_joint_array: np.ndarray, | |||
| side: Literal["right", "left"], | |||
| position_history: list | None = None, | |||
| ) -> Tuple[bool, Dict[str, float]]: | |||
| """ | |||
| Process 3D pose data to generate joint positions for a robotic arm. | |||
| Parameters: | |||
| human_shoulder_coords: The shoulder coordinates in depth camera frame | |||
| human_elbow_coords: The elbow coordinates in depth camera frame | |||
| human_hand_coords: The hand coordinates in depth camera frame | |||
| current_joint_array: Current joint angles (degrees) | |||
| position_history: List of recent positions for smoothing | |||
| Returns: | |||
| Tuple containing: | |||
| - bool: True if the arm was updated, False otherwise | |||
| - Dict[str, float]: Dictionary mapping joint names to angle values in degrees (e.g., 'r_shoulder_pitch': 45.0) | |||
| This function extracts 3D coordinates for the shoulder, elbow, and hand from camera data, | |||
| converts them to the robot's coordinate system, and calculates inverse kinematics | |||
| to determine joint angles needed to position the arm. | |||
| """ | |||
| if ( | |||
| human_shoulder_coords is None | |||
| or human_hand_coords is None | |||
| or human_elbow_coords is None | |||
| ): | |||
| return False, {} | |||
| # Convert to Reachy coordinate frame | |||
| target_ee_coords = get_reachy_coordinates( | |||
| human_hand_coords, human_shoulder_coords, 1.0, side | |||
| ) | |||
| target_elbow_coords = get_reachy_coordinates( | |||
| human_elbow_coords, human_shoulder_coords, 1.0, side | |||
| ) | |||
| # Calculate current end effector position using forward kinematics | |||
| current_elbow_coords, current_ee_coords = forward_kinematics( | |||
| current_joint_array, length=[0.28, 0.25, 0.075], side=side | |||
| ) | |||
| # Process the new position and determine if update is needed | |||
| should_update, target_ee_coords_smoothed = process_new_position( | |||
| target_ee_coords, | |||
| current_ee_coords, | |||
| position_history, | |||
| ) | |||
| if should_update: | |||
| # ! We're not smoothing the elbow position here | |||
| # Calculate IK and update joint positions | |||
| successful_update, joint_dict = calculate_joint_positions_custom_ik( | |||
| target_ee_coords_smoothed, | |||
| target_elbow_coords, | |||
| current_joint_array, | |||
| side, | |||
| ) | |||
| return successful_update, joint_dict | |||
| return False, {} | |||
| if __name__ == "__main__": | |||
| node = Node() | |||
| current_joint_array = np.array([0, 0, 0, 0, 0, 0, 0]) | |||
| position_history = None | |||
| side = os.getenv("SIDE", "left").lower() | |||
| ids = get_landmark_indices(side, mp_pose) | |||
| current_joint_array = initial_pose[:7] | |||
| for event in node: | |||
| if event["type"] != "INPUT": | |||
| continue | |||
| # if event["id"] == "robot_pose": | |||
| # current_joint_array = event["value"].to_pylist() | |||
| elif event["id"] == "human_pose": | |||
| # Process human pose data | |||
| values = event["value"].to_numpy().reshape(-1, 3) | |||
| shoulder = values[ids["shoulder"]] | |||
| elbow = values[ids["elbow"]] | |||
| index = values[ids["index"]] | |||
| pinky = values[ids["pinky"]] | |||
| hand = (index + pinky) / 2.0 | |||
| if hand[0] == 0 and hand[1] == 0 and hand[2] == 0: | |||
| continue | |||
| elif shoulder[0] == 0 and shoulder[1] == 0 and shoulder[2] == 0: | |||
| continue | |||
| elif elbow[0] == 0 and elbow[1] == 0 and elbow[2] == 0: | |||
| continue | |||
| success, joint_dict = entry_point( | |||
| shoulder, | |||
| elbow, | |||
| hand, | |||
| current_joint_array, | |||
| side, | |||
| position_history, | |||
| ) | |||
| if success: | |||
| current_joint_array = np.array(list(joint_dict.values())).ravel()[:7] | |||
| # Send joint_dict to the robot | |||
| current_joint_array = np.deg2rad(current_joint_array) | |||
| node.send_output( | |||
| output_id="pose", | |||
| data=pa.array(current_joint_array), | |||
| metadata={}, | |||
| ) | |||
| else: | |||
| print("Failed to update arm positions.") | |||
| @@ -0,0 +1,39 @@ | |||
| nodes: | |||
| - id: camera | |||
| build: pip install -e ../../node-hub/dora-ios-lidar | |||
| path: dora-ios-lidar | |||
| inputs: | |||
| tick: dora/timer/millis/100 | |||
| outputs: | |||
| - image | |||
| - depth | |||
| - id: dora-mediapipe | |||
| build: pip install -e ../../node-hub/dora-mediapipe | |||
| path: dora-mediapipe | |||
| inputs: | |||
| image: camera/image | |||
| depth: camera/depth | |||
| outputs: | |||
| - points3d | |||
| - id: human-shadowing | |||
| build: pip install scipy | |||
| path: human_shadowing.py | |||
| inputs: | |||
| human_pose: dora-mediapipe/points3d | |||
| outputs: | |||
| - pose | |||
| - id: plot | |||
| build: pip install -e ../../node-hub/dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| realsense/image: camera/image | |||
| realsense/depth: camera/depth | |||
| realsense/points3d: dora-mediapipe/points3d | |||
| jointstate_reachy: human-shadowing/pose | |||
| env: | |||
| # git clone git clone https://github.com/aubrune/reachy_description.git | |||
| # Replace package path with local path relative to the urdf. Ex: meshes/... | |||
| reachy_urdf: reachy_description/reachy.urdf | |||
| @@ -0,0 +1,38 @@ | |||
| nodes: | |||
| - id: camera | |||
| build: pip install -e ../../node-hub/dora-pyrealsense | |||
| path: dora-pyrealsense | |||
| inputs: | |||
| tick: dora/timer/millis/100 | |||
| outputs: | |||
| - image | |||
| - depth | |||
| - id: dora-mediapipe | |||
| build: pip install -e ../../node-hub/dora-mediapipe | |||
| path: dora-mediapipe | |||
| inputs: | |||
| image: camera/image | |||
| depth: camera/depth | |||
| outputs: | |||
| - points3d | |||
| - id: human-shadowing | |||
| build: pip install scipy | |||
| path: human_shadowing.py | |||
| inputs: | |||
| human_pose: dora-mediapipe/points3d | |||
| outputs: | |||
| - pose | |||
| - id: plot | |||
| build: pip install -e ../../node-hub/dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| realsense/image: camera/image | |||
| realsense/depth: camera/depth | |||
| realsense/points3d: dora-mediapipe/points3d | |||
| jointstate_reachy: human-shadowing/pose | |||
| env: | |||
| # git clone chttps://github.com/haixuanTao/reachy_description | |||
| reachy_urdf: reachy_description/reachy.urdf | |||
| @@ -0,0 +1,50 @@ | |||
| { | |||
| "shoulder_pan": { | |||
| "id": 1, | |||
| "model": "sts3215", | |||
| "torque": true, | |||
| "pwm_to_logical": { "90": 0, "180": 90, "0": -90, "270": 180, "360": -90 }, | |||
| "logical_to_pwm": { "0": 90, "90": 180, "-180": -90, "-90": 0, "180": 270 } | |||
| }, | |||
| "shoulder_lift": { | |||
| "id": 2, | |||
| "model": "sts3215", | |||
| "torque": true, | |||
| "pwm_to_logical": { "270": -90, "180": 0, "0": 180, "90": 90, "360": -180 }, | |||
| "logical_to_pwm": { "-90": 270, "0": 180, "-180": 360, "90": 90, "180": 0 } | |||
| }, | |||
| "elbow_flex": { | |||
| "id": 3, | |||
| "model": "sts3215", | |||
| "torque": true, | |||
| "pwm_to_logical": { "90": 90, "180": 0, "0": 180, "270": -90, "360": -180 }, | |||
| "logical_to_pwm": { "90": 90, "0": 180, "-180": 360, "-90": 270, "180": 0 } | |||
| }, | |||
| "wrist_flex": { | |||
| "id": 4, | |||
| "model": "sts3215", | |||
| "torque": true, | |||
| "pwm_to_logical": { "180": 0, "270": 90, "0": -180, "90": -90, "360": 180 }, | |||
| "logical_to_pwm": { "0": 180, "90": 270, "-180": 0, "-90": 90, "180": 360 } | |||
| }, | |||
| "wrist_roll": { | |||
| "id": 5, | |||
| "model": "sts3215", | |||
| "torque": true, | |||
| "pwm_to_logical": { "270": -90, "0": 0, "90": -90, "180": -180, "360": 0 }, | |||
| "logical_to_pwm": { | |||
| "-90": 270, | |||
| "0": 360, | |||
| "-180": 180, | |||
| "90": 450, | |||
| "180": 540 | |||
| } | |||
| }, | |||
| "gripper": { | |||
| "id": 6, | |||
| "model": "sts3215", | |||
| "torque": true, | |||
| "pwm_to_logical": { "180": 0, "270": -90, "0": 180, "90": 90, "360": -180 }, | |||
| "logical_to_pwm": { "0": 180, "-90": 270, "-180": 360, "90": 90, "180": 0 } | |||
| } | |||
| } | |||
| @@ -0,0 +1,405 @@ | |||
| <?xml version="1.0" encoding="utf-8"?> | |||
| <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) | |||
| Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634 | |||
| For more information, please see http://wiki.ros.org/sw_urdf_exporter --> | |||
| <robot | |||
| name="SO_5DOF_ARM100_8j_URDF.SLDASM"> | |||
| <link | |||
| name="base_link"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-2.45960666746703E-07 0.0311418169687909 0.0175746661003382" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.193184127927598" /> | |||
| <inertia | |||
| ixx="0.000137030709467877" | |||
| ixy="2.10136126944992E-08" | |||
| ixz="4.24087422551286E-09" | |||
| iyy="0.000169089551209259" | |||
| iyz="2.26514711036514E-05" | |||
| izz="0.000145097720857224" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Base.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Base.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <link | |||
| name="Rotation_Pitch"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-9.07886224712597E-05 0.0590971820568318 0.031089016892169" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.119226314127197" /> | |||
| <inertia | |||
| ixx="5.90408775624429E-05" | |||
| ixy="4.90800532852998E-07" | |||
| ixz="-5.90451772654387E-08" | |||
| iyy="3.21498601038881E-05" | |||
| iyz="-4.58026206663885E-06" | |||
| izz="5.86058514263952E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Rotation_Pitch.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Rotation_Pitch.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Rotation" | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 -0.0452 0.0165" | |||
| rpy="1.5708 0 0" /> | |||
| <parent | |||
| link="base_link" /> | |||
| <child | |||
| link="Rotation_Pitch" /> | |||
| <axis | |||
| xyz="0 -1 0" /> | |||
| <!-- note for the so100 arm there is no well defined effort/velocity limits at the moment --> | |||
| <limit | |||
| lower="-2.1" | |||
| upper="2.1" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Upper_Arm"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-1.7205170190925E-05 0.0701802156327694 0.00310545118155671" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.162409284599177" /> | |||
| <inertia | |||
| ixx="0.000167153146617081" | |||
| ixy="1.03902689187701E-06" | |||
| ixz="-1.20161820645189E-08" | |||
| iyy="7.01946992214245E-05" | |||
| iyz="2.11884806298698E-06" | |||
| izz="0.000213280241160769" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Upper_Arm.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Upper_Arm.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Pitch" | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 0.1025 0.0306" | |||
| rpy="1.5708 0 0" /> | |||
| <parent | |||
| link="Rotation_Pitch" /> | |||
| <child | |||
| link="Upper_Arm" /> | |||
| <axis | |||
| xyz="-1 0 0" /> | |||
| <limit | |||
| lower="-0.1" | |||
| upper="3.45" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Lower_Arm"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00339603710186651 0.00137796353960074 0.0768006751156044" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.147967774582291" /> | |||
| <inertia | |||
| ixx="0.000105333995841409" | |||
| ixy="1.73059237226499E-07" | |||
| ixz="-1.1720305455211E-05" | |||
| iyy="0.000138766654485212" | |||
| iyz="1.77429964684103E-06" | |||
| izz="5.08741652515214E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Lower_Arm.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Lower_Arm.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Elbow" | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 0.11257 0.028" | |||
| rpy="-1.5708 0 0" /> | |||
| <parent | |||
| link="Upper_Arm" /> | |||
| <child | |||
| link="Lower_Arm" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| <limit | |||
| lower="-0.2" | |||
| upper="3.14159" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Wrist_Pitch_Roll"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00852653127372418 -0.0352278997897927 -2.34622481569413E-05" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.066132067097723" /> | |||
| <inertia | |||
| ixx="1.95717492443445E-05" | |||
| ixy="-6.62714374412293E-07" | |||
| ixz="5.20089016442066E-09" | |||
| iyy="2.38028417569933E-05" | |||
| iyz="4.09549055863776E-08" | |||
| izz="3.4540143384536E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Pitch_Roll.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Pitch_Roll.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Wrist_Pitch" | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 0.0052 0.1349" | |||
| rpy="-1.5708 0 0" /> | |||
| <parent | |||
| link="Lower_Arm" /> | |||
| <child | |||
| link="Wrist_Pitch_Roll" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| <limit | |||
| lower="-1.8" | |||
| upper="1.8" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Fixed_Jaw"> | |||
| <inertial> | |||
| <origin | |||
| xyz="0.00552376906426563 -0.0280167153359021 0.000483582592841092" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.0929859131176897" /> | |||
| <inertia | |||
| ixx="4.3328249304211E-05" | |||
| ixy="7.09654328670947E-06" | |||
| ixz="5.99838530879484E-07" | |||
| iyy="3.04451747368212E-05" | |||
| iyz="-1.58743247545413E-07" | |||
| izz="5.02460913506734E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Fixed_Jaw.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <!-- <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Fixed_Jaw.STL" /> | |||
| </geometry> | |||
| </collision> --> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Roll_Follower_SO101.stl" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Wrist_Roll" | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 -0.0601 0" | |||
| rpy="0 1.5708 0" /> | |||
| <parent | |||
| link="Wrist_Pitch_Roll" /> | |||
| <child | |||
| link="Fixed_Jaw" /> | |||
| <axis | |||
| xyz="0 -1 0" /> | |||
| <limit | |||
| lower="-3.14159" | |||
| upper="3.14159" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Moving Jaw"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00161744605468241 -0.0303472584046471 0.000449645961853651" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.0202443794940372" /> | |||
| <inertia | |||
| ixx="1.10911325081525E-05" | |||
| ixy="-5.35076503033314E-07" | |||
| ixz="-9.46105662101403E-09" | |||
| iyy="3.03576451001973E-06" | |||
| iyz="-1.71146075110632E-07" | |||
| izz="8.9916083370498E-06" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving Jaw.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving_Jaw_SO101.stl" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Jaw" | |||
| type="revolute"> | |||
| <origin | |||
| xyz="-0.0202 -0.0244 0" | |||
| rpy="3.1416 0 3.33" /> | |||
| <parent | |||
| link="Fixed_Jaw" /> | |||
| <child | |||
| link="Moving Jaw" /> | |||
| <axis | |||
| xyz="0 0 1" /> | |||
| <limit | |||
| lower="0" | |||
| upper="1.7" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| </robot> | |||
| @@ -0,0 +1,365 @@ | |||
| <?xml version="1.0" encoding="utf-8"?> | |||
| <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) | |||
| Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634 | |||
| For more information, please see http://wiki.ros.org/sw_urdf_exporter --> | |||
| <robot | |||
| name="SO_5DOF_ARM100_8j_URDF.SLDASM"> | |||
| <link | |||
| name="Base"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-2.45960666746703E-07 0.0311418169687909 0.0175746661003382" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.193184127927598" /> | |||
| <inertia | |||
| ixx="0.000137030709467877" | |||
| ixy="2.10136126944992E-08" | |||
| ixz="4.24087422551286E-09" | |||
| iyy="0.000169089551209259" | |||
| iyz="2.26514711036514E-05" | |||
| izz="0.000145097720857224" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Base.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Base.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <link | |||
| name="Rotation_Pitch"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-9.07886224712597E-05 0.0590971820568318 0.031089016892169" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.119226314127197" /> | |||
| <inertia | |||
| ixx="5.90408775624429E-05" | |||
| ixy="4.90800532852998E-07" | |||
| ixz="-5.90451772654387E-08" | |||
| iyy="3.21498601038881E-05" | |||
| iyz="-4.58026206663885E-06" | |||
| izz="5.86058514263952E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Rotation_Pitch.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Rotation_Pitch.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Rotation" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 -0.0452 0.0165" | |||
| rpy="1.5708 0 0" /> | |||
| <parent | |||
| link="Base" /> | |||
| <child | |||
| link="Rotation_Pitch" /> | |||
| <axis | |||
| xyz="0 1 0" /> | |||
| </joint> | |||
| <link | |||
| name="Upper_Arm"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-1.7205170190925E-05 0.0701802156327694 0.00310545118155671" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.162409284599177" /> | |||
| <inertia | |||
| ixx="0.000167153146617081" | |||
| ixy="1.03902689187701E-06" | |||
| ixz="-1.20161820645189E-08" | |||
| iyy="7.01946992214245E-05" | |||
| iyz="2.11884806298698E-06" | |||
| izz="0.000213280241160769" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Upper_Arm.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Upper_Arm.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Pitch" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 0.1025 0.0306" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Rotation_Pitch" /> | |||
| <child | |||
| link="Upper_Arm" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Lower_Arm"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00339603710186651 0.00137796353960074 0.0768006751156044" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.147967774582291" /> | |||
| <inertia | |||
| ixx="0.000105333995841409" | |||
| ixy="1.73059237226499E-07" | |||
| ixz="-1.1720305455211E-05" | |||
| iyy="0.000138766654485212" | |||
| iyz="1.77429964684103E-06" | |||
| izz="5.08741652515214E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Lower_Arm.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Lower_Arm.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Elbow" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 0.11257 0.028" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Upper_Arm" /> | |||
| <child | |||
| link="Lower_Arm" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Wrist_Pitch_Roll"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00852653127372418 -0.0352278997897927 -2.34622481569413E-05" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.066132067097723" /> | |||
| <inertia | |||
| ixx="1.95717492443445E-05" | |||
| ixy="-6.62714374412293E-07" | |||
| ixz="5.20089016442066E-09" | |||
| iyy="2.38028417569933E-05" | |||
| iyz="4.09549055863776E-08" | |||
| izz="3.4540143384536E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Pitch_Roll.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Pitch_Roll.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Wrist_Pitch" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 0.0052 0.1349" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Lower_Arm" /> | |||
| <child | |||
| link="Wrist_Pitch_Roll" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Fixed_Jaw"> | |||
| <inertial> | |||
| <origin | |||
| xyz="0.00552376906426563 -0.0280167153359021 0.000483582592841092" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.0929859131176897" /> | |||
| <inertia | |||
| ixx="4.3328249304211E-05" | |||
| ixy="7.09654328670947E-06" | |||
| ixz="5.99838530879484E-07" | |||
| iyy="3.04451747368212E-05" | |||
| iyz="-1.58743247545413E-07" | |||
| izz="5.02460913506734E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Roll_Follower_SO101.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Roll_Follower_SO101.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Wrist_Roll" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 -0.0601 0" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Wrist_Pitch_Roll" /> | |||
| <child | |||
| link="Fixed_Jaw" /> | |||
| <axis | |||
| xyz="0 1 0" /> | |||
| </joint> | |||
| <link | |||
| name="Moving Jaw"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00161744605468241 -0.0303472584046471 0.000449645961853651" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.0202443794940372" /> | |||
| <inertia | |||
| ixx="1.10911325081525E-05" | |||
| ixy="-5.35076503033314E-07" | |||
| ixz="-9.46105662101403E-09" | |||
| iyy="3.03576451001973E-06" | |||
| iyz="-1.71146075110632E-07" | |||
| izz="8.9916083370498E-06" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving_Jaw_SO101.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving_Jaw_SO101.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Jaw" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="-0.0202 -0.0244 0" | |||
| rpy="3.1416 0 3.1416" /> | |||
| <parent | |||
| link="Fixed_Jaw" /> | |||
| <child | |||
| link="Moving Jaw" /> | |||
| <axis | |||
| xyz="0 0 1" /> | |||
| </joint> | |||
| </robot> | |||
| @@ -0,0 +1,34 @@ | |||
| nodes: | |||
| - id: so100 | |||
| build: pip install -e ../../node-hub/feetech-client | |||
| path: feetech-client | |||
| inputs: | |||
| pull_position: dora/timer/millis/33 | |||
| outputs: | |||
| - position | |||
| env: | |||
| PORT: /dev/tty.usbmodem585A0080971 | |||
| CONFIG: follower.right.json | |||
| - id: pytorch-kinematics | |||
| build: pip install -e ../../node-hub/dora-pytorch-kinematics | |||
| path: dora-pytorch-kinematics | |||
| inputs: | |||
| pose: so100/position | |||
| outputs: | |||
| - pose | |||
| env: | |||
| URDF_PATH: so100_2.urdf | |||
| END_EFFECTOR_LINK: "Moving Jaw" | |||
| so100_transform: 0 -0.3 0 | |||
| - id: plot | |||
| build: pip install -e ../../node-hub/dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| # series_so100: so100/position | |||
| series_pose: pytorch-kinematics/pose | |||
| #jointstate_so100: so100/position | |||
| env: | |||
| so100_urdf: so100_2.urdf | |||
| so100_transform: 0 -0.3 0 | |||
| @@ -135,7 +135,7 @@ class DemoApp: | |||
| "width": depth.shape[1], | |||
| "height": depth.shape[0], | |||
| "encoding": "mono16", | |||
| "focal": [ | |||
| "focal_length": [ | |||
| int(f_0), | |||
| int(f_1), | |||
| ], | |||
| @@ -0,0 +1,40 @@ | |||
| # dora-mediapipe | |||
| ## Getting started | |||
| - Install it with uv: | |||
| ```bash | |||
| uv venv -p 3.11 --seed | |||
| uv pip install -e . | |||
| ``` | |||
| ## Contribution Guide | |||
| - Format with [ruff](https://docs.astral.sh/ruff/): | |||
| ```bash | |||
| uv pip install ruff | |||
| uv run ruff check . --fix | |||
| ``` | |||
| - Lint with ruff: | |||
| ```bash | |||
| uv run ruff check . | |||
| ``` | |||
| - Test with [pytest](https://github.com/pytest-dev/pytest) | |||
| ```bash | |||
| uv pip install pytest | |||
| uv run pytest . # Test | |||
| ``` | |||
| ## YAML Specification | |||
| ## Examples | |||
| ## License | |||
| dora-mediapipe's code are released under the MIT License | |||
| @@ -0,0 +1,13 @@ | |||
| """TODO: Add docstring.""" | |||
| import os | |||
| # Define the path to the README file relative to the package directory | |||
| readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md") | |||
| # Read the content of the README file | |||
| try: | |||
| with open(readme_path, encoding="utf-8") as f: | |||
| __doc__ = f.read() | |||
| except FileNotFoundError: | |||
| __doc__ = "README file not found." | |||
| @@ -0,0 +1,6 @@ | |||
| """TODO: Add docstring.""" | |||
| from .main import main | |||
| if __name__ == "__main__": | |||
| main() | |||
| @@ -0,0 +1,136 @@ | |||
| """TODO: Add docstring.""" | |||
| import cv2 | |||
| import mediapipe as mp | |||
| import numpy as np | |||
| import pyarrow as pa | |||
| from dora import Node | |||
| # Initialiser MediaPipe Pose | |||
| mp_pose = mp.solutions.pose | |||
| pose = mp_pose.Pose() | |||
| mp_draw = mp.solutions.drawing_utils | |||
| def get_3d_coordinates(landmark, depth_frame, w, h, resolution, focal_length): | |||
| """Convert 2D landmark coordinates to 3D coordinates.""" | |||
| cx, cy = int(landmark.x * w), int(landmark.y * h) | |||
| if 0 < cx < w and 0 < cy < h: | |||
| depth = depth_frame[cy, cx] / 1_000.0 | |||
| if depth > 0: | |||
| fx, fy = focal_length | |||
| ppx, ppy = resolution | |||
| x = (cy - ppy) * depth / fy | |||
| y = (cx - ppx) * depth / fx | |||
| # Convert to right-handed coordinate system | |||
| return [x, -y, depth] | |||
| return [0, 0, 0] | |||
| def get_image(event: dict) -> np.ndarray: | |||
| """Convert the image from the event to a numpy array. | |||
| Args: | |||
| event (dict): The event containing the image data. | |||
| """ | |||
| storage = event["value"] | |||
| metadata = event["metadata"] | |||
| encoding = metadata["encoding"] | |||
| width = metadata["width"] | |||
| height = metadata["height"] | |||
| if ( | |||
| encoding == "bgr8" | |||
| or encoding == "rgb8" | |||
| or encoding in ["jpeg", "jpg", "jpe", "bmp", "webp", "png"] | |||
| ): | |||
| channels = 3 | |||
| storage_type = np.uint8 | |||
| else: | |||
| raise RuntimeError(f"Unsupported image encoding: {encoding}") | |||
| if encoding == "bgr8": | |||
| frame = ( | |||
| storage.to_numpy().astype(storage_type).reshape((height, width, channels)) | |||
| ) | |||
| frame = frame[:, :, ::-1] # OpenCV image (BGR to RGB) | |||
| elif encoding == "rgb8": | |||
| frame = ( | |||
| storage.to_numpy().astype(storage_type).reshape((height, width, channels)) | |||
| ) | |||
| elif encoding in ["jpeg", "jpg", "jpe", "bmp", "webp", "png"]: | |||
| storage = storage.to_numpy() | |||
| frame = cv2.imdecode(storage, cv2.IMREAD_COLOR) | |||
| frame = frame[:, :, ::-1] # OpenCV image (BGR to RGB) | |||
| else: | |||
| raise RuntimeError(f"Unsupported image encoding: {encoding}") | |||
| return frame | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| node = Node() | |||
| depth = None | |||
| focal_length = None | |||
| resolution = None | |||
| for event in node: | |||
| if event["type"] == "INPUT": | |||
| event_id = event["id"] | |||
| if "image" in event_id: | |||
| rgb_image = get_image(event) | |||
| width = rgb_image.shape[1] | |||
| height = rgb_image.shape[0] | |||
| pose_results = pose.process(rgb_image) | |||
| if pose_results.pose_landmarks: | |||
| values = pose_results.pose_landmarks.landmark | |||
| values = np.array( | |||
| [ | |||
| [landmark.x * width, landmark.y * height] | |||
| for landmark in pose_results.pose_landmarks.landmark | |||
| ] | |||
| ) | |||
| # Warning: Make sure to add my_output_id and my_input_id within the dataflow. | |||
| node.send_output( | |||
| output_id="points2d", | |||
| data=pa.array(values.ravel()), | |||
| metadata={}, | |||
| ) | |||
| if depth is not None: | |||
| values = np.array( | |||
| [ | |||
| get_3d_coordinates( | |||
| landmark, | |||
| depth, | |||
| width, | |||
| height, | |||
| resolution, | |||
| focal_length, | |||
| ) | |||
| for landmark in pose_results.pose_landmarks.landmark | |||
| ] | |||
| ) | |||
| # Warning: Make sure to add my_output_id and my_input_id within the dataflow. | |||
| node.send_output( | |||
| output_id="points3d", | |||
| data=pa.array(values.ravel()), | |||
| metadata={}, | |||
| ) | |||
| else: | |||
| print("No pose landmarks detected.") | |||
| elif "depth" in event_id: | |||
| metadata = event["metadata"] | |||
| _encoding = metadata["encoding"] | |||
| width = metadata["width"] | |||
| height = metadata["height"] | |||
| focal_length = metadata["focal_length"] | |||
| resolution = metadata["resolution"] | |||
| depth = event["value"].to_numpy().reshape((height, width)) | |||
| if __name__ == "__main__": | |||
| main() | |||
| @@ -0,0 +1,25 @@ | |||
| [project] | |||
| name = "dora-mediapipe" | |||
| version = "0.0.0" | |||
| authors = [{ name = "Your Name", email = "email@email.com" }] | |||
| description = "dora-mediapipe" | |||
| license = { text = "MIT" } | |||
| readme = "README.md" | |||
| requires-python = ">=3.8" | |||
| dependencies = [ | |||
| "dora-rs >= 0.3.9", | |||
| "mediapipe>=0.10.14", | |||
| ] | |||
| [dependency-groups] | |||
| dev = ["pytest >=8.1.1", "ruff >=0.9.1"] | |||
| [project.scripts] | |||
| dora-mediapipe = "dora_mediapipe.main:main" | |||
| [tool.ruff.lint] | |||
| extend-select = [ | |||
| "D", # pydocstyle | |||
| "UP" | |||
| ] | |||
| @@ -0,0 +1,13 @@ | |||
| """Test module for dora_mediapipe package.""" | |||
| import pytest | |||
| def test_import_main(): | |||
| """Test importing and running the main function.""" | |||
| from dora_mediapipe.main import main | |||
| # Check that everything is working, and catch Dora RuntimeError | |||
| # as we're not running in a Dora dataflow. | |||
| with pytest.raises(RuntimeError): | |||
| main() | |||
| @@ -0,0 +1,40 @@ | |||
| # dora-pytorch-kinematics | |||
| ## Getting started | |||
| - Install it with uv: | |||
| ```bash | |||
| uv venv -p 3.11 --seed | |||
| uv pip install -e . | |||
| ``` | |||
| ## Contribution Guide | |||
| - Format with [ruff](https://docs.astral.sh/ruff/): | |||
| ```bash | |||
| uv pip install ruff | |||
| uv run ruff check . --fix | |||
| ``` | |||
| - Lint with ruff: | |||
| ```bash | |||
| uv run ruff check . | |||
| ``` | |||
| - Test with [pytest](https://github.com/pytest-dev/pytest) | |||
| ```bash | |||
| uv pip install pytest | |||
| uv run pytest . # Test | |||
| ``` | |||
| ## YAML Specification | |||
| ## Examples | |||
| ## License | |||
| dora-pytorch-kinematics's code are released under the MIT License | |||
| @@ -0,0 +1,13 @@ | |||
| """TODO: Add docstring.""" | |||
| import os | |||
| # Define the path to the README file relative to the package directory | |||
| readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md") | |||
| # Read the content of the README file | |||
| try: | |||
| with open(readme_path, encoding="utf-8") as f: | |||
| __doc__ = f.read() | |||
| except FileNotFoundError: | |||
| __doc__ = "README file not found." | |||
| @@ -0,0 +1,6 @@ | |||
| """TODO: Add docstring.""" | |||
| from .main import main | |||
| if __name__ == "__main__": | |||
| main() | |||
| @@ -0,0 +1,251 @@ | |||
| """TODO: Add docstring.""" | |||
| import os | |||
| from typing import List, Optional, Tuple, Union | |||
| import numpy as np | |||
| import pyarrow as pa | |||
| import pytorch_kinematics as pk | |||
| import torch | |||
| from dora import Node | |||
| from pytorch_kinematics.transforms.rotation_conversions import matrix_to_euler_angles | |||
| def get_xyz_rpy_array_from_transform3d( | |||
| transform: "pytorch_kinematics.transforms.Transform3d", convention: str = "XYZ" | |||
| ) -> torch.Tensor: | |||
| """Extracts translation (xyz) and rotation (RPY Euler angles in radians) | |||
| from a pytorch_kinematics Transform3d object and returns them concatenated | |||
| into a single tensor. | |||
| Args: | |||
| transform: A Transform3d object or a batch of Transform3d objects. | |||
| Expected to have a method like get_matrix() or directly | |||
| access attributes like .R and .T. | |||
| convention: The Euler angle convention (e.g., "XYZ", "ZYX"). | |||
| RPY typically corresponds to "XYZ" (roll around X, | |||
| pitch around Y, yaw around Z). Adjust if needed. | |||
| Returns: | |||
| A single tensor of shape (..., 6) where the last dimension contains | |||
| [x, y, z, roll, pitch, yaw] in radians. | |||
| """ | |||
| # Attempt to get the full 4x4 transformation matrix | |||
| # Common methods are get_matrix() or accessing internal properties. | |||
| # This might need adjustment based on the specific API version. | |||
| matrix = transform.get_matrix() # Shape (..., 4, 4) | |||
| # Extract translation (first 3 elements of the last column) | |||
| xyz = matrix[..., :3, 3] | |||
| # Extract rotation matrix (top-left 3x3 submatrix) | |||
| rotation_matrix = matrix[..., :3, :3] | |||
| # Convert the rotation matrix to Euler angles in radians | |||
| # The matrix_to_euler_angles function likely exists based on pytorch3d's structure | |||
| rpy = matrix_to_euler_angles( | |||
| rotation_matrix, convention=convention | |||
| ) # Shape (..., 3) | |||
| # Concatenate xyz and rpy along the last dimension | |||
| return torch.cat((xyz, rpy), dim=-1) # Shape (..., 6) | |||
| class RobotKinematics: | |||
| """Concise wrapper for pytorch_kinematics FK and IK operations, | |||
| closely mirroring library usage patterns. | |||
| """ | |||
| def __init__( | |||
| self, | |||
| urdf_path: str, | |||
| end_effector_link: str, | |||
| device: Union[str, torch.device] = "cpu", | |||
| ): | |||
| """Initializes the kinematic chain from a URDF. | |||
| Args: | |||
| urdf_path (str): Path to the URDF file. | |||
| end_effector_link (str): Name of the end-effector link. | |||
| device (Union[str, torch.device]): Computation device ('cpu' or 'cuda'). | |||
| """ | |||
| self.device = torch.device(device) | |||
| try: | |||
| # Load kinematic chain (core pytorch_kinematics object) | |||
| self.chain = pk.build_serial_chain_from_urdf( | |||
| open(urdf_path, mode="rb").read(), end_effector_link | |||
| ).to(device=self.device) | |||
| except Exception as e: | |||
| raise RuntimeError(f"Failed to build chain from URDF: {urdf_path}") from e | |||
| self.num_joints = len(self.chain.get_joint_parameter_names(exclude_fixed=True)) | |||
| # Default initial guess for IK if none provided | |||
| self._default_q = torch.zeros( | |||
| (1, self.num_joints), device=self.device, dtype=torch.float32 | |||
| ) | |||
| # print(f"Initialized RobotKinematicsConcise: {self.num_joints} joints, EE='{end_effector_link}', device='{self.device}'") # Optional print | |||
| def _prepare_joint_tensor( | |||
| self, joints: Union[List[float], torch.Tensor], batch_dim_required: bool = True | |||
| ) -> torch.Tensor: | |||
| """Validates and formats joint angles input tensor.""" | |||
| if isinstance(joints, list): | |||
| j = torch.tensor(joints, dtype=torch.float32, device=self.device) | |||
| elif isinstance(joints, np.ndarray): | |||
| j = torch.tensor(joints, device=self.device, dtype=torch.float32) | |||
| elif isinstance(joints, torch.Tensor): | |||
| j = joints.to(device=self.device, dtype=torch.float32) | |||
| else: | |||
| raise TypeError( | |||
| "Joints must be a list or torch.Tensor and got: ", joints.type | |||
| ) | |||
| if j.ndim == 1: | |||
| if j.shape[0] != self.num_joints: | |||
| raise ValueError(f"Expected {self.num_joints} joints, got {j.shape[0]}") | |||
| if batch_dim_required: | |||
| j = j.unsqueeze(0) # Add batch dimension if needed (e.g., shape (1, N)) | |||
| elif j.ndim == 2: | |||
| if j.shape[1] != self.num_joints: | |||
| raise ValueError( | |||
| f"Expected {self.num_joints} joints (dim 1), got {j.shape[1]}" | |||
| ) | |||
| if batch_dim_required and j.shape[0] != 1: | |||
| # Most common IK solvers expect batch=1 for initial guess, FK can handle batches | |||
| # Relaxing this check slightly, but user should be aware for IK | |||
| pass | |||
| else: | |||
| raise ValueError(f"Joint tensor must have 1 or 2 dimensions, got {j.ndim}") | |||
| return j | |||
| def compute_fk( | |||
| self, joint_angles: Union[List[float], torch.Tensor] | |||
| ) -> pk.Transform3d: | |||
| """Computes Forward Kinematics (FK). | |||
| Args: | |||
| joint_angles (Union[List[float], torch.Tensor]): Joint angles (radians). | |||
| Shape (num_joints,) or (B, num_joints). | |||
| Returns: | |||
| pk.Transform3d: End-effector pose(s). Batched if input is batched. | |||
| """ | |||
| # robot frame | |||
| pos = torch.tensor([0.0, 0.0, 0.0]) | |||
| rot = torch.tensor([0.0, 0.0, 0.0]) | |||
| rob_tf = pk.Transform3d(pos=pos, rot=rot) | |||
| angles_tensor = self._prepare_joint_tensor( | |||
| joint_angles, batch_dim_required=False | |||
| ) # FK handles batches naturally | |||
| # Direct call to pytorch_kinematics FK | |||
| goal_in_rob_frame_tf = self.chain.forward_kinematics( | |||
| angles_tensor, end_only=True | |||
| ) | |||
| goal_tf = rob_tf.compose(goal_in_rob_frame_tf) | |||
| return goal_tf | |||
| def compute_ik( | |||
| self, | |||
| target_pose: pk.Transform3d, | |||
| initial_guess: Optional[Union[List[float], torch.Tensor]] = None, | |||
| iterations: int = 100, | |||
| lr: float = 0.1, | |||
| error_tolerance: float = 1e-4, | |||
| ) -> Tuple[torch.Tensor, bool]: | |||
| """Computes Inverse Kinematics (IK) using PseudoInverseIK. | |||
| Args: | |||
| target_pose (pk.Transform3d): Target end-effector pose (batch size 1). | |||
| initial_guess (Optional): Initial joint angles guess. Uses zeros if None. | |||
| Shape (num_joints,) or (1, num_joints). | |||
| iterations (int): Max solver iterations. | |||
| lr (float): Solver learning rate. | |||
| error_tolerance (float): Solver convergence tolerance. | |||
| Returns: | |||
| Tuple[torch.Tensor, bool]: Solution joint angles (1, num_joints), convergence status. | |||
| """ | |||
| if not isinstance(target_pose, pk.Transform3d): | |||
| raise TypeError("target_pose must be a pk.Transform3d") | |||
| target_pose = target_pose.to(device=self.device) | |||
| if target_pose.get_matrix().shape[0] != 1: | |||
| raise ValueError( | |||
| "target_pose must have batch size 1 for this IK method." | |||
| ) # Common limitation | |||
| # Prepare initial guess (q_init) | |||
| q_init = self._prepare_joint_tensor( | |||
| initial_guess if initial_guess is not None else self._default_q, | |||
| batch_dim_required=True, | |||
| ) | |||
| if q_init.shape[0] != 1: | |||
| raise ValueError( | |||
| "initial_guess must result in batch size 1 for this IK setup." | |||
| ) # Enforce batch=1 for guess | |||
| q_init = q_init.requires_grad_(True) # Need gradient for solver | |||
| # Instantiate and run the IK solver (core pytorch_kinematics objects/methods) | |||
| ik_solver = pk.PseudoInverseIK( | |||
| self.chain, | |||
| max_iterations=300, | |||
| retry_configs=q_init, | |||
| joint_limits=torch.tensor(self.chain.get_joint_limits()), | |||
| early_stopping_any_converged=True, | |||
| early_stopping_no_improvement="all", | |||
| debug=False, | |||
| lr=0.05, | |||
| ) | |||
| solution_angles = ik_solver.solve(target_pose) | |||
| return solution_angles.solutions.detach() | |||
| def main(): | |||
| """TODO: Add docstring.""" | |||
| node = Node() | |||
| urdf_path = os.getenv("URDF_PATH") | |||
| end_effector_link = os.getenv("END_EFFECTOR_LINK") | |||
| robot = RobotKinematics(urdf_path, end_effector_link) | |||
| last_known_state = None | |||
| for event in node: | |||
| if event["type"] == "INPUT": | |||
| metadata = event["metadata"] | |||
| match metadata["encoding"]: | |||
| case "xyzrpy": | |||
| # Apply Inverse Kinematics | |||
| if last_known_state is not None: | |||
| target = event["value"].to_numpy() | |||
| target = target.astype(np.float32) | |||
| target = pk.Transform3d( | |||
| pos=target[:3], | |||
| rot=pk.transforms.euler_angles_to_matrix( | |||
| torch.tensor(target[3:6]), convention="XYZ" | |||
| ), | |||
| ) | |||
| solution = robot.compute_ik(target, last_known_state) | |||
| metadata["encoding"] = "jointstate" | |||
| last_known_state = solution.numpy().ravel() | |||
| solution = pa.array(last_known_state) | |||
| node.send_output(event["id"], solution, metadata=metadata) | |||
| print(solution) | |||
| case "jointstate": | |||
| target = event["value"].to_numpy() | |||
| last_known_state = target | |||
| # Apply Forward Kinematics | |||
| target = robot.compute_fk(target) | |||
| target = np.array(get_xyz_rpy_array_from_transform3d(target)) | |||
| target = pa.array(target.ravel()) | |||
| metadata["encoding"] = "xyzrpy" | |||
| node.send_output(event["id"], target, metadata=metadata) | |||
| if __name__ == "__main__": | |||
| main() | |||
| @@ -0,0 +1,25 @@ | |||
| [project] | |||
| name = "dora-pytorch-kinematics" | |||
| version = "0.0.0" | |||
| authors = [{ name = "Your Name", email = "email@email.com" }] | |||
| description = "dora-pytorch-kinematics" | |||
| license = { text = "MIT" } | |||
| readme = "README.md" | |||
| requires-python = ">=3.8" | |||
| dependencies = [ | |||
| "dora-rs >= 0.3.9", | |||
| "pytorch-kinematics>=0.7.5", | |||
| ] | |||
| [dependency-groups] | |||
| dev = ["pytest >=8.1.1", "ruff >=0.9.1"] | |||
| [project.scripts] | |||
| dora-pytorch-kinematics = "dora_pytorch_kinematics.main:main" | |||
| [tool.ruff.lint] | |||
| extend-select = [ | |||
| "D", # pydocstyle | |||
| "UP" | |||
| ] | |||
| @@ -0,0 +1,13 @@ | |||
| """Test module for dora_pytorch_kinematics package.""" | |||
| import pytest | |||
| def test_import_main(): | |||
| """Test importing and running the main function.""" | |||
| from dora_pytorch_kinematics.main import main | |||
| # Check that everything is working, and catch Dora RuntimeError | |||
| # as we're not running in a Dora dataflow. | |||
| with pytest.raises(RuntimeError): | |||
| main() | |||
| @@ -28,6 +28,7 @@ pyo3 = { workspace = true, features = [ | |||
| "generate-import-lib", | |||
| ], optional = true } | |||
| bytemuck = "1.20.0" | |||
| rand = "0.9.1" | |||
| [lib] | |||
| @@ -3,12 +3,14 @@ | |||
| use std::{collections::HashMap, env::VarError, path::Path}; | |||
| use dora_node_api::{ | |||
| arrow::array::{Array, Float32Array, Float64Array, StringArray, UInt16Array, UInt8Array}, | |||
| arrow::{array::AsArray, datatypes::Float32Type}, | |||
| arrow::{ | |||
| array::{Array, AsArray, Float64Array, StringArray, UInt16Array, UInt8Array}, | |||
| datatypes::Float32Type, | |||
| }, | |||
| dora_core::config::DataId, | |||
| DoraNode, Event, Parameter, | |||
| into_vec, DoraNode, Event, Parameter, | |||
| }; | |||
| use eyre::{eyre, Context, ContextCompat, Result}; | |||
| use eyre::{eyre, Context, Result}; | |||
| use rerun::{ | |||
| components::ImageBuffer, | |||
| @@ -32,6 +34,7 @@ pub fn lib_main() -> Result<()> { | |||
| // Setup an image cache to paint depth images. | |||
| let mut image_cache = HashMap::new(); | |||
| let mut mask_cache: HashMap<DataId, Vec<bool>> = HashMap::new(); | |||
| let mut color_cache: HashMap<DataId, rerun::Color> = HashMap::new(); | |||
| let mut options = SpawnOptions::default(); | |||
| let memory_limit = match std::env::var("RERUN_MEMORY_LIMIT") { | |||
| @@ -316,11 +319,7 @@ pub fn lib_main() -> Result<()> { | |||
| }; | |||
| mask_cache.insert(id.clone(), masks.clone()); | |||
| } else if id.as_str().contains("jointstate") { | |||
| let buffer: &Float32Array = data | |||
| .as_any() | |||
| .downcast_ref() | |||
| .context("jointstate is not float32")?; | |||
| let mut positions: Vec<f32> = buffer.values().to_vec(); | |||
| let mut positions: Vec<f32> = into_vec(&data)?; | |||
| // Match file name | |||
| let mut id = id.as_str().replace("jointstate_", ""); | |||
| @@ -340,10 +339,41 @@ pub fn lib_main() -> Result<()> { | |||
| update_visualization(&rec, chain, &id, &positions)?; | |||
| } else { | |||
| println!("Could not find chain for {}", id); | |||
| println!( | |||
| "Could not find chain for {}. Only contains: {:#?}", | |||
| id, | |||
| chains.keys() | |||
| ); | |||
| } | |||
| } else if id.as_str().contains("series") { | |||
| update_series(&rec, id, data).context("could not plot series")?; | |||
| } else if id.as_str().contains("points3d") { | |||
| // Get color or assign random color in cache | |||
| let color = color_cache.get(&id); | |||
| let color = if let Some(color) = color { | |||
| color.clone() | |||
| } else { | |||
| let color = | |||
| rerun::Color::from_rgb(rand::random::<u8>(), 180, rand::random::<u8>()); | |||
| color_cache.insert(id.clone(), color.clone()); | |||
| color | |||
| }; | |||
| let dataid = id; | |||
| // get a random color | |||
| if let Ok(buffer) = into_vec::<f32>(&data) { | |||
| let mut points = vec![]; | |||
| let mut colors = vec![]; | |||
| buffer.chunks(3).for_each(|chunk| { | |||
| points.push((chunk[0], chunk[1], chunk[2])); | |||
| colors.push(color); | |||
| }); | |||
| let points = Points3D::new(points).with_radii(vec![0.013; colors.len()]); | |||
| rec.log(dataid.as_str(), &points.with_colors(colors)) | |||
| .context("could not log points")?; | |||
| } | |||
| } else { | |||
| println!("Could not find handler for {}", id); | |||
| } | |||
| @@ -88,8 +88,8 @@ pub fn init_urdf(rec: &RecordingStream) -> Result<HashMap<String, Chain<f32>>> { | |||
| ), | |||
| ) | |||
| .unwrap(); | |||
| chains.insert(path, chain); | |||
| } | |||
| chains.insert(path, chain); | |||
| } | |||
| Ok(chains) | |||
| @@ -113,11 +113,14 @@ pub fn update_visualization( | |||
| .world_transform() | |||
| .context("Could not get world transform")?; | |||
| let link_to_parent = if link_name != "base_link" { | |||
| let parent = link.parent().context("could not get parent")?; | |||
| parent | |||
| .world_transform() | |||
| .context("Could not get world transform")? | |||
| .inv_mul(&link_to_world) | |||
| if let Some(parent) = link.parent() { | |||
| parent | |||
| .world_transform() | |||
| .context("Could not get world transform")? | |||
| .inv_mul(&link_to_world) | |||
| } else { | |||
| link_to_world | |||
| } | |||
| } else { | |||
| link_to_world | |||
| }; | |||
| @@ -0,0 +1,12 @@ | |||
| [package] | |||
| name = "dora-rustypot" | |||
| version = "0.1.0" | |||
| edition = "2021" | |||
| # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html | |||
| [dependencies] | |||
| dora-node-api = "0.3.11" | |||
| eyre = "0.6.12" | |||
| rustypot = { git = "https://github.com/pollen-robotics/rustypot", branch = "next-release-1.0" } | |||
| serialport = "4.7.1" | |||
| @@ -0,0 +1,76 @@ | |||
| use dora_node_api::dora_core::config::DataId; | |||
| use dora_node_api::{into_vec, DoraNode, Event, IntoArrow, Parameter}; | |||
| use eyre::{Context, Result}; | |||
| use rustypot::servo::feetech::sts3215::Sts3215Controller; | |||
| use std::collections::BTreeMap; | |||
| use std::time::Duration; | |||
| fn main() -> Result<()> { | |||
| let (mut node, mut events) = DoraNode::init_from_env()?; | |||
| let serialportname: String = std::env::var("PORT").context("Serial port name not provided")?; | |||
| let baudrate: u32 = std::env::var("BAUDRATE") | |||
| .unwrap_or_else(|_| "1000000".to_string()) | |||
| .parse() | |||
| .context("Invalid baudrate")?; | |||
| let ids = std::env::var("IDS") | |||
| .unwrap_or_else(|_| "1,2,3,4,5,6".to_string()) | |||
| .split(&[',', ' '][..]) | |||
| .map(|s| s.parse::<u8>().unwrap()) | |||
| .collect::<Vec<u8>>(); | |||
| let torque = std::env::var("TORQUE") | |||
| .unwrap_or_else(|_| "false".to_string()) | |||
| .parse::<bool>() | |||
| .context("Invalid torque")?; | |||
| let serial_port = serialport::new(serialportname, baudrate) | |||
| .timeout(Duration::from_millis(1000)) | |||
| .open()?; | |||
| let mut c = Sts3215Controller::new() | |||
| .with_protocol_v1() | |||
| .with_serial_port(serial_port); | |||
| if torque { | |||
| let truthies = vec![true; ids.len()]; | |||
| c.write_torque_enable(&ids, &truthies) | |||
| .expect("could not enable torque"); | |||
| } else { | |||
| let falsies = vec![false; ids.len()]; | |||
| c.write_torque_enable(&ids, &falsies) | |||
| .expect("could not enable torque"); | |||
| } | |||
| while let Some(event) = events.recv() { | |||
| match event { | |||
| Event::Input { | |||
| id, | |||
| metadata: _, | |||
| data, | |||
| } => match id.as_str() { | |||
| "tick" => { | |||
| if let Ok(joints) = c.read_present_position(&ids) { | |||
| let mut parameter = BTreeMap::new(); | |||
| parameter.insert( | |||
| "encoding".to_string(), | |||
| Parameter::String("jointstate".to_string()), | |||
| ); | |||
| node.send_output( | |||
| DataId::from("pose".to_string()), | |||
| parameter, | |||
| joints.into_arrow(), | |||
| ) | |||
| .unwrap(); | |||
| }; | |||
| } | |||
| "pose" => { | |||
| let data: Vec<f64> = into_vec(&data).expect("could not cast values"); | |||
| c.write_goal_position(&ids, &data).unwrap(); | |||
| } | |||
| other => eprintln!("Received input `{other}`"), | |||
| }, | |||
| _ => {} | |||
| } | |||
| } | |||
| Ok(()) | |||
| } | |||
| @@ -1,4 +1,5 @@ | |||
| """TODO: Add docstring.""" | |||
| import enum | |||
| from typing import Union | |||
| @@ -150,7 +151,9 @@ class FeetechBus: | |||
| ] | |||
| values = [ | |||
| np.uint32(32767 - value.as_py()) if value < 0 else np.uint32(value.as_py()) | |||
| np.uint32(32767 - value.as_py()) | |||
| if value.as_py() < 0 | |||
| else np.uint32(value.as_py()) | |||
| for value in data.field("values") | |||
| ] | |||
| @@ -243,7 +246,9 @@ class FeetechBus: | |||
| values = pa.array( | |||
| [ | |||
| self.group_readers[group_key].getData( | |||
| idx, packet_address, packet_bytes_size, | |||
| idx, | |||
| packet_address, | |||
| packet_bytes_size, | |||
| ) | |||
| for idx in motor_ids | |||
| ], | |||
| @@ -0,0 +1,200 @@ | |||
| """Module for configuring and setting up the SO100 robot hardware. | |||
| This module provides functionality for initializing and configuring the SO100 robot's | |||
| servo motors and other hardware components. | |||
| The program will: | |||
| 1. Disable all torque motors of provided SO100. | |||
| 2. Ask the user to move the SO100 to the position 1 (see CONFIGURING.md for more details). | |||
| 3. Record the position of the SO100. | |||
| 4. Ask the user to move the SO100 to the position 2 (see CONFIGURING.md for more details). | |||
| 5. Record the position of the SO100. | |||
| 8. Calculate the offset and inverted mode of the SO100. | |||
| 9. Let the user verify in real time that the SO100 is working properly. | |||
| It will also enable all appropriate operating modes for the SO100. | |||
| """ | |||
| import argparse | |||
| import json | |||
| import time | |||
| import pyarrow as pa | |||
| from pwm_position_control.functions import construct_control_table | |||
| from pwm_position_control.tables import ( | |||
| construct_logical_to_pwm_conversion_table_arrow, | |||
| construct_pwm_to_logical_conversion_table_arrow, | |||
| ) | |||
| from pwm_position_control.transform import pwm_to_logical_arrow, wrap_joints_and_values | |||
| from .bus import FeetechBus, OperatingMode, TorqueMode | |||
| FULL_ARM = pa.array( | |||
| [ | |||
| "shoulder_pan", | |||
| "shoulder_lift", | |||
| "elbow_flex", | |||
| "wrist_flex", | |||
| "wrist_roll", | |||
| "gripper", | |||
| ], | |||
| type=pa.string(), | |||
| ) | |||
| ARM_WITHOUT_GRIPPER = pa.array( | |||
| ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"], | |||
| type=pa.string(), | |||
| ) | |||
| GRIPPER = pa.array(["gripper"], type=pa.string()) | |||
| def pause(): | |||
| """Pause execution and wait for user input.""" | |||
| input("Press Enter to continue...") | |||
| def configure_servos(bus: FeetechBus): | |||
| """Configure the servos with default settings. | |||
| Args: | |||
| bus: The FeetechBus instance to configure. | |||
| """ | |||
| bus.write_torque_enable( | |||
| wrap_joints_and_values(FULL_ARM, [TorqueMode.DISABLED.value] * 6), | |||
| ) | |||
| bus.write_operating_mode( | |||
| wrap_joints_and_values(FULL_ARM, [OperatingMode.ONE_TURN.value] * 6), | |||
| ) | |||
| bus.write_max_angle_limit( | |||
| wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6), | |||
| ) | |||
| bus.write_min_angle_limit( | |||
| wrap_joints_and_values(FULL_ARM, [pa.scalar(0, pa.uint32())] * 6), | |||
| ) | |||
| def main(): | |||
| """Run the servo configuration process.""" | |||
| parser = argparse.ArgumentParser( | |||
| description="SO100 Auto Configure: This program is used to automatically configure the Low Cost Robot (SO100) " | |||
| "for the user.", | |||
| ) | |||
| parser.add_argument( | |||
| "--port", | |||
| type=str, | |||
| required=True, | |||
| help="The port of the SO100.", | |||
| ) | |||
| parser.add_argument( | |||
| "--right", | |||
| action="store_true", | |||
| help="If the SO100 is on the right side of the user.", | |||
| ) | |||
| parser.add_argument( | |||
| "--left", | |||
| action="store_true", | |||
| help="If the SO100 is on the left side of the user.", | |||
| ) | |||
| args = parser.parse_args() | |||
| if args.right and args.left: | |||
| raise ValueError("You cannot specify both --right and --left.") | |||
| args = parser.parse_args() | |||
| targets = ( | |||
| wrap_joints_and_values(FULL_ARM, [0, -90, 90, 0, -90, 0]), | |||
| wrap_joints_and_values(FULL_ARM, [90, 0, 0, 90, 0, -90]), | |||
| ) | |||
| arm = FeetechBus( | |||
| args.port, | |||
| { | |||
| "shoulder_pan": (1, "st3215"), | |||
| "shoulder_lift": (2, "st3215"), | |||
| "elbow_flex": (3, "st3215"), | |||
| "wrist_flex": (4, "st3215"), | |||
| "wrist_roll": (5, "st3215"), | |||
| "gripper": (6, "st3215"), | |||
| }, | |||
| ) | |||
| configure_servos(arm) | |||
| print("Please move the SO100 to the first position.") | |||
| pause() | |||
| pwm_position_1 = arm.read_position(FULL_ARM) | |||
| print(pwm_position_1) | |||
| print("Please move the SO100 to the second position.") | |||
| pause() | |||
| pwm_position_2 = arm.read_position(FULL_ARM) | |||
| print(pwm_position_2) | |||
| print("Configuration completed.") | |||
| pwm_positions = (pwm_position_1, pwm_position_2) | |||
| pwm_to_logical_conversion_table = construct_pwm_to_logical_conversion_table_arrow( | |||
| pwm_positions, | |||
| targets, | |||
| ) | |||
| logical_to_pwm_conversion_table = construct_logical_to_pwm_conversion_table_arrow( | |||
| pwm_positions, | |||
| targets, | |||
| ) | |||
| control_table_json = {} | |||
| for i in range(len(FULL_ARM)): | |||
| control_table_json[FULL_ARM[i].as_py()] = { | |||
| "id": i + 1, | |||
| "model": "sts3215", | |||
| "torque": True, | |||
| "pwm_to_logical": pwm_to_logical_conversion_table[FULL_ARM[i].as_py()], | |||
| "logical_to_pwm": logical_to_pwm_conversion_table[FULL_ARM[i].as_py()], | |||
| } | |||
| left = "left" if args.left else "right" | |||
| path = ( | |||
| input( | |||
| f"Please enter the path of the configuration file (default is follower.{left}.json): ", | |||
| ) | |||
| or f"follower.{left}.json" | |||
| ) | |||
| with open(path, "w") as file: | |||
| json.dump(control_table_json, file) | |||
| control_table = construct_control_table( | |||
| pwm_to_logical_conversion_table, | |||
| logical_to_pwm_conversion_table, | |||
| ) | |||
| while True: | |||
| try: | |||
| pwm_position = arm.read_position(FULL_ARM) | |||
| logical_position = pwm_to_logical_arrow( | |||
| pwm_position, | |||
| control_table, | |||
| ranged=True, | |||
| ).field("values") | |||
| print(f"Logical Position: {logical_position}") | |||
| except ConnectionError: | |||
| print( | |||
| "Connection error occurred. Please check the connection and try again.", | |||
| ) | |||
| time.sleep(0.5) | |||
| if __name__ == "__main__": | |||
| main() | |||
| @@ -4,6 +4,7 @@ import argparse | |||
| import json | |||
| import os | |||
| import numpy as np | |||
| import pyarrow as pa | |||
| from dora import Node | |||
| @@ -65,9 +66,16 @@ class Client: | |||
| def pull_position(self, node, metadata): | |||
| """TODO: Add docstring.""" | |||
| try: | |||
| struct = self.bus.read_position(self.config["joints"]) | |||
| metadata["encoding"] = "jointstate" | |||
| node.send_output( | |||
| "position", | |||
| self.bus.read_position(self.config["joints"]), | |||
| pa.array( | |||
| np.deg2rad( | |||
| ((struct.flatten()[1].to_numpy() - 2048) / 4096.0) * 360 | |||
| ), | |||
| type=pa.float32(), | |||
| ), | |||
| metadata, | |||
| ) | |||
| @@ -166,21 +174,12 @@ def main(): | |||
| "torque": wrap_joints_and_values( | |||
| pa.array(config.keys(), pa.string()), | |||
| pa.array( | |||
| [ | |||
| ( | |||
| TorqueMode.ENABLED.value | |||
| if config[joint]["torque"] | |||
| else TorqueMode.DISABLED.value | |||
| ) | |||
| for joint in joints | |||
| ], | |||
| [(TorqueMode.DISABLED.value) for joint in joints], | |||
| type=pa.uint32(), | |||
| ), | |||
| ), | |||
| } | |||
| print("Feetech Client Configuration: ", bus, flush=True) | |||
| client = Client(bus) | |||
| client.run() | |||
| client.close() | |||
| @@ -1,16 +1,17 @@ | |||
| [project] | |||
| name = "feetech-client" | |||
| version = "0.1" | |||
| authors = [{ name = "Hennzau", email = "<dev@enzo-le-van.fr>"}] | |||
| authors = [{ name = "Hennzau", email = "<dev@enzo-le-van.fr>" }] | |||
| description = "Dora Node client for feetech motors." | |||
| license = { text = "MIT" } | |||
| readme = "README.md" | |||
| requires-python = ">=3.9" | |||
| dependencies = [ | |||
| "dora-rs == 0.3.5", | |||
| "numpy <= 2.0.0", | |||
| "feetech-servo-sdk == 1.0.0", | |||
| "dora-rs == 0.3.11", | |||
| "numpy <= 2.0.0", | |||
| "feetech-servo-sdk == 1.0.0", | |||
| "pwm-position-control", | |||
| ] | |||
| [dependency-groups] | |||
| @@ -18,7 +19,7 @@ dev = ["pytest >=8.1.1", "ruff >=0.9.1"] | |||
| [project.scripts] | |||
| feetech-client = "feetech_client.main:main" | |||
| feetech-configure = "feetech_client.configure:main" | |||
| [build-system] | |||
| requires = ["poetry-core>=1.8.0"] | |||
| build-backend = "poetry.core.masonry.api" | |||
| @@ -34,3 +35,6 @@ extend-select = [ | |||
| "N", # Ruff's N rule | |||
| "I", # Ruff's I rule | |||
| ] | |||
| [tool.uv.sources] | |||
| pwm-position-control = { git = "https://github.com/Hennzau/pwm-position-control" } | |||
| @@ -1,5 +1,4 @@ | |||
| version = 1 | |||
| revision = 1 | |||
| requires-python = ">=3.9" | |||
| [[package]] | |||
| @@ -31,6 +30,17 @@ wheels = [ | |||
| { url = "https://files.pythonhosted.org/packages/de/d8/a6bfeb961b8a15c8e73a51a839482783154a2f8927fef267d57d039043dc/dora_rs-0.3.5-cp37-abi3-win_amd64.whl", hash = "sha256:88b4fe5e5569562fcdb3817abb89532f4abca913e8bd02e4ec228833716cbd09", size = 6129593 }, | |||
| ] | |||
| [[package]] | |||
| name = "dynamixel-sdk" | |||
| version = "3.7.31" | |||
| source = { registry = "https://pypi.org/simple" } | |||
| dependencies = [ | |||
| { name = "pyserial" }, | |||
| ] | |||
| wheels = [ | |||
| { url = "https://files.pythonhosted.org/packages/03/e8/2c65d11d312fdb1a2308563d32f63f93c39123bcfb4909d947e0b294c794/dynamixel_sdk-3.7.31-py3-none-any.whl", hash = "sha256:74e8c112ca6b0b869b196dd8c6a44ffd5dd5c1a3cb9fe2030e9933922406b466", size = 23664 }, | |||
| ] | |||
| [[package]] | |||
| name = "exceptiongroup" | |||
| version = "1.2.2" | |||
| @@ -48,6 +58,7 @@ dependencies = [ | |||
| { name = "dora-rs" }, | |||
| { name = "feetech-servo-sdk" }, | |||
| { name = "numpy" }, | |||
| { name = "pwm-position-control" }, | |||
| ] | |||
| [package.dev-dependencies] | |||
| @@ -61,6 +72,7 @@ requires-dist = [ | |||
| { name = "dora-rs", specifier = "==0.3.5" }, | |||
| { name = "feetech-servo-sdk", specifier = "==1.0.0" }, | |||
| { name = "numpy", specifier = "<=2.0.0" }, | |||
| { name = "pwm-position-control", git = "https://github.com/Hennzau/pwm-position-control" }, | |||
| ] | |||
| [package.metadata.requires-dev] | |||
| @@ -89,54 +101,45 @@ wheels = [ | |||
| [[package]] | |||
| name = "numpy" | |||
| version = "2.0.0" | |||
| version = "1.26.4" | |||
| source = { registry = "https://pypi.org/simple" } | |||
| sdist = { url = "https://files.pythonhosted.org/packages/05/35/fb1ada118002df3fe91b5c3b28bc0d90f879b881a5d8f68b1f9b79c44bfe/numpy-2.0.0.tar.gz", hash = "sha256:cf5d1c9e6837f8af9f92b6bd3e86d513cdc11f60fd62185cc49ec7d1aba34864", size = 18326228 } | |||
| sdist = { url = "https://files.pythonhosted.org/packages/65/6e/09db70a523a96d25e115e71cc56a6f9031e7b8cd166c1ac8438307c14058/numpy-1.26.4.tar.gz", hash = "sha256:2a02aba9ed12e4ac4eb3ea9421c420301a0c6460d9830d74a9df87efa4912010", size = 15786129 } | |||
| wheels = [ | |||
| { url = "https://files.pythonhosted.org/packages/3a/83/24dafa898f172e198a1c164eb01675bbcbf5895ac8f9b1f8078ea5c2fdb5/numpy-2.0.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:04494f6ec467ccb5369d1808570ae55f6ed9b5809d7f035059000a37b8d7e86f", size = 21214540 }, | |||
| { url = "https://files.pythonhosted.org/packages/b6/8f/780b1719bee25794115b23dafd022aa4a835002077df58d4234ca6a23143/numpy-2.0.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:2635dbd200c2d6faf2ef9a0d04f0ecc6b13b3cad54f7c67c61155138835515d2", size = 13307901 }, | |||
| { url = "https://files.pythonhosted.org/packages/3b/61/e1e77694c4ed929c8edebde7d2ac30dbf3ed452c1988b633569d3d7ff271/numpy-2.0.0-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:0a43f0974d501842866cc83471bdb0116ba0dffdbaac33ec05e6afed5b615238", size = 5238781 }, | |||
| { url = "https://files.pythonhosted.org/packages/fc/1f/34b58ba54b5f202728083b5007d4b27dfcfd0edc616addadb0b35c7817d7/numpy-2.0.0-cp310-cp310-macosx_14_0_x86_64.whl", hash = "sha256:8d83bb187fb647643bd56e1ae43f273c7f4dbcdf94550d7938cfc32566756514", size = 6882511 }, | |||
| { url = "https://files.pythonhosted.org/packages/e1/5f/e51e3ebdaad1bccffdf9ba4b979c8b2fe2bd376d10bf9e9b59e1c6972a1a/numpy-2.0.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:79e843d186c8fb1b102bef3e2bc35ef81160ffef3194646a7fdd6a73c6b97196", size = 13904765 }, | |||
| { url = "https://files.pythonhosted.org/packages/d6/a8/6a2419c40c7b6f7cb4ef52c532c88e55490c4fa92885964757d507adddce/numpy-2.0.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6d7696c615765091cc5093f76fd1fa069870304beaccfd58b5dcc69e55ef49c1", size = 19282097 }, | |||
| { url = "https://files.pythonhosted.org/packages/52/d3/74989fffc21c74fba73eb05591cf3a56aaa135ee2427826217487028abd0/numpy-2.0.0-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:b4c76e3d4c56f145d41b7b6751255feefae92edbc9a61e1758a98204200f30fc", size = 19699985 }, | |||
| { url = "https://files.pythonhosted.org/packages/23/8a/a5cac659347f916cfaf2343eba577e98c83edd1ad6ada5586018961bf667/numpy-2.0.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:acd3a644e4807e73b4e1867b769fbf1ce8c5d80e7caaef0d90dcdc640dfc9787", size = 14406309 }, | |||
| { url = "https://files.pythonhosted.org/packages/8b/c4/858aadfd1f3f2f815c03be62556115f43796b805943755a9aef5b6b29b04/numpy-2.0.0-cp310-cp310-win32.whl", hash = "sha256:cee6cc0584f71adefe2c908856ccc98702baf95ff80092e4ca46061538a2ba98", size = 6358424 }, | |||
| { url = "https://files.pythonhosted.org/packages/9c/de/7d17991e0683f84bcfefcf4e3f43da6b37155b9e6a0429942494f044a7ef/numpy-2.0.0-cp310-cp310-win_amd64.whl", hash = "sha256:ed08d2703b5972ec736451b818c2eb9da80d66c3e84aed1deeb0c345fefe461b", size = 16507217 }, | |||
| { url = "https://files.pythonhosted.org/packages/58/52/a1aea658c7134ea0977542fc4d1aa6f1f9876c6a14ffeecd9394d839bc16/numpy-2.0.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:ad0c86f3455fbd0de6c31a3056eb822fc939f81b1618f10ff3406971893b62a5", size = 21218342 }, | |||
| { url = "https://files.pythonhosted.org/packages/77/4d/ba4a60298c55478b34f13c97a0ac2cf8d225320322976252a250ed04040a/numpy-2.0.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:e7f387600d424f91576af20518334df3d97bc76a300a755f9a8d6e4f5cadd289", size = 13272871 }, | |||
| { url = "https://files.pythonhosted.org/packages/01/4a/611a907421d8098d5edc8c2b10c3583796ee8da4156f8f7de52c2f4c9d90/numpy-2.0.0-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:34f003cb88b1ba38cb9a9a4a3161c1604973d7f9d5552c38bc2f04f829536609", size = 5237037 }, | |||
| { url = "https://files.pythonhosted.org/packages/4f/c1/42d1789f1dff7b65f2d3237eb88db258a5a7fdfb981b895509887c92838d/numpy-2.0.0-cp311-cp311-macosx_14_0_x86_64.whl", hash = "sha256:b6f6a8f45d0313db07d6d1d37bd0b112f887e1369758a5419c0370ba915b3871", size = 6886342 }, | |||
| { url = "https://files.pythonhosted.org/packages/cd/37/595f27a95ff976e8086bc4be1ede21ed24ca4bc127588da59197a65d066f/numpy-2.0.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5f64641b42b2429f56ee08b4f427a4d2daf916ec59686061de751a55aafa22e4", size = 13913798 }, | |||
| { url = "https://files.pythonhosted.org/packages/d1/27/2a7bd6855dc717aeec5f553073a3c426b9c816126555f8e616392eab856b/numpy-2.0.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:a7039a136017eaa92c1848152827e1424701532ca8e8967fe480fe1569dae581", size = 19292279 }, | |||
| { url = "https://files.pythonhosted.org/packages/1b/54/966a3f5a93d709672ad851f6db52461c0584bab52f2230cf76be482302c6/numpy-2.0.0-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:46e161722e0f619749d1cd892167039015b2c2817296104487cd03ed4a955995", size = 19709770 }, | |||
| { url = "https://files.pythonhosted.org/packages/cc/8b/9340ac45b6cd8bb92a03f797dbe9b7949f5b3789482e1d824cbebc80fda7/numpy-2.0.0-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:0e50842b2295ba8414c8c1d9d957083d5dfe9e16828b37de883f51fc53c4016f", size = 14417906 }, | |||
| { url = "https://files.pythonhosted.org/packages/fa/46/614507d78ca8ce1567ac2c3bf7a79bfd413d6fc96dc6b415abaeb3734c0a/numpy-2.0.0-cp311-cp311-win32.whl", hash = "sha256:2ce46fd0b8a0c947ae047d222f7136fc4d55538741373107574271bc00e20e8f", size = 6357084 }, | |||
| { url = "https://files.pythonhosted.org/packages/9b/0f/022ca4783b6e6239a53b988a4d315d67f9ae7126227fb2255054a558bd72/numpy-2.0.0-cp311-cp311-win_amd64.whl", hash = "sha256:fbd6acc766814ea6443628f4e6751d0da6593dae29c08c0b2606164db026970c", size = 16511678 }, | |||
| { url = "https://files.pythonhosted.org/packages/b7/c8/899826a2d5c94f607f5e4a6f1a0e8b07c8fea3a5b674c5706115b8aad9bb/numpy-2.0.0-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:354f373279768fa5a584bac997de6a6c9bc535c482592d7a813bb0c09be6c76f", size = 20944792 }, | |||
| { url = "https://files.pythonhosted.org/packages/fe/ec/8ae7750d33565769c8bb7ba925d4e73ecb2de6cd8eaa6fd527fbd52797ee/numpy-2.0.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:4d2f62e55a4cd9c58c1d9a1c9edaedcd857a73cb6fda875bf79093f9d9086f85", size = 13042186 }, | |||
| { url = "https://files.pythonhosted.org/packages/3f/ab/1dc9f176d3084a2546cf76eb213dc61586d015ef59b3b17947b0e40038af/numpy-2.0.0-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:1e72728e7501a450288fc8e1f9ebc73d90cfd4671ebbd631f3e7857c39bd16f2", size = 4977729 }, | |||
| { url = "https://files.pythonhosted.org/packages/a0/97/61ed64cedc1b94a7939e3ab3db587822320d90a77bef70fcb586ea7c1931/numpy-2.0.0-cp312-cp312-macosx_14_0_x86_64.whl", hash = "sha256:84554fc53daa8f6abf8e8a66e076aff6ece62de68523d9f665f32d2fc50fd66e", size = 6610230 }, | |||
| { url = "https://files.pythonhosted.org/packages/bb/31/1f050169270d51ef0346d4c84c7df1c45af16ea304ed5f7151584788d32e/numpy-2.0.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:c73aafd1afca80afecb22718f8700b40ac7cab927b8abab3c3e337d70e10e5a2", size = 13619789 }, | |||
| { url = "https://files.pythonhosted.org/packages/28/95/b56fc6b2abe37c03923b50415df483cf93e09e7438872280a5486131d804/numpy-2.0.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:49d9f7d256fbc804391a7f72d4a617302b1afac1112fac19b6c6cec63fe7fe8a", size = 18993635 }, | |||
| { url = "https://files.pythonhosted.org/packages/df/16/4c165a5194fc70e4a131f8db463e6baf34e0d191ed35d40a161ee4c885d4/numpy-2.0.0-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:0ec84b9ba0654f3b962802edc91424331f423dcf5d5f926676e0150789cb3d95", size = 19408219 }, | |||
| { url = "https://files.pythonhosted.org/packages/00/4c/440bad868bd3aff4fe4e293175a20da70cddff8674b3654eb2f112868ccf/numpy-2.0.0-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:feff59f27338135776f6d4e2ec7aeeac5d5f7a08a83e80869121ef8164b74af9", size = 14101574 }, | |||
| { url = "https://files.pythonhosted.org/packages/26/18/49f1e851f4157198c50f67ea3462797283aa36dd4b0c24b15f63e8118481/numpy-2.0.0-cp312-cp312-win32.whl", hash = "sha256:c5a59996dc61835133b56a32ebe4ef3740ea5bc19b3983ac60cc32be5a665d54", size = 6060205 }, | |||
| { url = "https://files.pythonhosted.org/packages/ad/9c/4a93b8e395b755c53628573d75d7b21985d9a0f416e978d637084ccc8ec3/numpy-2.0.0-cp312-cp312-win_amd64.whl", hash = "sha256:a356364941fb0593bb899a1076b92dfa2029f6f5b8ba88a14fd0984aaf76d0df", size = 16208660 }, | |||
| { url = "https://files.pythonhosted.org/packages/79/56/fb78389e7a1b1d0aa20dd0cbda5110d68f5df77b0a704180f0959b4f8ad1/numpy-2.0.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:e61155fae27570692ad1d327e81c6cf27d535a5d7ef97648a17d922224b216de", size = 21219203 }, | |||
| { url = "https://files.pythonhosted.org/packages/c6/ae/cc990cc3e9a211365391c193805496e7c7df93854d577e6a03d8a2319a12/numpy-2.0.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:4554eb96f0fd263041baf16cf0881b3f5dafae7a59b1049acb9540c4d57bc8cb", size = 13281472 }, | |||
| { url = "https://files.pythonhosted.org/packages/95/ed/3a23463e2608b54af1fbd3649cd403e81b82993685d2a21006291b879122/numpy-2.0.0-cp39-cp39-macosx_14_0_arm64.whl", hash = "sha256:903703372d46bce88b6920a0cd86c3ad82dae2dbef157b5fc01b70ea1cfc430f", size = 5245754 }, | |||
| { url = "https://files.pythonhosted.org/packages/d6/a1/8e8f40820ffe78ea09233b58c0f8719707b738ef36efbdc34377989b7ea5/numpy-2.0.0-cp39-cp39-macosx_14_0_x86_64.whl", hash = "sha256:3e8e01233d57639b2e30966c63d36fcea099d17c53bf424d77f088b0f4babd86", size = 6887414 }, | |||
| { url = "https://files.pythonhosted.org/packages/3b/89/abc57eebba1da98f615c7cb5d5b04bc105f00bda34d27048772d1be5a9fb/numpy-2.0.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1cde1753efe513705a0c6d28f5884e22bdc30438bf0085c5c486cdaff40cd67a", size = 13910703 }, | |||
| { url = "https://files.pythonhosted.org/packages/87/d3/74e627205462a170f39e7d7ddd2b4166a0d8ab163377592c7f4fa935cc8c/numpy-2.0.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:821eedb7165ead9eebdb569986968b541f9908979c2da8a4967ecac4439bae3d", size = 19285329 }, | |||
| { url = "https://files.pythonhosted.org/packages/c8/2e/14e7d5dd9930993797e95121176acbc3ffc1bb0ccbd2f8f7be36285ebde0/numpy-2.0.0-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:9a1712c015831da583b21c5bfe15e8684137097969c6d22e8316ba66b5baabe4", size = 19705286 }, | |||
| { url = "https://files.pythonhosted.org/packages/82/2d/f89a5cce068cd178c52e9fdc10fc227966d9da0cce368610775e75111d24/numpy-2.0.0-cp39-cp39-musllinux_1_2_aarch64.whl", hash = "sha256:9c27f0946a3536403efb0e1c28def1ae6730a72cd0d5878db38824855e3afc44", size = 14409686 }, | |||
| { url = "https://files.pythonhosted.org/packages/51/e7/8ab01e44772d376efd3e1f48df618c0f6ed6aeac5e2242387f0c21a77ff7/numpy-2.0.0-cp39-cp39-win32.whl", hash = "sha256:63b92c512d9dbcc37f9d81b123dec99fdb318ba38c8059afc78086fe73820275", size = 6363194 }, | |||
| { url = "https://files.pythonhosted.org/packages/6a/1e/1d76829f03b7ac9c90e2b158f06b69cddf9a06b96667dd7e2d96acdc0593/numpy-2.0.0-cp39-cp39-win_amd64.whl", hash = "sha256:3f6bed7f840d44c08ebdb73b1825282b801799e325bcbdfa6bc5c370e5aecc65", size = 16513565 }, | |||
| { url = "https://files.pythonhosted.org/packages/9a/e0/97d246e03f9597e7275dc2f0b24f6845fbb5380ef0fac330cb1b087229f8/numpy-2.0.0-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:9416a5c2e92ace094e9f0082c5fd473502c91651fb896bc17690d6fc475128d6", size = 21076646 }, | |||
| { url = "https://files.pythonhosted.org/packages/18/66/10c93572d97b410f71ad9b59b20f2a23dcdd871f025bd5376a732b408520/numpy-2.0.0-pp39-pypy39_pp73-macosx_14_0_x86_64.whl", hash = "sha256:17067d097ed036636fa79f6a869ac26df7db1ba22039d962422506640314933a", size = 6743807 }, | |||
| { url = "https://files.pythonhosted.org/packages/80/1a/354ad1a6627dbac1d4167591db51ce59ed972064bfb9979f9a37a7782900/numpy-2.0.0-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:38ecb5b0582cd125f67a629072fed6f83562d9dd04d7e03256c9829bdec027ad", size = 19089752 }, | |||
| { url = "https://files.pythonhosted.org/packages/5f/9f/fe311331410759da4d441d6d08dd54b80065f4946374e817611f4f9c527f/numpy-2.0.0-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:cef04d068f5fb0518a77857953193b6bb94809a806bd0a14983a8f12ada060c9", size = 16431128 }, | |||
| { url = "https://files.pythonhosted.org/packages/a7/94/ace0fdea5241a27d13543ee117cbc65868e82213fb31a8eb7fe9ff23f313/numpy-1.26.4-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:9ff0f4f29c51e2803569d7a51c2304de5554655a60c5d776e35b4a41413830d0", size = 20631468 }, | |||
| { url = "https://files.pythonhosted.org/packages/20/f7/b24208eba89f9d1b58c1668bc6c8c4fd472b20c45573cb767f59d49fb0f6/numpy-1.26.4-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:2e4ee3380d6de9c9ec04745830fd9e2eccb3e6cf790d39d7b98ffd19b0dd754a", size = 13966411 }, | |||
| { url = "https://files.pythonhosted.org/packages/fc/a5/4beee6488160798683eed5bdb7eead455892c3b4e1f78d79d8d3f3b084ac/numpy-1.26.4-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:d209d8969599b27ad20994c8e41936ee0964e6da07478d6c35016bc386b66ad4", size = 14219016 }, | |||
| { url = "https://files.pythonhosted.org/packages/4b/d7/ecf66c1cd12dc28b4040b15ab4d17b773b87fa9d29ca16125de01adb36cd/numpy-1.26.4-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ffa75af20b44f8dba823498024771d5ac50620e6915abac414251bd971b4529f", size = 18240889 }, | |||
| { url = "https://files.pythonhosted.org/packages/24/03/6f229fe3187546435c4f6f89f6d26c129d4f5bed40552899fcf1f0bf9e50/numpy-1.26.4-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:62b8e4b1e28009ef2846b4c7852046736bab361f7aeadeb6a5b89ebec3c7055a", size = 13876746 }, | |||
| { url = "https://files.pythonhosted.org/packages/39/fe/39ada9b094f01f5a35486577c848fe274e374bbf8d8f472e1423a0bbd26d/numpy-1.26.4-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:a4abb4f9001ad2858e7ac189089c42178fcce737e4169dc61321660f1a96c7d2", size = 18078620 }, | |||
| { url = "https://files.pythonhosted.org/packages/d5/ef/6ad11d51197aad206a9ad2286dc1aac6a378059e06e8cf22cd08ed4f20dc/numpy-1.26.4-cp310-cp310-win32.whl", hash = "sha256:bfe25acf8b437eb2a8b2d49d443800a5f18508cd811fea3181723922a8a82b07", size = 5972659 }, | |||
| { url = "https://files.pythonhosted.org/packages/19/77/538f202862b9183f54108557bfda67e17603fc560c384559e769321c9d92/numpy-1.26.4-cp310-cp310-win_amd64.whl", hash = "sha256:b97fe8060236edf3662adfc2c633f56a08ae30560c56310562cb4f95500022d5", size = 15808905 }, | |||
| { url = "https://files.pythonhosted.org/packages/11/57/baae43d14fe163fa0e4c47f307b6b2511ab8d7d30177c491960504252053/numpy-1.26.4-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:4c66707fabe114439db9068ee468c26bbdf909cac0fb58686a42a24de1760c71", size = 20630554 }, | |||
| { url = "https://files.pythonhosted.org/packages/1a/2e/151484f49fd03944c4a3ad9c418ed193cfd02724e138ac8a9505d056c582/numpy-1.26.4-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:edd8b5fe47dab091176d21bb6de568acdd906d1887a4584a15a9a96a1dca06ef", size = 13997127 }, | |||
| { url = "https://files.pythonhosted.org/packages/79/ae/7e5b85136806f9dadf4878bf73cf223fe5c2636818ba3ab1c585d0403164/numpy-1.26.4-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:7ab55401287bfec946ced39700c053796e7cc0e3acbef09993a9ad2adba6ca6e", size = 14222994 }, | |||
| { url = "https://files.pythonhosted.org/packages/3a/d0/edc009c27b406c4f9cbc79274d6e46d634d139075492ad055e3d68445925/numpy-1.26.4-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:666dbfb6ec68962c033a450943ded891bed2d54e6755e35e5835d63f4f6931d5", size = 18252005 }, | |||
| { url = "https://files.pythonhosted.org/packages/09/bf/2b1aaf8f525f2923ff6cfcf134ae5e750e279ac65ebf386c75a0cf6da06a/numpy-1.26.4-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:96ff0b2ad353d8f990b63294c8986f1ec3cb19d749234014f4e7eb0112ceba5a", size = 13885297 }, | |||
| { url = "https://files.pythonhosted.org/packages/df/a0/4e0f14d847cfc2a633a1c8621d00724f3206cfeddeb66d35698c4e2cf3d2/numpy-1.26.4-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:60dedbb91afcbfdc9bc0b1f3f402804070deed7392c23eb7a7f07fa857868e8a", size = 18093567 }, | |||
| { url = "https://files.pythonhosted.org/packages/d2/b7/a734c733286e10a7f1a8ad1ae8c90f2d33bf604a96548e0a4a3a6739b468/numpy-1.26.4-cp311-cp311-win32.whl", hash = "sha256:1af303d6b2210eb850fcf03064d364652b7120803a0b872f5211f5234b399f20", size = 5968812 }, | |||
| { url = "https://files.pythonhosted.org/packages/3f/6b/5610004206cf7f8e7ad91c5a85a8c71b2f2f8051a0c0c4d5916b76d6cbb2/numpy-1.26.4-cp311-cp311-win_amd64.whl", hash = "sha256:cd25bcecc4974d09257ffcd1f098ee778f7834c3ad767fe5db785be9a4aa9cb2", size = 15811913 }, | |||
| { url = "https://files.pythonhosted.org/packages/95/12/8f2020a8e8b8383ac0177dc9570aad031a3beb12e38847f7129bacd96228/numpy-1.26.4-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:b3ce300f3644fb06443ee2222c2201dd3a89ea6040541412b8fa189341847218", size = 20335901 }, | |||
| { url = "https://files.pythonhosted.org/packages/75/5b/ca6c8bd14007e5ca171c7c03102d17b4f4e0ceb53957e8c44343a9546dcc/numpy-1.26.4-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:03a8c78d01d9781b28a6989f6fa1bb2c4f2d51201cf99d3dd875df6fbd96b23b", size = 13685868 }, | |||
| { url = "https://files.pythonhosted.org/packages/79/f8/97f10e6755e2a7d027ca783f63044d5b1bc1ae7acb12afe6a9b4286eac17/numpy-1.26.4-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:9fad7dcb1aac3c7f0584a5a8133e3a43eeb2fe127f47e3632d43d677c66c102b", size = 13925109 }, | |||
| { url = "https://files.pythonhosted.org/packages/0f/50/de23fde84e45f5c4fda2488c759b69990fd4512387a8632860f3ac9cd225/numpy-1.26.4-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:675d61ffbfa78604709862923189bad94014bef562cc35cf61d3a07bba02a7ed", size = 17950613 }, | |||
| { url = "https://files.pythonhosted.org/packages/4c/0c/9c603826b6465e82591e05ca230dfc13376da512b25ccd0894709b054ed0/numpy-1.26.4-cp312-cp312-musllinux_1_1_aarch64.whl", hash = "sha256:ab47dbe5cc8210f55aa58e4805fe224dac469cde56b9f731a4c098b91917159a", size = 13572172 }, | |||
| { url = "https://files.pythonhosted.org/packages/76/8c/2ba3902e1a0fc1c74962ea9bb33a534bb05984ad7ff9515bf8d07527cadd/numpy-1.26.4-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:1dda2e7b4ec9dd512f84935c5f126c8bd8b9f2fc001e9f54af255e8c5f16b0e0", size = 17786643 }, | |||
| { url = "https://files.pythonhosted.org/packages/28/4a/46d9e65106879492374999e76eb85f87b15328e06bd1550668f79f7b18c6/numpy-1.26.4-cp312-cp312-win32.whl", hash = "sha256:50193e430acfc1346175fcbdaa28ffec49947a06918b7b92130744e81e640110", size = 5677803 }, | |||
| { url = "https://files.pythonhosted.org/packages/16/2e/86f24451c2d530c88daf997cb8d6ac622c1d40d19f5a031ed68a4b73a374/numpy-1.26.4-cp312-cp312-win_amd64.whl", hash = "sha256:08beddf13648eb95f8d867350f6a018a4be2e5ad54c8d8caed89ebca558b2818", size = 15517754 }, | |||
| { url = "https://files.pythonhosted.org/packages/7d/24/ce71dc08f06534269f66e73c04f5709ee024a1afe92a7b6e1d73f158e1f8/numpy-1.26.4-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:7349ab0fa0c429c82442a27a9673fc802ffdb7c7775fad780226cb234965e53c", size = 20636301 }, | |||
| { url = "https://files.pythonhosted.org/packages/ae/8c/ab03a7c25741f9ebc92684a20125fbc9fc1b8e1e700beb9197d750fdff88/numpy-1.26.4-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:52b8b60467cd7dd1e9ed082188b4e6bb35aa5cdd01777621a1658910745b90be", size = 13971216 }, | |||
| { url = "https://files.pythonhosted.org/packages/6d/64/c3bcdf822269421d85fe0d64ba972003f9bb4aa9a419da64b86856c9961f/numpy-1.26.4-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:d5241e0a80d808d70546c697135da2c613f30e28251ff8307eb72ba696945764", size = 14226281 }, | |||
| { url = "https://files.pythonhosted.org/packages/54/30/c2a907b9443cf42b90c17ad10c1e8fa801975f01cb9764f3f8eb8aea638b/numpy-1.26.4-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f870204a840a60da0b12273ef34f7051e98c3b5961b61b0c2c1be6dfd64fbcd3", size = 18249516 }, | |||
| { url = "https://files.pythonhosted.org/packages/43/12/01a563fc44c07095996d0129b8899daf89e4742146f7044cdbdb3101c57f/numpy-1.26.4-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:679b0076f67ecc0138fd2ede3a8fd196dddc2ad3254069bcb9faf9a79b1cebcd", size = 13882132 }, | |||
| { url = "https://files.pythonhosted.org/packages/16/ee/9df80b06680aaa23fc6c31211387e0db349e0e36d6a63ba3bd78c5acdf11/numpy-1.26.4-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:47711010ad8555514b434df65f7d7b076bb8261df1ca9bb78f53d3b2db02e95c", size = 18084181 }, | |||
| { url = "https://files.pythonhosted.org/packages/28/7d/4b92e2fe20b214ffca36107f1a3e75ef4c488430e64de2d9af5db3a4637d/numpy-1.26.4-cp39-cp39-win32.whl", hash = "sha256:a354325ee03388678242a4d7ebcd08b5c727033fcff3b2f536aea978e15ee9e6", size = 5976360 }, | |||
| { url = "https://files.pythonhosted.org/packages/b5/42/054082bd8220bbf6f297f982f0a8f5479fcbc55c8b511d928df07b965869/numpy-1.26.4-cp39-cp39-win_amd64.whl", hash = "sha256:3373d5d70a5fe74a2c1bb6d2cfd9609ecf686d47a2d7b1d37a8f3b6bf6003aea", size = 15814633 }, | |||
| { url = "https://files.pythonhosted.org/packages/3f/72/3df6c1c06fc83d9cfe381cccb4be2532bbd38bf93fbc9fad087b6687f1c0/numpy-1.26.4-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:afedb719a9dcfc7eaf2287b839d8198e06dcd4cb5d276a3df279231138e83d30", size = 20455961 }, | |||
| { url = "https://files.pythonhosted.org/packages/8e/02/570545bac308b58ffb21adda0f4e220ba716fb658a63c151daecc3293350/numpy-1.26.4-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:95a7476c59002f2f6c590b9b7b998306fba6a5aa646b1e22ddfeaf8f78c3a29c", size = 18061071 }, | |||
| { url = "https://files.pythonhosted.org/packages/f4/5f/fafd8c51235f60d49f7a88e2275e13971e90555b67da52dd6416caec32fe/numpy-1.26.4-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:7e50d0a0cc3189f9cb0aeb3a6a6af18c16f59f004b866cd2be1c14b36134a4a0", size = 15709730 }, | |||
| ] | |||
| [[package]] | |||
| @@ -157,53 +160,53 @@ wheels = [ | |||
| { url = "https://files.pythonhosted.org/packages/88/5f/e351af9a41f866ac3f1fac4ca0613908d9a41741cfcf2228f4ad853b697d/pluggy-1.5.0-py3-none-any.whl", hash = "sha256:44e1ad92c8ca002de6377e165f3e0f1be63266ab4d554740532335b9d75ea669", size = 20556 }, | |||
| ] | |||
| [[package]] | |||
| name = "pwm-position-control" | |||
| version = "0.1" | |||
| source = { git = "https://github.com/Hennzau/pwm-position-control#91d4e2ece8d72cbb8037ffb408e3ef3abe6b470d" } | |||
| dependencies = [ | |||
| { name = "dynamixel-sdk" }, | |||
| { name = "numpy" }, | |||
| { name = "pyarrow" }, | |||
| ] | |||
| [[package]] | |||
| name = "pyarrow" | |||
| version = "19.0.1" | |||
| version = "17.0.0" | |||
| source = { registry = "https://pypi.org/simple" } | |||
| sdist = { url = "https://files.pythonhosted.org/packages/7f/09/a9046344212690f0632b9c709f9bf18506522feb333c894d0de81d62341a/pyarrow-19.0.1.tar.gz", hash = "sha256:3bf266b485df66a400f282ac0b6d1b500b9d2ae73314a153dbe97d6d5cc8a99e", size = 1129437 } | |||
| dependencies = [ | |||
| { name = "numpy" }, | |||
| ] | |||
| sdist = { url = "https://files.pythonhosted.org/packages/27/4e/ea6d43f324169f8aec0e57569443a38bab4b398d09769ca64f7b4d467de3/pyarrow-17.0.0.tar.gz", hash = "sha256:4beca9521ed2c0921c1023e68d097d0299b62c362639ea315572a58f3f50fd28", size = 1112479 } | |||
| wheels = [ | |||
| { url = "https://files.pythonhosted.org/packages/36/01/b23b514d86b839956238d3f8ef206fd2728eee87ff1b8ce150a5678d9721/pyarrow-19.0.1-cp310-cp310-macosx_12_0_arm64.whl", hash = "sha256:fc28912a2dc924dddc2087679cc8b7263accc71b9ff025a1362b004711661a69", size = 30688914 }, | |||
| { url = "https://files.pythonhosted.org/packages/c6/68/218ff7cf4a0652a933e5f2ed11274f724dd43b9813cb18dd72c0a35226a2/pyarrow-19.0.1-cp310-cp310-macosx_12_0_x86_64.whl", hash = "sha256:fca15aabbe9b8355800d923cc2e82c8ef514af321e18b437c3d782aa884eaeec", size = 32102866 }, | |||
| { url = "https://files.pythonhosted.org/packages/98/01/c295050d183014f4a2eb796d7d2bbfa04b6cccde7258bb68aacf6f18779b/pyarrow-19.0.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ad76aef7f5f7e4a757fddcdcf010a8290958f09e3470ea458c80d26f4316ae89", size = 41147682 }, | |||
| { url = "https://files.pythonhosted.org/packages/40/17/a6c3db0b5f3678f33bbb552d2acbc16def67f89a72955b67b0109af23eb0/pyarrow-19.0.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d03c9d6f2a3dffbd62671ca070f13fc527bb1867b4ec2b98c7eeed381d4f389a", size = 42179192 }, | |||
| { url = "https://files.pythonhosted.org/packages/cf/75/c7c8e599300d8cebb6cb339014800e1c720c9db2a3fcb66aa64ec84bac72/pyarrow-19.0.1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:65cf9feebab489b19cdfcfe4aa82f62147218558d8d3f0fc1e9dea0ab8e7905a", size = 40517272 }, | |||
| { url = "https://files.pythonhosted.org/packages/ef/c9/68ab123ee1528699c4d5055f645ecd1dd68ff93e4699527249d02f55afeb/pyarrow-19.0.1-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:41f9706fbe505e0abc10e84bf3a906a1338905cbbcf1177b71486b03e6ea6608", size = 42069036 }, | |||
| { url = "https://files.pythonhosted.org/packages/54/e3/d5cfd7654084e6c0d9c3ce949e5d9e0ccad569ae1e2d5a68a3ec03b2be89/pyarrow-19.0.1-cp310-cp310-win_amd64.whl", hash = "sha256:c6cb2335a411b713fdf1e82a752162f72d4a7b5dbc588e32aa18383318b05866", size = 25277951 }, | |||
| { url = "https://files.pythonhosted.org/packages/a0/55/f1a8d838ec07fe3ca53edbe76f782df7b9aafd4417080eebf0b42aab0c52/pyarrow-19.0.1-cp311-cp311-macosx_12_0_arm64.whl", hash = "sha256:cc55d71898ea30dc95900297d191377caba257612f384207fe9f8293b5850f90", size = 30713987 }, | |||
| { url = "https://files.pythonhosted.org/packages/13/12/428861540bb54c98a140ae858a11f71d041ef9e501e6b7eb965ca7909505/pyarrow-19.0.1-cp311-cp311-macosx_12_0_x86_64.whl", hash = "sha256:7a544ec12de66769612b2d6988c36adc96fb9767ecc8ee0a4d270b10b1c51e00", size = 32135613 }, | |||
| { url = "https://files.pythonhosted.org/packages/2f/8a/23d7cc5ae2066c6c736bce1db8ea7bc9ac3ef97ac7e1c1667706c764d2d9/pyarrow-19.0.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:0148bb4fc158bfbc3d6dfe5001d93ebeed253793fff4435167f6ce1dc4bddeae", size = 41149147 }, | |||
| { url = "https://files.pythonhosted.org/packages/a2/7a/845d151bb81a892dfb368bf11db584cf8b216963ccce40a5cf50a2492a18/pyarrow-19.0.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f24faab6ed18f216a37870d8c5623f9c044566d75ec586ef884e13a02a9d62c5", size = 42178045 }, | |||
| { url = "https://files.pythonhosted.org/packages/a7/31/e7282d79a70816132cf6cae7e378adfccce9ae10352d21c2fecf9d9756dd/pyarrow-19.0.1-cp311-cp311-manylinux_2_28_aarch64.whl", hash = "sha256:4982f8e2b7afd6dae8608d70ba5bd91699077323f812a0448d8b7abdff6cb5d3", size = 40532998 }, | |||
| { url = "https://files.pythonhosted.org/packages/b8/82/20f3c290d6e705e2ee9c1fa1d5a0869365ee477e1788073d8b548da8b64c/pyarrow-19.0.1-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:49a3aecb62c1be1d822f8bf629226d4a96418228a42f5b40835c1f10d42e4db6", size = 42084055 }, | |||
| { url = "https://files.pythonhosted.org/packages/ff/77/e62aebd343238863f2c9f080ad2ef6ace25c919c6ab383436b5b81cbeef7/pyarrow-19.0.1-cp311-cp311-win_amd64.whl", hash = "sha256:008a4009efdb4ea3d2e18f05cd31f9d43c388aad29c636112c2966605ba33466", size = 25283133 }, | |||
| { url = "https://files.pythonhosted.org/packages/78/b4/94e828704b050e723f67d67c3535cf7076c7432cd4cf046e4bb3b96a9c9d/pyarrow-19.0.1-cp312-cp312-macosx_12_0_arm64.whl", hash = "sha256:80b2ad2b193e7d19e81008a96e313fbd53157945c7be9ac65f44f8937a55427b", size = 30670749 }, | |||
| { url = "https://files.pythonhosted.org/packages/7e/3b/4692965e04bb1df55e2c314c4296f1eb12b4f3052d4cf43d29e076aedf66/pyarrow-19.0.1-cp312-cp312-macosx_12_0_x86_64.whl", hash = "sha256:ee8dec072569f43835932a3b10c55973593abc00936c202707a4ad06af7cb294", size = 32128007 }, | |||
| { url = "https://files.pythonhosted.org/packages/22/f7/2239af706252c6582a5635c35caa17cb4d401cd74a87821ef702e3888957/pyarrow-19.0.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:4d5d1ec7ec5324b98887bdc006f4d2ce534e10e60f7ad995e7875ffa0ff9cb14", size = 41144566 }, | |||
| { url = "https://files.pythonhosted.org/packages/fb/e3/c9661b2b2849cfefddd9fd65b64e093594b231b472de08ff658f76c732b2/pyarrow-19.0.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f3ad4c0eb4e2a9aeb990af6c09e6fa0b195c8c0e7b272ecc8d4d2b6574809d34", size = 42202991 }, | |||
| { url = "https://files.pythonhosted.org/packages/fe/4f/a2c0ed309167ef436674782dfee4a124570ba64299c551e38d3fdaf0a17b/pyarrow-19.0.1-cp312-cp312-manylinux_2_28_aarch64.whl", hash = "sha256:d383591f3dcbe545f6cc62daaef9c7cdfe0dff0fb9e1c8121101cabe9098cfa6", size = 40507986 }, | |||
| { url = "https://files.pythonhosted.org/packages/27/2e/29bb28a7102a6f71026a9d70d1d61df926887e36ec797f2e6acfd2dd3867/pyarrow-19.0.1-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:b4c4156a625f1e35d6c0b2132635a237708944eb41df5fbe7d50f20d20c17832", size = 42087026 }, | |||
| { url = "https://files.pythonhosted.org/packages/16/33/2a67c0f783251106aeeee516f4806161e7b481f7d744d0d643d2f30230a5/pyarrow-19.0.1-cp312-cp312-win_amd64.whl", hash = "sha256:5bd1618ae5e5476b7654c7b55a6364ae87686d4724538c24185bbb2952679960", size = 25250108 }, | |||
| { url = "https://files.pythonhosted.org/packages/2b/8d/275c58d4b00781bd36579501a259eacc5c6dfb369be4ddeb672ceb551d2d/pyarrow-19.0.1-cp313-cp313-macosx_12_0_arm64.whl", hash = "sha256:e45274b20e524ae5c39d7fc1ca2aa923aab494776d2d4b316b49ec7572ca324c", size = 30653552 }, | |||
| { url = "https://files.pythonhosted.org/packages/a0/9e/e6aca5cc4ef0c7aec5f8db93feb0bde08dbad8c56b9014216205d271101b/pyarrow-19.0.1-cp313-cp313-macosx_12_0_x86_64.whl", hash = "sha256:d9dedeaf19097a143ed6da37f04f4051aba353c95ef507764d344229b2b740ae", size = 32103413 }, | |||
| { url = "https://files.pythonhosted.org/packages/6a/fa/a7033f66e5d4f1308c7eb0dfcd2ccd70f881724eb6fd1776657fdf65458f/pyarrow-19.0.1-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:6ebfb5171bb5f4a52319344ebbbecc731af3f021e49318c74f33d520d31ae0c4", size = 41134869 }, | |||
| { url = "https://files.pythonhosted.org/packages/2d/92/34d2569be8e7abdc9d145c98dc410db0071ac579b92ebc30da35f500d630/pyarrow-19.0.1-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f2a21d39fbdb948857f67eacb5bbaaf36802de044ec36fbef7a1c8f0dd3a4ab2", size = 42192626 }, | |||
| { url = "https://files.pythonhosted.org/packages/0a/1f/80c617b1084fc833804dc3309aa9d8daacd46f9ec8d736df733f15aebe2c/pyarrow-19.0.1-cp313-cp313-manylinux_2_28_aarch64.whl", hash = "sha256:99bc1bec6d234359743b01e70d4310d0ab240c3d6b0da7e2a93663b0158616f6", size = 40496708 }, | |||
| { url = "https://files.pythonhosted.org/packages/e6/90/83698fcecf939a611c8d9a78e38e7fed7792dcc4317e29e72cf8135526fb/pyarrow-19.0.1-cp313-cp313-manylinux_2_28_x86_64.whl", hash = "sha256:1b93ef2c93e77c442c979b0d596af45e4665d8b96da598db145b0fec014b9136", size = 42075728 }, | |||
| { url = "https://files.pythonhosted.org/packages/40/49/2325f5c9e7a1c125c01ba0c509d400b152c972a47958768e4e35e04d13d8/pyarrow-19.0.1-cp313-cp313-win_amd64.whl", hash = "sha256:d9d46e06846a41ba906ab25302cf0fd522f81aa2a85a71021826f34639ad31ef", size = 25242568 }, | |||
| { url = "https://files.pythonhosted.org/packages/3f/72/135088d995a759d4d916ec4824cb19e066585b4909ebad4ab196177aa825/pyarrow-19.0.1-cp313-cp313t-macosx_12_0_arm64.whl", hash = "sha256:c0fe3dbbf054a00d1f162fda94ce236a899ca01123a798c561ba307ca38af5f0", size = 30702371 }, | |||
| { url = "https://files.pythonhosted.org/packages/2e/01/00beeebd33d6bac701f20816a29d2018eba463616bbc07397fdf99ac4ce3/pyarrow-19.0.1-cp313-cp313t-macosx_12_0_x86_64.whl", hash = "sha256:96606c3ba57944d128e8a8399da4812f56c7f61de8c647e3470b417f795d0ef9", size = 32116046 }, | |||
| { url = "https://files.pythonhosted.org/packages/1f/c9/23b1ea718dfe967cbd986d16cf2a31fe59d015874258baae16d7ea0ccabc/pyarrow-19.0.1-cp313-cp313t-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:8f04d49a6b64cf24719c080b3c2029a3a5b16417fd5fd7c4041f94233af732f3", size = 41091183 }, | |||
| { url = "https://files.pythonhosted.org/packages/3a/d4/b4a3aa781a2c715520aa8ab4fe2e7fa49d33a1d4e71c8fc6ab7b5de7a3f8/pyarrow-19.0.1-cp313-cp313t-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:5a9137cf7e1640dce4c190551ee69d478f7121b5c6f323553b319cac936395f6", size = 42171896 }, | |||
| { url = "https://files.pythonhosted.org/packages/23/1b/716d4cd5a3cbc387c6e6745d2704c4b46654ba2668260d25c402626c5ddb/pyarrow-19.0.1-cp313-cp313t-manylinux_2_28_aarch64.whl", hash = "sha256:7c1bca1897c28013db5e4c83944a2ab53231f541b9e0c3f4791206d0c0de389a", size = 40464851 }, | |||
| { url = "https://files.pythonhosted.org/packages/ed/bd/54907846383dcc7ee28772d7e646f6c34276a17da740002a5cefe90f04f7/pyarrow-19.0.1-cp313-cp313t-manylinux_2_28_x86_64.whl", hash = "sha256:58d9397b2e273ef76264b45531e9d552d8ec8a6688b7390b5be44c02a37aade8", size = 42085744 }, | |||
| { url = "https://files.pythonhosted.org/packages/16/26/0ec396ebe98adefaffc0fff8e0dc14c8912e61093226284cf4b76faffd22/pyarrow-19.0.1-cp39-cp39-macosx_12_0_arm64.whl", hash = "sha256:b9766a47a9cb56fefe95cb27f535038b5a195707a08bf61b180e642324963b46", size = 30701112 }, | |||
| { url = "https://files.pythonhosted.org/packages/ba/10/c35d96686bf7f13e55bb87f06fe06e7d95533c271ef7f9a5a76e26b16fc2/pyarrow-19.0.1-cp39-cp39-macosx_12_0_x86_64.whl", hash = "sha256:6c5941c1aac89a6c2f2b16cd64fe76bcdb94b2b1e99ca6459de4e6f07638d755", size = 32117180 }, | |||
| { url = "https://files.pythonhosted.org/packages/8c/0d/81881a55302b6847ea2ea187517faa039c219d80b55050904e354c2eddde/pyarrow-19.0.1-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:fd44d66093a239358d07c42a91eebf5015aa54fccba959db899f932218ac9cc8", size = 41161334 }, | |||
| { url = "https://files.pythonhosted.org/packages/af/17/ea60a07ec6f6bb0740f11715e0d22ab8fdfcc94bc729832321f498370d75/pyarrow-19.0.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:335d170e050bcc7da867a1ed8ffb8b44c57aaa6e0843b156a501298657b1e972", size = 42190375 }, | |||
| { url = "https://files.pythonhosted.org/packages/f2/87/4ef05a088b18082cde4950bdfca752dd31effb3ec201b8026e4816d0f3fa/pyarrow-19.0.1-cp39-cp39-manylinux_2_28_aarch64.whl", hash = "sha256:1c7556165bd38cf0cd992df2636f8bcdd2d4b26916c6b7e646101aff3c16f76f", size = 40530649 }, | |||
| { url = "https://files.pythonhosted.org/packages/59/1e/9fb9a66a64eae4ff332a8f149d803d8c6c556714803d20d54ed2e9524a3b/pyarrow-19.0.1-cp39-cp39-manylinux_2_28_x86_64.whl", hash = "sha256:699799f9c80bebcf1da0983ba86d7f289c5a2a5c04b945e2f2bcf7e874a91911", size = 42081576 }, | |||
| { url = "https://files.pythonhosted.org/packages/1b/ee/c110d8da8bdde8e832ccf1ff90be747cb684874e2dc8acf26840058b0c32/pyarrow-19.0.1-cp39-cp39-win_amd64.whl", hash = "sha256:8464c9fbe6d94a7fe1599e7e8965f350fd233532868232ab2596a71586c5a429", size = 25465593 }, | |||
| { url = "https://files.pythonhosted.org/packages/39/5d/78d4b040bc5ff2fc6c3d03e80fca396b742f6c125b8af06bcf7427f931bc/pyarrow-17.0.0-cp310-cp310-macosx_10_15_x86_64.whl", hash = "sha256:a5c8b238d47e48812ee577ee20c9a2779e6a5904f1708ae240f53ecbee7c9f07", size = 28994846 }, | |||
| { url = "https://files.pythonhosted.org/packages/3b/73/8ed168db7642e91180330e4ea9f3ff8bab404678f00d32d7df0871a4933b/pyarrow-17.0.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:db023dc4c6cae1015de9e198d41250688383c3f9af8f565370ab2b4cb5f62655", size = 27165908 }, | |||
| { url = "https://files.pythonhosted.org/packages/81/36/e78c24be99242063f6d0590ef68c857ea07bdea470242c361e9a15bd57a4/pyarrow-17.0.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:da1e060b3876faa11cee287839f9cc7cdc00649f475714b8680a05fd9071d545", size = 39264209 }, | |||
| { url = "https://files.pythonhosted.org/packages/18/4c/3db637d7578f683b0a8fb8999b436bdbedd6e3517bd4f90c70853cf3ad20/pyarrow-17.0.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:75c06d4624c0ad6674364bb46ef38c3132768139ddec1c56582dbac54f2663e2", size = 39862883 }, | |||
| { url = "https://files.pythonhosted.org/packages/81/3c/0580626896c842614a523e66b351181ed5bb14e5dfc263cd68cea2c46d90/pyarrow-17.0.0-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:fa3c246cc58cb5a4a5cb407a18f193354ea47dd0648194e6265bd24177982fe8", size = 38723009 }, | |||
| { url = "https://files.pythonhosted.org/packages/ee/fb/c1b47f0ada36d856a352da261a44d7344d8f22e2f7db3945f8c3b81be5dd/pyarrow-17.0.0-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:f7ae2de664e0b158d1607699a16a488de3d008ba99b3a7aa5de1cbc13574d047", size = 39855626 }, | |||
| { url = "https://files.pythonhosted.org/packages/19/09/b0a02908180a25d57312ab5919069c39fddf30602568980419f4b02393f6/pyarrow-17.0.0-cp310-cp310-win_amd64.whl", hash = "sha256:5984f416552eea15fd9cee03da53542bf4cddaef5afecefb9aa8d1010c335087", size = 25147242 }, | |||
| { url = "https://files.pythonhosted.org/packages/f9/46/ce89f87c2936f5bb9d879473b9663ce7a4b1f4359acc2f0eb39865eaa1af/pyarrow-17.0.0-cp311-cp311-macosx_10_15_x86_64.whl", hash = "sha256:1c8856e2ef09eb87ecf937104aacfa0708f22dfeb039c363ec99735190ffb977", size = 29028748 }, | |||
| { url = "https://files.pythonhosted.org/packages/8d/8e/ce2e9b2146de422f6638333c01903140e9ada244a2a477918a368306c64c/pyarrow-17.0.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:2e19f569567efcbbd42084e87f948778eb371d308e137a0f97afe19bb860ccb3", size = 27190965 }, | |||
| { url = "https://files.pythonhosted.org/packages/3b/c8/5675719570eb1acd809481c6d64e2136ffb340bc387f4ca62dce79516cea/pyarrow-17.0.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:6b244dc8e08a23b3e352899a006a26ae7b4d0da7bb636872fa8f5884e70acf15", size = 39269081 }, | |||
| { url = "https://files.pythonhosted.org/packages/5e/78/3931194f16ab681ebb87ad252e7b8d2c8b23dad49706cadc865dff4a1dd3/pyarrow-17.0.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0b72e87fe3e1db343995562f7fff8aee354b55ee83d13afba65400c178ab2597", size = 39864921 }, | |||
| { url = "https://files.pythonhosted.org/packages/d8/81/69b6606093363f55a2a574c018901c40952d4e902e670656d18213c71ad7/pyarrow-17.0.0-cp311-cp311-manylinux_2_28_aarch64.whl", hash = "sha256:dc5c31c37409dfbc5d014047817cb4ccd8c1ea25d19576acf1a001fe07f5b420", size = 38740798 }, | |||
| { url = "https://files.pythonhosted.org/packages/4c/21/9ca93b84b92ef927814cb7ba37f0774a484c849d58f0b692b16af8eebcfb/pyarrow-17.0.0-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:e3343cb1e88bc2ea605986d4b94948716edc7a8d14afd4e2c097232f729758b4", size = 39871877 }, | |||
| { url = "https://files.pythonhosted.org/packages/30/d1/63a7c248432c71c7d3ee803e706590a0b81ce1a8d2b2ae49677774b813bb/pyarrow-17.0.0-cp311-cp311-win_amd64.whl", hash = "sha256:a27532c38f3de9eb3e90ecab63dfda948a8ca859a66e3a47f5f42d1e403c4d03", size = 25151089 }, | |||
| { url = "https://files.pythonhosted.org/packages/d4/62/ce6ac1275a432b4a27c55fe96c58147f111d8ba1ad800a112d31859fae2f/pyarrow-17.0.0-cp312-cp312-macosx_10_15_x86_64.whl", hash = "sha256:9b8a823cea605221e61f34859dcc03207e52e409ccf6354634143e23af7c8d22", size = 29019418 }, | |||
| { url = "https://files.pythonhosted.org/packages/8e/0a/dbd0c134e7a0c30bea439675cc120012337202e5fac7163ba839aa3691d2/pyarrow-17.0.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:f1e70de6cb5790a50b01d2b686d54aaf73da01266850b05e3af2a1bc89e16053", size = 27152197 }, | |||
| { url = "https://files.pythonhosted.org/packages/cb/05/3f4a16498349db79090767620d6dc23c1ec0c658a668d61d76b87706c65d/pyarrow-17.0.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:0071ce35788c6f9077ff9ecba4858108eebe2ea5a3f7cf2cf55ebc1dbc6ee24a", size = 39263026 }, | |||
| { url = "https://files.pythonhosted.org/packages/c2/0c/ea2107236740be8fa0e0d4a293a095c9f43546a2465bb7df34eee9126b09/pyarrow-17.0.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:757074882f844411fcca735e39aae74248a1531367a7c80799b4266390ae51cc", size = 39880798 }, | |||
| { url = "https://files.pythonhosted.org/packages/f6/b0/b9164a8bc495083c10c281cc65064553ec87b7537d6f742a89d5953a2a3e/pyarrow-17.0.0-cp312-cp312-manylinux_2_28_aarch64.whl", hash = "sha256:9ba11c4f16976e89146781a83833df7f82077cdab7dc6232c897789343f7891a", size = 38715172 }, | |||
| { url = "https://files.pythonhosted.org/packages/f1/c4/9625418a1413005e486c006e56675334929fad864347c5ae7c1b2e7fe639/pyarrow-17.0.0-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:b0c6ac301093b42d34410b187bba560b17c0330f64907bfa4f7f7f2444b0cf9b", size = 39874508 }, | |||
| { url = "https://files.pythonhosted.org/packages/ae/49/baafe2a964f663413be3bd1cf5c45ed98c5e42e804e2328e18f4570027c1/pyarrow-17.0.0-cp312-cp312-win_amd64.whl", hash = "sha256:392bc9feabc647338e6c89267635e111d71edad5fcffba204425a7c8d13610d7", size = 25099235 }, | |||
| { url = "https://files.pythonhosted.org/packages/43/e0/a898096d35be240aa61fb2d54db58b86d664b10e1e51256f9300f47565e8/pyarrow-17.0.0-cp39-cp39-macosx_10_15_x86_64.whl", hash = "sha256:13d7a460b412f31e4c0efa1148e1d29bdf18ad1411eb6757d38f8fbdcc8645fb", size = 29007881 }, | |||
| { url = "https://files.pythonhosted.org/packages/59/22/f7d14907ed0697b5dd488d393129f2738629fa5bcba863e00931b7975946/pyarrow-17.0.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:9b564a51fbccfab5a04a80453e5ac6c9954a9c5ef2890d1bcf63741909c3f8df", size = 27178117 }, | |||
| { url = "https://files.pythonhosted.org/packages/bf/ee/661211feac0ed48467b1d5c57298c91403809ec3ab78b1d175e1d6ad03cf/pyarrow-17.0.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:32503827abbc5aadedfa235f5ece8c4f8f8b0a3cf01066bc8d29de7539532687", size = 39273896 }, | |||
| { url = "https://files.pythonhosted.org/packages/af/61/bcd9b58e38ead6ad42b9ed00da33a3f862bc1d445e3d3164799c25550ac2/pyarrow-17.0.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:a155acc7f154b9ffcc85497509bcd0d43efb80d6f733b0dc3bb14e281f131c8b", size = 39875438 }, | |||
| { url = "https://files.pythonhosted.org/packages/75/63/29d1bfcc57af73cde3fc3baccab2f37548de512dbe0ab294b033cd203516/pyarrow-17.0.0-cp39-cp39-manylinux_2_28_aarch64.whl", hash = "sha256:dec8d129254d0188a49f8a1fc99e0560dc1b85f60af729f47de4046015f9b0a5", size = 38735092 }, | |||
| { url = "https://files.pythonhosted.org/packages/39/f4/90258b4de753df7cc61cefb0312f8abcf226672e96cc64996e66afce817a/pyarrow-17.0.0-cp39-cp39-manylinux_2_28_x86_64.whl", hash = "sha256:a48ddf5c3c6a6c505904545c25a4ae13646ae1f8ba703c4df4a1bfe4f4006bda", size = 39867610 }, | |||
| { url = "https://files.pythonhosted.org/packages/e7/f6/b75d4816c32f1618ed31a005ee635dd1d91d8164495d94f2ea092f594661/pyarrow-17.0.0-cp39-cp39-win_amd64.whl", hash = "sha256:42bf93249a083aca230ba7e2786c5f673507fa97bbd9725a1e2754715151a204", size = 25148611 }, | |||
| ] | |||
| [[package]] | |||