Compare commits
14
Commits
ceb80a8b17
...
master
| Author | SHA1 | Date | |
|---|---|---|---|
 LiuzhengSJ
|
149a23c12c | ||
|
LiuzhengSJ
|
8ff29b1cc9 | ||
|
LiuzhengSJ
|
0c121a7425 | ||
|
LiuzhengSJ
|
e001f2e1f1 | ||
|
LiuzhengSJ
|
ace5dea9a2 | ||
|
LiuzhengSJ
|
2713c54707 | ||
|
LiuzhengSJ
|
9849466430 | ||
|
LiuzhengSJ
|
86c40ee380 | ||
|
LiuzhengSJ
|
9513dfa4ce | ||
|
LiuzhengSJ
|
e1d833812b | ||
 ZhengLiu-cart
|
377a676863 | ||
|
ZhengLiu-cart
|
9db480c474 | ||
|
ZhengLiu-cart
|
3a8aecbf71 | ||
|
ZhengLiu-cart
|
7c8beaa3b9 |
@@ -11,9 +11,9 @@ Key specifications:
|
||||
2. Success rate
|
||||
3. Minial joint variation.
|
||||
|
||||
Next:\
|
||||
Comparison with Realman official IK method.
|
||||
Embedded with current demo.
|
||||
Cost:
|
||||
|
||||
<img src="img/cost.jpg" alt="Cost" width="400">
|
||||
|
||||
|
||||
### Comparison (05June2026):
|
||||
@@ -46,3 +46,8 @@ In each iteration, update optimization formula:
|
||||
<img src="img/cons.png" alt="Cost" width="400">
|
||||
|
||||
<img src="img/osqp.png" alt="Cost" width="400">
|
||||
|
||||
Visualization:
|
||||
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 69 KiB |
+17
-11
@@ -8,6 +8,10 @@ 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
|
||||
@@ -35,14 +39,20 @@ def main():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
# Create controller
|
||||
robot_mjk = MuJoCoPositionController()
|
||||
robot_mjk = MuJoCoPositionController(urdf_path="./urdf_rm75/RM75-SCI.urdf")
|
||||
|
||||
|
||||
# ----------- rm75 qp based kine ------------
|
||||
robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
|
||||
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')
|
||||
|
||||
|
||||
# ---------- rm75 official algorithm -----------
|
||||
robot_kine_rm = kine_rm()
|
||||
robot_kine_rm.add_tool_frames(tools_in_ee)
|
||||
@@ -56,8 +66,6 @@ def main():
|
||||
print(f'pose = {pose}')
|
||||
print('-'*100)
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -96,16 +104,13 @@ def main():
|
||||
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)
|
||||
robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
|
||||
if d_p_ik < 0.01:
|
||||
result[0][1] += 1
|
||||
|
||||
while 1:
|
||||
time.sleep(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)
|
||||
@@ -126,6 +131,7 @@ def main():
|
||||
|
||||
print(f'results with qp and rm for ik are {result}')
|
||||
print(f'solve_sum is {solve_sum}')
|
||||
robot_mjk.stop()
|
||||
|
||||
|
||||
def cal_pose_deviation(pose1, pose2):
|
||||
|
||||
+478
-400
File diff suppressed because it is too large
Load Diff
@@ -102,7 +102,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"):
|
||||
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work", step_arm_angle = 15.0):
|
||||
'''
|
||||
:param target_position: list of position values, m
|
||||
:param target_rpy: list of rpy values, rad
|
||||
@@ -126,10 +126,33 @@ class rm75_kine_api():
|
||||
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, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||
# print(f'the arm angle is ret = {ret}, and phi = {phi}')
|
||||
params = rm_inverse_kinematics_params_t(q_ref,
|
||||
target, 1)
|
||||
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, 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)
|
||||
|
||||
@@ -0,0 +1,555 @@
|
||||
<?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>
|
||||
@@ -0,0 +1,507 @@
|
||||
<?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>
|
||||
@@ -0,0 +1,516 @@
|
||||
<?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>
|
||||
@@ -0,0 +1,516 @@
|
||||
<?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>
|
||||
@@ -469,7 +469,7 @@
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/sci.stl"
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
@@ -481,7 +481,7 @@
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/sci.stl"
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
@@ -491,6 +491,26 @@
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 0 0" rpy="3.14159 0 0" />
|
||||
<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>
|
||||
|
||||
@@ -0,0 +1,516 @@
|
||||
<?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.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 796 KiB |
@@ -3,7 +3,7 @@ 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_user()
|
||||
- your own IK solver inside solve_ik()
|
||||
|
||||
This script computes:
|
||||
- IK success rate
|
||||
@@ -57,9 +57,13 @@ GRID_RESOLUTION = 0.05 # 5 cm. Use 0.02 for finer but slower.
|
||||
|
||||
num_orientations = 120
|
||||
|
||||
tool_name = "v_minis"
|
||||
tool_name = "scissor"
|
||||
|
||||
output_csv = "rm75b_comfort_workspace" + tool_name + ".csv"
|
||||
|
||||
|
||||
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-SCI.urdf'
|
||||
|
||||
output_csv = "workspace" + tool_name + URDF_PATH.split('/')[-1].split('.')[0] + ".csv"
|
||||
|
||||
|
||||
# Comfort thresholds
|
||||
@@ -95,11 +99,10 @@ ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||
lb = -ub
|
||||
|
||||
|
||||
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
|
||||
MESH_DIR = str(Path(URDF_PATH).parent)
|
||||
|
||||
# ----------- rm75 qp based kine ------------
|
||||
robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR)
|
||||
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)
|
||||
|
||||
@@ -164,40 +167,12 @@ def make_task_orientations(num_orientations=num_orientations, seed=1):
|
||||
|
||||
return orientations
|
||||
|
||||
#
|
||||
# def euler_angles_to_rotation_matrix(rx, ry, rz):
|
||||
# """
|
||||
# Official RM convention:
|
||||
# R = Rz @ Ry @ Rx
|
||||
# This matches scipy:
|
||||
# Rotation.from_euler("xyz", [rx, ry, rz]).as_matrix()
|
||||
# """
|
||||
# Rx = np.array([
|
||||
# [1, 0, 0],
|
||||
# [0, np.cos(rx), -np.sin(rx)],
|
||||
# [0, np.sin(rx), np.cos(rx)]
|
||||
# ])
|
||||
#
|
||||
# Ry = np.array([
|
||||
# [ np.cos(ry), 0, np.sin(ry)],
|
||||
# [0, 1, 0],
|
||||
# [-np.sin(ry), 0, np.cos(ry)]
|
||||
# ])
|
||||
#
|
||||
# Rz = np.array([
|
||||
# [np.cos(rz), -np.sin(rz), 0],
|
||||
# [np.sin(rz), np.cos(rz), 0],
|
||||
# [0, 0, 1]
|
||||
# ])
|
||||
#
|
||||
# return Rz @ Ry @ Rx
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 3. YOUR IK FUNCTION GOES HERE
|
||||
# 3. IK FUNCTION GOES HERE
|
||||
# ============================================================
|
||||
|
||||
def solve_ik_user(target_position, target_rotation):
|
||||
def solve_ik(target_position, target_rotation):
|
||||
"""
|
||||
Replace this function with your own IK solver.
|
||||
|
||||
@@ -229,23 +204,20 @@ def solve_ik_user(target_position, target_rotation):
|
||||
[joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
|
||||
"""
|
||||
|
||||
# ========================================================
|
||||
# INSERT YOUR IK CODE HERE
|
||||
# ========================================================
|
||||
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:
|
||||
return q
|
||||
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:
|
||||
pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
|
||||
# print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}')
|
||||
return q
|
||||
if not robot_kine_qp.collision_detect(q, stop_at_first_collision=True, verbose=True):
|
||||
return q
|
||||
|
||||
|
||||
return None
|
||||
@@ -564,7 +536,7 @@ def evaluate_workspace():
|
||||
attempted += 1
|
||||
|
||||
|
||||
ik_result = solve_ik_user(point, rpy)
|
||||
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)
|
||||
|
||||
Reference in New Issue
Block a user