ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

Compare commits

merge into: ZhongyuWang-cart/IK_qp:class_version
Branches Tags
ZhongyuWang-cart/IK_qp:master ZhongyuWang-cart/IK_qp:class_version ZhengLiu-cart/IK_qp:class_version ZhengLiu-cart/IK_qp:rm75_kinematics ZhengLiu-cart/IK_qp:master
..
pull from: ZhengLiu-cart/IK_qp:master
Branches Tags
ZhengLiu-cart/IK_qp:class_version ZhengLiu-cart/IK_qp:rm75_kinematics ZhengLiu-cart/IK_qp:master ZhongyuWang-cart/IK_qp:master ZhongyuWang-cart/IK_qp:class_version
31 Commits
class_version .. master
Author SHA1 Message Date
LiuzhengSJ 149a23c12c Merge remote-tracking branch 'origin/master' 
# Conflicts:
#	README.md
 2026-08-05 13:17:17 +01:00
LiuzhengSJ 8ff29b1cc9 add mini scissor stl file, as well as urdf file. 
modify the Dual_arm.urdf, in line with real arm installation.
 2026-07-31 16:08:17 +01:00
LiuzhengSJ 0c121a7425 In qp inverse kinematic file, the tool frames self.tcps is defined as [] if not given a value. 2026-07-30 15:41:10 +01:00
LiuzhengSJ e001f2e1f1 add ik q_solved refinement function, to make it as close as possible to last_q. 
update the rm75_kine_rm.py to included a series of arm-angle for inverse kinematics calculation.
 2026-07-30 15:28:14 +01:00
LiuzhengSJ ace5dea9a2 add collision detection in workspace cal 2026-07-29 11:30:32 +01:00
LiuzhengSJ 2713c54707 add collision detection in workspace cal 2026-07-29 11:25:09 +01:00
LiuzhengSJ 9849466430 add collision detection 2026-07-28 22:41:35 +01:00
LiuzhengSJ 86c40ee380 add urdf files, dual arm. 2026-07-28 19:40:27 +01:00
LiuzhengSJ 9513dfa4ce add urdf file, scissor included. 2026-07-28 16:06:21 +01:00
LiuzhengSJ e1d833812b the scissor stl file is added. 2026-07-27 21:31:52 +01:00
LiuzhengSJ ceb80a8b17 the scissor stl file is added. 2026-07-27 20:34:02 +01:00
LiuzhengSJ d1080638c1 Merge remote-tracking branch 'origin/class_version' into class_version 
# Conflicts:
#	kine_ctrl/workspace_comfortable/workspace_cal.py
 2026-07-27 15:15:11 +01:00
emboddied 5cac8c5a73 code for virtical sci 2026-07-27 21:59:54 +08:00
LiuzhengSJ d0ca4d6115 update teh contour plot method 2026-07-24 10:46:09 +01:00
LiuzhengSJ 579abe6b67 update teh contour plot method 2026-07-22 11:41:55 +01:00
emboddied 2bd6bf510f use the tool 'minisci' 2026-07-20 21:09:15 +08:00
LiuzhengSJ a58e1b9f59 with tool, expand the calculation space 2026-07-18 21:45:24 +01:00
LiuzhengSJ 6a50db91a4 add vertical tool installation. 2026-07-18 21:39:15 +01:00
emboddied c6458248a7 collision detection ik 2026-07-15 16:32:18 +08:00
LiuzhengSJ 4add432f53 add collision detection 2026-07-13 15:05:34 +01:00
LiuzhengSJ 58e84c6a33 add collision detection 2026-07-13 14:54:33 +01:00
ZhengLiu-cart 7050c93c84 Upload files to "kine_ctrl/workspace_comfortable" 
add
 2026-07-13 18:57:58 +08:00
ZhengLiu-cart 6db8b2f254 Upload files to "kine_ctrl/workspace_comfortable" 
add
 2026-07-13 18:57:44 +08:00
ZhengLiu-cart a5fb40d1a6 Upload files to "kine_ctrl/workspace_comfortable" 
results
 2026-07-13 18:57:11 +08:00
ZhengLiu-cart 223b29f37d Upload files to "kine_ctrl/workspace_comfortable" 
ik success rate. no self-collision detection used. no tool installed
 2026-07-13 18:56:17 +08:00
emboddied cb51ecf2eb test_120Ori+0.05m 2026-07-13 16:27:35 +08:00
emboddied 75ba51c609 after calculation 2026-07-10 16:55:08 +08:00
ZhengLiu-cart 377a676863 Update README.md 2026-06-14 04:17:40 +08:00
ZhengLiu-cart 9db480c474 Upload files to "img" 2026-06-14 04:13:40 +08:00
ZhengLiu-cart 3a8aecbf71 Update README.md 
add visualization
 2026-06-09 17:28:57 +08:00
ZhengLiu-cart 7c8beaa3b9 Upload files to "kine_ctrl/visual/mjc_ik_test1" 2026-06-09 17:23:17 +08:00
47 changed files with 67831 additions and 465 deletions
Expand all files Collapse all files
README.md
+9 -4
View File
@@ -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):
@@ -45,4 +45,9 @@ In each iteration, update optimization formula:
<img src="img/cons.png" alt="Cost" width="400">
<img src="img/osqp.png" alt="Cost" width="400">
<img src="img/osqp.png" alt="Cost" width="400">
Visualization:
![demo](kine_ctrl/visual/mjc_ik_test1/mjc_ik.gif)
img/cost.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 69 KiB

