ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: ZhengLiu-cart/IK_qp:9513dfa4cefef332798977fba161e9ef31d72d72
Branches Tags
ZhengLiu-cart/IK_qp:master ZhengLiu-cart/IK_qp:class_version ZhengLiu-cart/IK_qp:rm75_kinematics
..
compare: ZhengLiu-cart/IK_qp:master
Branches Tags
ZhengLiu-cart/IK_qp:class_version ZhengLiu-cart/IK_qp:rm75_kinematics ZhengLiu-cart/IK_qp:master
12 Commits
9513dfa4ce .. 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
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
17 changed files with 2113 additions and 466 deletions
Expand all files Collapse all files
README.md
+8 -3
View File
@@ -11,9 +11,9 @@ Key specifications:
2. Success rate 2. Success rate
3. Minial joint variation. 3. Minial joint variation.
Next:\ Cost:
Comparison with Realman official IK method.
Embedded with current demo. <img src="img/cost.jpg" alt="Cost" width="400">
### Comparison (05June2026): ### Comparison (05June2026):
@@ -46,3 +46,8 @@ In each iteration, update optimization formula:
<img src="img/cons.png" alt="Cost" width="400"> <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
+10 -11
View File
@@ -43,12 +43,15 @@ def main():
# ----------- rm75 qp based kine ------------ # ----------- rm75 qp based kine ------------
robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./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.add_tool_frames(tools_in_ee)
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
fp = robot_kine_qp.forward_kinematics(np.zeros(7)) fp = robot_kine_qp.forward_kinematics(np.ones(7)*0.3,tool='scissor_tcp')
print(f'forward kine res = {fp}') 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 ----------- # ---------- rm75 official algorithm -----------
robot_kine_rm = kine_rm() robot_kine_rm = kine_rm()
@@ -63,8 +66,6 @@ def main():
print(f'pose = {pose}') print(f'pose = {pose}')
print('-'*100) print('-'*100)
time.sleep(5)
@@ -103,16 +104,13 @@ def main():
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) 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) 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}') 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: if d_p_ik < 0.01:
result[0][1] += 1 result[0][1] += 1
# while 1: # robot_mjk.send_command(q)
# time.sleep(1) # robot_mjk.wait_until_reached()
# robot_mjk.print_state()
robot_mjk.send_command(q)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
else: else:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) 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) d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
@@ -133,6 +131,7 @@ def main():
print(f'results with qp and rm for ik are {result}') print(f'results with qp and rm for ik are {result}')
print(f'solve_sum is {solve_sum}') print(f'solve_sum is {solve_sum}')
robot_mjk.stop()
def cal_pose_deviation(pose1, pose2): def cal_pose_deviation(pose1, pose2):
kine_ctrl/rm75_kine_qp.py
+471 -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) 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_position: list of position values, m
:param target_rpy: list of rpy values, rad :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 ] q_ref = [ 180/math.pi * ig for ig in initial_guess ]
else: else:
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] 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) ret, phi0 = 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)
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) 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_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
pose_dis = cal_pose_deviation(pose_fk, target) 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-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/workspace_cal.py
+13 -41
View File
@@ -3,7 +3,7 @@ RM75-B comfortable workspace evaluator.
You provide: You provide:
- URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf' - 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: This script computes:
- IK success rate - IK success rate
@@ -57,9 +57,13 @@ GRID_RESOLUTION = 0.05 # 5 cm. Use 0.02 for finer but slower.
num_orientations = 120 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 # 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 lb = -ub
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
MESH_DIR = str(Path(URDF_PATH).parent) MESH_DIR = str(Path(URDF_PATH).parent)
# ----------- rm75 qp based kine ------------ # ----------- 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.add_tool_frames(tools_in_ee)
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) 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 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. Replace this function with your own IK solver.
@@ -229,22 +204,19 @@ def solve_ik_user(target_position, target_rotation):
[joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7] [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
""" """
# ========================================================
# INSERT YOUR IK CODE HERE
# ========================================================
initial_guess = [0.1] * 7 initial_guess = [0.1] * 7
ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250) 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}') # print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
if ret_qp == 0: if ret_qp == 0:
if not robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True):
return q 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) 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}') # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
if ret_rm == 0: if ret_rm == 0:
pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name) if not robot_kine_qp.collision_detect(q, stop_at_first_collision=True, verbose=True):
# print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}')
return q return q
@@ -564,7 +536,7 @@ def evaluate_workspace():
attempted += 1 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}') # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
candidate_solutions = normalize_ik_solutions(ik_result) candidate_solutions = normalize_ik_solutions(ik_result)