delete unnecessary files.

Integrate both method into one comprehensive file.
2026-08-05 16:14:39 +01:00 · 2026-08-05 16:04:40 +01:00 · 2026-08-05 16:01:12 +01:00 · 2026-08-05 14:10:06 +01:00 · 2026-08-05 13:08:38 +01:00 · 2026-07-31 16:08:17 +01:00
66 changed files with 4039 additions and 66375 deletions
@@ -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:
+```aiignore
-1. Time consumption.
+from  rm75_kinematics import rm75_kinematics
-2. Success rate
+robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
-3. Minial joint variation.
+                             mesh_dir='./urdf_rm75',
-
+                             tcps=["scissor_tcp", "camera_tcp"],
-Next:\
+                             tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
 Comparison with Realman official IK method.
 Embedded with current demo.
 ### Comparison (05June2026):
 - With current dual arm joint limit, 
 ```
-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">
@@ -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__}')"
@@ -1,144 +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(10):
            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}')
                # robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
                if d_p_ik < 0.01:
                    result[0][1] += 1
                while 1:
                    time.sleep(1)
                robot_mjk.send_command(q)
                robot_mjk.wait_until_reached()
                robot_mjk.print_state()
            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()
@@ -1,9 +0,0 @@
 numpy
 pandas
 matplotlib
 tqdm
 scipy
 urdfpy
 pin
 osqp
 Robotic_Arm
@@ -1,916 +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, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
        self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
        self.geom_model.addAllCollisionPairs()
        self.remove_adjacent_collision_pairs(verbose=True)
        self.geom_data = pin.GeometryData(self.geom_model)
        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:
            collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
            if collision is False:
                # return best_solution, True, best_error, iter_count
                return 0, best_solution.tolist()
            else:
                return -2, q[:7].copy().tolist()
        else:
            # return q[:7].copy(), False, error_norm, iter_count
            return -1, q[:7].copy().tolist()
    def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ):
        q = np.asarray(q, dtype=np.float64).reshape(-1)
        if q.shape[0] != self.model.nq:
            raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}")
        # Update robot kinematics
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateGeometryPlacements(
            self.model,
            self.data,
            self.geom_model,
            self.geom_data,
            q
        )
        # Now compute collisions on the updated geometry model
        collision = pin.computeCollisions(
            self.geom_model,
            self.geom_data,
            stop_at_first_collision
        )
        if verbose:
            print(f"the collision is {collision}\n")
            for k, cr in enumerate(self.geom_data.collisionResults):
                if cr.isCollision():
                    cp = self.geom_model.collisionPairs[k]
                    geom1 = self.geom_model.geometryObjects[cp.first]
                    geom2 = self.geom_model.geometryObjects[cp.second]
                    print(
                        f"collision pair {k}: "
                        f"{geom1.name} <--> {geom2.name}"
                    )
        return bool(collision)
    def remove_adjacent_collision_pairs(self, verbose=True):
        """
        Remove collision pairs between same/adjacent parent joints.
        This avoids false positives such as:
            base_link_0 <--> link_1_0
        """
        pairs_to_remove = []
        for pair_id, pair in enumerate(self.geom_model.collisionPairs):
            geom1 = self.geom_model.geometryObjects[pair.first]
            geom2 = self.geom_model.geometryObjects[pair.second]
            j1 = geom1.parentJoint
            j2 = geom2.parentJoint
            # Same body or directly connected bodies
            if j1 == j2 or abs(j1 - j2) <= 1:
                pairs_to_remove.append(pair_id)
                if verbose:
                    print(
                        "Removing adjacent pair:",
                        pair_id,
                        geom1.name,
                        "<-->",
                        geom2.name,
                        "parentJoint:",
                        j1,
                        j2,
                    )
        for pair_id in reversed(pairs_to_remove):
            self.geom_model.removeCollisionPair(
                self.geom_model.collisionPairs[pair_id]
            )
        # Important: recreate geometry data after modifying pairs
        self.geom_data = pin.GeometryData(self.geom_model)
        if verbose:
            print("Remaining collision pairs:", len(self.geom_model.collisionPairs))
    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)
