luckyrobots/open_phantom
13
0
Fork 0
mirror of https://github.com/luckyrobots/open_phantom.git synced 2025-04-03 10:32:19 +00:00
Code Issues 4 Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
Ethan Clark 2025-03-21 12:51:16 -07:00
commit 2bea6dc790
30 changed files with 4912 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
**__pycache__**
repomix-output.xml

19
.vscode/launch.json vendored Normal file
View File

@ -0,0 +1,19 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "Python Debugger: Current File",
"type": "debugpy",
"request": "launch",
"python": "${config:python.pythonPath}",
"program": "${fileDirname}/main.py",
"console": "integratedTerminal",
"cwd": "${fileDirname}",
"args": [
]
}
]
}

3
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,3 @@
{
"python.pythonPath": "/home/eejmeister/anaconda3/envs/luckyrobots/bin/python"
}

201
LICENSE Normal file
View File

@ -0,0 +1,201 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

0
README.md Normal file
View File

0
phantom/__init__.py Normal file
View File

524
phantom/hand_processor.py Normal file
View File

@ -0,0 +1,524 @@
import os
import cv2
import time
import torch
import numpy as np
import open3d as o3d
import mediapipe as mp
from tqdm import tqdm
from utils.visualizations import *
from phantom.robot_manager import RobotManager
# from diffusers import StableDiffusionInpaintPipeline
class HandProcessor:
def __init__(self) -> None:
self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
self.mp_hands = mp.solutions.hands
self.mp_drawing = mp.solutions.drawing_utils
self.mp_drawing_styles = mp.solutions.drawing_styles
self.hands = self.mp_hands.Hands(
model_complexity=1,
max_num_hands=1,
static_image_mode=False,
)
# NOTE: Look into better depth estimation models
# Initialize MiDaS for depth estimation
print("Loading MiDaS model...")
self.midas = torch.hub.load("intel-isl/MiDaS", "DPT_Hybrid")
self.midas.to(self.device)
self.midas.eval()
# Load MiDaS transforms to resize and normalize input images
self.midas_transforms = torch.hub.load("intel-isl/MiDaS", "transforms")
self.transform = self.midas_transforms.dpt_transform
# Segment hand from image using MediaPipe Hands landmarks
def _create_mask(self, image: np.ndarray, landmarks: list, thickness: int=5, padding:int=10) -> np.ndarray:
height, width = image.shape[:2]
mask = np.zeros((height, width), dtype=np.uint8)
points = [(int(landmark.x * width), int(landmark.y * height)) for landmark in landmarks.landmark]
# Draw filled circles at each landmark
for point in points:
cv2.circle(mask, point, thickness, 255, -1)
# Connect all landmarks with thick lines
for connection in self.mp_hands.HAND_CONNECTIONS:
start_idx, end_idx = connection
cv2.line(mask, points[start_idx], points[end_idx], 255, thickness)
# Create palm by connecting base of fingers with wrist
palm_points = [points[0], points[1], points[5], points[9], points[13], points[17]]
cv2.fillPoly(mask, [np.array(palm_points)], 255)
# Create shape between fingers
finger_bases = [(1,5), (5,9), (9,13), (13,17)]
for base1, base2 in finger_bases:
triangle = np.array([points[0], points[base1], points[base2]])
cv2.fillPoly(mask, [triangle], 255)
# Dilate to smooth and expand slightly
kernel = np.ones((padding, padding), np.uint8)
mask = cv2.dilate(mask, kernel, iterations=1)
# Smooth the mask
mask = cv2.GaussianBlur(mask, (21, 21), 0)
_, mask = cv2.threshold(mask, 50, 255, cv2.THRESH_BINARY)
return mask
"""
Transform input image to match MiDaS model requirements
Estimate depth map using MiDaS model
"""
# TODO: Look into why screen goes black sometimes
def _estimate_depth(self, image: np.ndarray) -> tuple:
img = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
# Transform img for MiDaS model
input_batch = self.transform(img).to(self.device)
with torch.inference_mode():
prediction = self.midas(input_batch)
# Interpolate to original size
prediction = torch.nn.functional.interpolate(
prediction.unsqueeze(1),
size=image.shape[:2],
mode="bicubic",
align_corners=False,
).squeeze()
# Convert to numpy and normalize to 0-255 for visualization
depth_map = prediction.cpu().numpy()
depth_min = depth_map.min()
depth_max = depth_map.max()
depth_map_normalized = 255 * (depth_map - depth_min) / (depth_max - depth_min)
return depth_map, depth_map_normalized.astype(np.uint8)
# TODO: Look into better depth scaling
def _create_cloud(self, depth_map: np.ndarray, hand_mask: np.ndarray) -> np.ndarray:
focal_x, focal_y, center_x, center_y = self.camera_intrinsics
v_coords, u_coords = np.where(hand_mask > 0)
z_values = depth_map[v_coords, u_coords]
# Filter out zero depth values
valid_indices = z_values > 0
u_coords = u_coords[valid_indices]
v_coords = v_coords[valid_indices]
z_values = z_values[valid_indices]
# NOTE: Abritrary scaling factor for depth
z_metric = z_values * 0.5
# Back-project to 3D using camera intrinsics
x_values = (u_coords - center_x) * z_metric / focal_x
y_values = (v_coords - center_y) * z_metric / focal_y
points = np.column_stack((x_values, y_values, z_metric))
return points
# TODO: Look into better Z scaling
def _create_mesh(self, landmarks: list, image_dims: tuple) -> np.ndarray:
width, height = image_dims
vertices = []
for landmark in landmarks.landmark:
vertices.append([
landmark.x * width,
landmark.y * height,
landmark.z * width
])
# Define faces (triangles) connecting landmarks
faces = [
# Palm
[0, 1, 5], [0, 5, 9], [0, 9, 13], [0, 13, 17],
# Thumb
[1, 2, 3], [2, 3, 4],
# Index finger
[5, 6, 7], [6, 7, 8],
# Middle finger
[9, 10, 11], [10, 11, 12],
# Ring finger
[13, 14, 15], [14, 15, 16],
# Pinky
[17, 18, 19], [18, 19, 20]
]
dense_vertices = list(vertices)
# Add interpolated vertices along finger segments
connections = self.mp_hands.HAND_CONNECTIONS
for connection in connections:
start_idx, end_idx = connection
start_point = np.array(vertices[start_idx])
end_point = np.array(vertices[end_idx])
# Add 2 interpolated points between each connected pair
for t in [0.33, 0.66]:
interp_point = start_point * (1-t) + end_point * t
dense_vertices.append(interp_point.tolist())
return np.array(dense_vertices), np.array(faces)
def _icp_registration(self, cloud: np.ndarray, vertices: np.ndarray) -> np.ndarray:
# Normalize both point clouds to handle scale differences
cloud_centroid = np.mean(cloud, axis=0)
vertices_centroid = np.mean(vertices, axis=0)
cloud_centered = cloud - cloud_centroid
vertices_centered = vertices - vertices_centroid
cloud_scale = np.max(np.linalg.norm(cloud_centered, axis=1))
vertices_scale = np.max(np.linalg.norm(vertices_centered, axis=1))
cloud_normalized = cloud_centered / cloud_scale
vertices_normalized = vertices_centered / vertices_scale
# Create Open3D point clouds
source = o3d.geometry.PointCloud()
source.points = o3d.utility.Vector3dVector(vertices_normalized)
target = o3d.geometry.PointCloud()
target.points = o3d.utility.Vector3dVector(cloud_normalized)
# Optional: Downsample for better performance
source = source.voxel_down_sample(voxel_size=0.01)
target = target.voxel_down_sample(voxel_size=0.01)
# Initial identity transformation
trans_init = np.eye(4)
# Run ICP with better parameters
result = o3d.pipelines.registration.registration_icp(
source,
target,
max_correspondence_distance=0.2, # Increased for better matching
init=trans_init,
estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane(),
criteria=o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=100)
)
# Create denormalization transformation
denorm_transform = np.eye(4)
denorm_transform[:3, :3] *= (cloud_scale / vertices_scale)
# Combine transformations
transform_to_origin = np.eye(4)
transform_to_origin[:3, 3] = -vertices_centroid
transform_from_origin = np.eye(4)
transform_from_origin[:3, 3] = cloud_centroid
final_transform = np.matmul(transform_from_origin,
np.matmul(denorm_transform,
np.matmul(result.transformation, transform_to_origin)))
return final_transform
def _refine_landmarks(self, landmarks: list, transform: int, image_dims: tuple):
width, height = image_dims
refined_landmarks = []
for landmark in landmarks.landmark:
point = np.array([
landmark.x * width,
landmark.y * height,
landmark.z * width, # TODO: Figure out better scaling factor
1.0
])
# Apply transformation to 3D point
transformed = np.dot(transform, point)
refined_landmarks.append(transformed[:3])
return refined_landmarks
def _apply_constraints(self, landmarks: list):
constrained = np.array(landmarks)
# Define finger joint indices
# MediaPipe hand model: Wrist is 0, thumb is 1-4, index is 5-8, etc.
thumb_indices = [1, 2, 3, 4]
index_indices = [5, 6, 7, 8]
# Constrain the last two joints of thumb and index finger
# as mentioned in the paper
for finger_indices in [thumb_indices, index_indices]:
# Get the last three joints (two segments)
if len(finger_indices) >= 3:
# Get joint positions
joint1 = constrained[finger_indices[-3]]
joint2 = constrained[finger_indices[-2]]
joint3 = constrained[finger_indices[-1]]
# Direction of the first segment
dir1 = joint2 - joint1
dir1 = dir1 / np.linalg.norm(dir1)
# Instead of full ball joint, constrain the last joint's direction
# to follow similar direction as the previous segment
ideal_dir = dir1.copy()
actual_dir = joint3 - joint2
actual_length = np.linalg.norm(actual_dir)
# Force the direction to be within a cone of the previous segment
# (limiting to single degree of freedom approximately)
corrected_dir = ideal_dir * actual_length
# Apply the correction
constrained[finger_indices[-1]] = joint2 + corrected_dir
return constrained
def _get_robot_params(self, refined_landmarks) -> tuple:
# Extract key landmarks
wrist = refined_landmarks[0] # Wrist landmark
thumb_tip = refined_landmarks[4] # Thumb tip
index_tip = refined_landmarks[8] # Index finger tip
# Calculate end effector position (midpoint between thumb and index tips)
position = (thumb_tip + index_tip) / 2
# Calculate vectors for orientation
v1 = thumb_tip - wrist # Vector from wrist to thumb tip
v2 = index_tip - wrist # Vector from wrist to index tip
# Calculate normal to hand plane
normal = np.cross(v1, v2)
if np.linalg.norm(normal) > 0:
normal = normal / np.linalg.norm(normal)
else:
# Default if vectors are collinear
normal = np.array([0, 0, 1])
# Calculate main direction along thumb
direction = thumb_tip - wrist
if np.linalg.norm(direction) > 0:
direction = direction / np.linalg.norm(direction)
else:
# Default if thumb is at wrist (unlikely)
direction = np.array([1, 0, 0])
# Calculate gripper width
gripper_width = np.linalg.norm(thumb_tip - index_tip)
return position, direction, normal, gripper_width
def record_video(self) -> str:
output_dir = os.path.join(os.getcwd(), 'recordings')
os.makedirs(output_dir, exist_ok=True)
timestamp = time.strftime("%Y%m%d-%H%M%S")
video_filename = os.path.join(output_dir, f"recorded_video_{timestamp}.mp4")
cap = cv2.VideoCapture(0)
assert cap.isOpened(), "Failed to open camera."
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = int(cap.get(cv2.CAP_PROP_FPS))
# Update camera intrinsics based on video dimensions
self.camera_intrinsics = (width * 0.8, height * 0.8, width / 2, height / 2)
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = None
recording = False
did_record = False
print("Camera is active. Press 'r' to start/stop recording, 'q' to quit.")
while cap.isOpened():
success, frame = cap.read()
assert success, "Failed to read from camera."
# Mirror the image for more intuitive viewing
frame = cv2.flip(frame, 1)
# Create a separate display frame for showing the recording indicator
display_frame = frame.copy()
# Display recording indicator ONLY on the display frame
if recording:
cv2.circle(display_frame, (30, 30), 15, (0, 0, 255), -1)
cv2.putText(display_frame, "RECORDING", (50, 40), cv2.FONT_HERSHEY_SIMPLEX,
1, (0, 0, 255), 2, cv2.LINE_AA)
# Show the display frame (with indicator if recording)
cv2.imshow('Video Recording', display_frame)
# Write the original frame (without indicator) to video file if recording
if recording and out is not None:
out.write(frame)
key = cv2.waitKey(1) & 0xFF
# NOTE: Haven't tested what happens if user records multiple videos in one session (probably overwrites?)
if key == ord('q'):
break
elif key == ord('r'):
recording = not recording
if recording:
print(f"Started recording to {video_filename}")
out = cv2.VideoWriter(video_filename, fourcc, fps, (width, height))
else:
if out is not None:
out.release()
print(f"Stopped recording. Video saved to {video_filename}")
did_record = True
if out is not None:
out.release()
cap.release()
cv2.destroyAllWindows()
return video_filename if did_record else None
def process_video(self, video_path: str) -> tuple:
assert video_path, "Video path is required."
cap = cv2.VideoCapture(video_path)
assert cap.isOpened(), f"Failed to open video file {video_path}"
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = int(cap.get(cv2.CAP_PROP_FPS))
total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
# Update camera intrinsics based on video dimensions
self.camera_intrinsics = (width * 0.8, height * 0.8, width / 2, height / 2)
base_path = os.path.splitext(video_path)[0]
segmented_output_path = f"{base_path}_masked.mp4"
depth_output_path = f"{base_path}_depth.mp4"
mesh_output_path = f"{base_path}_mesh.mp4"
registration_output_path = f"{base_path}_registration.mp4"
constraints_output_path = f"{base_path}_constraints.mp4"
robot_output_path = f"{base_path}_robot.mp4"
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
segmented_out = cv2.VideoWriter(segmented_output_path, fourcc, fps, (width, height))
depth_out = cv2.VideoWriter(depth_output_path, fourcc, fps, (width, height))
mesh_out = cv2.VideoWriter(mesh_output_path, fourcc, fps, (width, height))
reg_out = cv2.VideoWriter(registration_output_path, fourcc, fps, (640, 480)) # Fixed size
constraints_out = cv2.VideoWriter(constraints_output_path, fourcc, fps, (width, height))
robot_out = cv2.VideoWriter(robot_output_path, fourcc, fps, (width, height))
print(f"Processing video with {total_frames} frames...")
for _ in tqdm(range(total_frames)):
success, frame = cap.read()
assert success, "Failed to read frame from video."
# Convert image to RGB for MediaPipe
image_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
# Process with MediaPipe Hands
results = self.hands.process(image_rgb)
# Initialize output frames
segmented_frame = frame.copy()
depth_frame = np.zeros((height, width, 3), dtype=np.uint8)
mesh_frame = frame.copy()
reg_frame = np.ones((480, 640, 3), dtype=np.uint8) * 255 # Fixed size, white background
constraints_frame = frame.copy()
robot_frame = frame.copy()
# Process if hand is detected
if results.multi_hand_landmarks:
for hand_landmarks in results.multi_hand_landmarks:
# Segment hand
hand_mask = self._create_mask(frame, hand_landmarks)
mask_overlay = frame.copy()
mask_overlay[hand_mask > 0] = [0, 0, 255] # Red color for mask
segmented_frame = cv2.addWeighted(frame, 0.7, mask_overlay, 0.3, 0)
# Depth estimation
depth_map, depth_vis = self._estimate_depth(frame)
depth_colored = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET)
depth_frame = depth_colored.copy()
# Create hand mesh
hand_vertices, hand_faces = self._create_mesh(hand_landmarks, (width, height))
mesh_frame = visualize_mesh(frame, hand_vertices, hand_faces)
# Create point cloud from depth
cloud = self._create_cloud(depth_map, hand_mask)
# Perform ICP registration
icp_transform = self._icp_registration(cloud, hand_vertices)
reg_frame = visualize_registration(cloud, hand_vertices, icp_transform)
# Refine landmarks using ICP transformation
refined_landmarks = self._refine_landmarks(hand_landmarks, icp_transform, (width, height))
# Store pre-constraint landmarks for visualization
original_refined = refined_landmarks.copy()
# Apply anatomical constraints
constrained_landmarks = self._apply_constraints(refined_landmarks)
constraints_frame = visualize_constraints(frame, original_refined, constrained_landmarks, self.camera_intrinsics)
# # Calculate robot parameters
# position, direction, normal, gripper_width = self._get_robot_params(constrained_landmarks)
# robot_frame = visualize_robot_params(frame, position, direction, normal, gripper_width)
segmented_out.write(segmented_frame)
depth_out.write(depth_frame)
mesh_out.write(mesh_frame)
reg_out.write(reg_frame)
constraints_out.write(constraints_frame)
robot_out.write(robot_frame)
display_scale = 0.5
display_size = (int(width * display_scale), int(height * display_scale))
reg_display_size = (int(640 * display_scale), int(480 * display_scale))
cv2.imshow('Segmented', cv2.resize(segmented_frame, display_size))
cv2.imshow('Depth', cv2.resize(depth_frame, display_size))
cv2.imshow('Mesh', cv2.resize(mesh_frame, display_size))
cv2.imshow('Registration', cv2.resize(reg_frame, reg_display_size))
cv2.imshow('Constraints', cv2.resize(constraints_frame, display_size))
cv2.imshow('Robot Parameters', cv2.resize(robot_frame, display_size))
if cv2.waitKey(1) & 0xFF == 27: # ESC to exit
break
cap.release()
segmented_out.release()
depth_out.release()
mesh_out.release()
reg_out.release()
constraints_out.release()
robot_out.release()
cv2.destroyAllWindows()
print(f"Processing complete. Results saved to:")
print(f"- Hand mask: {segmented_output_path}")
print(f"- Depth visualization: {depth_output_path}")
print(f"- Mesh visualization: {mesh_output_path}")
print(f"- Registration visualization: {registration_output_path}")
print(f"- Constraints visualization: {constraints_output_path}")
print(f"- Robot parameters: {robot_output_path}")
return {
"segmented": segmented_output_path,
"depth": depth_output_path,
"mesh": mesh_output_path,
"registration": registration_output_path,
"constraints": constraints_output_path,
"robot": robot_output_path
}

