delete unnecessary files.

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

README.md

@@ -1,48 +1,42 @@
 ### This repo is for inverse kinematics and verification
 
-In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
+In this branch, the **integrated** inverse kinematics method is packed into one python class **rm75_kinematics** in ``rm75_kinematics.py``. 
 
-Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
+The user can call it as in `test1.py`, `test2.py`.
 
-Verification is done with Mujoco simulation.
+How to use
 
-Key specifications:
-1. Time consumption.
-2. Success rate
-3. Minial joint variation.
-
-Next:\
-Comparison with Realman official IK method.
-Embedded with current demo.
-
-
-### Comparison (05June2026):
-
-- With current dual arm joint limit, 
+```aiignore
+from  rm75_kinematics import rm75_kinematics
+robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
+                             mesh_dir='./urdf_rm75',
+                             tcps=["scissor_tcp", "camera_tcp"],
+                             tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
 ```
-ub = np.array([150.0, 110.0, 170.0,  130, 175.0, 125.0,  179.0])
-lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0,  -179.0])
+
+Parameter definition:
+
+- urdf_path: the robot description file, ending with `.urdf`.
+- mesh_dir: the robot parts, ending with `.stl`.
+- tcps: the tool central points defined in urdf file, if no, ignore it.
+- tools_in_ee: the installation of different tools attached to the end-effector of the arm (jont7+link7).
+
+
+### Current functions ###
+
+1. `get_ik_result`
+```aiignore
+ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], 
+                                    target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
+                                    initial_guess=[0.1] * 7, tool=tool_name)
 ```
-the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
-At least one solver works out the ik, rate = **74%**.
 
-- With realman-75 physical joint limit,
+2. `get_fk_result`
+```aiignore
+p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
 ```
-ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
-lb = -ub
+
+3. `get_self_collision`
+```aiignore
+self_collision_sts = robot_kine.get_self_collision(q)
 ```
-the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
-At least one solver works out the ik, rate = **84%**.
-
-### update(1st July 2026)
-
-In each iteration, update optimization formula:
-
-- new cost item for distance from middle of the joint range. 
-- set up different weight for different joints motion.
-
-<img src="img/optimization.png" alt="Cost" width="400">
-
-<img src="img/cons.png" alt="Cost" width="400">
-
-<img src="img/osqp.png" alt="Cost" width="400">

kine_ctrl/fix_robotics_env.sh

@@ -1,6 +0,0 @@
-#!/bin/bash
-echo "Fixing robotics environment..."
-conda activate coppeliasim
-export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
-pip install osqp==0.6.2.post8 --force-reinstall
-python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"

kine_ctrl/main.py

@@ -1,140 +0,0 @@
-
-
-#   conda activate coppeliasim
-#   env fix, in terminal:  fix_robotics_env.sh
-
-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
-import numpy as np
-
-#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
-#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
-tools_in_ee = {
-    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
-    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
-    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
-    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
-}
-
-# joint limit
-# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
-# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
-
-
-ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
-lb = -ub
-
-tool_name = "scissor"
-
-def main():
-    """Demonstrate pure position control"""
-
-    # Create controller
-    robot_mjk = MuJoCoPositionController()
-
-
-    #  -----------      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.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)
-
-    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)
-
-    time.sleep(5)
-
-
-
-
-
-#   --------------  for comparison   ----------------
-    print(f'in the comparison part')
-
-    if True:
-
-        result = np.array([[0,0],[0,0]], dtype=np.int32)        # to collect ik result   qp_fk, qp_ik, rm_fk, rm_ik
-
-        solve_sum = 0
-
-        for i in range(1000):
-            print(f'\n-------------- in i = {i} ----------------')
-            joint_rand = np.random.uniform(ub, lb)
-            print(f'the predefined joints are {joint_rand}')
-
-            #   -------------- fk ------------------
-            fk_qp_p1 = robot_kine_qp.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
-
-            fk_rm_p1 = robot_kine_rm.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
-
-            d_fk = cal_pose_deviation(pose1=fk_rm_p1, pose2=fk_qp_p1)
-            print(f'fk_qp_p1 = {fk_qp_p1}, fk_rm_p1 = {fk_rm_p1}, d_fk = {d_fk}\n')
-
-
-            #   ----------- ik ----------------
-            t_p = fk_rm_p1
-            joint_rand_init = np.random.uniform(ub, lb)
-            print(f'the guess is {joint_rand_init}')
-
-            ret_qp, q = robot_kine_qp.inverse_kinematics( target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
-
-            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}')
-                if d_p_ik < 0.01:
-                    result[0][1] += 1
-
-                # robot_mjk.send_command(q)
-                # robot_mjk.wait_until_reached()
-                # robot_mjk.print_state()
-            else:
-                fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
-                d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
-                print(f'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
-
-            ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
-            if ret_rm == 0:
-                fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
-                d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
-                print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
-                if d_p_ik < 0.01:
-                    result[1][1] += 1
-            else:
-                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}')
-
-
-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] > pi:
-            d_fk_p1[j] -= 2 * pi
-        while d_fk_p1[j] < -pi:
-            d_fk_p1[j] += 2 * pi
-    d_fk = np.linalg.norm(d_fk_p1)
-    return d_fk
-
-
-
-if __name__ == "__main__":
-    main()

kine_ctrl/requirements.txt

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

kine_ctrl/rm75_kine_qp.py