@@ -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
@@ -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()
@@ -1,106 +0,0 @@
 from pathlib import Path
 import matplotlib.pyplot as plt
 import numpy as np
 import pandas as pd
 # --------------------------------------------------
 # 1. Load the data
 # --------------------------------------------------
 file_name = "rm75b_comfort_workspace_v_minis_collision.csv"
 csv_path = Path(file_name)
 # The file has no column names, so header=None is important.
 df_csv = pd.read_csv(
    csv_path,
 )
 rate_res = df_csv.iloc[:, :4]
 try:
    rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
 except:
    rate_res_sort = rate_res
 DECIMALS = 4
 try:
    x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
    y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
    z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
 except:
    x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
    y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
    z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
 nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
 ik_rates = rate_res_sort.to_numpy()
 # --------------------------------------------------
 # 2. Create an output directory
 # --------------------------------------------------
 output_dir = Path(file_name.split(".")[0])
 output_dir.mkdir(exist_ok=True)
 # --------------------------------------------------
 # 3. Use the same colour scale for every z-plane
 # --------------------------------------------------
 df = rate_res_sort
 value_min = df["ik_success_rate"].min()
 value_max = df["ik_success_rate"].max()
 # More levels give a smoother-looking contour plot.
 levels = np.linspace(value_min, value_max, 51)
 # --------------------------------------------------
 # 4. Draw one contour plot for each z-plane
 # --------------------------------------------------
 for z_value, plane in df.groupby("z", sort=True):
    # Rows become y-coordinates, columns become x-coordinates.
    grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
    x = grid.columns.to_numpy()
    y = grid.index.to_numpy()
    ik_grid = grid.to_numpy()
    X, Y = np.meshgrid(x, y)
    fig, ax = plt.subplots(figsize=(7, 6))
    contour = ax.contourf(
        X,
        Y,
        ik_grid,
        levels=levels,
        cmap="viridis",
        extend="both",
    )
    highlight_levels = [0.6, 0.7]
    # Only plot if the levels are within the data range (optional)
    if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
        lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
                           colors='red', linewidths=2, linestyles='solid')
        # Optionally label the lines
        ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
    colorbar = fig.colorbar(contour, ax=ax)
    colorbar.set_label("IK rate")
    ax.set_title(f"IK rate at z = {z_value:.2f}")
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_aspect("equal")
    fig.tight_layout()
    output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
    fig.savefig(output_path, dpi=200, bbox_inches="tight")
    plt.close(fig)
 print(f"Plots saved to: {output_dir.resolve()}")
@@ -1,109 +0,0 @@
 from pathlib import Path
 import matplotlib.pyplot as plt
 import numpy as np
 import pandas as pd
 # --------------------------------------------------
 # 1. Load the data
 # --------------------------------------------------
 file_name = "workspace minisci collision.csv"
 csv_path = Path(file_name)
 # The file has no column names, so header=None is important.
 df_csv = pd.read_csv(
    csv_path,
    header=None,                     # the file has no header row
    names=['x', 'y', 'z', 'ik_success_rate']   # assign names
 )
 rate_res = df_csv.iloc[:, :4]
 try:
    rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
 except:
    rate_res_sort = rate_res
 DECIMALS = 4
 try:
    x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
    y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
    z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
 except:
    x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
    y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
    z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
 nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
 ik_rates = rate_res_sort.to_numpy()
 # --------------------------------------------------
 # 2. Create an output directory
 # --------------------------------------------------
 output_dir = Path(file_name.split(".")[0])
 output_dir.mkdir(exist_ok=True)
 # --------------------------------------------------
 # 3. Use the same colour scale for every z-plane
 # --------------------------------------------------
 df = rate_res_sort
 value_min = df["ik_success_rate"].min()
 value_max = df["ik_success_rate"].max()
 # More levels give a smoother-looking contour plot.
 levels = np.linspace(value_min, value_max, 51)
 # --------------------------------------------------
 # 4. Draw one contour plot for each z-plane
 # --------------------------------------------------
 for z_value, plane in df.groupby("z", sort=True):
    # Rows become y-coordinates, columns become x-coordinates.
    grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
    x = grid.columns.to_numpy()
    y = grid.index.to_numpy()
    ik_grid = grid.to_numpy()
    X, Y = np.meshgrid(x, y)
    fig, ax = plt.subplots(figsize=(7, 6))
    contour = ax.contourf(
        X,
        Y,
        ik_grid,
        levels=levels,
        cmap="viridis",
        extend="both",
    )
    highlight_levels = [0.6, 0.7]
    # Only plot if the levels are within the data range (optional)
    if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
        lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
                           colors='red', linewidths=2, linestyles='solid')
        # Optionally label the lines
        ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
    colorbar = fig.colorbar(contour, ax=ax)
    colorbar.set_label("IK rate")
    ax.set_title(f"IK rate at z = {z_value:.2f}")
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_aspect("equal")
    fig.tight_layout()
    output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
    fig.savefig(output_path, dpi=200, bbox_inches="tight")
    plt.close(fig)
 print(f"Plots saved to: {output_dir.resolve()}")
@@ -1,754 +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.7, 0.7)
 Y_RANGE = (-0.7, 0.7)
 Z_RANGE = (-0.10, 0.8)
 GRID_RESOLUTION = 0.05  # 5 cm. Use 0.02 for finer but slower.
 num_orientations =  120
 tool_name = "v_minis"
 output_csv = "rm75b_comfort_workspace"  + tool_name + ".csv"
 # Comfort thresholds
 MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
 MAX_CONDITION_NUMBER = 150.0
 MIN_MANIPULABILITY_RATIO = 0.10
 # Scoring weights
 WEIGHT_IK_SUCCESS = 0.70
 WEIGHT_JOINT_LIMIT = 0.10
 WEIGHT_MANIPULABILITY = 0.1
 WEIGHT_SINGULARITY = 0.1
 #   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
 #   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
 tools_in_ee = {
    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'v_minis': np.array([[0.0, 0.1, 0.1, -np.sqrt(2) * 0.5, 0.0, 0.0, np.sqrt(2) * 0.5],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
 }
 # joint limit
 # ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
 # lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
 #
 #
 ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
 lb = -ub
 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=num_orientations, seed=1):
    """
    Random orientation sampling using RM's Euler convention:
        R = Rz @ Ry @ Rx
    Note:
        This samples Euler angles randomly.
        It is useful, but not perfectly uniform over SO(3).
    """
    rng = np.random.default_rng(seed)
    orientations = []
    for _ in range(num_orientations):
        rx = rng.uniform(-np.pi, np.pi)
        ry = rng.uniform(-np.pi / 2.0, np.pi / 2.0)
        rz = rng.uniform(-np.pi, np.pi)
        orientations.append([rx, ry, rz])
    return orientations
 #
 # 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
            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()
    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)
