Compare commits
| Author | SHA1 | Date | |
|---|---|---|---|
 ZhongyuWang-cart
|
cce375e4a5 |
@@ -1,7 +1,5 @@
|
||||
### This repo is for inverse kinematics and verification
|
||||
|
||||
In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
|
||||
|
||||
Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
|
||||
|
||||
Verification is done with Mujoco simulation.
|
||||
@@ -26,6 +24,9 @@ lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
|
||||
the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
|
||||
At least one solver works out the ik, rate = **74%**.
|
||||
|
||||
|
||||
|
||||
|
||||
- With realman-75 physical joint limit,
|
||||
```
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
|
||||
@@ -34,18 +35,7 @@ lb = -ub
|
||||
the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
|
||||
At least one solver works out the ik, rate = **84%**.
|
||||
|
||||
### update(1st July 2026)
|
||||
|
||||
In each iteration, update optimization formula:
|
||||
|
||||
- new cost item for distance from middle of the joint range.
|
||||
- set up different weight for different joints motion.
|
||||
|
||||
<img src="img/optimization.png" alt="Cost" width="400">
|
||||
|
||||
<img src="img/cons.png" alt="Cost" width="400">
|
||||
|
||||
<img src="img/osqp.png" alt="Cost" width="400">
|
||||
|
||||
Visualization:
|
||||
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 25 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 44 KiB |
@@ -1,6 +0,0 @@
|
||||
#!/bin/bash
|
||||
echo "Fixing robotics environment..."
|
||||
conda activate coppeliasim
|
||||
export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
|
||||
pip install osqp==0.6.2.post8 --force-reinstall
|
||||
python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"
|
||||
+21
-56
@@ -8,65 +8,22 @@ from rm75_kine_rm import rm75_kine_api as kine_rm
|
||||
from rm75_mjc import MuJoCoPositionController
|
||||
from Robotic_Arm.rm_robot_interface import *
|
||||
|
||||
import os
|
||||
cwd = os.getcwd()
|
||||
|
||||
|
||||
import time
|
||||
from math import radians, degrees, pi, cos, sin
|
||||
import numpy as np
|
||||
|
||||
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
|
||||
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
|
||||
tools_in_ee = {
|
||||
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
}
|
||||
|
||||
# joint limit
|
||||
# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
|
||||
# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
|
||||
|
||||
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||
lb = -ub
|
||||
|
||||
tool_name = "scissor"
|
||||
|
||||
def main():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
# Create controller
|
||||
robot_mjk = MuJoCoPositionController(urdf_path="./urdf_rm75/RM75-SCI.urdf")
|
||||
|
||||
robot_mjk = MuJoCoPositionController()
|
||||
|
||||
tool_name = "scissor"
|
||||
# ----------- rm75 qp based kine ------------
|
||||
robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75', tcps=["scissor_tcp", "camera_tcp"])
|
||||
robot_kine_qp.add_tool_frames(tools_in_ee)
|
||||
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
fp = robot_kine_qp.forward_kinematics(np.ones(7)*0.3,tool='scissor_tcp')
|
||||
print(f'forward kine res = {fp}')
|
||||
ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=fp[0:3], target_rpy=fp[3:6], initial_guess=np.zeros(7),
|
||||
tool='scissor_tcp')
|
||||
|
||||
robot_kine_qp = kine_qp()
|
||||
|
||||
# ---------- rm75 official algorithm -----------
|
||||
robot_kine_rm = kine_rm()
|
||||
robot_kine_rm.add_tool_frames(tools_in_ee)
|
||||
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool="no_tool")
|
||||
|
||||
print(f'ret_rm = {ret_rm}, q = {q}')
|
||||
pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
|
||||
print(f'pose = {pose}')
|
||||
print('-'*100)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -75,6 +32,16 @@ def main():
|
||||
|
||||
if True:
|
||||
|
||||
# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
|
||||
# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
|
||||
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||
lb = -ub
|
||||
|
||||
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
result = np.array([[0,0],[0,0]], dtype=np.int32) # to collect ik result qp_fk, qp_ik, rm_fk, rm_ik
|
||||
|
||||
solve_sum = 0
|
||||
@@ -103,35 +70,33 @@ def main():
|
||||
if ret_qp == 0:
|
||||
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
|
||||
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
|
||||
print(f'---- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
|
||||
robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
|
||||
print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
|
||||
if d_p_ik < 0.01:
|
||||
result[0][1] += 1
|
||||
|
||||
# robot_mjk.send_command(q)
|
||||
# robot_mjk.wait_until_reached()
|
||||
# robot_mjk.print_state()
|
||||
robot_mjk.send_command(q)
|
||||
robot_mjk.wait_until_reached()
|
||||
robot_mjk.print_state()
|
||||
else:
|
||||
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
|
||||
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
|
||||
print(f'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
|
||||
print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
|
||||
|
||||
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
|
||||
if ret_rm == 0:
|
||||
fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
|
||||
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
|
||||
print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
|
||||
print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
|
||||
if d_p_ik < 0.01:
|
||||
result[1][1] += 1
|
||||
else:
|
||||
print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
|
||||
print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
|
||||
|
||||
if ret_qp == 0 or ret_rm == 0:
|
||||
solve_sum += 1
|
||||
|
||||
print(f'results with qp and rm for ik are {result}')
|
||||
print(f'result is {result}')
|
||||
print(f'solve_sum is {solve_sum}')
|
||||
robot_mjk.stop()
|
||||
|
||||
|
||||
def cal_pose_deviation(pose1, pose2):
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
conda install -c conda-forge osqp scipy tqdm matplotlib pandas "numpy<1.24" pinocchio -y
|
||||
pip install urdfpy mujoco "networkx>=2.8.4"
|
||||
pip install Robotic_Arm
|
||||
@@ -1,9 +0,0 @@
|
||||
numpy
|
||||
pandas
|
||||
matplotlib
|
||||
tqdm
|
||||
scipy
|
||||
urdfpy
|
||||
pin
|
||||
osqp
|
||||
Robotic_Arm
|
||||
+473
-644
File diff suppressed because it is too large
Load Diff
+15
-97
@@ -7,18 +7,23 @@ class rm75_kine_api():
|
||||
def __init__(self):
|
||||
# ---------- rm75 official algorithm -----------
|
||||
print(f'------- the realman official kinematic initialising -------')
|
||||
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm
|
||||
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm
|
||||
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
|
||||
# Initialize the robotic arm model and sensor type in the algorithm
|
||||
self.robot_kine_rm = Algo(arm_model, force_type)
|
||||
|
||||
self.cfg_j_limit()
|
||||
|
||||
self.tool_frames = {
|
||||
'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
|
||||
'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.144, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
|
||||
'camera': rm_frame_t(frame_name="camera", pose=(0.05, 0.02, 0.10, -1.57, 0, -1.57), payload=1, x=0, y=0, z=72)
|
||||
}
|
||||
self.work_frames = {
|
||||
'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
|
||||
'work': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
|
||||
}
|
||||
|
||||
self.tool_name = "no_tool"
|
||||
self.tool_name = "ee"
|
||||
self.work_name = "work"
|
||||
|
||||
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
|
||||
@@ -27,8 +32,6 @@ class rm75_kine_api():
|
||||
if min_j is None:
|
||||
min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
|
||||
|
||||
max_j = np.array(max_j)
|
||||
min_j = np.array(min_j)
|
||||
if rad_flag:
|
||||
self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
|
||||
self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
|
||||
@@ -48,46 +51,7 @@ class rm75_kine_api():
|
||||
def get_tool_frame(self):
|
||||
return self.robot_kine_rm.rm_algo_get_curr_toolframe()
|
||||
|
||||
def quaternion_to_euler(self, q):
|
||||
"""
|
||||
Convert quaternion to Euler angles (roll, pitch, yaw)
|
||||
|
||||
Args:
|
||||
qx, qy, qz, qw: quaternion components
|
||||
|
||||
Returns:
|
||||
tuple: (roll, pitch, yaw) in radians
|
||||
"""
|
||||
# Roll (x-axis rotation)
|
||||
sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
|
||||
cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
|
||||
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# Pitch (y-axis rotation)
|
||||
sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
|
||||
if abs(sinp) >= 1:
|
||||
pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range
|
||||
else:
|
||||
pitch = np.arcsin(sinp)
|
||||
|
||||
# Yaw (z-axis rotation)
|
||||
siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
|
||||
cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
|
||||
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return [roll, pitch, yaw]
|
||||
|
||||
def add_tool_frames(self, dict_frames):
|
||||
self.tool_frames = {}
|
||||
for tool_name in dict_frames:
|
||||
tool_attr = dict_frames[tool_name]
|
||||
position = tool_attr[0][0:3]
|
||||
rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
|
||||
f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
|
||||
|
||||
self.tool_frames.update({tool_name:f})
|
||||
|
||||
def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", work="work"):
|
||||
def forward_kinematics(self, joint_angles, flag = 1 , tool="ee", work="work"):
|
||||
'''
|
||||
:param joint_angles: list of joint values, in rad
|
||||
:param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
|
||||
@@ -102,7 +66,7 @@ class rm75_kine_api():
|
||||
|
||||
return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles] , flag=flag)
|
||||
|
||||
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work", step_arm_angle = 15.0):
|
||||
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"):
|
||||
'''
|
||||
:param target_position: list of position values, m
|
||||
:param target_rpy: list of rpy values, rad
|
||||
@@ -120,60 +84,14 @@ class rm75_kine_api():
|
||||
self.work_name = work
|
||||
self.cfg_work_frame(work)
|
||||
|
||||
target = list(target_position) + list(target_rpy)
|
||||
target = target_position + target_rpy
|
||||
|
||||
if initial_guess is not None:
|
||||
q_ref = [ 180/math.pi * ig for ig in initial_guess ]
|
||||
else:
|
||||
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
|
||||
ret, phi0 = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||
params = rm_inverse_kinematics_params_t(q_ref, target, 1)
|
||||
|
||||
offsets = [0.0]
|
||||
arm_angle = step_arm_angle
|
||||
while arm_angle <= 180.0:
|
||||
offsets += [arm_angle, -arm_angle]
|
||||
arm_angle += step_arm_angle
|
||||
|
||||
best_ret, best_q_out, best_dis = -1, None, None
|
||||
for offset in offsets:
|
||||
phi = ((phi0 + offset + 180.0) % 360.0) - 180.0
|
||||
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
if int(ret) != 0:
|
||||
if best_q_out is None:
|
||||
best_ret, best_q_out = ret, q_out
|
||||
continue
|
||||
|
||||
p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
|
||||
pose_dis = cal_pose_deviation(p_fk, target)
|
||||
if pose_dis < 0.01:
|
||||
# success in ik calculation
|
||||
return ret, [q / 180 * math.pi for q in q_out]
|
||||
|
||||
if best_dis is None or pose_dis < best_dis:
|
||||
best_ret, best_q_out, best_dis = -10, q_out, pose_dis
|
||||
|
||||
ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||
params = rm_inverse_kinematics_params_t(q_ref,
|
||||
target, 1)
|
||||
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
|
||||
pose_dis = cal_pose_deviation(pose_fk, target)
|
||||
|
||||
# print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
|
||||
#
|
||||
# print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
|
||||
# print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
|
||||
if int(ret) < 0:
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
elif pose_dis < 0.01:
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
else:
|
||||
return -10, [ q/180*math.pi for q in q_out]
|
||||
|
||||
def cal_pose_deviation(pose1, pose2):
|
||||
d_fk_p1 = np.array(pose1) - np.array(pose2)
|
||||
for j in [3, 4, 5]:
|
||||
while d_fk_p1[j] > math.pi:
|
||||
d_fk_p1[j] -= 2 * math.pi
|
||||
while d_fk_p1[j] < -math.pi:
|
||||
d_fk_p1[j] += 2 * math.pi
|
||||
d_fk = np.linalg.norm(d_fk_p1)
|
||||
return d_fk
|
||||
return ret, [ q/180*math.pi for q in q_out]
|
||||
@@ -1,555 +0,0 @@
|
||||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<robot name="rm75_dual_arm">
|
||||
<!--Shared supporting base. Adjust the two mount joint origins to match the CAD mounting frames.-->
|
||||
<link name="dual_arm_base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="dual_arm_base_material">
|
||||
<color rgba="0.5 0.5 0.5 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="omnipic_base_link">
|
||||
<inertial>
|
||||
<origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
|
||||
<mass value="1.862" />
|
||||
<inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="omnipic_link_1">
|
||||
<inertial>
|
||||
<origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
|
||||
<mass value="1.574" />
|
||||
<inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_1" type="revolute">
|
||||
<origin xyz="0 0 0.2405" rpy="0 0 0" />
|
||||
<parent link="omnipic_base_link" />
|
||||
<child link="omnipic_link_1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_2">
|
||||
<inertial>
|
||||
<origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
|
||||
<mass value="1.217" />
|
||||
<inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_2" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="omnipic_link_1" />
|
||||
<child link="omnipic_link_2" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_3">
|
||||
<inertial>
|
||||
<origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
|
||||
<mass value="1.11" />
|
||||
<inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_3" type="revolute">
|
||||
<origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
|
||||
<parent link="omnipic_link_2" />
|
||||
<child link="omnipic_link_3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_4">
|
||||
<inertial>
|
||||
<origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
|
||||
<mass value="0.685" />
|
||||
<inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_4" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="omnipic_link_3" />
|
||||
<child link="omnipic_link_4" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_5">
|
||||
<inertial>
|
||||
<origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
|
||||
<mass value="0.619" />
|
||||
<inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_5" type="revolute">
|
||||
<origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
|
||||
<parent link="omnipic_link_4" />
|
||||
<child link="omnipic_link_5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_6">
|
||||
<inertial>
|
||||
<origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
|
||||
<mass value="0.602" />
|
||||
<inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_6" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="omnipic_link_5" />
|
||||
<child link="omnipic_link_6" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_7">
|
||||
<inertial>
|
||||
<origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
|
||||
<mass value="0.107" />
|
||||
<inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_7" type="revolute">
|
||||
<origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
|
||||
<parent link="omnipic_link_6" />
|
||||
<child link="omnipic_link_7" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_gripper_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="omnipic_OmniPic_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_OmniPic_fixed_joint" type="fixed">
|
||||
<parent link="omnipic_link_7" />
|
||||
<child link="omnipic_gripper_link" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="omnipic_OmniPic_tcp" />
|
||||
<joint name="omnipic_OmniPic_tcp_fixed" type="fixed">
|
||||
<parent link="omnipic_gripper_link" />
|
||||
<child link="omnipic_OmniPic_tcp" />
|
||||
<origin xyz="0 0 0.14" rpy="0 0 0" />
|
||||
</joint>
|
||||
<!--Omnipic arm mount: edit xyz/rpy to match dual_arm_base.stl.-->
|
||||
<joint name="omnipic_base_mount_joint" type="fixed">
|
||||
<parent link="dual_arm_base_link" />
|
||||
<child link="omnipic_base_link" />
|
||||
<origin xyz="0.03 0 0" rpy="0 -1.57 3.141593" />
|
||||
</joint>
|
||||
<link name="scissor_base_link">
|
||||
<inertial>
|
||||
<origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
|
||||
<mass value="1.862" />
|
||||
<inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="scissor_link_1">
|
||||
<inertial>
|
||||
<origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
|
||||
<mass value="1.574" />
|
||||
<inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_1" type="revolute">
|
||||
<origin xyz="0 0 0.2405" rpy="0 0 0" />
|
||||
<parent link="scissor_base_link" />
|
||||
<child link="scissor_link_1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_2">
|
||||
<inertial>
|
||||
<origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
|
||||
<mass value="1.217" />
|
||||
<inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_2" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="scissor_link_1" />
|
||||
<child link="scissor_link_2" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_3">
|
||||
<inertial>
|
||||
<origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
|
||||
<mass value="1.11" />
|
||||
<inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_3" type="revolute">
|
||||
<origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
|
||||
<parent link="scissor_link_2" />
|
||||
<child link="scissor_link_3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_4">
|
||||
<inertial>
|
||||
<origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
|
||||
<mass value="0.685" />
|
||||
<inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_4" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="scissor_link_3" />
|
||||
<child link="scissor_link_4" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_5">
|
||||
<inertial>
|
||||
<origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
|
||||
<mass value="0.619" />
|
||||
<inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_5" type="revolute">
|
||||
<origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
|
||||
<parent link="scissor_link_4" />
|
||||
<child link="scissor_link_5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_6">
|
||||
<inertial>
|
||||
<origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
|
||||
<mass value="0.602" />
|
||||
<inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_6" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="scissor_link_5" />
|
||||
<child link="scissor_link_6" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_7">
|
||||
<inertial>
|
||||
<origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
|
||||
<mass value="0.107" />
|
||||
<inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_7" type="revolute">
|
||||
<origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
|
||||
<parent link="scissor_link_6" />
|
||||
<child link="scissor_link_7" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_scissor_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/scissor.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/scissor.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_scissor_fixed_joint" type="fixed">
|
||||
<parent link="scissor_link_7" />
|
||||
<child link="scissor_scissor_link" />
|
||||
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="scissor_scissor_tcp" />
|
||||
<joint name="scissor_scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_scissor_link" />
|
||||
<child link="scissor_scissor_tcp" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="scissor_camera_tcp" />
|
||||
<joint name="scissor_camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_scissor_link" />
|
||||
<child link="scissor_camera_tcp" />
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57" />
|
||||
</joint>
|
||||
<!--Scissor arm mount: edit xyz/rpy to match dual_arm_base.stl.-->
|
||||
<joint name="scissor_base_mount_joint" type="fixed">
|
||||
<parent link="dual_arm_base_link" />
|
||||
<child link="scissor_base_link" />
|
||||
<origin xyz="-0.030 0 0" rpy="0 1.57 3.141593" />
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -1,507 +0,0 @@
|
||||
<?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="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="gripper_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/OmniPic.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="OmniPic_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/OmniPic.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the gripper to the final wrist link -->
|
||||
<joint name="OmniPic_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="gripper_link" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="OmniPic_tcp"/>
|
||||
|
||||
<joint name="OmniPic_tcp_fixed" type="fixed">
|
||||
<parent link="gripper_link"/>
|
||||
<child link="OmniPic_tcp"/>
|
||||
<origin xyz="0 0 0.14" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -1,516 +0,0 @@
|
||||
<?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="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 -0.12 0.03" rpy="1.5707963 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.5707963"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -1,516 +0,0 @@
|
||||
<?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="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="-0.12 0 0.063" rpy="0 -1.5707963 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -1,516 +0,0 @@
|
||||
<?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="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -1,516 +0,0 @@
|
||||
<?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="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/mini_scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/mini_scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.08 0 -0.135" rpy="0 0 1.57"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,106 +0,0 @@
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 1. Load the data
|
||||
# --------------------------------------------------
|
||||
file_name = "rm75b_comfort_workspace_v_minis_collision.csv"
|
||||
csv_path = Path(file_name)
|
||||
|
||||
# The file has no column names, so header=None is important.
|
||||
df_csv = pd.read_csv(
|
||||
csv_path,
|
||||
)
|
||||
rate_res = df_csv.iloc[:, :4]
|
||||
try:
|
||||
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
|
||||
except:
|
||||
rate_res_sort = rate_res
|
||||
|
||||
DECIMALS = 4
|
||||
|
||||
try:
|
||||
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
|
||||
except:
|
||||
x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
|
||||
|
||||
nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
|
||||
|
||||
|
||||
ik_rates = rate_res_sort.to_numpy()
|
||||
|
||||
# --------------------------------------------------
|
||||
# 2. Create an output directory
|
||||
# --------------------------------------------------
|
||||
output_dir = Path(file_name.split(".")[0])
|
||||
output_dir.mkdir(exist_ok=True)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 3. Use the same colour scale for every z-plane
|
||||
# --------------------------------------------------
|
||||
df = rate_res_sort
|
||||
value_min = df["ik_success_rate"].min()
|
||||
value_max = df["ik_success_rate"].max()
|
||||
|
||||
# More levels give a smoother-looking contour plot.
|
||||
levels = np.linspace(value_min, value_max, 51)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 4. Draw one contour plot for each z-plane
|
||||
# --------------------------------------------------
|
||||
for z_value, plane in df.groupby("z", sort=True):
|
||||
|
||||
# Rows become y-coordinates, columns become x-coordinates.
|
||||
grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
|
||||
|
||||
x = grid.columns.to_numpy()
|
||||
y = grid.index.to_numpy()
|
||||
ik_grid = grid.to_numpy()
|
||||
|
||||
X, Y = np.meshgrid(x, y)
|
||||
|
||||
fig, ax = plt.subplots(figsize=(7, 6))
|
||||
|
||||
contour = ax.contourf(
|
||||
X,
|
||||
Y,
|
||||
ik_grid,
|
||||
levels=levels,
|
||||
cmap="viridis",
|
||||
extend="both",
|
||||
)
|
||||
|
||||
highlight_levels = [0.6, 0.7]
|
||||
# Only plot if the levels are within the data range (optional)
|
||||
if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
|
||||
lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
|
||||
colors='red', linewidths=2, linestyles='solid')
|
||||
# Optionally label the lines
|
||||
ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
|
||||
|
||||
|
||||
colorbar = fig.colorbar(contour, ax=ax)
|
||||
colorbar.set_label("IK rate")
|
||||
|
||||
ax.set_title(f"IK rate at z = {z_value:.2f}")
|
||||
ax.set_xlabel("x")
|
||||
ax.set_ylabel("y")
|
||||
ax.set_aspect("equal")
|
||||
|
||||
fig.tight_layout()
|
||||
|
||||
output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
|
||||
fig.savefig(output_path, dpi=200, bbox_inches="tight")
|
||||
plt.close(fig)
|
||||
|
||||
print(f"Plots saved to: {output_dir.resolve()}")
|
||||
@@ -1,109 +0,0 @@
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 1. Load the data
|
||||
# --------------------------------------------------
|
||||
file_name = "workspace minisci collision.csv"
|
||||
csv_path = Path(file_name)
|
||||
|
||||
# The file has no column names, so header=None is important.
|
||||
df_csv = pd.read_csv(
|
||||
csv_path,
|
||||
header=None, # the file has no header row
|
||||
names=['x', 'y', 'z', 'ik_success_rate'] # assign names
|
||||
)
|
||||
|
||||
rate_res = df_csv.iloc[:, :4]
|
||||
try:
|
||||
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
|
||||
except:
|
||||
rate_res_sort = rate_res
|
||||
|
||||
DECIMALS = 4
|
||||
|
||||
try:
|
||||
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
|
||||
except:
|
||||
x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
|
||||
y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
|
||||
z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
|
||||
|
||||
nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
|
||||
|
||||
|
||||
ik_rates = rate_res_sort.to_numpy()
|
||||
|
||||
# --------------------------------------------------
|
||||
# 2. Create an output directory
|
||||
# --------------------------------------------------
|
||||
output_dir = Path(file_name.split(".")[0])
|
||||
output_dir.mkdir(exist_ok=True)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 3. Use the same colour scale for every z-plane
|
||||
# --------------------------------------------------
|
||||
df = rate_res_sort
|
||||
value_min = df["ik_success_rate"].min()
|
||||
value_max = df["ik_success_rate"].max()
|
||||
|
||||
# More levels give a smoother-looking contour plot.
|
||||
levels = np.linspace(value_min, value_max, 51)
|
||||
|
||||
|
||||
# --------------------------------------------------
|
||||
# 4. Draw one contour plot for each z-plane
|
||||
# --------------------------------------------------
|
||||
for z_value, plane in df.groupby("z", sort=True):
|
||||
|
||||
# Rows become y-coordinates, columns become x-coordinates.
|
||||
grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
|
||||
|
||||
x = grid.columns.to_numpy()
|
||||
y = grid.index.to_numpy()
|
||||
ik_grid = grid.to_numpy()
|
||||
|
||||
X, Y = np.meshgrid(x, y)
|
||||
|
||||
fig, ax = plt.subplots(figsize=(7, 6))
|
||||
|
||||
contour = ax.contourf(
|
||||
X,
|
||||
Y,
|
||||
ik_grid,
|
||||
levels=levels,
|
||||
cmap="viridis",
|
||||
extend="both",
|
||||
)
|
||||
|
||||
highlight_levels = [0.6, 0.7]
|
||||
# Only plot if the levels are within the data range (optional)
|
||||
if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
|
||||
lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
|
||||
colors='red', linewidths=2, linestyles='solid')
|
||||
# Optionally label the lines
|
||||
ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
|
||||
|
||||
|
||||
colorbar = fig.colorbar(contour, ax=ax)
|
||||
colorbar.set_label("IK rate")
|
||||
|
||||
ax.set_title(f"IK rate at z = {z_value:.2f}")
|
||||
ax.set_xlabel("x")
|
||||
ax.set_ylabel("y")
|
||||
ax.set_aspect("equal")
|
||||
|
||||
fig.tight_layout()
|
||||
|
||||
output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
|
||||
fig.savefig(output_path, dpi=200, bbox_inches="tight")
|
||||
plt.close(fig)
|
||||
|
||||
print(f"Plots saved to: {output_dir.resolve()}")
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 246 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 442 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 120 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 123 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 128 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 133 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 134 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 136 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 129 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 123 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 119 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 115 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 109 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 101 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 90 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 77 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 67 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 57 KiB |
-10626
File diff suppressed because it is too large
Load Diff
-10625
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,726 +0,0 @@
|
||||
"""
|
||||
RM75-B comfortable workspace evaluator.
|
||||
|
||||
You provide:
|
||||
- URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
|
||||
- your own IK solver inside solve_ik()
|
||||
|
||||
This script computes:
|
||||
- IK success rate
|
||||
- joint-limit comfort
|
||||
- manipulability
|
||||
- singularity / condition number score
|
||||
- final comfort score
|
||||
|
||||
Recommended install:
|
||||
pip install numpy scipy urdfpy pandas matplotlib tqdm
|
||||
|
||||
Optional:
|
||||
pip install plotly
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from tqdm import tqdm
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from urdfpy import URDF
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
# 1. Get the absolute path of the directory containing this current script
|
||||
current_dir = Path(__file__).resolve().parent
|
||||
|
||||
# 2. Get the parent (upper) directory
|
||||
parent_dir = current_dir.parent
|
||||
|
||||
# 3. Add the parent directory to the system path
|
||||
sys.path.insert(0, str(parent_dir))
|
||||
|
||||
from rm75_kine_qp import KinematicsSolver as kine_qp
|
||||
from rm75_kine_rm import rm75_kine_api as kine_rm
|
||||
from rm75_mjc import MuJoCoPositionController
|
||||
from Robotic_Arm.rm_robot_interface import *
|
||||
|
||||
import time
|
||||
from math import radians, degrees, pi, cos, sin
|
||||
|
||||
# Cartesian workspace grid, in meters.
|
||||
# Adjust according to your robot placement and task.
|
||||
X_RANGE = (-0.7, 0.7)
|
||||
Y_RANGE = (-0.7, 0.7)
|
||||
Z_RANGE = (-0.10, 0.8)
|
||||
|
||||
GRID_RESOLUTION = 0.05 # 5 cm. Use 0.02 for finer but slower.
|
||||
|
||||
num_orientations = 120
|
||||
|
||||
tool_name = "scissor"
|
||||
|
||||
|
||||
|
||||
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-SCI.urdf'
|
||||
|
||||
output_csv = "workspace" + tool_name + URDF_PATH.split('/')[-1].split('.')[0] + ".csv"
|
||||
|
||||
|
||||
# Comfort thresholds
|
||||
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
|
||||
MAX_CONDITION_NUMBER = 150.0
|
||||
MIN_MANIPULABILITY_RATIO = 0.10
|
||||
|
||||
# Scoring weights
|
||||
WEIGHT_IK_SUCCESS = 0.70
|
||||
WEIGHT_JOINT_LIMIT = 0.10
|
||||
WEIGHT_MANIPULABILITY = 0.1
|
||||
WEIGHT_SINGULARITY = 0.1
|
||||
|
||||
|
||||
|
||||
|
||||
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
|
||||
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
|
||||
tools_in_ee = {
|
||||
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'v_minis': np.array([[0.0, 0.1, 0.1, -np.sqrt(2) * 0.5, 0.0, 0.0, np.sqrt(2) * 0.5],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
}
|
||||
|
||||
# joint limit
|
||||
# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
|
||||
# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
|
||||
#
|
||||
#
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||
lb = -ub
|
||||
|
||||
|
||||
MESH_DIR = str(Path(URDF_PATH).parent)
|
||||
|
||||
# ----------- rm75 qp based kine ------------
|
||||
robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR, tcps=["scissor_tcp", "camera_tcp"])
|
||||
robot_kine_qp.add_tool_frames(tools_in_ee)
|
||||
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
# ---------- rm75 official algorithm -----------
|
||||
robot_kine_rm = kine_rm()
|
||||
robot_kine_rm.add_tool_frames(tools_in_ee)
|
||||
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
|
||||
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 1. USER SETTINGS
|
||||
# ============================================================
|
||||
|
||||
|
||||
|
||||
BASE_LINK = "base_link"
|
||||
TCP_LINK = "link_7"
|
||||
|
||||
JOINT_NAMES = [
|
||||
"joint_1",
|
||||
"joint_2",
|
||||
"joint_3",
|
||||
"joint_4",
|
||||
"joint_5",
|
||||
"joint_6",
|
||||
"joint_7",
|
||||
]
|
||||
|
||||
|
||||
|
||||
# Numerical Jacobian settings
|
||||
JACOBIAN_EPS = 1e-5
|
||||
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 2. TASK ORIENTATION SAMPLING
|
||||
# ============================================================
|
||||
|
||||
def make_task_orientations(num_orientations=num_orientations, seed=1):
|
||||
"""
|
||||
Random orientation sampling using RM's Euler convention:
|
||||
|
||||
R = Rz @ Ry @ Rx
|
||||
|
||||
Note:
|
||||
This samples Euler angles randomly.
|
||||
It is useful, but not perfectly uniform over SO(3).
|
||||
"""
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
orientations = []
|
||||
|
||||
for _ in range(num_orientations):
|
||||
rx = rng.uniform(-np.pi, np.pi)
|
||||
ry = rng.uniform(-np.pi / 2.0, np.pi / 2.0)
|
||||
rz = rng.uniform(-np.pi, np.pi)
|
||||
|
||||
orientations.append([rx, ry, rz])
|
||||
|
||||
return orientations
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 3. IK FUNCTION GOES HERE
|
||||
# ============================================================
|
||||
|
||||
def solve_ik(target_position, target_rotation):
|
||||
"""
|
||||
Replace this function with your own IK solver.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
target_position : np.ndarray, shape (3,)
|
||||
Desired TCP position in base_link frame.
|
||||
|
||||
target_rotation : np.ndarray, shape (3, 3)
|
||||
Desired TCP rotation matrix in base_link frame.
|
||||
|
||||
Returns
|
||||
-------
|
||||
None
|
||||
If IK fails.
|
||||
|
||||
or
|
||||
|
||||
np.ndarray, shape (7,)
|
||||
One IK solution.
|
||||
|
||||
or
|
||||
|
||||
list[np.ndarray]
|
||||
Multiple IK solutions.
|
||||
|
||||
Important:
|
||||
Joint order must be:
|
||||
[joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
|
||||
"""
|
||||
|
||||
initial_guess = [0.1] * 7
|
||||
|
||||
ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250)
|
||||
# print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
|
||||
if ret_qp == 0:
|
||||
if not robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True):
|
||||
return q
|
||||
|
||||
|
||||
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
|
||||
# print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
|
||||
if ret_rm == 0:
|
||||
if not robot_kine_qp.collision_detect(q, stop_at_first_collision=True, verbose=True):
|
||||
return q
|
||||
|
||||
|
||||
return None
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 4. URDF / FK UTILITIES
|
||||
# ============================================================
|
||||
|
||||
def load_robot_and_limits(urdf_path):
|
||||
robot = URDF.load(urdf_path)
|
||||
|
||||
joints = []
|
||||
lower = []
|
||||
upper = []
|
||||
|
||||
joint_map = {j.name: j for j in robot.joints}
|
||||
|
||||
for name in JOINT_NAMES:
|
||||
joint = joint_map[name]
|
||||
joints.append(joint)
|
||||
|
||||
if joint.limit is None:
|
||||
raise ValueError(f"Joint {name} has no limit in URDF.")
|
||||
|
||||
lower.append(joint.limit.lower)
|
||||
upper.append(joint.limit.upper)
|
||||
|
||||
lower = np.asarray(lower, dtype=float)
|
||||
upper = np.asarray(upper, dtype=float)
|
||||
|
||||
return robot, lower, upper
|
||||
|
||||
|
||||
# def q_to_cfg(q):
|
||||
# """
|
||||
# Convert joint vector to urdfpy FK config dictionary.
|
||||
# """
|
||||
# return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
|
||||
|
||||
|
||||
# def fk_transform(robot, q):
|
||||
# """
|
||||
# Forward kinematics from base_link to TCP_LINK.
|
||||
#
|
||||
# Returns
|
||||
# -------
|
||||
# T : np.ndarray, shape (4, 4)
|
||||
# """
|
||||
# cfg = q_to_cfg(q)
|
||||
# fk = robot.link_fk(cfg=cfg)
|
||||
# tcp_link = robot.link_map[TCP_LINK]
|
||||
# return fk[tcp_link]
|
||||
#
|
||||
#
|
||||
# def fk_position(robot, q):
|
||||
# T = fk_transform(robot, q)
|
||||
# return T[:3, 3]
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 5. COMFORT METRICS
|
||||
# ============================================================
|
||||
|
||||
def is_within_joint_limits(q, lower, upper, tol=1e-8):
|
||||
q = np.asarray(q)
|
||||
return np.all(q >= lower - tol) and np.all(q <= upper + tol)
|
||||
|
||||
|
||||
def joint_limit_score(q, lower, upper):
|
||||
"""
|
||||
Score in [0, 1].
|
||||
1 means every joint is at center of its range.
|
||||
0 means at least one joint is at its limit.
|
||||
"""
|
||||
|
||||
q = np.asarray(q)
|
||||
mid = 0.5 * (lower + upper)
|
||||
half_range = 0.5 * (upper - lower)
|
||||
|
||||
per_joint_score = 1.0 - np.abs(q - mid) / half_range
|
||||
per_joint_score = np.clip(per_joint_score, 0.0, 1.0)
|
||||
|
||||
# Conservative: one bad joint makes the whole pose less comfortable.
|
||||
return float(np.min(per_joint_score))
|
||||
|
||||
|
||||
def joint_margin(q, lower, upper):
|
||||
"""
|
||||
Minimum normalized distance to joint limits.
|
||||
|
||||
0.15 means the closest joint is 15% away from its limit.
|
||||
"""
|
||||
q = np.asarray(q)
|
||||
margin_lower = (q - lower) / (upper - lower)
|
||||
margin_upper = (upper - q) / (upper - lower)
|
||||
margin = np.minimum(margin_lower, margin_upper)
|
||||
return float(np.min(margin))
|
||||
|
||||
|
||||
def q_to_cfg(q):
|
||||
"""
|
||||
Convert joint vector to urdfpy FK config dictionary.
|
||||
"""
|
||||
return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
|
||||
|
||||
|
||||
def fk_transform(robot, q):
|
||||
"""
|
||||
Forward kinematics from base_link to TCP_LINK.
|
||||
|
||||
Returns
|
||||
-------
|
||||
T : np.ndarray, shape (4, 4)
|
||||
"""
|
||||
cfg = q_to_cfg(q)
|
||||
fk = robot.link_fk(cfg=cfg)
|
||||
tcp_link = robot.link_map[TCP_LINK]
|
||||
return fk[tcp_link]
|
||||
|
||||
|
||||
def numerical_geometric_jacobian(robot, q, eps=1e-5):
|
||||
"""
|
||||
Numerical 6D geometric-like Jacobian, shape (6, 7).
|
||||
|
||||
Top 3 rows:
|
||||
linear velocity approximation
|
||||
|
||||
Bottom 3 rows:
|
||||
angular velocity approximation as rotation-vector difference
|
||||
|
||||
This is useful for manipulability and singularity checks.
|
||||
"""
|
||||
q = np.asarray(q, dtype=float)
|
||||
n = len(q)
|
||||
|
||||
J = np.zeros((6, n))
|
||||
|
||||
T0 = fk_transform(robot, q)
|
||||
p0 = T0[:3, 3]
|
||||
R0 = T0[:3, :3]
|
||||
|
||||
for i in range(n):
|
||||
q_plus = q.copy()
|
||||
q_minus = q.copy()
|
||||
|
||||
q_plus[i] += eps
|
||||
q_minus[i] -= eps
|
||||
|
||||
T_plus = fk_transform(robot, q_plus)
|
||||
T_minus = fk_transform(robot, q_minus)
|
||||
|
||||
p_plus = T_plus[:3, 3]
|
||||
p_minus = T_minus[:3, 3]
|
||||
|
||||
R_plus = T_plus[:3, :3]
|
||||
R_minus = T_minus[:3, :3]
|
||||
|
||||
# Linear part
|
||||
J[:3, i] = (p_plus - p_minus) / (2.0 * eps)
|
||||
|
||||
# Angular part
|
||||
# Relative rotation from minus to plus.
|
||||
dR = R_plus @ R_minus.T
|
||||
rotvec = R.from_matrix(dR).as_rotvec()
|
||||
J[3:, i] = rotvec / (2.0 * eps)
|
||||
|
||||
return J
|
||||
|
||||
|
||||
def manipulability_score_from_jacobian(J):
|
||||
"""
|
||||
Yoshikawa-style manipulability.
|
||||
|
||||
For a 6x7 Jacobian:
|
||||
w = sqrt(det(J J.T))
|
||||
|
||||
To improve numerical robustness, compute from singular values.
|
||||
"""
|
||||
singular_values = np.linalg.svd(J, compute_uv=False)
|
||||
|
||||
# Product of singular values.
|
||||
# For a 6x7 Jacobian, there are 6 singular values.
|
||||
w = float(np.prod(singular_values))
|
||||
|
||||
return w
|
||||
|
||||
|
||||
def condition_number_from_jacobian(J, min_sigma=1e-9):
|
||||
singular_values = np.linalg.svd(J, compute_uv=False)
|
||||
sigma_max = np.max(singular_values)
|
||||
sigma_min = np.min(singular_values)
|
||||
|
||||
if sigma_min < min_sigma:
|
||||
return np.inf
|
||||
|
||||
return float(sigma_max / sigma_min)
|
||||
|
||||
|
||||
def singularity_score(condition_number):
|
||||
"""
|
||||
Score in [0, 1].
|
||||
Higher is better.
|
||||
|
||||
condition_number = 1 is ideal.
|
||||
Very large means near singularity.
|
||||
"""
|
||||
if not np.isfinite(condition_number):
|
||||
return 0.0
|
||||
|
||||
return float(1.0 / condition_number)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 6. IK RESULT HANDLING
|
||||
# ============================================================
|
||||
|
||||
def normalize_ik_solutions(ik_result):
|
||||
"""
|
||||
Your IK returns:
|
||||
- None if failed
|
||||
- one list/array of 7 joint values if successful
|
||||
"""
|
||||
if ik_result is None:
|
||||
return []
|
||||
|
||||
q = np.asarray(ik_result, dtype=float).reshape(-1)
|
||||
|
||||
if q.shape[0] != 7:
|
||||
return []
|
||||
|
||||
return [q]
|
||||
|
||||
|
||||
def evaluate_single_solution(robot, q, lower, upper):
|
||||
"""
|
||||
Evaluate one IK solution.
|
||||
|
||||
Returns a dictionary with metrics.
|
||||
"""
|
||||
|
||||
if q.shape[0] != 7:
|
||||
return None
|
||||
|
||||
if not is_within_joint_limits(q, lower, upper):
|
||||
return None
|
||||
|
||||
jl_score = joint_limit_score(q, lower, upper)
|
||||
jl_margin = joint_margin(q, lower, upper)
|
||||
|
||||
J = numerical_geometric_jacobian(robot, q, eps=JACOBIAN_EPS)
|
||||
|
||||
manip = manipulability_score_from_jacobian(J)
|
||||
cond = condition_number_from_jacobian(J)
|
||||
sing_score = singularity_score(cond)
|
||||
|
||||
valid_by_thresholds = (
|
||||
jl_margin >= MIN_JOINT_MARGIN
|
||||
and cond <= MAX_CONDITION_NUMBER
|
||||
)
|
||||
|
||||
return {
|
||||
"q": q,
|
||||
"joint_limit_score": jl_score,
|
||||
"joint_margin": jl_margin,
|
||||
"manipulability": manip,
|
||||
"condition_number": cond,
|
||||
"singularity_score": sing_score,
|
||||
"valid_by_thresholds": valid_by_thresholds,
|
||||
}
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 7. MAIN WORKSPACE EVALUATION
|
||||
# ============================================================
|
||||
|
||||
def make_grid():
|
||||
xs = np.arange(X_RANGE[0], X_RANGE[1] + 1e-9, GRID_RESOLUTION)
|
||||
ys = np.arange(Y_RANGE[0], Y_RANGE[1] + 1e-9, GRID_RESOLUTION)
|
||||
zs = np.arange(Z_RANGE[0], Z_RANGE[1] + 1e-9, GRID_RESOLUTION)
|
||||
|
||||
points = []
|
||||
|
||||
for x in xs:
|
||||
for y in ys:
|
||||
for z in zs:
|
||||
points.append(np.array([x, y, z], dtype=float))
|
||||
|
||||
return points
|
||||
|
||||
|
||||
def evaluate_workspace():
|
||||
robot, lower, upper = load_robot_and_limits(URDF_PATH)
|
||||
orientations = make_task_orientations()
|
||||
grid_points = make_grid()
|
||||
|
||||
rows = []
|
||||
|
||||
# First pass stores raw manipulability.
|
||||
# Later we normalize manipulability by max observed value.
|
||||
all_valid_solution_metrics = []
|
||||
|
||||
print(f"Loaded robot from: {URDF_PATH}")
|
||||
print(f"Grid points: {len(grid_points)}")
|
||||
print(f"Orientations per point: {len(orientations)}")
|
||||
print("Evaluating IK reachability and raw metrics...")
|
||||
|
||||
for point in tqdm(grid_points):
|
||||
point_solution_metrics = []
|
||||
|
||||
attempted = 0
|
||||
ik_success_count = 0
|
||||
|
||||
|
||||
for rpy in orientations:
|
||||
attempted += 1
|
||||
|
||||
|
||||
ik_result = solve_ik(point, rpy)
|
||||
|
||||
# print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
|
||||
candidate_solutions = normalize_ik_solutions(ik_result)
|
||||
|
||||
if len(candidate_solutions) == 0:
|
||||
continue
|
||||
|
||||
evaluated_solutions = []
|
||||
|
||||
for q in candidate_solutions:
|
||||
# pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name)
|
||||
# print(f'the fk of q is {pose}\n')
|
||||
metrics = evaluate_single_solution(robot, q, lower, upper)
|
||||
# print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
|
||||
if metrics is not None:
|
||||
evaluated_solutions.append(metrics)
|
||||
|
||||
if len(evaluated_solutions) == 0:
|
||||
continue
|
||||
|
||||
ik_success_count += 1
|
||||
|
||||
# Use the best solution for this pose.
|
||||
# At this stage, manipulability is not normalized,
|
||||
# so use joint score + singularity score as temporary ranking.
|
||||
best = max(
|
||||
evaluated_solutions,
|
||||
key=lambda m: 0.6 * m["joint_limit_score"] + 0.4 * m["singularity_score"]
|
||||
)
|
||||
|
||||
point_solution_metrics.append(best)
|
||||
all_valid_solution_metrics.append(best)
|
||||
print(f'this position+all orientations, the point_solution_metrics = {point_solution_metrics}')
|
||||
ik_success_rate = ik_success_count / attempted if attempted > 0 else 0.0
|
||||
|
||||
if len(point_solution_metrics) == 0:
|
||||
rows.append({
|
||||
"x": point[0],
|
||||
"y": point[1],
|
||||
"z": point[2],
|
||||
"ik_success_rate": 0.0,
|
||||
"joint_limit_score": 0.0,
|
||||
"joint_margin": 0.0,
|
||||
"manipulability": 0.0,
|
||||
"manipulability_score": 0.0,
|
||||
"condition_number": np.inf,
|
||||
"singularity_score": 0.0,
|
||||
"comfort_score": 0.0,
|
||||
"comfortable": False,
|
||||
"reachable": False,
|
||||
})
|
||||
else:
|
||||
# Average over task orientations.
|
||||
rows.append({
|
||||
"x": point[0],
|
||||
"y": point[1],
|
||||
"z": point[2],
|
||||
"ik_success_rate": ik_success_rate,
|
||||
"joint_limit_score": np.mean([m["joint_limit_score"] for m in point_solution_metrics]),
|
||||
"joint_margin": np.mean([m["joint_margin"] for m in point_solution_metrics]),
|
||||
"manipulability": np.mean([m["manipulability"] for m in point_solution_metrics]),
|
||||
"manipulability_score": 0.0, # filled later
|
||||
"condition_number": np.mean([m["condition_number"] for m in point_solution_metrics]),
|
||||
"singularity_score": np.mean([m["singularity_score"] for m in point_solution_metrics]),
|
||||
"comfort_score": 0.0, # filled later
|
||||
"comfortable": False,
|
||||
"reachable": True,
|
||||
})
|
||||
|
||||
df = pd.DataFrame(rows)
|
||||
|
||||
# Normalize manipulability by maximum observed value.
|
||||
max_manip = df["manipulability"].replace([np.inf, -np.inf], np.nan).max()
|
||||
|
||||
if max_manip is None or not np.isfinite(max_manip) or max_manip <= 0:
|
||||
max_manip = 1.0
|
||||
|
||||
df["manipulability_score"] = df["manipulability"] / max_manip
|
||||
df["manipulability_score"] = df["manipulability_score"].clip(0.0, 1.0)
|
||||
|
||||
# Final comfort score.
|
||||
df["comfort_score"] = (
|
||||
WEIGHT_IK_SUCCESS * df["ik_success_rate"]
|
||||
+ WEIGHT_JOINT_LIMIT * df["joint_limit_score"]
|
||||
+ WEIGHT_MANIPULABILITY * df["manipulability_score"]
|
||||
+ WEIGHT_SINGULARITY * df["singularity_score"]
|
||||
)
|
||||
|
||||
# Comfortable binary classification.
|
||||
df["comfortable"] = (
|
||||
(df["reachable"] == True)
|
||||
& (df["ik_success_rate"] >= 0.80)
|
||||
& (df["joint_margin"] >= MIN_JOINT_MARGIN)
|
||||
& (df["condition_number"] <= MAX_CONDITION_NUMBER)
|
||||
& (df["manipulability_score"] >= MIN_MANIPULABILITY_RATIO)
|
||||
)
|
||||
|
||||
return df
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 8. PLOTTING
|
||||
# ============================================================
|
||||
|
||||
def plot_workspace(df):
|
||||
"""
|
||||
3D scatter plot:
|
||||
gray/low = low comfort
|
||||
brighter = higher comfort
|
||||
"""
|
||||
|
||||
reachable = df[df["reachable"] == True]
|
||||
|
||||
if len(reachable) == 0:
|
||||
print("No reachable points found. Check your IK function.")
|
||||
return
|
||||
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection="3d")
|
||||
|
||||
sc = ax.scatter(
|
||||
reachable["x"],
|
||||
reachable["y"],
|
||||
reachable["z"],
|
||||
c=reachable["comfort_score"],
|
||||
s=12,
|
||||
alpha=0.8,
|
||||
)
|
||||
|
||||
ax.set_title("RM75-B Comfortable Workspace")
|
||||
ax.set_xlabel("X [m]")
|
||||
ax.set_ylabel("Y [m]")
|
||||
ax.set_zlabel("Z [m]")
|
||||
|
||||
fig.colorbar(sc, ax=ax, label="Comfort score")
|
||||
plt.show()
|
||||
|
||||
|
||||
def plot_comfortable_only(df):
|
||||
comfortable = df[df["comfortable"] == True]
|
||||
|
||||
if len(comfortable) == 0:
|
||||
print("No comfortable points found under current thresholds.")
|
||||
return
|
||||
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection="3d")
|
||||
|
||||
ax.scatter(
|
||||
comfortable["x"],
|
||||
comfortable["y"],
|
||||
comfortable["z"],
|
||||
c=comfortable["comfort_score"],
|
||||
s=16,
|
||||
alpha=0.9,
|
||||
)
|
||||
|
||||
ax.set_title("RM75-B Comfortable Region Only")
|
||||
ax.set_xlabel("X [m]")
|
||||
ax.set_ylabel("Y [m]")
|
||||
ax.set_zlabel("Z [m]")
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 9. ENTRY POINT
|
||||
# ============================================================
|
||||
|
||||
if __name__ == "__main__":
|
||||
df = evaluate_workspace()
|
||||
|
||||
df.to_csv(output_csv, index=False)
|
||||
|
||||
print(f"\nSaved result to: {output_csv}")
|
||||
|
||||
print("\nSummary:")
|
||||
print(f"Total grid points: {len(df)}")
|
||||
print(f"Reachable points: {df['reachable'].sum()}")
|
||||
print(f"Comfortable points: {df['comfortable'].sum()}")
|
||||
|
||||
if df["reachable"].sum() > 0:
|
||||
print(f"Max comfort score: {df['comfort_score'].max():.3f}")
|
||||
print(f"Mean comfort score: {df[df['reachable']]['comfort_score'].mean():.3f}")
|
||||
|
||||
plot_workspace(df)
|
||||
plot_comfortable_only(df)
|
||||
Reference in New Issue
Block a user