33
phantom/main.py Normal file
View File

@ -0,0 +1,33 @@
import os
from phantom.hand_processor import HandProcessor
def main():
processor = HandProcessor()
record_option = input("Record a new video? (y/n): ")
if record_option.lower() == 'y':
print("Starting video recording...")
video_path = processor.record_video()
if not video_path:
print("Video recording failed.")
return
print(f"Video recorded successfully to {video_path}")
process_option = input("Process the video now? (y/n): ")
if process_option.lower() == "n":
print("Video processing skipped.")
return
else:
video_path = input("Enter the path to a video file: ")
while not os.path.exists(video_path):
print("Error: Video file not found.")
video_path = input("Enter the path to the video file: ")
processor.process_video(video_path)
if __name__ == "__main__":
main()

158
phantom/robot_manager.py Normal file
View File

@ -0,0 +1,158 @@
import os
import cv2
import numpy as np
import pybullet as p
from utils.math import *
from utils.handle_urdf import handle_urdf
from scipy.spatial.transform import Rotation as R
class RobotManager:
def __init__(self, urdf_path: str, camera_intrinsics: tuple) -> None:
self.physics_client = p.connect(p.DIRECT)
robot_urdf = handle_urdf(urdf_path)
self.robot_id = self._load_robot(robot_urdf)
self.joint_count = p.getNumJoints(self.robot_id)
self.end_effector_index = self._find_end_effector()
self.fx, self.fy, self.cx, self.cy = camera_intrinsics
# Set up rendering parameters
self.img_width = int(self.cx * 2)
self.img_height = int(self.cy * 2)
# Load the robot URDF into PyBullet
def _load_robot(self, robot_urdf: str) -> int:
try:
robot_id = p.loadURDF(
robot_urdf,
basePosition=[0, 0, 0],
useFixedBase=True,
flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_INERTIA_FROM_FILE
)
except p.error as e:
print(f"PyBullet error when loading URDF: {e}")
raise e
robot_name = p.getBodyInfo(robot_id)[1].decode('utf-8')
print(f"Successfully loaded {robot_name} robot with ID: {robot_id}")
return robot_id
# NOTE: Only applicable if the robot has one end effector
def _find_end_effector(self) -> int:
assert self.joint_count > 0, "Robot has no joints"
# Keywords to look for in joint names to identify end effector
keywords = ['gripper', 'tool', 'tcp', 'end_effector', 'hand']
for i in range(self.joint_count):
info = p.getJointInfo(self.robot_id, i)
joint_name = info[1].decode('utf-8').lower()
# Check if any keyword is in the joint name
if any(keyword in joint_name for keyword in keywords):
return i
# If no specific end effector found, use the last joint in the chain
return self.joint_count - 1
# TODO: Use inverse kinematics to set the robot pose
def set_robot_pose(self, position: np.ndarray, orientation_vectors: np.ndarray, gripper_width: float) -> None:
pass
# Render the robot in some scene using some camera parameters
def render_robot(self, bg_image=None, camera_params=None):
assert self.robot_id >= 0, "Robot not properly loaded"
# Set up camera parameters
if camera_params is None:
# Default camera setup
cam_target = [0, 0, 0]
cam_distance = 1.0
cam_yaw = 0
cam_pitch = -30
cam_roll = 0
else:
cam_target, cam_distance, cam_yaw, cam_pitch, cam_roll = camera_params
# Compute view matrix
view_matrix = p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=cam_target,
distance=cam_distance,
yaw=cam_yaw,
pitch=cam_pitch,
roll=cam_roll,
upAxisIndex=2
)
# Compute projection matrix
aspect = self.img_width / self.img_height
proj_matrix = p.computeProjectionMatrixFOV(
fov=60,
aspect=aspect,
nearVal=0.01,
farVal=100.0
)
# Render the scene
img_arr = p.getCameraImage(
width=self.img_width,
height=self.img_height,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL
)
# Extract RGB and depth
rgb = np.reshape(img_arr[2], (self.img_height, self.img_width, 4))
rgb = rgb[:, :, :3] # Remove alpha channel
depth = np.reshape(img_arr[3], (self.img_height, self.img_width))
# If background image is provided, composite
if bg_image is not None:
# Resize background if needed
bg_h, bg_w = bg_image.shape[:2]
if bg_w != self.img_width or bg_h != self.img_height:
bg_resized = cv2.resize(bg_image, (self.img_width, self.img_height))
else:
bg_resized = bg_image
# Create mask from depth
mask = (depth < 0.99).astype(np.float32)
mask = np.stack([mask, mask, mask], axis=2)
# Composite
composite = bg_resized * (1 - mask) + rgb * mask
return composite.astype(np.uint8)
return rgb.astype(np.uint8)
def __del__(self) -> None:
if hasattr(self, 'physics_client'):
try:
p.disconnect(self.physics_client)
except:
pass
if __name__ == "__main__":
cwd = os.getcwd()
urdf_path = os.path.join(cwd, "notebook/phantom/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf")
camera_intrinsics = (320, 240, 320, 240) # Random intrinsics for example
robot_vis = RobotManager(urdf_path, camera_intrinsics)
rendered_image = robot_vis.render_robot()
# Option 1: Display the image using OpenCV
cv2.imshow("Robot Render", rendered_image)
cv2.waitKey(0) # Wait for a key press
cv2.destroyAllWindows()
# Option 2: Save the image to a file
output_path = "robot_render.png"
cv2.imwrite(output_path, rendered_image)
print(f"Render saved to {output_path}")