@@ -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()
@@ -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()
@@ -0,0 +1,555 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot name="rm75_dual_arm">
  <!--Shared supporting base. Adjust the two mount joint origins to match the CAD mounting frames.-->
  <link name="dual_arm_base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
      </geometry>
      <material name="dual_arm_base_material">
        <color rgba="0.5 0.5 0.5 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <link name="omnipic_base_link">
    <inertial>
      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
      <mass value="1.862" />
      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/base_link.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link name="omnipic_link_1">
    <inertial>
      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
      <mass value="1.574" />
      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_1.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_1" type="revolute">
    <origin xyz="0 0 0.2405" rpy="0 0 0" />
    <parent link="omnipic_base_link" />
    <child link="omnipic_link_1" />
    <axis xyz="0 0 1" />
    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
  </joint>
  <link name="omnipic_link_2">
    <inertial>
      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
      <mass value="1.217" />
      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_2.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_2" type="revolute">
    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
    <parent link="omnipic_link_1" />
    <child link="omnipic_link_2" />
    <axis xyz="0 0 1" />
    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
  </joint>
  <link name="omnipic_link_3">
    <inertial>
      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
      <mass value="1.11" />
      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_3.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_3" type="revolute">
    <origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
    <parent link="omnipic_link_2" />
    <child link="omnipic_link_3" />
    <axis xyz="0 0 1" />
    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
  </joint>
  <link name="omnipic_link_4">
    <inertial>
      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
      <mass value="0.685" />
      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_4.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_4" type="revolute">
    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
    <parent link="omnipic_link_3" />
    <child link="omnipic_link_4" />
    <axis xyz="0 0 1" />
    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
  </joint>
  <link name="omnipic_link_5">
    <inertial>
      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
      <mass value="0.619" />
      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_5.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_5" type="revolute">
    <origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
    <parent link="omnipic_link_4" />
    <child link="omnipic_link_5" />
    <axis xyz="0 0 1" />
    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
  </joint>
  <link name="omnipic_link_6">
    <inertial>
      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
      <mass value="0.602" />
      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_6.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_6" type="revolute">
    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
    <parent link="omnipic_link_5" />
    <child link="omnipic_link_6" />
    <axis xyz="0 0 1" />
    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
  </joint>
  <link name="omnipic_link_7">
    <inertial>
      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
      <mass value="0.107" />
      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_7.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_joint_7" type="revolute">
    <origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
    <parent link="omnipic_link_6" />
    <child link="omnipic_link_7" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
  </joint>
  <link name="omnipic_gripper_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.1" />
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
      </geometry>
      <material name="omnipic_OmniPic_material">
        <color rgba="0.7 0.7 0.7 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <joint name="omnipic_OmniPic_fixed_joint" type="fixed">
    <parent link="omnipic_link_7" />
    <child link="omnipic_gripper_link" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>
  <link name="omnipic_OmniPic_tcp" />
  <joint name="omnipic_OmniPic_tcp_fixed" type="fixed">
    <parent link="omnipic_gripper_link" />
    <child link="omnipic_OmniPic_tcp" />
    <origin xyz="0 0 0.14" rpy="0 0 0" />
  </joint>
  <!--Omnipic arm mount: edit xyz/rpy to match dual_arm_base.stl.-->
  <joint name="omnipic_base_mount_joint" type="fixed">
    <parent link="dual_arm_base_link" />
    <child link="omnipic_base_link" />
    <origin xyz="0.03 0 0" rpy="0 -1.57 3.141593" />
  </joint>
  <link name="scissor_base_link">
    <inertial>
      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
      <mass value="1.862" />
      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/base_link.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link name="scissor_link_1">
    <inertial>
      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
      <mass value="1.574" />
      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_1.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_1" type="revolute">
    <origin xyz="0 0 0.2405" rpy="0 0 0" />
    <parent link="scissor_base_link" />
    <child link="scissor_link_1" />
    <axis xyz="0 0 1" />
    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
  </joint>
  <link name="scissor_link_2">
    <inertial>
      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
      <mass value="1.217" />
      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_2.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_2" type="revolute">
    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
    <parent link="scissor_link_1" />
    <child link="scissor_link_2" />
    <axis xyz="0 0 1" />
    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
  </joint>
  <link name="scissor_link_3">
    <inertial>
      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
      <mass value="1.11" />
      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_3.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_3" type="revolute">
    <origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
    <parent link="scissor_link_2" />
    <child link="scissor_link_3" />
    <axis xyz="0 0 1" />
    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
  </joint>
  <link name="scissor_link_4">
    <inertial>
      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
      <mass value="0.685" />
      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_4.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_4" type="revolute">
    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
    <parent link="scissor_link_3" />
    <child link="scissor_link_4" />
    <axis xyz="0 0 1" />
    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
  </joint>
  <link name="scissor_link_5">
    <inertial>
      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
      <mass value="0.619" />
      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_5.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_5" type="revolute">
    <origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
    <parent link="scissor_link_4" />
    <child link="scissor_link_5" />
    <axis xyz="0 0 1" />
    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
  </joint>
  <link name="scissor_link_6">
    <inertial>
      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
      <mass value="0.602" />
      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_6.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_6" type="revolute">
    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
    <parent link="scissor_link_5" />
    <child link="scissor_link_6" />
    <axis xyz="0 0 1" />
    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
  </joint>
  <link name="scissor_link_7">
    <inertial>
      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
      <mass value="0.107" />
      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_7.STL" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_joint_7" type="revolute">
    <origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
    <parent link="scissor_link_6" />
    <child link="scissor_link_7" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
  </joint>
  <link name="scissor_scissor_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.1" />
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/scissor.stl" scale="1 1 1" />
      </geometry>
      <material name="scissor_scissor_material">
        <color rgba="0.7 0.7 0.7 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="meshes/scissor.stl" scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <joint name="scissor_scissor_fixed_joint" type="fixed">
    <parent link="scissor_link_7" />
    <child link="scissor_scissor_link" />
    <origin xyz="0 0 0.165" rpy="0 0 0" />
  </joint>
  <link name="scissor_scissor_tcp" />
  <joint name="scissor_scissor_tcp_fixed" type="fixed">
    <parent link="scissor_scissor_link" />
    <child link="scissor_scissor_tcp" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>
  <link name="scissor_camera_tcp" />
  <joint name="scissor_camera_tcp_fixed" type="fixed">
    <parent link="scissor_scissor_link" />
    <child link="scissor_camera_tcp" />
    <origin xyz="0.042 0 -0.0755" rpy="0 0 1.57" />
  </joint>
  <!--Scissor arm mount: edit xyz/rpy to match dual_arm_base.stl.-->
  <joint name="scissor_base_mount_joint" type="fixed">
    <parent link="dual_arm_base_link" />
    <child link="scissor_base_link" />
    <origin xyz="-0.030 0 0" rpy="0 1.57 3.141593" />
  </joint>
 </robot>