kine_ctrl/main.py
+15 -5
View File
@@ -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)
@@ -71,7 +79,7 @@ def main():
solve_sum = 0
for i in range(1000):
for i in range(10):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub, lb)
print(f'the predefined joints are {joint_rand}')
@@ -96,6 +104,7 @@ 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)
if d_p_ik < 0.01:
result[0][1] += 1
@@ -122,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):
kine_ctrl/req.txt
+3
View File
@@ -0,0 +1,3 @@
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
kine_ctrl/rm75_kine_qp.py
+571 -401
View File
File diff suppressed because it is too large Load Diff
kine_ctrl/rm75_kine_rm.py
+28 -5
View File
@@ -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)
kine_ctrl/urdf_rm75/Dual_arm.urdf
+555
View File
@@ -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>
kine_ctrl/urdf_rm75/RM75-OmniPicker.urdf
+507
View File
@@ -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>
kine_ctrl/urdf_rm75/RM75-SCI-V1.urdf
+516
View File
@@ -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>
kine_ctrl/urdf_rm75/RM75-SCI-V2.urdf
+516
View File
@@ -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>
kine_ctrl/urdf_rm75/RM75-SCI.urdf
+516
View File
@@ -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 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>
kine_ctrl/urdf_rm75/RM75-newSCI.urdf
+516
View File
@@ -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>
kine_ctrl/urdf_rm75/meshes/OmniPic.stl
BIN
View File
Binary file not shown.
kine_ctrl/urdf_rm75/meshes/OmniPic_original.stl
BIN
View File
Binary file not shown.
kine_ctrl/urdf_rm75/meshes/dual_arm_base.stl
BIN
View File
Binary file not shown.
kine_ctrl/urdf_rm75/meshes/dual_arm_base_original.stl
BIN
View File
Binary file not shown.
kine_ctrl/urdf_rm75/meshes/mini_scissor.stl
BIN
View File
Binary file not shown.
kine_ctrl/urdf_rm75/meshes/scissor.stl
BIN
View File
Binary file not shown.
kine_ctrl/urdf_rm75/meshes/scissor_original.stl
BIN
View File
Binary file not shown.
kine_ctrl/visual/mjc_ik_test1/mjc_ik.gif
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 796 KiB

kine_ctrl/workspace_comfortable/Contour_plot.py
+106
View File
@@ -0,0 +1,106 @@
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()}")
kine_ctrl/workspace_comfortable/Contour_plot_no_header.py
+109
View File
@@ -0,0 +1,109 @@
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()}")
kine_ctrl/workspace_comfortable/no_collision_detection_res/Comfortable_Region.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 246 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/Comfortable_Workspace.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 442 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.00.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 120 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.05.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 123 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.10.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 128 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.15.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 133 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.20.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 134 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.25.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 136 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.30.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 132 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.35.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 129 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.40.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 123 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.45.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 119 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.50.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 115 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.55.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 109 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.60.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 101 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.65.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 90 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.70.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 77 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.75.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 67 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/ik_contour_z_0.80.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

kine_ctrl/workspace_comfortable/no_collision_detection_res/rm75b_comfort_workspace.csv
+10626
View File
File diff suppressed because it is too large Load Diff
kine_ctrl/workspace_comfortable/no_collision_detection_res/workspace_nocollisiondetection.csv
+10625
View File
File diff suppressed because it is too large Load Diff
kine_ctrl/workspace_comfortable/rm75b_comfort_workspace_collision_minisci.csv
+15980
View File
File diff suppressed because it is too large Load Diff
kine_ctrl/workspace_comfortable/rm75b_comfort_workspace_no_collision_no_tool.csv
+10626
View File
File diff suppressed because it is too large Load Diff
kine_ctrl/workspace_comfortable/rm75b_comfort_workspace_v_minis_collision.csv
+15980
View File
File diff suppressed because it is too large Load Diff
kine_ctrl/workspace_comfortable/workspace_cal.py
+27 -50
View File
@@ -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
@@ -49,11 +49,22 @@ from math import radians, degrees, pi, cos, sin
# Cartesian workspace grid, in meters.
# Adjust according to your robot placement and task.
X_RANGE = (-0.6, 0.6)
Y_RANGE = (-0.6, 0.6)
Z_RANGE = (0.0, 0.8)
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"
GRID_RESOLUTION = 0.3 # 5 cm. Use 0.02 for finer but slower.
# Comfort thresholds
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
@@ -75,6 +86,7 @@ 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),
}
@@ -86,13 +98,11 @@ tools_in_ee = {
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
lb = -ub
tool_name = "no_tool"
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)
@@ -133,7 +143,7 @@ JACOBIAN_EPS = 1e-5
# 2. TASK ORIENTATION SAMPLING
# ============================================================
def make_task_orientations(num_orientations=200, seed=1):
def make_task_orientations(num_orientations=num_orientations, seed=1):
"""
Random orientation sampling using RM's Euler convention:
@@ -157,40 +167,12 @@ def make_task_orientations(num_orientations=200, 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.
@@ -222,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
@@ -556,9 +535,8 @@ def evaluate_workspace():
for rpy in orientations:
attempted += 1
# print(f"\n - target point: {point}, target orientation: {rpy}")
rpy = [1.2022060487764064, -1.0097962261845583, -0.6518417572686532]
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)
@@ -731,7 +709,6 @@ def plot_comfortable_only(df):
if __name__ == "__main__":
df = evaluate_workspace()
output_csv = "rm75b_comfort_workspace.csv"
df.to_csv(output_csv, index=False)
print(f"\nSaved result to: {output_csv}")