0
phantom/utils/__init__.py Normal file
View File

94
phantom/utils/handle_urdf.py Normal file
View File

@ -0,0 +1,94 @@
import os
import xml.dom.minidom as minidom
import xml.etree.ElementTree as ET
"""
Fixes a specific path that uses the ROS-style 'package://' prefix.
The 'package://' prefix is used in URDF files to refer to files in the same package.
However, when we're not running in a ROS environment, the paths are not valid.
This function tries to find the absolute path of the mesh file.
If the mesh file is not found, the original path is used.
"""
def fix_path(path: str, urdf_dir: str) -> str:
if path.startswith('package://'):
parts = path[len('package://'):].split('/', 1)
if len(parts) == 2:
package_name, rel_path = parts
# Try potential locations for the mesh
potential_paths = [
os.path.join(urdf_dir, rel_path),
os.path.join(urdf_dir, '../meshes', rel_path),
os.path.join(urdf_dir, f'../{package_name}', rel_path),
os.path.join(urdf_dir, '../..', rel_path)
]
for possible_path in potential_paths:
if os.path.exists(possible_path):
return possible_path
print(f"Failed to find mesh for package path: {path}")
return path
"""
Iterates through the URDF file and fixes the paths of all mesh files.
The URDF file is parsed and the mesh paths are modified in-place.
"""
def fix_mesh_paths(urdf_path:str, urdf_dir: str) -> str:
root = ET.parse(urdf_path).getroot()
try:
for mesh in root.findall('.//mesh'):
if 'filename' in mesh.attrib:
mesh.attrib['filename'] = fix_path(mesh.attrib['filename'], urdf_dir)
except Exception as e:
print(f"Error fixing mesh paths: {e}")
raise e
fixed_path = urdf_path.replace('.urdf', '_fixed.urdf')
return root, fixed_path
"""
Formats the XML tree to be human-readable and writes it to a file.
"""
def format_xml(root: ET.Element, fixed_path: str) -> None:
xml_str = ET.tostring(root, encoding='utf-8')
dom = minidom.parseString(xml_str)
with open(fixed_path, 'w', encoding='utf-8') as f:
# Write with nice indentation but remove extra whitespace
pretty_xml = dom.toprettyxml(indent=' ')
# Remove extra blank lines that minidom sometimes adds
pretty_xml = '\n'.join([line for line in pretty_xml.split('\n') if line.strip()])
f.write(pretty_xml)
def handle_urdf(urdf_path: str) -> str:
# Check if URDF is valid
if not os.path.exists(urdf_path):
print(f"Invalid URDF path: {urdf_path}")
return None
# FIXME: Add check to see if URDF needs to be processed
try:
urdf_dir = os.path.dirname(urdf_path)
root, fixed_path = fix_mesh_paths(urdf_path, urdf_dir)
format_xml(root, fixed_path)
print(f"Successfully processed URDF: {fixed_path}")
return fixed_path
except Exception as e:
print(f"Failed to process URDF: {e}")
raise e
if __name__ == '__main__':
# Example usage
cwd = os.getcwd()
urdf_path = os.path.join(cwd, "notebook/phantom/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf")
handle_urdf(urdf_path)