@@ -0,0 +1,507 @@
 <?xml version="1.0" encoding="utf-8"?>
 <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot
  name="RM75-B">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0.00049987 5.2709E-05 0.060019"
        rpy="0 0 0" />
      <mass
        value="1.862" />
      <inertia
        ixx="0.0017232"
        ixy="-3.1058E-06"
        ixz="-3.7924E-05"
        iyy="0.0017051"
        iyz="1.3691E-06"
        izz="0.00090158" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="link_1">
    <inertial>
      <origin
        xyz="0.000241 -0.013273 -0.00995"
        rpy="0 0 0" />
      <mass
        value="1.574" />
      <inertia
        ixx="0.002487573"
        ixy="0.000009663"
        ixz="-0.000007909"
        iyy="0.002321038"
        iyz="0.000179393"
        izz="0.001450554" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_1"
    type="revolute">
    <origin
      xyz="0 0 0.2405"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="link_1" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_2">
    <inertial>
      <origin
        xyz="-0.000357 -0.106789 0.005329"
        rpy="0 0 0" />
      <mass
        value="1.217" />
      <inertia
        ixx="0.003494121"
        ixy="0.000002921"
        ixz="-0.000005613"
        iyy="0.000892721"
        iyz="-0.000583884"
        izz="0.003444080" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_2"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_1" />
    <child
      link="link_2" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.2689"
      upper="2.2689"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_3">
    <inertial>
      <origin
        xyz="0.000003 -0.01398 -0.011324"
        rpy="0 0 0" />
      <mass
        value="1.11" />
      <inertia
        ixx="0.001836663"
        ixy="0.000002259"
        ixz="-0.000004216"
        iyy="0.001498875"
        iyz="0.000037167"
        izz="0.001062545" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_3"
    type="revolute">
    <origin
      xyz="0 -0.256 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_2" />
    <child
      link="link_3" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_4">
    <inertial>
      <origin
        xyz="-0.000005 -0.084658 0.004747"
        rpy="0 0 0" />
      <mass
        value="0.685" />
      <inertia
        ixx="0.001282444"
        ixy="-0.000000551"
        ixz="-0.000000630"
        iyy="0.000373013"
        iyz="-0.000232084"
        izz="0.001256177" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_4"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_3" />
    <child
      link="link_4" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.356"
      upper="2.356"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_5">
    <inertial>
      <origin
        xyz="0.000078 -0.012937 -0.008781"
        rpy="0 0 0" />
      <mass
        value="0.619" />
      <inertia
        ixx="0.000627336"
        ixy="0.000001636"
        ixz="-0.000001345"
        iyy="0.000542455"
        iyz="0.000034970"
        izz="0.000370291" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_5"
    type="revolute">
    <origin
      xyz="0 -0.21 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_4" />
    <child
      link="link_5" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_6">
    <inertial>
      <origin
        xyz="-0.000014 -0.078524 0.002819"
        rpy="0 0 0" />
      <mass
        value="0.602" />
      <inertia
        ixx="0.000780774"
        ixy="-0.000000121"
        ixz="-0.000000469"
        iyy="0.000289973"
        iyz="-0.000120513"
        izz="0.000763955" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_6"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_5" />
    <child
      link="link_6" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.234"
      upper="2.234"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_7">
    <inertial>
      <origin
        xyz="0.001094 -0.000077 -0.010119"
        rpy="0 0 0" />
      <mass
        value="0.107" />
      <inertia
        ixx="0.000044123"
        ixy="-0.000000064"
        ixz="0.0000003"
        iyy="0.000035078"
        iyz="-0.000000029"
        izz="0.000065445" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_7"
    type="revolute">
    <origin
      xyz="0 -0.144 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_6" />
    <child
      link="link_7" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-6.28"
      upper="6.28"
      effort="10"
      velocity="3.14" />
  </joint>
    <!-- Scissor end-effector -->
  <link name="gripper_link">
    <inertial>
      <!-- Replace these values with CAD-derived mass properties -->
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.1" />
      <inertia
        ixx="0.0001"
        ixy="0"
        ixz="0"
        iyy="0.0001"
        iyz="0"
        izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/OmniPic.stl"
          scale="1 1 1" />
      </geometry>
      <material name="OmniPic_material">
        <color rgba="0.7 0.7 0.7 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/OmniPic.stl"
          scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <!-- Rigidly attach the gripper to the final wrist link -->
  <joint name="OmniPic_fixed_joint" type="fixed">
    <parent link="link_7" />
    <child link="gripper_link" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>
  <link name="OmniPic_tcp"/>
 <joint name="OmniPic_tcp_fixed" type="fixed">
  <parent link="gripper_link"/>
  <child link="OmniPic_tcp"/>
  <origin xyz="0 0 0.14" rpy="0 0 0"/>
 </joint>
 </robot>
