RM75-SCI scissor collision_detect

modify the Dual_arm.urdf, in line with real arm installation.

.gitignore

@@ -0,0 +1 @@
+kine_ctrl/__pycache__/

kine_ctrl/main.py

@@ -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
@@ -22,12 +26,12 @@ tools_in_ee = {
 }
 
 # joint limit
-ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
+# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
-lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
+# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
 
 
-# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
-# lb = -ub
+lb = -ub
 
 tool_name = "scissor"
 
@@ -35,19 +39,35 @@ 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)
     robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
+    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool="no_tool")
+
+    print(f'ret_rm = {ret_rm}, q = {q}')
+    pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
+    print(f'pose = {pose}')
+    print('-'*100)
+
+
+
 
 
 #   --------------  for comparison   ----------------
@@ -83,33 +103,35 @@ def main():
             if ret_qp == 0:
                 fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
-                print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
+                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
 
-                robot_mjk.send_command(q)
+                # robot_mjk.send_command(q)
-                robot_mjk.wait_until_reached()
+                # robot_mjk.wait_until_reached()
-                robot_mjk.print_state()
+                # robot_mjk.print_state()
             else:
                 fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
-                print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
+                print(f'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
 
             ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
             if ret_rm == 0:
                 fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
-                print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
+                print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
                 if d_p_ik < 0.01:
                     result[1][1] += 1
             else:
-                print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
+                print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
 
             if ret_qp == 0 or ret_rm == 0:
                 solve_sum += 1
 
         print(f'results with qp and rm for ik are {result}')
         print(f'solve_sum is {solve_sum}')
+    robot_mjk.stop()
 
 
 def cal_pose_deviation(pose1, pose2):

kine_ctrl/req.txt

@@ -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/requirements.txt

@@ -0,0 +1,9 @@
+numpy
+pandas
+matplotlib
+tqdm
+scipy
+urdfpy
+pin
+osqp
+Robotic_Arm

kine_ctrl/rm75_kine_rm.py

@@ -7,7 +7,7 @@ class rm75_kine_api():
     def __init__(self):
         #   ----------      rm75 official algorithm         -----------
         print(f'-------  the realman official kinematic initialising -------')
-        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 Robotic arm
         force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
         # Initialize the robotic arm model and sensor type in the algorithm
         self.robot_kine_rm = Algo(arm_model, force_type)
@@ -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
@@ -120,14 +120,60 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = target_position + target_rpy
+        target = list(target_position) + list(target_rpy)
 
         if initial_guess is not None:
             q_ref = [ 180/math.pi * ig for ig in  initial_guess ]
         else:
             q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
-        ret, 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)
-        params = rm_inverse_kinematics_params_t(q_ref,
+        params = rm_inverse_kinematics_params_t(q_ref, target, 1)
-                                                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)
+
+        # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
+        #
+        # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
+        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+        if int(ret) < 0:
             return ret, [ q/180*math.pi for q in q_out]
+        elif pose_dis < 0.01:
+            return ret, [ q/180*math.pi for q in q_out]
+        else:
+            return -10, [ q/180*math.pi for q in q_out]
+
+def cal_pose_deviation(pose1, pose2):
+    d_fk_p1 = np.array(pose1) - np.array(pose2)
+    for j in [3, 4, 5]:
+        while d_fk_p1[j] > math.pi:
+            d_fk_p1[j] -= 2 * math.pi
+        while d_fk_p1[j] < -math.pi:
+            d_fk_p1[j] += 2 * math.pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk

kine_ctrl/urdf_rm75/Dual_arm.urdf

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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/workspace_comfortable/Contour_plot.py

@@ -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

@@ -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/workspace_cal.py