0
phantom/utils/math.py Normal file
View File

263
phantom/utils/visualizations.py Normal file
View File

@ -0,0 +1,263 @@
import cv2
import numpy as np
def visualize_mesh(frame, hand_vertices, faces):
"""Visualize the hand mesh on the frame"""
mesh_vis = frame.copy()
height, width = frame.shape[:2]
# Draw the base vertices of the mesh (original landmarks)
for i in range(21):
pt = tuple(map(int, hand_vertices[i][:2]))
if 0 <= pt[0] < width and 0 <= pt[1] < height:
cv2.circle(mesh_vis, pt, 4, (255, 0, 0), -1) # Red circles for base vertices
# Draw the interpolated vertices
for i in range(21, len(hand_vertices)):
pt = tuple(map(int, hand_vertices[i][:2]))
if 0 <= pt[0] < width and 0 <= pt[1] < height:
cv2.circle(mesh_vis, pt, 2, (0, 255, 255), -1) # Yellow circles for interpolated vertices
# Draw the faces of the mesh
for face in faces:
if len(face) == 3: # Triangle face
pt1 = tuple(map(int, hand_vertices[face[0]][:2]))
pt2 = tuple(map(int, hand_vertices[face[1]][:2]))
pt3 = tuple(map(int, hand_vertices[face[2]][:2]))
if (0 <= pt1[0] < width and 0 <= pt1[1] < height and
0 <= pt2[0] < width and 0 <= pt2[1] < height and
0 <= pt3[0] < width and 0 <= pt3[1] < height):
cv2.line(mesh_vis, pt1, pt2, (0, 255, 0), 1) # Green lines for mesh edges
cv2.line(mesh_vis, pt2, pt3, (0, 255, 0), 1)
cv2.line(mesh_vis, pt3, pt1, (0, 255, 0), 1)
# Add explanation text
cv2.putText(mesh_vis, "Red: Base vertices", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2)
cv2.putText(mesh_vis, "Yellow: Interpolated vertices", (10, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2)
cv2.putText(mesh_vis, "Green: Mesh edges", (10, 90),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
return mesh_vis
def visualize_registration(cloud, vertices, icp_transform):
"""Visualize the ICP registration process"""
height, width = 480, 640 # Fixed size visualization
reg_vis = np.ones((height, width, 3), dtype=np.uint8) * 255 # White background
# Define visualization boundaries
margin = 50
view_width = width - 2 * margin
view_height = height - 2 * margin
# Find 2D min/max values for scaling
all_points_2d = np.vstack([cloud[:, :2], vertices[:, :2]])
min_vals = np.min(all_points_2d, axis=0)
max_vals = np.max(all_points_2d, axis=0)
# Function to scale points to fit visualization
def scale_point(point):
scaled = (point - min_vals) / (max_vals - min_vals)
x = int(scaled[0] * view_width) + margin
y = int(scaled[1] * view_height) + margin
return (x, y)
# Draw original point cloud (red)
for point in cloud[::10]: # Downsample for visualization
pt = scale_point(point[:2])
cv2.circle(reg_vis, pt, 1, (0, 0, 255), -1) # Red dots
# Draw original mesh vertices (blue)
for vertex in vertices:
pt = scale_point(vertex[:2])
cv2.circle(reg_vis, pt, 2, (255, 0, 0), -1) # Blue dots
# Apply transformation to mesh vertices
transformed_vertices = []
for vertex in vertices:
# Convert to homogeneous coordinates
v_hom = np.append(vertex, 1.0)
# Apply transformation
v_transformed = np.dot(icp_transform, v_hom)
transformed_vertices.append(v_transformed[:3])
# Draw transformed mesh vertices (green)
for vertex in transformed_vertices:
pt = scale_point(vertex[:2])
cv2.circle(reg_vis, pt, 2, (0, 255, 0), -1) # Green dots
# Add transformation matrix display
cv2.putText(reg_vis, "ICP Transformation Matrix:", (10, height - 120),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
for i in range(4):
matrix_text = f"[{icp_transform[i][0]:.2f}, {icp_transform[i][1]:.2f}, {icp_transform[i][2]:.2f}, {icp_transform[i][3]:.2f}]"
cv2.putText(reg_vis, matrix_text, (10, height - 90 + i * 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 0), 1)
# Add legend
cv2.putText(reg_vis, "Red: Point Cloud", (width - 200, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
cv2.putText(reg_vis, "Blue: Original Mesh", (width - 200, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
cv2.putText(reg_vis, "Green: Transformed Mesh", (width - 200, 90),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
return reg_vis
def visualize_constraints(frame, refined_landmarks, constrained_landmarks, camera_intrinsics):
"""Visualize before and after applying anatomical constraints"""
constraints_vis = frame.copy()
height, width = frame.shape[:2]
focal_x, focal_y, center_x, center_y = camera_intrinsics
# Define finger indices
thumb_indices = [1, 2, 3, 4]
index_indices = [5, 6, 7, 8]
highlighted_indices = thumb_indices + index_indices
# Draw original refined landmarks
for i, landmark in enumerate(refined_landmarks):
x, y, z = landmark
if z > 0:
u = int(x * focal_x / z + center_x)
v = int(y * focal_y / z + center_y)
if 0 <= u < width and 0 <= v < height:
if i in highlighted_indices:
# Draw larger circles for thumb and index fingers (the constrained ones)
cv2.circle(constraints_vis, (u, v), 5, (0, 0, 255), -1) # Red circles
else:
cv2.circle(constraints_vis, (u, v), 3, (0, 0, 255), -1)
# Draw connections for original landmarks
for i in range(len(refined_landmarks) - 1):
if i + 1 in thumb_indices and i in thumb_indices:
# Draw thumb connections
start = refined_landmarks[i]
end = refined_landmarks[i + 1]
if start[2] > 0 and end[2] > 0:
start_u = int(start[0] * focal_x / start[2] + center_x)
start_v = int(start[1] * focal_y / start[2] + center_y)
end_u = int(end[0] * focal_x / end[2] + center_x)
end_v = int(end[1] * focal_y / end[2] + center_y)
if (0 <= start_u < width and 0 <= start_v < height and
0 <= end_u < width and 0 <= end_v < height):
cv2.line(constraints_vis, (start_u, start_v), (end_u, end_v), (0, 0, 255), 2)
# Draw constrained landmarks
for i, landmark in enumerate(constrained_landmarks):
x, y, z = landmark
if z > 0:
u = int(x * focal_x / z + center_x)
v = int(y * focal_y / z + center_y)
if 0 <= u < width and 0 <= v < height:
if i in highlighted_indices:
# Draw larger circles for thumb and index fingers
cv2.circle(constraints_vis, (u, v), 5, (0, 255, 0), -1) # Green circles
else:
cv2.circle(constraints_vis, (u, v), 3, (0, 255, 0), -1)
# Draw connections for constrained landmarks
for i in range(len(constrained_landmarks) - 1):
if i + 1 in thumb_indices and i in thumb_indices:
# Draw thumb connections
start = constrained_landmarks[i]
end = constrained_landmarks[i + 1]
if start[2] > 0 and end[2] > 0:
start_u = int(start[0] * focal_x / start[2] + center_x)
start_v = int(start[1] * focal_y / start[2] + center_y)
end_u = int(end[0] * focal_x / end[2] + center_x)
end_v = int(end[1] * focal_y / end[2] + center_y)
if (0 <= start_u < width and 0 <= start_v < height and
0 <= end_u < width and 0 <= end_v < height):
cv2.line(constraints_vis, (start_u, start_v), (end_u, end_v), (0, 255, 0), 2)
# Add legend
cv2.putText(constraints_vis, "Red: Before constraints", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.putText(constraints_vis, "Green: After constraints", (10, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
return constraints_vis
def visualize_robot_params(frame, position, direction, normal, gripper_width, camera_intrinsics):
"""Visualize robot parameters (position, orientation, gripper width)"""
robot_vis = frame.copy()
height, width = frame.shape[:2]
focal_x, focal_y, center_x, center_y = camera_intrinsics
# Project position to image coordinates
x, y, z = position
if z > 0:
u = int(x * focal_x / z + center_x)
v = int(y * focal_y / z + center_y)
if 0 <= u < width and 0 <= v < height:
# Draw end effector position
cv2.circle(robot_vis, (u, v), 10, (255, 0, 0), -1) # Blue circle
# Draw direction vector (X axis)
dx, dy, dz = direction
scale = 50 # Scale for better visualization
end_u = int((x + dx * scale) * focal_x / (z + dz * scale) + center_x)
end_v = int((y + dy * scale) * focal_y / (z + dz * scale) + center_y)
if 0 <= end_u < width and 0 <= end_v < height:
cv2.line(robot_vis, (u, v), (end_u, end_v), (0, 0, 255), 2) # Red line (X axis)
cv2.putText(robot_vis, "X", (end_u, end_v),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
# Draw normal vector (Z axis)
nx, ny, nz = normal
end_u = int((x + nx * scale) * focal_x / (z + nz * scale) + center_x)
end_v = int((y + ny * scale) * focal_y / (z + nz * scale) + center_y)
if 0 <= end_u < width and 0 <= end_v < height:
cv2.line(robot_vis, (u, v), (end_u, end_v), (0, 255, 0), 2) # Green line (Z axis)
cv2.putText(robot_vis, "Z", (end_u, end_v),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
# Calculate Y axis (cross product of Z and X for right-hand coordinate system)
y_axis = np.cross(normal, direction)
y_axis = y_axis / np.linalg.norm(y_axis)
yx, yy, yz = y_axis
end_u = int((x + yx * scale) * focal_x / (z + yz * scale) + center_x)
end_v = int((y + yy * scale) * focal_y / (z + yz * scale) + center_y)
if 0 <= end_u < width and 0 <= end_v < height:
cv2.line(robot_vis, (u, v), (end_u, end_v), (255, 0, 0), 2) # Blue line (Y axis)
cv2.putText(robot_vis, "Y", (end_u, end_v),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)
# Visualize gripper width
gripper_radius = int(gripper_width * 100) # Scale for better visualization
cv2.circle(robot_vis, (u, v), gripper_radius, (0, 255, 255), 2) # Yellow circle
# Add parameter values as text
y_offset = height - 160
cv2.putText(robot_vis, f"Position: ({x:.2f}, {y:.2f}, {z:.2f})", (10, y_offset),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
y_offset += 30
cv2.putText(robot_vis, f"Direction: ({dx:.2f}, {dy:.2f}, {dz:.2f})", (10, y_offset),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
y_offset += 30
cv2.putText(robot_vis, f"Normal: ({nx:.2f}, {ny:.2f}, {nz:.2f})", (10, y_offset),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
y_offset += 30
cv2.putText(robot_vis, f"Gripper Width: {gripper_width:.2f}", (10, y_offset),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
return robot_vis

90
requirements.txt Normal file
View File

@ -0,0 +1,90 @@
absl-py==2.2.0
aiohappyeyeballs @ file:///croot/aiohappyeyeballs_1734469393482/work
aiohttp @ file:///croot/aiohttp_1734687138658/work
aiosignal @ file:///tmp/build/80754af9/aiosignal_1637843061372/work
ansi2html @ file:///croot/ansi2html_1705049836845/work
async-timeout @ file:///croot/async-timeout_1732661977001/work
attrs @ file:///croot/attrs_1734533101012/work
blinker @ file:///croot/blinker_1737448726027/work
Brotli @ file:///croot/brotli-split_1736182456865/work
certifi @ file:///croot/certifi_1738623731865/work/certifi
cffi==1.17.1
charset-normalizer @ file:///croot/charset-normalizer_1721748349566/work
click @ file:///croot/click_1698129812380/work
contourpy==1.3.1
cycler==0.12.1
dash @ file:///croot/dash_1701446519353/work
filelock==3.18.0
Flask @ file:///croot/flask_1716545870149/work
Flask-Compress @ file:///croot/flask-compress_1668714942319/work
flatbuffers==25.2.10
fonttools==4.56.0
frozenlist @ file:///croot/frozenlist_1730902802621/work
fsspec==2025.3.0
huggingface-hub==0.29.3
idna==3.10
importlib_metadata @ file:///croot/importlib_metadata-suite_1732633488278/work
itsdangerous @ file:///croot/itsdangerous_1716533337253/work
jax==0.5.3
jaxlib==0.5.3
Jinja2 @ file:///croot/jinja2_1741710844255/work
kiwisolver==1.4.8
loguru @ file:///croot/loguru_1729800300339/work
MarkupSafe @ file:///croot/markupsafe_1738584038848/work
matplotlib==3.10.1
mediapipe==0.10.21
ml_dtypes==0.5.1
mpmath==1.3.0
msgpack @ file:///opt/conda/conda-bld/msgpack-python_1652362659880/work
multidict @ file:///croot/multidict_1730905498140/work
nest-asyncio @ file:///croot/nest-asyncio_1708532673751/work
networkx==3.4.2
numpy @ file:///croot/numpy_and_numpy_base_1708638617955/work/dist/numpy-1.26.4-cp310-cp310-linux_x86_64.whl#sha256=ce136f308f566617937abac6c8f280bd61b41c12ab7eb7d8784c5795c5be477b
nvidia-cublas-cu12==12.4.5.8
nvidia-cuda-cupti-cu12==12.4.127
nvidia-cuda-nvrtc-cu12==12.4.127
nvidia-cuda-runtime-cu12==12.4.127
nvidia-cudnn-cu12==9.1.0.70
nvidia-cufft-cu12==11.2.1.3
nvidia-curand-cu12==10.3.5.147
nvidia-cusolver-cu12==11.6.1.9
nvidia-cusparse-cu12==12.3.1.170
nvidia-cusparselt-cu12==0.6.2
nvidia-nccl-cu12==2.21.5
nvidia-nvjitlink-cu12==12.4.127
nvidia-nvtx-cu12==12.4.127
open3d-cpu @ file:///home/conda/feedstock_root/build_artifacts/open3d_1737748982201/work/build/lib/python_package/pip_package/open3d_cpu-0.19.0%2Bd28f9fd-cp310-cp310-manylinux_2_17_x86_64.whl#sha256=66e5d794f148bc721110a1db5a0a243a73bf58eeb83dc2f8cf88e7760f2fafd6
opencv-contrib-python==4.11.0.86
opt_einsum==3.4.0
packaging @ file:///croot/packaging_1734472117206/work
pillow==11.1.0
plotly @ file:///croot/plotly_1740385556434/work
propcache @ file:///croot/propcache_1732303986938/work
protobuf==4.25.6
pybullet @ file:///home/conda/feedstock_root/build_artifacts/bullet_1725366740765/work
pycparser==2.22
pyparsing==3.2.1
PySocks @ file:///home/builder/ci_310/pysocks_1640793678128/work
python-dateutil==2.9.0.post0
PyYAML==6.0.2
requests @ file:///croot/requests_1730999120400/work
retrying @ file:///Users/ktietz/demo/mc3/conda-bld/retrying_1629465456590/work
safetensors==0.5.3
scipy @ file:///croot/scipy_1737122901252/work/dist/scipy-1.15.1-cp310-cp310-linux_x86_64.whl#sha256=3a9bc1330ec4c8e1a3d5149a796c2a8a571e7b44f1336cfc344c247fed4c0eea
sentencepiece==0.2.0
six @ file:///tmp/build/80754af9/six_1644875935023/work
sounddevice==0.5.1
sympy==1.13.1
tenacity @ file:///croot/tenacity_1730302500303/work
timm==1.0.15
torch==2.6.0
torchvision==0.21.0
tqdm==4.67.1
triton==3.2.0
typing_extensions @ file:///croot/typing_extensions_1734714854207/work
urllib3 @ file:///croot/urllib3_1737133630106/work
vtk==9.3.1
Werkzeug @ file:///croot/werkzeug_1731005776056/work
wslink @ file:///home/conda/feedstock_root/build_artifacts/wslink_1740199482166/work
yarl @ file:///croot/yarl_1732546845924/work
zipp @ file:///croot/zipp_1732630741423/work

14
urdf/SO_5DOF_ARM100_05d.SLDASM/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(SO_5DOF_ARM100_05d.SLDASM)
find_package(catkin REQUIRED)
catkin_package()
find_package(roslaunch)
foreach(dir config launch meshes urdf)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)

1
urdf/SO_5DOF_ARM100_05d.SLDASM/config/joint_names_SO_5DOF_ARM100_05d.SLDASM.yaml Normal file
View File

@ -0,0 +1 @@
controller_joint_names: ['', 'Shoulder_Rotation', 'Shoulder_Pitch', 'Elbow', 'Wrist_Pitch', 'Wrist_Roll', 'Gripper', ]

2882
urdf/SO_5DOF_ARM100_05d.SLDASM/export.log Normal file
View File

File diff suppressed because one or more lines are too long

20
urdf/SO_5DOF_ARM100_05d.SLDASM/launch/display.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<arg
name="model" />
<param
name="robot_description"
textfile="$(find SO_5DOF_ARM100_05d.SLDASM)/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf" />
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher" />
<node
name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find SO_5DOF_ARM100_05d.SLDASM)/urdf.rviz" />
</launch>

20
urdf/SO_5DOF_ARM100_05d.SLDASM/launch/gazebo.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find SO_5DOF_ARM100_05d.SLDASM)/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf -urdf -model SO_5DOF_ARM100_05d.SLDASM"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL Normal file
View File

Binary file not shown.

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL Normal file
View File

Binary file not shown.

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL Normal file
View File

Binary file not shown.

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL Normal file
View File

Binary file not shown.

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL Normal file
View File

Binary file not shown.

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL Normal file
View File

Binary file not shown.

BIN
urdf/SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL Normal file
View File

Binary file not shown.

21
urdf/SO_5DOF_ARM100_05d.SLDASM/package.xml Normal file
View File

@ -0,0 +1,21 @@
<package format="2">
<name>SO_5DOF_ARM100_05d.SLDASM</name>
<version>1.0.0</version>
<description>
<p>URDF Description package for SO_5DOF_ARM100_05d.SLDASM</p>
<p>This package contains configuration data, 3D models and launch files
for SO_5DOF_ARM100_05d.SLDASM robot</p>
</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>rviz</depend>
<depend>joint_state_publisher_gui</depend>
<depend>gazebo</depend>
<export>
<architecture_independent />
</export>
</package>

8
urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM.csv Normal file
View File

@ -0,0 +1,8 @@
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
Base,-1.55220299024015E-10,0.0275980388649202,0.0272094138963763,0,0,0,0.146962928243327,9.5191642834079E-05,2.02405274856147E-12,1.46514387606669E-13,0.000123785814019492,1.84608762035329E-05,0.000137926707148466,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL,,Base_03-1;STS3215_02a-5,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
Shoulder_Rotation_Pitch,-0.00511938391873139,0.0678779339349912,-0.000127472379243391,0,0,0,0.111780100254674,7.03890301713851E-05,-1.55093016866869E-05,1.67387694867946E-07,3.32352621027575E-05,9.30705606418705E-07,7.08694473647387E-05,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL,,Rotation_Pitch_04c-2;STS3215_02a-4,Origin_Shoulder_Rotation,Axis_Shoulder_Rotation,Shoulder_Rotation,continuous,0,-0.0452,0.0181,1.5708,0,1.5708,Base,0,1,0,,,,,,,,,,,,
Upper_Arm,-0.0693113774468845,0.00293741346964818,-7.61279219025209E-07,0,0,0,0.167601391353176,7.75332201021328E-05,-2.10765620509824E-06,7.52685919931984E-07,0.000233751202018378,-1.63496162538793E-07,0.000180452754687364,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL,,SO_ARM100_06b-5;STS3215_02a-3,Origin_Shoulder_Pitch,Axis_Shoulder_Pitch,Shoulder_Pitch,continuous,0.000125,0.1086,0,3.1416,0,-1.5708,Shoulder_Rotation_Pitch,0,0,1,,,,,,,,,,,,
Lower_Arm,-0.0588290275819227,0.0021495318374051,0.000146772621039401,0,0,0,0.142523221917339,6.29078989235053E-05,3.79294618448135E-06,1.70733512134003E-06,0.000146811163948232,-2.1474403445678E-07,0.000102145070617562,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL,,SO_ARM100_06b-6;STS3215_02a-6,Origin_Elbow,Axis_Elbow,Elbow,continuous,-0.11238,0.0282,0,0,0,-2.2391,Upper_Arm,0,0,1,,,,,,,,,,,,
Wrist_Pitch_Roll,-6.28656116854598E-09,-0.0087849429576346,-0.0309177852835532,0,0,0,0.106401896179987,4.78947074364113E-05,-1.33871782943846E-11,-8.95740683864277E-12,7.01088408487287E-05,-5.49748507471695E-06,6.17958653539553E-05,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL,,Wrist_Roll_Pitch_04c-2;STS3215_02a-7,Origin_Wrist_Pitch,Axis_Wrist_Pitch,Wrist_Pitch,continuous,-0.1102,0.005375,0,0.90254,1.5708,0,Lower_Arm,1,0,0,,,,,,,,,,,,
Fixed_Gripper,-0.00772179942650385,-0.000555295978140996,0.0316941559340959,0,0,0,0.11710741874408,5.67526018031759E-05,1.04098982658207E-06,8.53596077253277E-06,5.78441834179299E-05,-2.86014969245207E-07,4.22399193495317E-05,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL,,Wrist_Roll_05g-1;STS3215_02a-8;Removable_Jaws_01e-1,Origin_Wrist_Roll,Axis_Wrist_Roll,Wrist_Roll,continuous,0,0.002,-0.0545,3.1416,0,3.1416,Wrist_Pitch_Roll,0,0,1,,,,,,,,,,,,
Moving_Jaw,-0.0033838985185846,-0.0322884362122416,0.000144458547748166,0,0,0,0.0347149174448153,1.36949844449711E-05,-5.63192124555278E-07,-5.74449907399212E-09,7.04089001130743E-06,-1.05361496046931E-07,8.28976960805291E-06,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL,,Moving_Jaw_04f-1;Removable_Jaws_01e-2,Origin_Gripper,Axis_Gripper,Gripper,continuous,0.0202,0,0.024375,-1.5708,0,0,Fixed_Gripper,0,0,1,,,,,,,,,,,,
1 Link Name Center of Mass X Center of Mass Y Center of Mass Z Center of Mass Roll Center of Mass Pitch Center of Mass Yaw Mass Moment Ixx Moment Ixy Moment Ixz Moment Iyy Moment Iyz Moment Izz Visual X Visual Y Visual Z Visual Roll Visual Pitch Visual Yaw Mesh Filename Color Red Color Green Color Blue Color Alpha Collision X Collision Y Collision Z Collision Roll Collision Pitch Collision Yaw Collision Mesh Filename Material Name SW Components Coordinate System Axis Name Joint Name Joint Type Joint Origin X Joint Origin Y Joint Origin Z Joint Origin Roll Joint Origin Pitch Joint Origin Yaw Parent Joint Axis X Joint Axis Y Joint Axis Z Limit Effort Limit Velocity Limit Lower Limit Upper Calibration rising Calibration falling Dynamics Damping Dynamics Friction Safety Soft Upper Safety Soft Lower Safety K Position Safety K Velocity
2 Base -1.55220299024015E-10 0.0275980388649202 0.0272094138963763 0 0 0 0.146962928243327 9.5191642834079E-05 2.02405274856147E-12 1.46514387606669E-13 0.000123785814019492 1.84608762035329E-05 0.000137926707148466 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL Base_03-1;STS3215_02a-5 Origin_global 0 0 0 0 0 0 0 0 0
3 Shoulder_Rotation_Pitch -0.00511938391873139 0.0678779339349912 -0.000127472379243391 0 0 0 0.111780100254674 7.03890301713851E-05 -1.55093016866869E-05 1.67387694867946E-07 3.32352621027575E-05 9.30705606418705E-07 7.08694473647387E-05 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL Rotation_Pitch_04c-2;STS3215_02a-4 Origin_Shoulder_Rotation Axis_Shoulder_Rotation Shoulder_Rotation continuous 0 -0.0452 0.0181 1.5708 0 1.5708 Base 0 1 0
4 Upper_Arm -0.0693113774468845 0.00293741346964818 -7.61279219025209E-07 0 0 0 0.167601391353176 7.75332201021328E-05 -2.10765620509824E-06 7.52685919931984E-07 0.000233751202018378 -1.63496162538793E-07 0.000180452754687364 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL SO_ARM100_06b-5;STS3215_02a-3 Origin_Shoulder_Pitch Axis_Shoulder_Pitch Shoulder_Pitch continuous 0.000125 0.1086 0 3.1416 0 -1.5708 Shoulder_Rotation_Pitch 0 0 1
5 Lower_Arm -0.0588290275819227 0.0021495318374051 0.000146772621039401 0 0 0 0.142523221917339 6.29078989235053E-05 3.79294618448135E-06 1.70733512134003E-06 0.000146811163948232 -2.1474403445678E-07 0.000102145070617562 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL SO_ARM100_06b-6;STS3215_02a-6 Origin_Elbow Axis_Elbow Elbow continuous -0.11238 0.0282 0 0 0 -2.2391 Upper_Arm 0 0 1
6 Wrist_Pitch_Roll -6.28656116854598E-09 -0.0087849429576346 -0.0309177852835532 0 0 0 0.106401896179987 4.78947074364113E-05 -1.33871782943846E-11 -8.95740683864277E-12 7.01088408487287E-05 -5.49748507471695E-06 6.17958653539553E-05 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL Wrist_Roll_Pitch_04c-2;STS3215_02a-7 Origin_Wrist_Pitch Axis_Wrist_Pitch Wrist_Pitch continuous -0.1102 0.005375 0 0.90254 1.5708 0 Lower_Arm 1 0 0
7 Fixed_Gripper -0.00772179942650385 -0.000555295978140996 0.0316941559340959 0 0 0 0.11710741874408 5.67526018031759E-05 1.04098982658207E-06 8.53596077253277E-06 5.78441834179299E-05 -2.86014969245207E-07 4.22399193495317E-05 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL Wrist_Roll_05g-1;STS3215_02a-8;Removable_Jaws_01e-1 Origin_Wrist_Roll Axis_Wrist_Roll Wrist_Roll continuous 0 0.002 -0.0545 3.1416 0 3.1416 Wrist_Pitch_Roll 0 0 1
8 Moving_Jaw -0.0033838985185846 -0.0322884362122416 0.000144458547748166 0 0 0 0.0347149174448153 1.36949844449711E-05 -5.63192124555278E-07 -5.74449907399212E-09 7.04089001130743E-06 -1.05361496046931E-07 8.28976960805291E-06 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL 0.792156862745098 0.819607843137255 0.933333333333333 1 0 0 0 0 0 0 package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL Moving_Jaw_04f-1;Removable_Jaws_01e-2 Origin_Gripper Axis_Gripper Gripper continuous 0.0202 0 0.024375 -1.5708 0 0 Fixed_Gripper 0 0 1

365
urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf Normal file
View File

@ -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_05d.SLDASM">
<link
name="Base">
<inertial>
<origin
xyz="-1.55220299024015E-10 0.0275980388649202 0.0272094138963763"
rpy="0 0 0" />
<mass
value="0.146962928243327" />
<inertia
ixx="9.5191642834079E-05"
ixy="2.02405274856147E-12"
ixz="1.46514387606669E-13"
iyy="0.000123785814019492"
iyz="1.84608762035329E-05"
izz="0.000137926707148466" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL" />
</geometry>
</collision>
</link>
<link
name="Shoulder_Rotation_Pitch">
<inertial>
<origin
xyz="-0.00511938391873139 0.0678779339349912 -0.000127472379243391"
rpy="0 0 0" />
<mass
value="0.111780100254674" />
<inertia
ixx="7.03890301713851E-05"
ixy="-1.55093016866869E-05"
ixz="1.67387694867946E-07"
iyy="3.32352621027575E-05"
iyz="9.30705606418705E-07"
izz="7.08694473647387E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL" />
</geometry>
</collision>
</link>
<joint
name="Shoulder_Rotation"
type="continuous">
<origin
xyz="0 -0.0452 0.0181"
rpy="1.5708 0 1.5708" />
<parent
link="Base" />
<child
link="Shoulder_Rotation_Pitch" />
<axis
xyz="0 1 0" />
</joint>
<link
name="Upper_Arm">
<inertial>
<origin
xyz="-0.0693113774468845 0.00293741346964818 -7.61279219025209E-07"
rpy="0 0 0" />
<mass
value="0.167601391353176" />
<inertia
ixx="7.75332201021328E-05"
ixy="-2.10765620509824E-06"
ixz="7.52685919931984E-07"
iyy="0.000233751202018378"
iyz="-1.63496162538793E-07"
izz="0.000180452754687364" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL" />
</geometry>
</collision>
</link>
<joint
name="Shoulder_Pitch"
type="continuous">
<origin
xyz="0.000125 0.1086 0"
rpy="3.1416 0 -1.5708" />
<parent
link="Shoulder_Rotation_Pitch" />
<child
link="Upper_Arm" />
<axis
xyz="0 0 1" />
</joint>
<link
name="Lower_Arm">
<inertial>
<origin
xyz="-0.0588290275819227 0.0021495318374051 0.000146772621039401"
rpy="0 0 0" />
<mass
value="0.142523221917339" />
<inertia
ixx="6.29078989235053E-05"
ixy="3.79294618448135E-06"
ixz="1.70733512134003E-06"
iyy="0.000146811163948232"
iyz="-2.1474403445678E-07"
izz="0.000102145070617562" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL" />
</geometry>
</collision>
</link>
<joint
name="Elbow"
type="continuous">
<origin
xyz="-0.11238 0.0282 0"
rpy="0 0 -2.2391" />
<parent
link="Upper_Arm" />
<child
link="Lower_Arm" />
<axis
xyz="0 0 1" />
</joint>
<link
name="Wrist_Pitch_Roll">
<inertial>
<origin
xyz="-6.28656116854598E-09 -0.0087849429576346 -0.0309177852835532"
rpy="0 0 0" />
<mass
value="0.106401896179987" />
<inertia
ixx="4.78947074364113E-05"
ixy="-1.33871782943846E-11"
ixz="-8.95740683864277E-12"
iyy="7.01088408487287E-05"
iyz="-5.49748507471695E-06"
izz="6.17958653539553E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL" />
</geometry>
</collision>
</link>
<joint
name="Wrist_Pitch"
type="continuous">
<origin
xyz="-0.1102 0.005375 0"
rpy="0.90254 1.5708 0" />
<parent
link="Lower_Arm" />
<child
link="Wrist_Pitch_Roll" />
<axis
xyz="1 0 0" />
</joint>
<link
name="Fixed_Gripper">
<inertial>
<origin
xyz="-0.00772179942650385 -0.000555295978140996 0.0316941559340959"
rpy="0 0 0" />
<mass
value="0.11710741874408" />
<inertia
ixx="5.67526018031759E-05"
ixy="1.04098982658207E-06"
ixz="8.53596077253277E-06"
iyy="5.78441834179299E-05"
iyz="-2.86014969245207E-07"
izz="4.22399193495317E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL" />
</geometry>
</collision>
</link>
<joint
name="Wrist_Roll"
type="continuous">
<origin
xyz="0 0.002 -0.0545"
rpy="3.1416 0 3.1416" />
<parent
link="Wrist_Pitch_Roll" />
<child
link="Fixed_Gripper" />
<axis
xyz="0 0 1" />
</joint>
<link
name="Moving_Jaw">
<inertial>
<origin
xyz="-0.0033838985185846 -0.0322884362122416 0.000144458547748166"
rpy="0 0 0" />
<mass
value="0.0347149174448153" />
<inertia
ixx="1.36949844449711E-05"
ixy="-5.63192124555278E-07"
ixz="-5.74449907399212E-09"
iyy="7.04089001130743E-06"
iyz="-1.05361496046931E-07"
izz="8.28976960805291E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL" />
</geometry>
</collision>
</link>
<joint
name="Gripper"
type="continuous">
<origin
xyz="0.0202 0 0.024375"
rpy="-1.5708 0 0" />
<parent
link="Fixed_Gripper" />
<child
link="Moving_Jaw" />
<axis
xyz="0 0 1" />
</joint>
</robot>

193
urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM_fixed.urdf Normal file
View File

@ -0,0 +1,193 @@
<?xml version="1.0" ?>
<robot name="SO_5DOF_ARM100_05d.SLDASM">
<link name="Base">
<inertial>
<origin xyz="-1.55220299024015E-10 0.0275980388649202 0.0272094138963763" rpy="0 0 0"/>
<mass value="0.146962928243327"/>
<inertia ixx="9.5191642834079E-05" ixy="2.02405274856147E-12" ixz="1.46514387606669E-13" iyy="0.000123785814019492" iyz="1.84608762035329E-05" izz="0.000137926707148466"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL"/>
</geometry>
</collision>
</link>
<link name="Shoulder_Rotation_Pitch">
<inertial>
<origin xyz="-0.00511938391873139 0.0678779339349912 -0.000127472379243391" rpy="0 0 0"/>
<mass value="0.111780100254674"/>
<inertia ixx="7.03890301713851E-05" ixy="-1.55093016866869E-05" ixz="1.67387694867946E-07" iyy="3.32352621027575E-05" iyz="9.30705606418705E-07" izz="7.08694473647387E-05"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL"/>
</geometry>
</collision>
</link>
<joint name="Shoulder_Rotation" type="continuous">
<origin xyz="0 -0.0452 0.0181" rpy="1.5708 0 1.5708"/>
<parent link="Base"/>
<child link="Shoulder_Rotation_Pitch"/>
<axis xyz="0 1 0"/>
</joint>
<link name="Upper_Arm">
<inertial>
<origin xyz="-0.0693113774468845 0.00293741346964818 -7.61279219025209E-07" rpy="0 0 0"/>
<mass value="0.167601391353176"/>
<inertia ixx="7.75332201021328E-05" ixy="-2.10765620509824E-06" ixz="7.52685919931984E-07" iyy="0.000233751202018378" iyz="-1.63496162538793E-07" izz="0.000180452754687364"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL"/>
</geometry>
</collision>
</link>
<joint name="Shoulder_Pitch" type="continuous">
<origin xyz="0.000125 0.1086 0" rpy="3.1416 0 -1.5708"/>
<parent link="Shoulder_Rotation_Pitch"/>
<child link="Upper_Arm"/>
<axis xyz="0 0 1"/>
</joint>
<link name="Lower_Arm">
<inertial>
<origin xyz="-0.0588290275819227 0.0021495318374051 0.000146772621039401" rpy="0 0 0"/>
<mass value="0.142523221917339"/>
<inertia ixx="6.29078989235053E-05" ixy="3.79294618448135E-06" ixz="1.70733512134003E-06" iyy="0.000146811163948232" iyz="-2.1474403445678E-07" izz="0.000102145070617562"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL"/>
</geometry>
</collision>
</link>
<joint name="Elbow" type="continuous">
<origin xyz="-0.11238 0.0282 0" rpy="0 0 -2.2391"/>
<parent link="Upper_Arm"/>
<child link="Lower_Arm"/>
<axis xyz="0 0 1"/>
</joint>
<link name="Wrist_Pitch_Roll">
<inertial>
<origin xyz="-6.28656116854598E-09 -0.0087849429576346 -0.0309177852835532" rpy="0 0 0"/>
<mass value="0.106401896179987"/>
<inertia ixx="4.78947074364113E-05" ixy="-1.33871782943846E-11" ixz="-8.95740683864277E-12" iyy="7.01088408487287E-05" iyz="-5.49748507471695E-06" izz="6.17958653539553E-05"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL"/>
</geometry>
</collision>
</link>
<joint name="Wrist_Pitch" type="continuous">
<origin xyz="-0.1102 0.005375 0" rpy="0.90254 1.5708 0"/>
<parent link="Lower_Arm"/>
<child link="Wrist_Pitch_Roll"/>
<axis xyz="1 0 0"/>
</joint>
<link name="Fixed_Gripper">
<inertial>
<origin xyz="-0.00772179942650385 -0.000555295978140996 0.0316941559340959" rpy="0 0 0"/>
<mass value="0.11710741874408"/>
<inertia ixx="5.67526018031759E-05" ixy="1.04098982658207E-06" ixz="8.53596077253277E-06" iyy="5.78441834179299E-05" iyz="-2.86014969245207E-07" izz="4.22399193495317E-05"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL"/>
</geometry>
</collision>
</link>
<joint name="Wrist_Roll" type="continuous">
<origin xyz="0 0.002 -0.0545" rpy="3.1416 0 3.1416"/>
<parent link="Wrist_Pitch_Roll"/>
<child link="Fixed_Gripper"/>
<axis xyz="0 0 1"/>
</joint>
<link name="Moving_Jaw">
<inertial>
<origin xyz="-0.0033838985185846 -0.0322884362122416 0.000144458547748166" rpy="0 0 0"/>
<mass value="0.0347149174448153"/>
<inertia ixx="1.36949844449711E-05" ixy="-5.63192124555278E-07" ixz="-5.74449907399212E-09" iyy="7.04089001130743E-06" iyz="-1.05361496046931E-07" izz="8.28976960805291E-06"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/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="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL"/>
</geometry>
</collision>
</link>
<joint name="Gripper" type="continuous">
<origin xyz="0.0202 0 0.024375" rpy="-1.5708 0 0"/>
<parent link="Fixed_Gripper"/>
<child link="Moving_Jaw"/>
<axis xyz="0 0 1"/>
</joint>
</robot>