@@ -0,0 +1,516 @@
 <?xml version="1.0" encoding="utf-8"?>
 <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot
  name="RM75-B">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0.00049987 5.2709E-05 0.060019"
        rpy="0 0 0" />
      <mass
        value="1.862" />
      <inertia
        ixx="0.0017232"
        ixy="-3.1058E-06"
        ixz="-3.7924E-05"
        iyy="0.0017051"
        iyz="1.3691E-06"
        izz="0.00090158" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="link_1">
    <inertial>
      <origin
        xyz="0.000241 -0.013273 -0.00995"
        rpy="0 0 0" />
      <mass
        value="1.574" />
      <inertia
        ixx="0.002487573"
        ixy="0.000009663"
        ixz="-0.000007909"
        iyy="0.002321038"
        iyz="0.000179393"
        izz="0.001450554" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_1"
    type="revolute">
    <origin
      xyz="0 0 0.2405"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="link_1" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_2">
    <inertial>
      <origin
        xyz="-0.000357 -0.106789 0.005329"
        rpy="0 0 0" />
      <mass
        value="1.217" />
      <inertia
        ixx="0.003494121"
        ixy="0.000002921"
        ixz="-0.000005613"
        iyy="0.000892721"
        iyz="-0.000583884"
        izz="0.003444080" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_2"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_1" />
    <child
      link="link_2" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.2689"
      upper="2.2689"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_3">
    <inertial>
      <origin
        xyz="0.000003 -0.01398 -0.011324"
        rpy="0 0 0" />
      <mass
        value="1.11" />
      <inertia
        ixx="0.001836663"
        ixy="0.000002259"
        ixz="-0.000004216"
        iyy="0.001498875"
        iyz="0.000037167"
        izz="0.001062545" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_3"
    type="revolute">
    <origin
      xyz="0 -0.256 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_2" />
    <child
      link="link_3" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_4">
    <inertial>
      <origin
        xyz="-0.000005 -0.084658 0.004747"
        rpy="0 0 0" />
      <mass
        value="0.685" />
      <inertia
        ixx="0.001282444"
        ixy="-0.000000551"
        ixz="-0.000000630"
        iyy="0.000373013"
        iyz="-0.000232084"
        izz="0.001256177" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_4"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_3" />
    <child
      link="link_4" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.356"
      upper="2.356"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_5">
    <inertial>
      <origin
        xyz="0.000078 -0.012937 -0.008781"
        rpy="0 0 0" />
      <mass
        value="0.619" />
      <inertia
        ixx="0.000627336"
        ixy="0.000001636"
        ixz="-0.000001345"
        iyy="0.000542455"
        iyz="0.000034970"
        izz="0.000370291" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_5"
    type="revolute">
    <origin
      xyz="0 -0.21 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_4" />
    <child
      link="link_5" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_6">
    <inertial>
      <origin
        xyz="-0.000014 -0.078524 0.002819"
        rpy="0 0 0" />
      <mass
        value="0.602" />
      <inertia
        ixx="0.000780774"
        ixy="-0.000000121"
        ixz="-0.000000469"
        iyy="0.000289973"
        iyz="-0.000120513"
        izz="0.000763955" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_6"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_5" />
    <child
      link="link_6" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.234"
      upper="2.234"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_7">
    <inertial>
      <origin
        xyz="0.001094 -0.000077 -0.010119"
        rpy="0 0 0" />
      <mass
        value="0.107" />
      <inertia
        ixx="0.000044123"
        ixy="-0.000000064"
        ixz="0.0000003"
        iyy="0.000035078"
        iyz="-0.000000029"
        izz="0.000065445" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_7"
    type="revolute">
    <origin
      xyz="0 -0.144 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_6" />
    <child
      link="link_7" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-6.28"
      upper="6.28"
      effort="10"
      velocity="3.14" />
  </joint>
    <!-- Scissor end-effector -->
  <link name="scissor_link">
    <inertial>
      <!-- Replace these values with CAD-derived mass properties -->
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.1" />
      <inertia
        ixx="0.0001"
        ixy="0"
        ixz="0"
        iyy="0.0001"
        iyz="0"
        izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/scissor.stl"
          scale="1 1 1" />
      </geometry>
      <material name="scissor_material">
        <color rgba="0.7 0.7 0.7 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/scissor.stl"
          scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <!-- Rigidly attach the scissor to the final wrist link -->
  <joint name="scissor_fixed_joint" type="fixed">
    <parent link="link_7" />
    <child link="scissor_link" />
    <origin xyz="0 -0.12 0.03" rpy="1.5707963 0 0" />
  </joint>
  <link name="scissor_tcp"/>
 <joint name="scissor_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="scissor_tcp"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
 </joint>
 <link name="camera_tcp"/>
 <joint name="camera_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="camera_tcp"/>
  <origin xyz="0.042 0 -0.0755" rpy="0 0 1.5707963"/>
 </joint>
 </robot>