@@ -1,824 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import os
-
-import pinocchio as pin
-import numpy as np
-import osqp
-from scipy import sparse
-from math import radians, degrees, pi, cos, sin
-import time
-import threading
-
-
-
-class KinematicsSolver():
-    def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
-        """
-        for realman 75b
-        Initialize robotic arm kinematics using Pinocchio (ROS2 version).
-        unit: m, rad
-        """
-        print(f' ------------  the qp based kinematic initialising   -----------')
-        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
-
-
-
-        self.cfg_j_limit()
-
-        q_range = (
-                self.model.upperPositionLimit[:7] -
-                self.model.lowerPositionLimit[:7]
-        )
-
-        self.w_q_limit = np.diag(1.0 / (q_range ** 2))
-
-        self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
-
-        # ----------  for reused qp_solver  ------------------
-        self.nv = 7
-
-        # Full dense symmetric matrix structure
-        # P_template = np.triu(np.ones((7, 7)))
-        self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
-
-        P_sparse = sparse.csc_matrix(self.P_pattern)
-
-        A_sparse = sparse.eye(7, format='csc')
-
-        self.osqp_solver = osqp.OSQP()
-
-        self.osqp_solver.setup(
-            P=P_sparse,
-            q=np.zeros(7),
-            A=A_sparse,
-            l=-np.ones(7),
-            u=np.ones(7),
-            verbose=False,
-            warm_start=True,
-            polish=False
-        )
-
-        self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
-        # Smaller value => joint moves more actively
-        # Larger value  => joint moves less / more lazy
-        self.joint_motion_weight = np.diag([
-            1.0, 1.0, 1.0, 1.0,
-            0.3, 0.3, 0.2
-        ])
-
-    def add_frame(self,frame_name, position, rotationXYZ):
-        '''
-        :param frame_name:  str
-        :param position: [x, y, z] target position (meters)
-        :param rotationXYZ: [x, y, z] target rotation (rad)
-        '''
-        camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0],  rotationXYZ[1],  rotationXYZ[2] )
-        camera_offset = pin.SE3(
-            camera_rotation,
-            np.array(position)
-        )
-        self.model.addFrame(  pin.Frame(  frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
-
-    def add_tool_frames(self,dict_frames):
-        self.tool_frames ={}
-        for tool_name in dict_frames:
-            tool_attr = dict_frames[tool_name]
-            position = tool_attr[0][0:3]
-            rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
-            self.add_frame(tool_name, position, rotationXYZ)
-            self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)})
-        self.data = self.model.createData()
-
-
-    def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
-        if min_j is None:
-            min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
-        if max_j is None:
-            max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
-        if rad_flag:
-            for i in range(7):
-                self.model.lowerPositionLimit[i] = min_j[i]
-                self.model.upperPositionLimit[i] = max_j[i]
-        else:
-            for i in range(7):
-                self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
-                self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
-
-    def forward_kinematics(self, joint_angles, tool="omnipic"):
-        """
-        Compute forward kinematics.
-
-        Args:
-            joint_angles: List or array of 7 joint angles (radians)
-            tool: Name of frame to compute
-
-        Returns:
-            dict: Position, rotation, rpy, quaternion
-                unit:   position:   m
-                        rpy:        rad
-        """
-        if len(joint_angles) != 7:
-            raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
-
-        # Create configuration vector
-        q = pin.neutral(self.model)
-        for i, angle in enumerate(joint_angles):
-            q[i] = angle
-
-        # Compute forward kinematics
-        pin.forwardKinematics(self.model, self.data, q)
-        pin.updateFramePlacements(self.model, self.data)
-
-        # Get frame transform
-        frame_id = self.tool_frames[tool]
-        frame_transform = self.data.oMf[frame_id]
-
-        # Extract results
-        position = frame_transform.translation.copy()
-        rotation = frame_transform.rotation.copy()
-
-        # Compute RPY
-        rpy = pin.rpy.matrixToRpy(rotation)
-
-        # Compute quaternion
-        # quat = pin.Quaternion(rotation)
-        pose = np.concatenate([position, rpy], axis=0)
-        return pose
-        # return {
-        #     'position': position,
-        #     # 'rotation': rotation,
-        #     'rpy': rpy,
-        #     'quaternion': [quat.x, quat.y, quat.z, quat.w],
-        #     # 'transform': frame_transform
-        # }
-
-    def inverse_kinematics(self, target_position, target_rpy=None,
-                           target_quat=None, initial_guess=None,
-                           max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
-        """
-        Compute inverse kinematics using differential IK with multiple strategies.
-
-        Args:
-            target_position: [x, y, z] target position (meters)
-            target_rpy: [roll, pitch, yaw] target orientation (radians)
-            target_quat: [x, y, z, w] target orientation as quaternion
-            initial_guess: Initial joint angles (radians)
-            max_iter: Maximum iterations
-            tolerance: Error tolerance
-            debug: Print debug information
-            tool: the frame name ('scissor', 'camera', 'ee')
-
-        Returns:
-            tuple: (joint_angles, success, error)
-        """
-        # Build target SE3 placement
-        if target_quat is not None:
-            quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
-            target_rotation = quat.matrix()
-        elif target_rpy is not None:
-            target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
-                                                  target_rpy[1],
-                                                  target_rpy[2])
-        else:
-            target_rotation = np.eye(3)
-
-        target_placement = pin.SE3(target_rotation, np.array(target_position))
-
-        # Try multiple initial guesses
-        initial_guesses = []
-
-        if initial_guess is not None:
-            initial_guesses.append(initial_guess)
-        else:
-            # Try different initial configurations
-            initial_guesses.append([0.1] * 7)  # Zero config
-
-
-        best_solution = None
-        best_error = float('inf')
-
-        for guess_idx, guess in enumerate(initial_guesses):
-            q = pin.neutral(self.model)
-            for i, angle in enumerate(guess):
-                if i < len(q):
-                    q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
-                                   self.model.upperPositionLimit[i])
-            q_ref = q.copy()
-
-            # Differential IK with adaptive damping
-            damping = 0.1
-            damping_reduction = 0.95
-            iter_count = 0
-            prev_error = float('inf')
-
-            ee_frame_id = self.tool_frames[tool]
-
-            J = pin.computeFrameJacobian(
-                self.model,
-                self.data,
-                q,
-                ee_frame_id,
-                pin.ReferenceFrame.LOCAL
-            )
-
-            pin.forwardKinematics(self.model, self.data, q)
-            pin.updateFramePlacements(self.model, self.data)
-
-            current_placement = self.data.oMf[ee_frame_id]
-
-            error_SE3 = current_placement.actInv(target_placement)
-            error_vec = pin.log(error_SE3).vector
-
-            # print("\n initial error =", np.linalg.norm(error_vec))
-            # print(error_vec)
-
-            while iter_count < max_iter:
-                # Compute forward kinematics
-
-                pin.computeJointJacobians(self.model, self.data, q)
-                pin.framesForwardKinematics(self.model, self.data, q)
-
-                # Get current end-effector placement
-                current_placement = self.data.oMf[ee_frame_id]
-
-                # Compute error
-                error_SE3 = current_placement.actInv(target_placement)
-                error_vec = pin.log(error_SE3).vector
-                error_norm = np.linalg.norm(error_vec)
-
-                if error_norm < tolerance:
-                    if error_norm < best_error:
-                        best_error = error_norm
-                        best_solution = q[:7].copy()
-                    break
-
-                # Check if error is increasing (diverging)
-                if error_norm > prev_error * 1.1 and iter_count > 10:
-                    damping = min(1.0, damping * 1.5)
-                else:
-                    damping = max(0.01, damping * damping_reduction)
-
-
-                J = pin.getFrameJacobian(
-                    self.model,
-                    self.data,
-                    ee_frame_id,
-                    pin.ReferenceFrame.LOCAL
-                )
-
-                # =========================
-                # QP-based IK
-                # =========================
-                w_ref = 0.0001
-                w_limit_mid = 0.00002
-
-                J_eff = pin.Jlog6(error_SE3) @ J #J #
-
-                H = J_eff.T @ self.W @ J_eff
-
-
-                H += damping * damping * self.joint_motion_weight
-                H += w_ref * np.eye(7)
-                H += w_limit_mid * self.w_q_limit
-
-                H_triu = sparse.triu(H).tocsc()
-
-                g = -J_eff.T @ self.W @ error_vec
-                g += w_ref * (q[:7] - q_ref[:7])
-                g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
-
-                # -------------------------
-                # Joint velocity constraints
-                # -------------------------
-                dq_limit =  np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ])  # rad per iteration
-
-                lb = -dq_limit * np.ones(7)
-                ub = dq_limit * np.ones(7)
-
-                # -------------------------
-                # Joint position constraints
-                # -------------------------
-
-                q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
-                q_max_step = self.model.upperPositionLimit[:7] - q[:7]
-
-                lb = np.maximum(lb, q_min_step)
-                ub = np.minimum(ub, q_max_step)
-
-                # -------------------------
-                # Solve QP
-                # ------------------------
-                # Update solver
-                self.osqp_solver.update(
-                    Px= H_triu.data, #H[np.triu_indices(7)], #
-                    q=g,
-                    l=lb,
-                    u=ub
-                )
-
-                # Solve
-                result = self.osqp_solver.solve()
-                if result.info.status != 'solved':
-                    break
-
-                dq = result.x
-
-                if dq is None:
-                    break
-
-                # Apply joint limits with scaling
-                alpha = 1.0
-                q = pin.integrate(self.model, q, alpha * dq)
-
-                prev_error = error_norm
-                iter_count += 1
-
-        if best_solution is not None:
-            # return best_solution, True, best_error, iter_count
-            return 0, best_solution.tolist()
-        else:
-            # return q[:7].copy(), False, error_norm, iter_count
-            return -1, q[:7].copy().tolist()
-
-    def quaternion_to_euler(self, q):
-        """
-        Convert quaternion to Euler angles (roll, pitch, yaw)
-
-        Args:
-            qx, qy, qz, qw: quaternion components
-
-        Returns:
-            tuple: (roll, pitch, yaw) in radians
-        """
-        # Roll (x-axis rotation)
-        sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
-        cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
-        roll = np.arctan2(sinr_cosp, cosr_cosp)
-
-        # Pitch (y-axis rotation)
-        sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
-        if abs(sinp) >= 1:
-            pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
-        else:
-            pitch = np.arcsin(sinp)
-
-        # Yaw (z-axis rotation)
-        siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
-        cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
-        yaw = np.arctan2(siny_cosp, cosy_cosp)
-
-        return [roll, pitch, yaw]
-
-    # def invese_kinematics_velocity(self, target_position, target_rpy=None,
-    #                        target_quat=None, initial_guess=None, tool="ee"):
-    #     """
-    #             Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
-    #
-    #             Args:
-    #                 target_position: [x, y, z] target position (meters)
-    #                 target_rpy: [roll, pitch, yaw] target orientation (radians)
-    #                 target_quat: [x, y, z, w] target orientation as quaternion
-    #                 initial_guess: Initial joint angles (radians)
-    #                 tool: the frame name ('scissor', 'camera', 'ee')
-    #
-    #             Returns:
-    #                 joint_velocity: np.array()
-    #             """
-    #     # Build target SE3 placement
-    #     if target_quat is not None:
-    #         quat = pin.Quaternion(target_quat[3], target_quat[0],
-    #                               target_quat[1], target_quat[2])
-    #         target_rotation = quat.matrix()
-    #     elif target_rpy is not None:
-    #         target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
-    #                                               target_rpy[1],
-    #                                               target_rpy[2])
-    #     else:
-    #         target_rotation = np.eye(3)
-    #
-    #     target_placement = pin.SE3(target_rotation, np.array(target_position))
-    #
-    #     # Try multiple initial guesses
-    #     initial_guesses = []
-    #
-    #     if initial_guess is not None:
-    #         initial_guesses.append(initial_guess)
-    #     else:
-    #         # Try different initial configurations
-    #         initial_guesses.append([0.1] * 7)  # Zero config
-    #         initial_guesses.append([radians(30), radians(45), radians(30),
-    #                                 radians(-45), radians(30), radians(-30), 0])
-    #         initial_guesses.append([radians(-30), radians(45), radians(-30),
-    #                                 radians(45), radians(30), radians(30), 0])
-    #
-    #     best_solution = None
-    #     best_error = float('inf')
-    #
-    #     for guess_idx, guess in enumerate(initial_guesses):
-    #         q = pin.neutral(self.model)
-    #         for i, angle in enumerate(guess):
-    #             if i < len(q):
-    #                 q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
-    #                                self.model.upperPositionLimit[i])
-    #
-    #         # Differential IK with adaptive damping
-    #         damping = 0.01
-    #         damping_reduction = 0.95
-    #         iter_count = 0
-    #         prev_error = float('inf')
-    #
-    #         ee_frame_id = self.tool_frames[tool]
-    #
-    #         J = pin.computeFrameJacobian(
-    #             self.model,
-    #             self.data,
-    #             q,
-    #             ee_frame_id,
-    #             pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
-    #         )
-    #
-    #         while iter_count < max_iter:
-    #             # Compute forward kinematics
-    #
-    #             pin.computeJointJacobians(self.model, self.data, q)
-    #             pin.framesForwardKinematics(self.model, self.data, q)
-    #
-    #             # Get current end-effector placement
-    #
-    #             current_placement = self.data.oMf[ee_frame_id]
-    #
-    #             # Compute error
-    #             error_SE3 = current_placement.actInv(target_placement)
-    #             error_vec = pin.log(error_SE3).vector
-    #             error_norm = np.linalg.norm(error_vec)
-    #
-    #             if error_norm < tolerance:
-    #                 joint_angles = q[:7].copy()
-    #                 fk_result = self.forward_kinematics(joint_angles, tool=tool)
-    #                 position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
-    #
-    #                 if position_error < best_error:
-    #                     best_error = position_error
-    #                     best_solution = joint_angles
-    #                 break
-    #
-    #             # Check if error is increasing (diverging)
-    #             if error_norm > prev_error * 1.1 and iter_count > 10:
-    #                 damping = min(1.0, damping * 1.5)
-    #             else:
-    #                 damping = max(0.01, damping * damping_reduction)
-    #
-    #             J = pin.getFrameJacobian(
-    #                 self.model,
-    #                 self.data,
-    #                 ee_frame_id,
-    #                 pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
-    #             )
-    #
-    #             # =========================
-    #             # QP-based IK
-    #             # =========================
-    #
-    #             H = J.T @ self.W @ J
-    #             H += damping * damping * np.eye(7)
-    #
-    #             H_triu = sparse.triu(H).tocsc()
-    #
-    #             g = -J.T @ self.W @ error_vec
-    #
-    #             # -------------------------
-    #             # Joint velocity constraints
-    #             # -------------------------
-    #
-    #             dq_limit = 0.05  # rad per iteration
-    #
-    #             lb = -dq_limit * np.ones(7)
-    #             ub = dq_limit * np.ones(7)
-    #
-    #             # -------------------------
-    #             # Joint position constraints
-    #             # -------------------------
-    #
-    #             q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
-    #             q_max_step = self.model.upperPositionLimit[:7] - q[:7]
-    #
-    #             lb = np.maximum(lb, q_min_step)
-    #             ub = np.minimum(ub, q_max_step)
-    #
-    #             # -------------------------
-    #             # Solve QP
-    #             # ------------------------
-    #             # Update solver
-    #             self.osqp_solver.update(
-    #                 Px=H_triu.data,
-    #                 q=g,
-    #                 l=lb,
-    #                 u=ub
-    #             )
-    #
-    #             # Solve
-    #             result = self.osqp_solver.solve()
-    #
-    #             if result.info.status != 'solved':
-    #                 break
-    #
-    #             dq = result.x
-    #
-    #             if dq is None:
-    #                 break
-    #
-    #             # Apply joint limits with scaling
-    #             alpha = 0.5
-    #             q = pin.integrate(self.model, q, alpha * dq)
-    #
-    #             prev_error = error_norm
-    #             iter_count += 1
-    #
-    #     if best_solution is not None:
-    #         return best_solution, True, best_error
-    #     else:
-    #         return None, False, None
-
-    def compute_jacobian(self, joint_angles, tool="ee"):
-        """Compute geometric Jacobian (6x7)"""
-        q = pin.neutral(self.model)
-        for i, angle in enumerate(joint_angles):
-            q[i] = angle
-
-        pin.forwardKinematics(self.model, self.data, q)
-        pin.updateFramePlacements(self.model, self.data)
-        ee_frame_id = self.tool_frames[tool]
-        J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id)
-
-        return J
-
-    def get_subchain_jacobian(self,
-                              joint_angles,
-                              frame_names
-                              ):
-
-        q = pin.neutral(self.model)
-
-        all_active_joints = self.get_active_joints_from_frame(frame_names)
-
-        for i in range(7):
-            q[i] = joint_angles[i]
-
-        pin.forwardKinematics(self.model, self.data, q)
-        pin.updateFramePlacements(self.model, self.data)
-        pin.computeJointJacobians(self.model, self.data, q)
-
-        Js = []
-
-        for  frame_name, active_joints in zip(frame_names, all_active_joints):
-            frame_id = self.model.getFrameId(frame_name)
-
-            J = pin.getFrameJacobian(
-                self.model,
-                self.data,
-                frame_id,
-                pin.ReferenceFrame.LOCAL
-            )
-            Js.append(J[:, active_joints])
-
-        return Js
-
-    def get_active_joints_from_frame(self, frame_names):
-        """
-        Return active joint indices affecting a frame.
-
-        Example:
-            frame_name='link_4'
-            -> [0,1,2,3]
-        """
-        all_active_joint_ids = []
-        for frame_name in frame_names:
-            frame_id = self.model.getFrameId(frame_name)
-
-            # Parent joint of this frame
-            joint_id = self.model.frames[frame_id].parentJoint
-
-            print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
-
-            active_joint_ids = []
-
-
-            # Traverse upward to root
-            while joint_id > 0:
-                # Pinocchio joint indexing:
-                # universe joint = 0
-                # robot joints start from 1
-
-                active_joint_ids.append(joint_id - 1)
-
-                # Move to parent joint
-                joint_id = self.model.parents[joint_id]
-
-            # Reverse so order becomes base -> tip
-            active_joint_ids.reverse()
-            all_active_joint_ids.append(active_joint_ids)
-
-        return all_active_joint_ids
-
-    def plan_cartesian_trajectory(self, start_pos, end_pos,
-                                  start_rpy=None, end_rpy=None,
-                                  num_steps=20, tool='ee'):
-        """
-        Plan a Cartesian trajectory with IK for each waypoint.
-        """
-        # Get current end-effector pose if start_rpy not provided
-        if start_rpy is None:
-            # Try to find a valid starting configuration
-            test_angles = [0.1] * 7
-            fk_test = self.forward_kinematics(test_angles,tool=tool)
-            start_rpy = fk_test['rpy']
-
-        if end_rpy is None:
-            end_rpy = start_rpy
-
-        # First, check if target is reachable
-        print(f"\nChecking if target is reachable...")
-        target_pos = end_pos
-        target_rpy = end_rpy
-
-        test_solution, success, error = self.inverse_kinematics(
-            target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
-        )
-
-        if not success:
-            print(f"Warning: Target may be unreachable or difficult to reach")
-            print(f"Trying with relaxed tolerance...")
-
-        # Initial guess for IK (start with zero configuration)
-        current_angles = [0.1] * 7
-        trajectory = []
-
-        print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
-        print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
-        print("-" * 60)
-
-        for i in range(num_steps + 1):
-            t = i / num_steps
-
-            # Interpolate position
-            pos = [
-                start_pos[0] + t * (end_pos[0] - start_pos[0]),
-                start_pos[1] + t * (end_pos[1] - start_pos[1]),
-                start_pos[2] + t * (end_pos[2] - start_pos[2])
-            ]
-
-            # Interpolate orientation
-            rpy = [
-                start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
-                start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
-                start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
-            ]
-
-            # Compute IK
-            joint_angles, success, error = self.inverse_kinematics(
-                pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
-            )
-
-            if not success:
-                print(f"  Waypoint {i}: IK failed!")
-                break
-
-            # Verify
-            fk_verify = self.forward_kinematics(joint_angles, tool=tool)
-
-            trajectory.append({
-                'step': i,
-                't': t,
-                'position': pos,
-                'rpy': rpy,
-                'joint_angles': joint_angles,
-                'actual_position': fk_verify['position'],
-                'error': error
-            })
-
-            # Update current angles for next iteration
-            current_angles = joint_angles
-
-            if i % 5 == 0 or i == num_steps:
-                print(f"  Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
-                      f"error={error:.6f}m")
-
-        return trajectory
-
-
-
-def main():
-    """Main test function"""
-
-
-    rm75 = KinematicsSolver()
-
-    # Test 1: Forward Kinematics
-    print("\n1. Forward Kinematics Test")
-    print("-" * 40)
-
-    tool_name = "scissor"
-    joint_angles_zero = [0.1] * 7
-    fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
-
-    print(f"Init configuration:")
-    print(f"  Position: ({fk_result['position'][0]:.3f}, "
-          f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
-
-    # Test 2: Inverse Kinematics with more reachable target
-    print("\n2. Inverse Kinematics Test")
-    print("-" * 40)
-
-    # Try a simpler target first
-    target_pos = [0.3, 0.2, 0.4]  # More reachable position
-    target_rpy = [0.0, 0.0, radians(45)]  # Simpler orientation
-
-    print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
-
-    import time
-    init_joints = [0.2] * 7
-    time0 = time.time()
-    for ii in range(100):
-        joint_solution, success, error = rm75.inverse_kinematics(
-            target_pos, target_rpy=target_rpy, initial_guess=init_joints,
-            max_iter=500, debug=False, tool=tool_name
-        )
-    time1 = time.time()
-    print(f"Time: {time1 - time0}")
-
-    if success:
-        print(f"✓ Solution found! Error: {error:.6f} m")
-        for i, angle in enumerate(joint_solution):
-            print(f"  Joint {i + 1}: {degrees(angle):7.2f}°")
-
-        # Verify
-        fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name)
-        print(
-            f"  Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
-    else:
-        print("✗ IK failed to find a solution!")
-
-    # Test 3: Jacobian
-    print("\n3. Jacobian Matrix")
-    print("-" * 40)
-
-    J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name)
-    print(f"Jacobian shape: {J.shape}")
-    for i in range(min(3, J.shape[0])):
-        row_str = "  ".join([f"{J[i, j]:7.3f}" for j in range(7)])
-        print(f"  Row {i + 1}: {row_str}")
-
-    # Test 4: Trajectory Planning with reachable positions
-    print("\n4. Cartesian Trajectory Planning")
-    print("-" * 40)
-
-    start_pos = [0.3, 0.0, 0.4]  # Start position
-    end_pos = [0.3, 0.0, 0.55]  # End position (smaller movement)
-
-    fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
-
-    trajectory = rm75.plan_cartesian_trajectory(
-        start_pos,
-        end_pos,
-        start_rpy=fk0['rpy'],
-        end_rpy=[
-            fk0['rpy'][0] + radians(10),
-            fk0['rpy'][1],
-            fk0['rpy'][2]
-        ],
-        num_steps=10,
-        tool=tool_name
-    )
-
-    if trajectory:
-        print(f"\n✓ Generated {len(trajectory)} waypoints")
-
-    if success:
-        print("✓ Inverse kinematics working (with simplified target)")
-    else:
-        print("⚠ Inverse kinematics may need tuning - try different targets")
-
-
-    print("\n" + "=" * 60)
-    print(f'test subchain Jacobian, for future obstacle avoidance')
-    frame_names = [
-        "link_2",
-        "link_4",
-        "link_7"
-    ]
-    Js_sub = rm75.get_subchain_jacobian(
-        joint_angles=joint_angles_zero,
-        frame_names=frame_names
-    )
-    print(f'Js_sub: {Js_sub}')
-
-    return rm75, trajectory
-
-
-if __name__ == "__main__":
-    rm75, trajectory = main()
-
-    print("\n" + "=" * 60)
-    print("All tests completed!")
-    print("=" * 60)