@@ -0,0 +1,726 @@
+"""
+RM75-B comfortable workspace evaluator.
+
+You provide:
+    - URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
+    - your own IK solver inside solve_ik()
+
+This script computes:
+    - IK success rate
+    - joint-limit comfort
+    - manipulability
+    - singularity / condition number score
+    - final comfort score
+
+Recommended install:
+    pip install numpy scipy urdfpy pandas matplotlib tqdm
+
+Optional:
+    pip install plotly
+"""
+
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+
+from tqdm import tqdm
+from scipy.spatial.transform import Rotation as R
+from urdfpy import URDF
+
+import sys
+from pathlib import Path
+
+# 1. Get the absolute path of the directory containing this current script
+current_dir = Path(__file__).resolve().parent
+
+# 2. Get the parent (upper) directory
+parent_dir = current_dir.parent
+
+# 3. Add the parent directory to the system path
+sys.path.insert(0, str(parent_dir))
+
+from rm75_kine_qp import KinematicsSolver as kine_qp
+from rm75_kine_rm import rm75_kine_api as kine_rm
+from rm75_mjc import MuJoCoPositionController
+from Robotic_Arm.rm_robot_interface import *
+
+import time
+from math import radians, degrees, pi, cos, sin
+
+# Cartesian workspace grid, in meters.
+# Adjust according to your robot placement and task.
+X_RANGE = (-0.7, 0.7)
+Y_RANGE = (-0.7, 0.7)
+Z_RANGE = (-0.10, 0.8)
+
+GRID_RESOLUTION = 0.05  # 5 cm. Use 0.02 for finer but slower.
+
+num_orientations =  120
+
+tool_name = "scissor"
+
+
+
+URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-SCI.urdf'
+
+output_csv = "workspace"  + tool_name + URDF_PATH.split('/')[-1].split('.')[0] + ".csv"
+
+
+# Comfort thresholds
+MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
+MAX_CONDITION_NUMBER = 150.0
+MIN_MANIPULABILITY_RATIO = 0.10
+
+# Scoring weights
+WEIGHT_IK_SUCCESS = 0.70
+WEIGHT_JOINT_LIMIT = 0.10
+WEIGHT_MANIPULABILITY = 0.1
+WEIGHT_SINGULARITY = 0.1
+
+
+
+
+#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
+#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
+tools_in_ee = {
+    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'v_minis': np.array([[0.0, 0.1, 0.1, -np.sqrt(2) * 0.5, 0.0, 0.0, np.sqrt(2) * 0.5],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
+}
+
+# joint limit
+# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
+# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
+#
+#
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
+
+
+MESH_DIR = str(Path(URDF_PATH).parent)
+
+#  -----------      rm75 qp based kine          ------------
+robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR, tcps=["scissor_tcp", "camera_tcp"])
+robot_kine_qp.add_tool_frames(tools_in_ee)
+robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+#   ----------      rm75 official algorithm         -----------
+robot_kine_rm = kine_rm()
+robot_kine_rm.add_tool_frames(tools_in_ee)
+robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+
+
+# ============================================================
+# 1. USER SETTINGS
+# ============================================================
+
+
+
+BASE_LINK = "base_link"
+TCP_LINK = "link_7"
+
+JOINT_NAMES = [
+    "joint_1",
+    "joint_2",
+    "joint_3",
+    "joint_4",
+    "joint_5",
+    "joint_6",
+    "joint_7",
+]
+
+
+
+# Numerical Jacobian settings
+JACOBIAN_EPS = 1e-5
+
+
+
+# ============================================================
+# 2. TASK ORIENTATION SAMPLING
+# ============================================================
+
+def make_task_orientations(num_orientations=num_orientations, seed=1):
+    """
+    Random orientation sampling using RM's Euler convention:
+
+        R = Rz @ Ry @ Rx
+
+    Note:
+        This samples Euler angles randomly.
+        It is useful, but not perfectly uniform over SO(3).
+    """
+
+    rng = np.random.default_rng(seed)
+
+    orientations = []
+
+    for _ in range(num_orientations):
+        rx = rng.uniform(-np.pi, np.pi)
+        ry = rng.uniform(-np.pi / 2.0, np.pi / 2.0)
+        rz = rng.uniform(-np.pi, np.pi)
+
+        orientations.append([rx, ry, rz])
+
+    return orientations
+
+
+# ============================================================
+# 3.  IK FUNCTION GOES HERE
+# ============================================================
+
+def solve_ik(target_position, target_rotation):
+    """
+    Replace this function with your own IK solver.
+
+    Parameters
+    ----------
+    target_position : np.ndarray, shape (3,)
+        Desired TCP position in base_link frame.
+
+    target_rotation : np.ndarray, shape (3, 3)
+        Desired TCP rotation matrix in base_link frame.
+
+    Returns
+    -------
+    None
+        If IK fails.
+
+    or
+
+    np.ndarray, shape (7,)
+        One IK solution.
+
+    or
+
+    list[np.ndarray]
+        Multiple IK solutions.
+
+    Important:
+        Joint order must be:
+        [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
+    """
+
+    initial_guess = [0.1] * 7
+
+    ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250)
+    # print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
+    if ret_qp == 0:
+        if not robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True):
+            return q
+
+
+    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
+    # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
+    if ret_rm == 0:
+        if not robot_kine_qp.collision_detect(q, stop_at_first_collision=True, verbose=True):
+            return q
+
+
+    return None
+
+
+# ============================================================
+# 4. URDF / FK UTILITIES
+# ============================================================
+
+def load_robot_and_limits(urdf_path):
+    robot = URDF.load(urdf_path)
+
+    joints = []
+    lower = []
+    upper = []
+
+    joint_map = {j.name: j for j in robot.joints}
+
+    for name in JOINT_NAMES:
+        joint = joint_map[name]
+        joints.append(joint)
+
+        if joint.limit is None:
+            raise ValueError(f"Joint {name} has no limit in URDF.")
+
+        lower.append(joint.limit.lower)
+        upper.append(joint.limit.upper)
+
+    lower = np.asarray(lower, dtype=float)
+    upper = np.asarray(upper, dtype=float)
+
+    return robot, lower, upper
+
+
+# def q_to_cfg(q):
+#     """
+#     Convert joint vector to urdfpy FK config dictionary.
+#     """
+#     return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
+
+
+# def fk_transform(robot, q):
+#     """
+#     Forward kinematics from base_link to TCP_LINK.
+#
+#     Returns
+#     -------
+#     T : np.ndarray, shape (4, 4)
+#     """
+#     cfg = q_to_cfg(q)
+#     fk = robot.link_fk(cfg=cfg)
+#     tcp_link = robot.link_map[TCP_LINK]
+#     return fk[tcp_link]
+#
+#
+# def fk_position(robot, q):
+#     T = fk_transform(robot, q)
+#     return T[:3, 3]
+
+
+# ============================================================
+# 5. COMFORT METRICS
+# ============================================================
+
+def is_within_joint_limits(q, lower, upper, tol=1e-8):
+    q = np.asarray(q)
+    return np.all(q >= lower - tol) and np.all(q <= upper + tol)
+
+
+def joint_limit_score(q, lower, upper):
+    """
+    Score in [0, 1].
+    1 means every joint is at center of its range.
+    0 means at least one joint is at its limit.
+    """
+
+    q = np.asarray(q)
+    mid = 0.5 * (lower + upper)
+    half_range = 0.5 * (upper - lower)
+
+    per_joint_score = 1.0 - np.abs(q - mid) / half_range
+    per_joint_score = np.clip(per_joint_score, 0.0, 1.0)
+
+    # Conservative: one bad joint makes the whole pose less comfortable.
+    return float(np.min(per_joint_score))
+
+
+def joint_margin(q, lower, upper):
+    """
+    Minimum normalized distance to joint limits.
+
+    0.15 means the closest joint is 15% away from its limit.
+    """
+    q = np.asarray(q)
+    margin_lower = (q - lower) / (upper - lower)
+    margin_upper = (upper - q) / (upper - lower)
+    margin = np.minimum(margin_lower, margin_upper)
+    return float(np.min(margin))
+
+
+def q_to_cfg(q):
+    """
+    Convert joint vector to urdfpy FK config dictionary.
+    """
+    return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
+
+
+def fk_transform(robot, q):
+    """
+    Forward kinematics from base_link to TCP_LINK.
+
+    Returns
+    -------
+    T : np.ndarray, shape (4, 4)
+    """
+    cfg = q_to_cfg(q)
+    fk = robot.link_fk(cfg=cfg)
+    tcp_link = robot.link_map[TCP_LINK]
+    return fk[tcp_link]
+
+
+def numerical_geometric_jacobian(robot, q, eps=1e-5):
+    """
+    Numerical 6D geometric-like Jacobian, shape (6, 7).
+
+    Top 3 rows:
+        linear velocity approximation
+
+    Bottom 3 rows:
+        angular velocity approximation as rotation-vector difference
+
+    This is useful for manipulability and singularity checks.
+    """
+    q = np.asarray(q, dtype=float)
+    n = len(q)
+
+    J = np.zeros((6, n))
+
+    T0 = fk_transform(robot, q)
+    p0 = T0[:3, 3]
+    R0 = T0[:3, :3]
+
+    for i in range(n):
+        q_plus = q.copy()
+        q_minus = q.copy()
+
+        q_plus[i] += eps
+        q_minus[i] -= eps
+
+        T_plus = fk_transform(robot, q_plus)
+        T_minus = fk_transform(robot, q_minus)
+
+        p_plus = T_plus[:3, 3]
+        p_minus = T_minus[:3, 3]
+
+        R_plus = T_plus[:3, :3]
+        R_minus = T_minus[:3, :3]
+
+        # Linear part
+        J[:3, i] = (p_plus - p_minus) / (2.0 * eps)
+
+        # Angular part
+        # Relative rotation from minus to plus.
+        dR = R_plus @ R_minus.T
+        rotvec = R.from_matrix(dR).as_rotvec()
+        J[3:, i] = rotvec / (2.0 * eps)
+
+    return J
+
+
+def manipulability_score_from_jacobian(J):
+    """
+    Yoshikawa-style manipulability.
+
+    For a 6x7 Jacobian:
+        w = sqrt(det(J J.T))
+
+    To improve numerical robustness, compute from singular values.
+    """
+    singular_values = np.linalg.svd(J, compute_uv=False)
+
+    # Product of singular values.
+    # For a 6x7 Jacobian, there are 6 singular values.
+    w = float(np.prod(singular_values))
+
+    return w
+
+
+def condition_number_from_jacobian(J, min_sigma=1e-9):
+    singular_values = np.linalg.svd(J, compute_uv=False)
+    sigma_max = np.max(singular_values)
+    sigma_min = np.min(singular_values)
+
+    if sigma_min < min_sigma:
+        return np.inf
+
+    return float(sigma_max / sigma_min)
+
+
+def singularity_score(condition_number):
+    """
+    Score in [0, 1].
+    Higher is better.
+
+    condition_number = 1 is ideal.
+    Very large means near singularity.
+    """
+    if not np.isfinite(condition_number):
+        return 0.0
+
+    return float(1.0 / condition_number)
+
+
+# ============================================================
+# 6. IK RESULT HANDLING
+# ============================================================
+
+def normalize_ik_solutions(ik_result):
+    """
+    Your IK returns:
+        - None if failed
+        - one list/array of 7 joint values if successful
+    """
+    if ik_result is None:
+        return []
+
+    q = np.asarray(ik_result, dtype=float).reshape(-1)
+
+    if q.shape[0] != 7:
+        return []
+
+    return [q]
+
+
+def evaluate_single_solution(robot, q, lower, upper):
+    """
+    Evaluate one IK solution.
+
+    Returns a dictionary with metrics.
+    """
+
+    if q.shape[0] != 7:
+        return None
+
+    if not is_within_joint_limits(q, lower, upper):
+        return None
+
+    jl_score = joint_limit_score(q, lower, upper)
+    jl_margin = joint_margin(q, lower, upper)
+
+    J = numerical_geometric_jacobian(robot, q, eps=JACOBIAN_EPS)
+
+    manip = manipulability_score_from_jacobian(J)
+    cond = condition_number_from_jacobian(J)
+    sing_score = singularity_score(cond)
+
+    valid_by_thresholds = (
+        jl_margin >= MIN_JOINT_MARGIN
+        and cond <= MAX_CONDITION_NUMBER
+    )
+
+    return {
+        "q": q,
+        "joint_limit_score": jl_score,
+        "joint_margin": jl_margin,
+        "manipulability": manip,
+        "condition_number": cond,
+        "singularity_score": sing_score,
+        "valid_by_thresholds": valid_by_thresholds,
+    }
+
+
+# ============================================================
+# 7. MAIN WORKSPACE EVALUATION
+# ============================================================
+
+def make_grid():
+    xs = np.arange(X_RANGE[0], X_RANGE[1] + 1e-9, GRID_RESOLUTION)
+    ys = np.arange(Y_RANGE[0], Y_RANGE[1] + 1e-9, GRID_RESOLUTION)
+    zs = np.arange(Z_RANGE[0], Z_RANGE[1] + 1e-9, GRID_RESOLUTION)
+
+    points = []
+
+    for x in xs:
+        for y in ys:
+            for z in zs:
+                points.append(np.array([x, y, z], dtype=float))
+
+    return points
+
+
+def evaluate_workspace():
+    robot, lower, upper = load_robot_and_limits(URDF_PATH)
+    orientations = make_task_orientations()
+    grid_points = make_grid()
+
+    rows = []
+
+    # First pass stores raw manipulability.
+    # Later we normalize manipulability by max observed value.
+    all_valid_solution_metrics = []
+
+    print(f"Loaded robot from: {URDF_PATH}")
+    print(f"Grid points: {len(grid_points)}")
+    print(f"Orientations per point: {len(orientations)}")
+    print("Evaluating IK reachability and raw metrics...")
+
+    for point in tqdm(grid_points):
+        point_solution_metrics = []
+
+        attempted = 0
+        ik_success_count = 0
+
+
+        for rpy in orientations:
+            attempted += 1
+
+
+            ik_result = solve_ik(point, rpy)
+
+            # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
+            candidate_solutions = normalize_ik_solutions(ik_result)
+
+            if len(candidate_solutions) == 0:
+                continue
+
+            evaluated_solutions = []
+
+            for q in candidate_solutions:
+                # pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name)
+                # print(f'the fk of q is {pose}\n')
+                metrics = evaluate_single_solution(robot, q, lower, upper)
+                # print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
+                if metrics is not None:
+                    evaluated_solutions.append(metrics)
+
+            if len(evaluated_solutions) == 0:
+                continue
+
+            ik_success_count += 1
+
+            # Use the best solution for this pose.
+            # At this stage, manipulability is not normalized,
+            # so use joint score + singularity score as temporary ranking.
+            best = max(
+                evaluated_solutions,
+                key=lambda m: 0.6 * m["joint_limit_score"] + 0.4 * m["singularity_score"]
+            )
+
+            point_solution_metrics.append(best)
+            all_valid_solution_metrics.append(best)
+        print(f'this position+all orientations, the point_solution_metrics = {point_solution_metrics}')
+        ik_success_rate = ik_success_count / attempted if attempted > 0 else 0.0
+
+        if len(point_solution_metrics) == 0:
+            rows.append({
+                "x": point[0],
+                "y": point[1],
+                "z": point[2],
+                "ik_success_rate": 0.0,
+                "joint_limit_score": 0.0,
+                "joint_margin": 0.0,
+                "manipulability": 0.0,
+                "manipulability_score": 0.0,
+                "condition_number": np.inf,
+                "singularity_score": 0.0,
+                "comfort_score": 0.0,
+                "comfortable": False,
+                "reachable": False,
+            })
+        else:
+            # Average over task orientations.
+            rows.append({
+                "x": point[0],
+                "y": point[1],
+                "z": point[2],
+                "ik_success_rate": ik_success_rate,
+                "joint_limit_score": np.mean([m["joint_limit_score"] for m in point_solution_metrics]),
+                "joint_margin": np.mean([m["joint_margin"] for m in point_solution_metrics]),
+                "manipulability": np.mean([m["manipulability"] for m in point_solution_metrics]),
+                "manipulability_score": 0.0,  # filled later
+                "condition_number": np.mean([m["condition_number"] for m in point_solution_metrics]),
+                "singularity_score": np.mean([m["singularity_score"] for m in point_solution_metrics]),
+                "comfort_score": 0.0,  # filled later
+                "comfortable": False,
+                "reachable": True,
+            })
+
+    df = pd.DataFrame(rows)
+
+    # Normalize manipulability by maximum observed value.
+    max_manip = df["manipulability"].replace([np.inf, -np.inf], np.nan).max()
+
+    if max_manip is None or not np.isfinite(max_manip) or max_manip <= 0:
+        max_manip = 1.0
+
+    df["manipulability_score"] = df["manipulability"] / max_manip
+    df["manipulability_score"] = df["manipulability_score"].clip(0.0, 1.0)
+
+    # Final comfort score.
+    df["comfort_score"] = (
+        WEIGHT_IK_SUCCESS * df["ik_success_rate"]
+        + WEIGHT_JOINT_LIMIT * df["joint_limit_score"]
+        + WEIGHT_MANIPULABILITY * df["manipulability_score"]
+        + WEIGHT_SINGULARITY * df["singularity_score"]
+    )
+
+    # Comfortable binary classification.
+    df["comfortable"] = (
+        (df["reachable"] == True)
+        & (df["ik_success_rate"] >= 0.80)
+        & (df["joint_margin"] >= MIN_JOINT_MARGIN)
+        & (df["condition_number"] <= MAX_CONDITION_NUMBER)
+        & (df["manipulability_score"] >= MIN_MANIPULABILITY_RATIO)
+    )
+
+    return df
+
+
+# ============================================================
+# 8. PLOTTING
+# ============================================================
+
+def plot_workspace(df):
+    """
+    3D scatter plot:
+        gray/low = low comfort
+        brighter = higher comfort
+    """
+
+    reachable = df[df["reachable"] == True]
+
+    if len(reachable) == 0:
+        print("No reachable points found. Check your IK function.")
+        return
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection="3d")
+
+    sc = ax.scatter(
+        reachable["x"],
+        reachable["y"],
+        reachable["z"],
+        c=reachable["comfort_score"],
+        s=12,
+        alpha=0.8,
+    )
+
+    ax.set_title("RM75-B Comfortable Workspace")
+    ax.set_xlabel("X [m]")
+    ax.set_ylabel("Y [m]")
+    ax.set_zlabel("Z [m]")
+
+    fig.colorbar(sc, ax=ax, label="Comfort score")
+    plt.show()
+
+
+def plot_comfortable_only(df):
+    comfortable = df[df["comfortable"] == True]
+
+    if len(comfortable) == 0:
+        print("No comfortable points found under current thresholds.")
+        return
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection="3d")
+
+    ax.scatter(
+        comfortable["x"],
+        comfortable["y"],
+        comfortable["z"],
+        c=comfortable["comfort_score"],
+        s=16,
+        alpha=0.9,
+    )
+
+    ax.set_title("RM75-B Comfortable Region Only")
+    ax.set_xlabel("X [m]")
+    ax.set_ylabel("Y [m]")
+    ax.set_zlabel("Z [m]")
+
+    plt.show()
+
+
+# ============================================================
+# 9. ENTRY POINT
+# ============================================================
+
+if __name__ == "__main__":
+    df = evaluate_workspace()
+
+    df.to_csv(output_csv, index=False)
+
+    print(f"\nSaved result to: {output_csv}")
+
+    print("\nSummary:")
+    print(f"Total grid points:       {len(df)}")
+    print(f"Reachable points:        {df['reachable'].sum()}")
+    print(f"Comfortable points:      {df['comfortable'].sum()}")
+
+    if df["reachable"].sum() > 0:
+        print(f"Max comfort score:       {df['comfort_score'].max():.3f}")
+        print(f"Mean comfort score:      {df[df['reachable']]['comfort_score'].mean():.3f}")
+
+    plot_workspace(df)
+    plot_comfortable_only(df)