@@ -0,0 +1,516 @@
 <?xml version="1.0" encoding="utf-8"?>
 <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot
  name="RM75-B">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0.00049987 5.2709E-05 0.060019"
        rpy="0 0 0" />
      <mass
        value="1.862" />
      <inertia
        ixx="0.0017232"
        ixy="-3.1058E-06"
        ixz="-3.7924E-05"
        iyy="0.0017051"
        iyz="1.3691E-06"
        izz="0.00090158" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="link_1">
    <inertial>
      <origin
        xyz="0.000241 -0.013273 -0.00995"
        rpy="0 0 0" />
      <mass
        value="1.574" />
      <inertia
        ixx="0.002487573"
        ixy="0.000009663"
        ixz="-0.000007909"
        iyy="0.002321038"
        iyz="0.000179393"
        izz="0.001450554" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_1"
    type="revolute">
    <origin
      xyz="0 0 0.2405"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="link_1" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_2">
    <inertial>
      <origin
        xyz="-0.000357 -0.106789 0.005329"
        rpy="0 0 0" />
      <mass
        value="1.217" />
      <inertia
        ixx="0.003494121"
        ixy="0.000002921"
        ixz="-0.000005613"
        iyy="0.000892721"
        iyz="-0.000583884"
        izz="0.003444080" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_2"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_1" />
    <child
      link="link_2" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.2689"
      upper="2.2689"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_3">
    <inertial>
      <origin
        xyz="0.000003 -0.01398 -0.011324"
        rpy="0 0 0" />
      <mass
        value="1.11" />
      <inertia
        ixx="0.001836663"
        ixy="0.000002259"
        ixz="-0.000004216"
        iyy="0.001498875"
        iyz="0.000037167"
        izz="0.001062545" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_3"
    type="revolute">
    <origin
      xyz="0 -0.256 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_2" />
    <child
      link="link_3" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_4">
    <inertial>
      <origin
        xyz="-0.000005 -0.084658 0.004747"
        rpy="0 0 0" />
      <mass
        value="0.685" />
      <inertia
        ixx="0.001282444"
        ixy="-0.000000551"
        ixz="-0.000000630"
        iyy="0.000373013"
        iyz="-0.000232084"
        izz="0.001256177" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_4"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_3" />
    <child
      link="link_4" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.356"
      upper="2.356"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_5">
    <inertial>
      <origin
        xyz="0.000078 -0.012937 -0.008781"
        rpy="0 0 0" />
      <mass
        value="0.619" />
      <inertia
        ixx="0.000627336"
        ixy="0.000001636"
        ixz="-0.000001345"
        iyy="0.000542455"
        iyz="0.000034970"
        izz="0.000370291" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_5"
    type="revolute">
    <origin
      xyz="0 -0.21 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_4" />
    <child
      link="link_5" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_6">
    <inertial>
      <origin
        xyz="-0.000014 -0.078524 0.002819"
        rpy="0 0 0" />
      <mass
        value="0.602" />
      <inertia
        ixx="0.000780774"
        ixy="-0.000000121"
        ixz="-0.000000469"
        iyy="0.000289973"
        iyz="-0.000120513"
        izz="0.000763955" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_6"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_5" />
    <child
      link="link_6" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.234"
      upper="2.234"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_7">
    <inertial>
      <origin
        xyz="0.001094 -0.000077 -0.010119"
        rpy="0 0 0" />
      <mass
        value="0.107" />
      <inertia
        ixx="0.000044123"
        ixy="-0.000000064"
        ixz="0.0000003"
        iyy="0.000035078"
        iyz="-0.000000029"
        izz="0.000065445" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_7"
    type="revolute">
    <origin
      xyz="0 -0.144 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_6" />
    <child
      link="link_7" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-6.28"
      upper="6.28"
      effort="10"
      velocity="3.14" />
  </joint>
    <!-- Scissor end-effector -->
  <link name="scissor_link">
    <inertial>
      <!-- Replace these values with CAD-derived mass properties -->
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.1" />
      <inertia
        ixx="0.0001"
        ixy="0"
        ixz="0"
        iyy="0.0001"
        iyz="0"
        izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/scissor.stl"
          scale="1 1 1" />
      </geometry>
      <material name="scissor_material">
        <color rgba="0.7 0.7 0.7 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/scissor.stl"
          scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <!-- Rigidly attach the scissor to the final wrist link -->
  <joint name="scissor_fixed_joint" type="fixed">
    <parent link="link_7" />
    <child link="scissor_link" />
    <origin xyz="-0.12 0 0.063" rpy="0 -1.5707963 0" />
  </joint>
  <link name="scissor_tcp"/>
 <joint name="scissor_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="scissor_tcp"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
 </joint>
 <link name="camera_tcp"/>
 <joint name="camera_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="camera_tcp"/>
  <origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
 </joint>
 </robot>