kine_ctrl/rm75_kine_rm.py

@@ -1,156 +0,0 @@
-
-from Robotic_Arm.rm_robot_interface import *
-import numpy as np
-import math
-
-class rm75_kine_api():
-    def __init__(self):
-        #   ----------      rm75 official algorithm         -----------
-        print(f'-------  the realman official kinematic initialising -------')
-        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 Robotic arm
-        force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
-        # Initialize the robotic arm model and sensor type in the algorithm
-        self.robot_kine_rm = Algo(arm_model, force_type)
-
-        self.cfg_j_limit()
-
-        self.work_frames = {
-            'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
-        }
-
-        self.tool_name = "no_tool"
-        self.work_name = "work"
-
-    def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
-        if max_j is None:
-            max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
-        if min_j is None:
-            min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159  ])
-
-        max_j = np.array(max_j)
-        min_j = np.array(min_j)
-        if rad_flag:
-            self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
-            self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
-        else:
-            self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
-            self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
-
-    def cfg_work_frame(self , frame_name):
-        self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
-
-    def get_work_frame(self):
-        return self.robot_kine_rm.rm_algo_get_curr_workframe()
-
-    def cfg_tool_frame(self, frame_name ):
-        self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
-
-    def get_tool_frame(self):
-        return self.robot_kine_rm.rm_algo_get_curr_toolframe()
-
-    def quaternion_to_euler(self, q):
-        """
-        Convert quaternion to Euler angles (roll, pitch, yaw)
-
-        Args:
-            qx, qy, qz, qw: quaternion components
-
-        Returns:
-            tuple: (roll, pitch, yaw) in radians
-        """
-        # Roll (x-axis rotation)
-        sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
-        cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
-        roll = np.arctan2(sinr_cosp, cosr_cosp)
-
-        # Pitch (y-axis rotation)
-        sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
-        if abs(sinp) >= 1:
-            pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
-        else:
-            pitch = np.arcsin(sinp)
-
-        # Yaw (z-axis rotation)
-        siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
-        cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
-        yaw = np.arctan2(siny_cosp, cosy_cosp)
-
-        return [roll, pitch, yaw]
-
-    def add_tool_frames(self, dict_frames):
-        self.tool_frames = {}
-        for tool_name in dict_frames:
-            tool_attr = dict_frames[tool_name]
-            position = tool_attr[0][0:3]
-            rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
-            f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
-
-            self.tool_frames.update({tool_name:f})
-
-    def forward_kinematics(self, joint_angles, flag  = 1  , tool="omnipic", work="work"):
-        '''
-        :param joint_angles:   list of joint values, in rad
-        :param flag:       0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
-        :param return:      [x,y,z,rx,ry,rz], m & rad
-        '''
-        if tool != self.tool_name:
-            self.tool_name = tool
-            self.cfg_tool_frame(tool)
-        if work != self.work_name:
-            self.work_name = work
-            self.cfg_work_frame(work)
-
-        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"):
-        '''
-        :param target_position:     list of position values, m
-        :param target_rpy:          list of rpy values, rad
-        :param initial_guess:       initial guess of angles, rad
-        :param tool:                tool name, refer to self.tool_frames
-        :param work:                work name, refer to self.work_frames
-
-        return  ret:                 state of ik calculation, 0:success, -2: out of workspace
-                [q_]:               the ik calculated angles for joints, rad
-        '''
-        if tool != self.tool_name:
-            self.tool_name = tool
-            self.cfg_tool_frame(tool)
-        if work != self.work_name:
-            self.work_name = work
-            self.cfg_work_frame(work)
-
-        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)
-        # print(f'the arm angle is ret = {ret}, and phi = {phi}')
-        params = rm_inverse_kinematics_params_t(q_ref,
-                                                target, 1)
-        ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
-        pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
-        pose_dis = cal_pose_deviation(pose_fk, target)
-
-        # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
-        #
-        # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
-        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
-        if int(ret) < 0:
-            return ret, [ q/180*math.pi for q in q_out]
-        elif pose_dis < 0.01:
-            return ret, [ q/180*math.pi for q in q_out]
-        else:
-            return -10, [ q/180*math.pi for q in q_out]
-
-def cal_pose_deviation(pose1, pose2):
-    d_fk_p1 = np.array(pose1) - np.array(pose2)
-    for j in [3, 4, 5]:
-        while d_fk_p1[j] > math.pi:
-            d_fk_p1[j] -= 2 * math.pi
-        while d_fk_p1[j] < -math.pi:
-            d_fk_p1[j] += 2 * math.pi
-    d_fk = np.linalg.norm(d_fk_p1)
-    return d_fk