@@ -469,7 +469,7 @@
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
-          filename="meshes/sci.stl"
+          filename="meshes/scissor.stl"
          scale="1 1 1" />
      </geometry>
      <material name="scissor_material">
@@ -481,7 +481,7 @@
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
-          filename="meshes/sci.stl"
+          filename="meshes/scissor.stl"
          scale="1 1 1" />
      </geometry>
    </collision>
@@ -491,6 +491,26 @@
  <joint name="scissor_fixed_joint" type="fixed">
    <parent link="link_7" />
    <child link="scissor_link" />
-    <origin xyz="0 0 0" rpy="3.14159 0 0" />
+    <origin xyz="0 0 0.165" rpy="0 0 0" />
  </joint>
  <link name="scissor_tcp"/>
 <joint name="scissor_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="scissor_tcp"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
 </joint>
 <link name="camera_tcp"/>
 <joint name="camera_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="camera_tcp"/>
  <origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
 </joint>
 </robot>
@@ -0,0 +1,516 @@
 <?xml version="1.0" encoding="utf-8"?>
 <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot
  name="RM75-B">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0.00049987 5.2709E-05 0.060019"
        rpy="0 0 0" />
      <mass
        value="1.862" />
      <inertia
        ixx="0.0017232"
        ixy="-3.1058E-06"
        ixz="-3.7924E-05"
        iyy="0.0017051"
        iyz="1.3691E-06"
        izz="0.00090158" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="link_1">
    <inertial>
      <origin
        xyz="0.000241 -0.013273 -0.00995"
        rpy="0 0 0" />
      <mass
        value="1.574" />
      <inertia
        ixx="0.002487573"
        ixy="0.000009663"
        ixz="-0.000007909"
        iyy="0.002321038"
        iyz="0.000179393"
        izz="0.001450554" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_1"
    type="revolute">
    <origin
      xyz="0 0 0.2405"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="link_1" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_2">
    <inertial>
      <origin
        xyz="-0.000357 -0.106789 0.005329"
        rpy="0 0 0" />
      <mass
        value="1.217" />
      <inertia
        ixx="0.003494121"
        ixy="0.000002921"
        ixz="-0.000005613"
        iyy="0.000892721"
        iyz="-0.000583884"
        izz="0.003444080" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_2"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_1" />
    <child
      link="link_2" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.2689"
      upper="2.2689"
      effort="60"
      velocity="3.14" />
  </joint>
  <link
    name="link_3">
    <inertial>
      <origin
        xyz="0.000003 -0.01398 -0.011324"
        rpy="0 0 0" />
      <mass
        value="1.11" />
      <inertia
        ixx="0.001836663"
        ixy="0.000002259"
        ixz="-0.000004216"
        iyy="0.001498875"
        iyz="0.000037167"
        izz="0.001062545" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_3"
    type="revolute">
    <origin
      xyz="0 -0.256 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_2" />
    <child
      link="link_3" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_4">
    <inertial>
      <origin
        xyz="-0.000005 -0.084658 0.004747"
        rpy="0 0 0" />
      <mass
        value="0.685" />
      <inertia
        ixx="0.001282444"
        ixy="-0.000000551"
        ixz="-0.000000630"
        iyy="0.000373013"
        iyz="-0.000232084"
        izz="0.001256177" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_4"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_3" />
    <child
      link="link_4" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.356"
      upper="2.356"
      effort="30"
      velocity="3.14" />
  </joint>
  <link
    name="link_5">
    <inertial>
      <origin
        xyz="0.000078 -0.012937 -0.008781"
        rpy="0 0 0" />
      <mass
        value="0.619" />
      <inertia
        ixx="0.000627336"
        ixy="0.000001636"
        ixz="-0.000001345"
        iyy="0.000542455"
        iyz="0.000034970"
        izz="0.000370291" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_5"
    type="revolute">
    <origin
      xyz="0 -0.21 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_4" />
    <child
      link="link_5" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.106"
      upper="3.106"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_6">
    <inertial>
      <origin
        xyz="-0.000014 -0.078524 0.002819"
        rpy="0 0 0" />
      <mass
        value="0.602" />
      <inertia
        ixx="0.000780774"
        ixy="-0.000000121"
        ixz="-0.000000469"
        iyy="0.000289973"
        iyz="-0.000120513"
        izz="0.000763955" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_6"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="-1.5708 0 0" />
    <parent
      link="link_5" />
    <child
      link="link_6" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-2.234"
      upper="2.234"
      effort="10"
      velocity="3.14" />
  </joint>
  <link
    name="link_7">
    <inertial>
      <origin
        xyz="0.001094 -0.000077 -0.010119"
        rpy="0 0 0" />
      <mass
        value="0.107" />
      <inertia
        ixx="0.000044123"
        ixy="-0.000000064"
        ixz="0.0000003"
        iyy="0.000035078"
        iyz="-0.000000029"
        izz="0.000065445" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_7"
    type="revolute">
    <origin
      xyz="0 -0.144 0"
      rpy="1.5708 0 0" />
    <parent
      link="link_6" />
    <child
      link="link_7" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-6.28"
      upper="6.28"
      effort="10"
      velocity="3.14" />
  </joint>
    <!-- Scissor end-effector -->
  <link name="scissor_link">
    <inertial>
      <!-- Replace these values with CAD-derived mass properties -->
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.1" />
      <inertia
        ixx="0.0001"
        ixy="0"
        ixz="0"
        iyy="0.0001"
        iyz="0"
        izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/mini_scissor.stl"
          scale="1 1 1" />
      </geometry>
      <material name="scissor_material">
        <color rgba="0.7 0.7 0.7 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh
          filename="meshes/mini_scissor.stl"
          scale="1 1 1" />
      </geometry>
    </collision>
  </link>
  <!-- Rigidly attach the scissor to the final wrist link -->
  <joint name="scissor_fixed_joint" type="fixed">
    <parent link="link_7" />
    <child link="scissor_link" />
    <origin xyz="0 0 0.165" rpy="0 0 0" />
  </joint>
  <link name="scissor_tcp"/>
 <joint name="scissor_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="scissor_tcp"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
 </joint>
 <link name="camera_tcp"/>
 <joint name="camera_tcp_fixed" type="fixed">
  <parent link="scissor_link"/>
  <child link="camera_tcp"/>
  <origin xyz="0.08 0 -0.135" rpy="0 0 1.57"/>
 </joint>
 </robot>