kine_ctrl/rm75_mjc.py

@@ -1,297 +0,0 @@
-#!/usr/bin/env python3
-"""
-Pure Position Control for MuJoCo - No velocity commands, no forces
-Direct joint position control with smoothing
-"""
-
-import mujoco
-import mujoco.viewer
-import numpy as np
-import threading
-import time
-from pathlib import Path
-
-
-class MuJoCoPositionController:
-    """
-    Pure position control - directly sets joint positions
-    No velocity commands, no forces - completely stable
-    """
-
-    def __init__(self, urdf_path="./urdf_rm75/RM75-B.urdf", smoothness=0.05, enable_viewer=True):
-        """
-        Args:
-            urdf_path: Path to URDF file
-            smoothness: Motion smoothness (0.02=very smooth, 0.1=fast)
-            enable_viewer: Show MuJoCo viewer
-        """
-        # Load model
-        self.model = mujoco.MjModel.from_xml_path(urdf_path)
-        self.data = mujoco.MjData(self.model)
-
-        self.time_interval = 0.02
-
-        print(f'time interval: {self.model.opt.timestep}')
-
-        # Robot info
-        self.n_joints = self.model.njnt
-
-        # Get joint limits
-        self.joint_lower_limits = []
-        self.joint_upper_limits = []
-        for i in range(self.n_joints):
-            self.joint_lower_limits.append(self.model.jnt_range[i, 0])
-            self.joint_upper_limits.append(self.model.jnt_range[i, 1])
-
-        print(f"Loaded robot: {self.n_joints} joints")
-        for i in range(self.n_joints):
-            print(
-                f"  {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
-
-        # Target joint angles (in radians)
-        self.target_joints = self.data.qpos[:self.n_joints].copy()
-
-        # Smoothing factor (0-1, lower = smoother)
-        self.smoothness = smoothness
-
-        # Thread safety
-        self.command_lock = threading.Lock()
-        self.feedback_lock = threading.Lock()
-        self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
-
-        self.max_ang_inc = 0.02
-
-        # Control flags
-        self.running = False
-        self.simulation_thread = None
-
-        # Viewer
-        self.viewer = None
-        if enable_viewer:
-            try:
-                self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
-                print("Viewer launched")
-            except Exception as e:
-                print(f"Viewer warning: {e}")
-        self.start()
-
-    def start(self):
-        """Start the simulation thread"""
-        if self.running:
-            return
-
-        self.running = True
-        self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
-        self.simulation_thread.start()
-        print("Simulation thread started")
-
-    def stop(self):
-        """Stop the simulation thread"""
-        self.running = False
-        if self.simulation_thread:
-            self.simulation_thread.join(timeout=2.0)
-        if self.viewer:
-            self.viewer.close()
-        print("Simulation stopped")
-
-    def send_command(self, joint_positions):
-        """
-        Send target joint positions
-
-        Args:
-            joint_positions: Array of target joint angles (radians)
-        """
-        cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64)
-
-        # Apply joint limits
-        for i in range(self.n_joints):
-            cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
-
-        with self.command_lock:
-            self.target_joints = cmd
-
-    def get_feedback(self):
-        """Get current joint positions"""
-        with self.feedback_lock:
-            return self.current_feedback_joint.copy()
-
-    def get_target(self):
-        """Get current target positions"""
-        with self.command_lock:
-            return self.target_joints.copy()
-
-    def _simulation_loop(self):
-        """
-        Main simulation loop - PURE POSITION CONTROL
-        No velocity commands, no forces - just direct position setting
-        """
-        last_time = time.time()
-
-        # For smooth interpolation
-        current_joints = self.data.qpos[:self.n_joints].copy()
-
-        while self.running:
-            # Get target command
-            with self.command_lock:
-                target = self.target_joints.copy()
-
-            # Get current positions
-            current_joints = self.data.qpos[:self.n_joints].copy()
-
-            # Smooth interpolation toward target
-            # This creates natural motion without velocity commands
-            alpha = self.smoothness
-
-            next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
-
-            # DIRECT POSITION CONTROL - Set joint positions
-            self.data.qpos[:self.n_joints] = next_joints
-
-            # IMPORTANT: Set velocities to zero to prevent physics from moving joints
-            # This ensures pure kinematic control
-            self.data.qvel[:self.n_joints] = 0
-
-            # Step physics (this will apply gravity, collisions, etc. to other bodies)
-            mujoco.mj_step(self.model, self.data)
-
-            # After step, ensure our joint positions are maintained
-            # (Physics might have altered them slightly)
-            self.data.qpos[:self.n_joints] = next_joints
-            self.data.qvel[:self.n_joints] = 0
-
-            # Update feedback
-            with self.feedback_lock:
-                self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
-
-            # Sync viewer
-            if self.viewer:
-                self.viewer.sync()
-
-            # Maintain real-time speed
-            elapsed = time.time() - last_time
-            sleep_time = self.time_interval - elapsed
-            if sleep_time > 0:
-                time.sleep(sleep_time)
-            last_time = time.time()
-
-    def move_to_joints(self, target, duration=1.0):
-        """
-        Move to target joints over specified duration
-
-        Args:
-            target: Target joint joints
-            duration: Time to complete movement (seconds)
-        """
-        start_js = self.get_feedback()
-        end_js = np.array(target[:self.n_joints])
-
-        # Apply limits
-        for i in range(self.n_joints):
-            end_js[i] = np.clip(end_js[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
-
-        n_steps = int(duration / self.time_interval)
-
-        print(f"  Moving over {duration}s ({n_steps} steps)")
-
-        for step in range(n_steps):
-            alpha = (step + 1) / n_steps
-            # Use easing for smoother motion
-            ease_alpha = 1 - (1 - alpha) ** 2  # Quadratic ease-out
-            current_target = start_js + ease_alpha * (end_js - start_js)
-            self.send_command(current_target)
-            time.sleep(self.time_interval)
-
-        # Ensure exact target
-        self.send_command(end_js)
-        time.sleep(0.1)
-
-    def wait_until_reached(self, tolerance=0.01, timeout=10.0):
-        """
-        Wait until robot reaches target position
-
-        Args:
-            tolerance: Position error tolerance (radians)
-            timeout: Maximum wait time (seconds)
-        """
-        start_time = time.time()
-
-        while time.time() - start_time < timeout:
-            current = self.get_feedback()
-            target = self.get_target()
-            error = np.max(np.abs(target - current))
-
-            if error < tolerance:
-                return True
-
-            time.sleep(0.01)
-
-        return False
-
-    def print_state(self):
-        """Print current robot state"""
-        joints = self.get_feedback()
-        target = self.get_target()
-        print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
-        print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
-
-
-# Demo
-def demo_position_control():
-    """Demonstrate pure position control"""
-
-    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
-
-    if not Path(urdf_path).exists():
-        print(f"Error: URDF not found at {urdf_path}")
-        return
-
-    print("=" * 60)
-    print("Pure Position Control Demo")
-    print("=" * 60)
-
-    # Create controller
-    robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
-    robot.start()
-    time.sleep(1)
-
-    print("\n[Test 1] Move joint 1 to 45 degrees")
-    robot.send_command([0.785, 0, 0, 0, 0, 0, 0])
-    robot.wait_until_reached()
-    robot.print_state()
-    time.sleep(0.5)
-
-    print("\n[Test 2] Move joint 2 to -30 degrees")
-    robot.send_command([0, -0.524, 0, 0, 0, 0, 0])
-    robot.wait_until_reached()
-    robot.print_state()
-    time.sleep(0.5)
-
-    print("\n[Test 3] Move multiple joints simultaneously")
-    robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
-    robot.wait_until_reached()
-    robot.print_state()
-    time.sleep(0.5)
-
-    print("\n[Test 4] Return home")
-    robot.send_command([0, 0, 0, 0, 0, 0, 0])
-    robot.wait_until_reached()
-    robot.print_state()
-
-    print("\n" + "=" * 60)
-    print("✓ All tests passed! Robot is stable and controllable.")
-    print("=" * 60)
-    print("\nInteractive mode - close viewer to exit")
-
-    try:
-        while robot.viewer and robot.viewer.is_running():
-            time.sleep(0.1)
-    except KeyboardInterrupt:
-        pass
-
-    robot.stop()
-
-
-
-
-if __name__ == "__main__":
-    demo_position_control()

kine_ctrl/workspace_comfortable/workspace_cal.py

@@ -1,749 +0,0 @@
-"""
-RM75-B comfortable workspace evaluator.
-
-You provide:
-    - URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
-    - your own IK solver inside solve_ik_user()
-
-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.6, 0.6)
-Y_RANGE = (-0.6, 0.6)
-Z_RANGE = (0.0, 0.8)
-
-GRID_RESOLUTION = 0.3  # 5 cm. Use 0.02 for finer but slower.
-
-# Comfort thresholds
-MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
-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),
-    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
-}
-
-# joint limit
-# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
-# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
-#
-#
-ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
-lb = -ub
-
-tool_name = "no_tool"
-
-URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
-MESH_DIR = str(Path(URDF_PATH).parent)
-
-#  -----------      rm75 qp based kine          ------------
-robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR)
-robot_kine_qp.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=200, 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
-
-#
-# def euler_angles_to_rotation_matrix(rx, ry, rz):
-#     """
-#     Official RM convention:
-#         R = Rz @ Ry @ Rx
-#     This matches scipy:
-#         Rotation.from_euler("xyz", [rx, ry, rz]).as_matrix()
-#     """
-#     Rx = np.array([
-#         [1, 0, 0],
-#         [0, np.cos(rx), -np.sin(rx)],
-#         [0, np.sin(rx),  np.cos(rx)]
-#     ])
-#
-#     Ry = np.array([
-#         [ np.cos(ry), 0, np.sin(ry)],
-#         [0,           1, 0],
-#         [-np.sin(ry), 0, np.cos(ry)]
-#     ])
-#
-#     Rz = np.array([
-#         [np.cos(rz), -np.sin(rz), 0],
-#         [np.sin(rz),  np.cos(rz), 0],
-#         [0,           0,          1]
-#     ])
-#
-#     return Rz @ Ry @ Rx
-
-
-# ============================================================
-# 3. YOUR IK FUNCTION GOES HERE
-# ============================================================
-
-def solve_ik_user(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]
-    """
-
-    # ========================================================
-    # INSERT YOUR IK CODE HERE
-    # ========================================================
-    initial_guess = [0.1] * 7
-
-    ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250)
-    # print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
-    if ret_qp == 0:
-        return q
-
-
-    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
-    # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
-    if ret_rm == 0:
-        pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
-        # print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}')
-        return q
-
-
-    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
-
-            # print(f"\n - target point: {point}, target orientation: {rpy}")
-            rpy = [1.2022060487764064, -1.0097962261845583, -0.6518417572686532]
-            ik_result = solve_ik_user(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()
-
-    output_csv = "rm75b_comfort_workspace.csv"
-    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)

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

test1.py

@@ -0,0 +1,48 @@
+
+
+#   conda activate coppeliasim
+#   env fix, in terminal:  fix_robotics_env.sh
+
+from  rm75_kinematics import rm75_kinematics
+
+from math import pi
+import numpy as np
+
+#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
+#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
+tools_in_ee = {
+    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
+}
+
+# joint limit
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
+
+tool_name = "scissor"
+
+def main():
+    """Demonstrate pure position control"""
+
+    # Create controller
+
+    robot_kine = rm75_kinematics(urdf_path='urdf_rm75/RM75-SCI.urdf', mesh_dir='urdf_rm75',
+                                 tcps=["scissor_tcp", "camera_tcp"], tools_in_ee=tools_in_ee, min_j=lb, max_j=ub)
+
+    ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool=tool_name)
+
+    self_collision_sts = robot_kine.get_self_collision(q)
+
+    p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
+
+    print(f'self_collision_sts: {self_collision_sts}')
+
+
+
+
+
+if __name__ == "__main__":
+    main()

test2.py

@@ -0,0 +1,90 @@
+
+
+#   conda activate coppeliasim
+#   env fix, in terminal:  fix_robotics_env.sh
+
+from rm75_kinematics import rm75_kinematics
+
+from math import pi
+import numpy as np
+
+#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
+#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
+tools_in_ee = {
+    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
+}
+
+# joint limit
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
+
+tool_name = "scissor"
+
+def main():
+    """Demonstrate pure position control"""
+
+    # Create controller
+
+    robot_kine = rm75_kinematics(urdf_path='urdf_rm75/RM75-SCI.urdf', mesh_dir='urdf_rm75',
+                                 tcps=["scissor_tcp", "camera_tcp"], tools_in_ee=tools_in_ee, min_j=lb, max_j=ub)
+
+    ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool=tool_name)
+
+    self_collision_sts = robot_kine.get_self_collision(q)
+
+    p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
+
+    print(f'self_collision_sts: {self_collision_sts}')
+
+    import numpy as np
+
+    ik_suc = 0
+
+    for i in range(100):
+        joint_rand = np.random.uniform(ub, lb)
+
+        p_t = robot_kine.get_fk_result(joint_angles=joint_rand.tolist(), tool=tool_name)
+
+        joint_rand_init = np.random.uniform(ub, lb)
+
+        ret_ik, q = robot_kine.get_ik_result(target_position=p_t[0:3], target_rpy=p_t[3:6],
+                                             initial_guess=joint_rand_init, tool=tool_name)
+
+
+
+
+        p_fk = robot_kine.get_fk_result(joint_angles=q, tool=tool_name)
+
+        d_p_ik = cal_pose_deviation(pose1=p_t, pose2=p_fk)
+
+        coll_sts = robot_kine.get_self_collision(joint_angles=q)
+        if ret_ik == True:
+
+            print(f'\n---- success, in the ik, j_t = {joint_rand}, q = {q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
+
+            ik_suc += 1
+        else:
+            print(f'\n**** ik failed, in the ik, j_t = {joint_rand}, q={q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
+
+
+    print(f'ik_suc: {ik_suc}')
+
+
+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] > pi:
+            d_fk_p1[j] -= 2 * pi
+        while d_fk_p1[j] < -pi:
+            d_fk_p1[j] += 2 * pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk
+
+
+
+if __name__ == "__main__":
+    main()

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>

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>

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>

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>

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>

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>