Author	SHA1	Message	Date
LiuzhengSJ	d1e096cb3d	delete unnecessary files.	2026-08-05 16:14:39 +01:00
LiuzhengSJ	60eb37b0ed	delete unnecessary files.	2026-08-05 16:04:40 +01:00
LiuzhengSJ	c0e181cc50	Integrate both method into one comprehensive file.	2026-08-05 16:01:12 +01:00
LiuzhengSJ	2e25951bc0	add test files for the rm75 kinematics class.	2026-08-05 14:10:06 +01:00
LiuzhengSJ	d86438115f	the branch with concise robot kinematics class for rm75	2026-08-05 13:08:38 +01:00
LiuzhengSJ	8ff29b1cc9	add mini scissor stl file, as well as urdf file. modify the Dual_arm.urdf, in line with real arm installation.	2026-07-31 16:08:17 +01:00
LiuzhengSJ	0c121a7425	In qp inverse kinematic file, the tool frames self.tcps is defined as [] if not given a value.	2026-07-30 15:41:10 +01:00
LiuzhengSJ	e001f2e1f1	add ik q_solved refinement function, to make it as close as possible to last_q. update the rm75_kine_rm.py to included a series of arm-angle for inverse kinematics calculation.	2026-07-30 15:28:14 +01:00
LiuzhengSJ	ace5dea9a2	add collision detection in workspace cal	2026-07-29 11:30:32 +01:00
LiuzhengSJ	2713c54707	add collision detection in workspace cal	2026-07-29 11:25:09 +01:00
LiuzhengSJ	9849466430	add collision detection	2026-07-28 22:41:35 +01:00
LiuzhengSJ	86c40ee380	add urdf files, dual arm.	2026-07-28 19:40:27 +01:00
LiuzhengSJ	9513dfa4ce	add urdf file, scissor included.	2026-07-28 16:06:21 +01:00
LiuzhengSJ	e1d833812b	the scissor stl file is added.	2026-07-27 21:31:52 +01:00