test push

Update README.md
Upload files to "img"
2026-07-07 17:08:33 +08:00 · 2026-06-14 04:17:40 +08:00 · 2026-06-14 04:13:40 +08:00 · 2026-06-09 17:28:57 +08:00 · 2026-06-09 17:23:17 +08:00
36 changed files with 1366 additions and 4535 deletions
@@ -0,0 +1 @@
+hhelloworrld
@@ -1,42 +1,43 @@
 ### This repo is for inverse kinematics and verification

-In this branch, the **integrated** inverse kinematics method is packed into one python class **rm75_kinematics** in ``rm75_kinematics.py``. 
+Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).

-The user can call it as in `test1.py`, `test2.py`.
+Verification is done with Mujoco simulation.

-How to use
+Key specifications:
+1. Time consumption.
+2. Success rate
+3. Minial joint variation.

-```aiignore
-from  rm75_kinematics import rm75_kinematics
-robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
-                             mesh_dir='./urdf_rm75',
-                             tcps=["scissor_tcp", "camera_tcp"],
-                             tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
+Cost:
+
+<img src="img/cost.jpg" alt="Cost" width="400">
+
+
+### Comparison (05June2026):
+
+- With current dual arm joint limit, 
 ```
-
-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)
+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])
 ```
+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%**.

-2. `get_fk_result`
-```aiignore
-p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
-```

-3. `get_self_collision`
-```aiignore
-self_collision_sts = robot_kine.get_self_collision(q)
+
+
+- With realman-75 physical joint limit,
 ```
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
+lb = -ub
+```
+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%**.
+
+
+
+Visualization:
+
+
+![demo](kine_ctrl/visual/mjc_ik_test1/mjc_ik.gif)
@@ -0,0 +1,115 @@
+
+
+#   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
+
+def main():
+    """Demonstrate pure position control"""
+
+    # Create controller
+    robot_mjk = MuJoCoPositionController()
+
+    tool_name = "scissor"
+    #  -----------      rm75 qp based kine          ------------
+    robot_kine_qp = kine_qp()
+
+    #   ----------      rm75 official algorithm         -----------
+    robot_kine_rm = kine_rm()
+
+
+
+#   --------------  for comparison   ----------------
+    print(f'in the comparison part')
+
+    if True:
+
+        # 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])
+
+        ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+        lb = -ub
+
+        robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+        robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=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}')
+                if d_p_ik < 0.01:
+                    result[0][1] += 1
+
+                robot_mjk.send_command(q)
+                robot_mjk.wait_until_reached()
+                robot_mjk.print_state()
+            else:
+                fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
+                d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
+                print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
+
+            ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
+            if ret_rm == 0:
+                fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
+                d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
+                print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
+                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'result is {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()
@@ -0,0 +1,823 @@
+#!/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
+
+
+
+class KinematicsSolver():
+    def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
+        """
+        for realman 75b
+        Initialize robotic arm kinematics using Pinocchio (ROS2 version).
+        unit: m, rad
+        """
+        print(f' ------------  the qp based kinematic initialising   -----------')
+        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
+
+        # -------------------------------------------------
+        # ee
+        # -------------------------------------------------
+        ee_offset = pin.SE3(np.eye(3), np.array([0, 0, 0.0]))
+        self.model.addFrame(
+            pin.Frame(
+                "ee",
+                self.model.getJointId("joint_7"),
+                self.model.getFrameId("link_7"),
+                ee_offset,
+                pin.FrameType.OP_FRAME
+            )
+        )
+
+
+        # -------------------------------------------------
+        # Scissor tool
+        # -------------------------------------------------
+        scissor_offset = pin.SE3(
+            np.eye(3),
+            np.array([0.0, 0.0, 0.144])
+        )
+        self.model.addFrame(
+            pin.Frame(
+                "scissor",
+                self.model.getJointId("joint_7"),
+                self.model.getFrameId("link_7"),
+                scissor_offset,
+                pin.FrameType.OP_FRAME
+            )
+        )
+
+        # -------------------------------------------------
+        # Camera tool
+        # -------------------------------------------------
+        camera_rotation = pin.rpy.rpyToMatrix(
+            radians(-90),
+            0,
+            radians(-90)
+        )
+        camera_offset = pin.SE3(
+            camera_rotation,
+            np.array([0.05, 0.02, 0.10])
+        )
+        self.model.addFrame(
+            pin.Frame(
+                "camera",
+                self.model.getJointId("joint_7"),
+                self.model.getFrameId("link_7"),
+                camera_offset,
+                pin.FrameType.OP_FRAME
+            )
+        )
+
+        # -------------------------------------------------
+        # Store tool frame IDs
+        # -------------------------------------------------
+
+        self.tool_frames = {
+            "scissor": self.model.getFrameId("scissor"),
+            "camera": self.model.getFrameId("camera"),
+            "ee":   self.model.getFrameId("ee")
+        }
+
+
+
+
+        self.data = self.model.createData()
+
+        self.cfg_j_limit()
+
+        # ----------  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])
+
+    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="ee"):
+        """
+        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_posture = 0.0001
+
+                J_eff = pin.Jlog6(error_SE3) @ J #J #
+
+                H = J_eff.T @ self.W @ J_eff
+
+
+                # H = J.T @ self.W @ J
+                H += damping * damping * np.eye(7)
+                H += w_posture * np.eye(7)
+
+                H_triu = sparse.triu(H).tocsc()
+
+                g = -J_eff.T @ self.W @ error_vec
+                g += w_posture * (q[:7] - q_ref[:7])
+                # 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, #H[np.triu_indices(7)], #
+                    q=g,
+                    l=lb,
+                    u=ub
+                )
+
+                # Solve
+                result = self.osqp_solver.solve()
+                if result.info.status != 'solved':
+                    break
+
+                dq = result.x
+
+                if dq is None:
+                    break
+
+                # Apply joint limits with scaling
+                alpha = 1.0
+                q = pin.integrate(self.model, q, alpha * dq)
+
+                prev_error = error_norm
+                iter_count += 1
+
+        if best_solution is not None:
+            # return best_solution, True, best_error, iter_count
+            return 0, best_solution
+        else:
+            # return q[:7].copy(), False, error_norm, iter_count
+            return -1, q[:7].copy()
+
+    # 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)
@@ -0,0 +1,97 @@
+
+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_65 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.tool_frames = {
+            'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
+            'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.144, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
+            'camera': rm_frame_t(frame_name="camera", pose=(0.05, 0.02, 0.10, -1.57, 0, -1.57), payload=1, x=0, y=0, z=72)
+        }
+        self.work_frames = {
+            'work': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
+        }
+
+        self.tool_name = "ee"
+        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  ])
+
+        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 forward_kinematics(self, joint_angles, flag  = 1  , tool="ee", 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="ee", 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 = target_position + 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)
+        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)
+        return ret, [ q/180*math.pi for q in q_out]
@@ -0,0 +1,297 @@
+#!/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,3 +0,0 @@
-conda install -c conda-forge osqp scipy tqdm matplotlib pandas "numpy<1.24" pinocchio -y
-pip install urdfpy mujoco "networkx>=2.8.4"
-pip install Robotic_Arm
@@ -1,48 +0,0 @@
-
-
-#   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()
@@ -1,90 +0,0 @@
-
-
-#   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()
@@ -1,555 +0,0 @@
-<?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>
@@ -1,507 +0,0 @@
-<?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>
@@ -1,516 +0,0 @@
-<?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>
@@ -1,516 +0,0 @@
-<?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>
@@ -1,516 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
-     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="RM75-B">
-  <link
-    name="base_link">
-    <inertial>
-      <origin
-        xyz="0.00049987 5.2709E-05 0.060019"
-        rpy="0 0 0" />
-      <mass
-        value="1.862" />
-      <inertia
-        ixx="0.0017232"
-        ixy="-3.1058E-06"
-        ixz="-3.7924E-05"
-        iyy="0.0017051"
-        iyz="1.3691E-06"
-        izz="0.00090158" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link_1">
-    <inertial>
-      <origin
-        xyz="0.000241 -0.013273 -0.00995"
-        rpy="0 0 0" />
-      <mass
-        value="1.574" />
-      <inertia
-        ixx="0.002487573"
-        ixy="0.000009663"
-        ixz="-0.000007909"
-        iyy="0.002321038"
-        iyz="0.000179393"
-        izz="0.001450554" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_1.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_1.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_1"
-    type="revolute">
-    <origin
-      xyz="0 0 0.2405"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="link_1" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.106"
-      upper="3.106"
-      effort="60"
-      velocity="3.14" />
-  </joint>
-  <link
-    name="link_2">
-    <inertial>
-      <origin
-        xyz="-0.000357 -0.106789 0.005329"
-        rpy="0 0 0" />
-      <mass
-        value="1.217" />
-      <inertia
-        ixx="0.003494121"
-        ixy="0.000002921"
-        ixz="-0.000005613"
-        iyy="0.000892721"
-        iyz="-0.000583884"
-        izz="0.003444080" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_2.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_2.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_2"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="-1.5708 0 0" />
-    <parent
-      link="link_1" />
-    <child
-      link="link_2" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.2689"
-      upper="2.2689"
-      effort="60"
-      velocity="3.14" />
-  </joint>
-  <link
-    name="link_3">
-    <inertial>
-      <origin
-        xyz="0.000003 -0.01398 -0.011324"
-        rpy="0 0 0" />
-      <mass
-        value="1.11" />
-      <inertia
-        ixx="0.001836663"
-        ixy="0.000002259"
-        ixz="-0.000004216"
-        iyy="0.001498875"
-        iyz="0.000037167"
-        izz="0.001062545" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_3.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_3.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_3"
-    type="revolute">
-    <origin
-      xyz="0 -0.256 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link_2" />
-    <child
-      link="link_3" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.106"
-      upper="3.106"
-      effort="30"
-      velocity="3.14" />
-  </joint>
-  <link
-    name="link_4">
-    <inertial>
-      <origin
-        xyz="-0.000005 -0.084658 0.004747"
-        rpy="0 0 0" />
-      <mass
-        value="0.685" />
-      <inertia
-        ixx="0.001282444"
-        ixy="-0.000000551"
-        ixz="-0.000000630"
-        iyy="0.000373013"
-        iyz="-0.000232084"
-        izz="0.001256177" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_4.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_4.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_4"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="-1.5708 0 0" />
-    <parent
-      link="link_3" />
-    <child
-      link="link_4" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.356"
-      upper="2.356"
-      effort="30"
-      velocity="3.14" />
-  </joint>
-  <link
-    name="link_5">
-    <inertial>
-      <origin
-        xyz="0.000078 -0.012937 -0.008781"
-        rpy="0 0 0" />
-      <mass
-        value="0.619" />
-      <inertia
-        ixx="0.000627336"
-        ixy="0.000001636"
-        ixz="-0.000001345"
-        iyy="0.000542455"
-        iyz="0.000034970"
-        izz="0.000370291" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_5.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_5.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_5"
-    type="revolute">
-    <origin
-      xyz="0 -0.21 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link_4" />
-    <child
-      link="link_5" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.106"
-      upper="3.106"
-      effort="10"
-      velocity="3.14" />
-  </joint>
-  <link
-    name="link_6">
-    <inertial>
-      <origin
-        xyz="-0.000014 -0.078524 0.002819"
-        rpy="0 0 0" />
-      <mass
-        value="0.602" />
-      <inertia
-        ixx="0.000780774"
-        ixy="-0.000000121"
-        ixz="-0.000000469"
-        iyy="0.000289973"
-        iyz="-0.000120513"
-        izz="0.000763955" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_6.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_6.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_6"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="-1.5708 0 0" />
-    <parent
-      link="link_5" />
-    <child
-      link="link_6" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.234"
-      upper="2.234"
-      effort="10"
-      velocity="3.14" />
-  </joint>
-  <link
-    name="link_7">
-    <inertial>
-      <origin
-        xyz="0.001094 -0.000077 -0.010119"
-        rpy="0 0 0" />
-      <mass
-        value="0.107" />
-      <inertia
-        ixx="0.000044123"
-        ixy="-0.000000064"
-        ixz="0.0000003"
-        iyy="0.000035078"
-        iyz="-0.000000029"
-        izz="0.000065445" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_7.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link_7.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_7"
-    type="revolute">
-    <origin
-      xyz="0 -0.144 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link_6" />
-    <child
-      link="link_7" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-6.28"
-      upper="6.28"
-      effort="10"
-      velocity="3.14" />
-  </joint>
-    <!-- Scissor end-effector -->
-  <link name="scissor_link">
-    <inertial>
-      <!-- Replace these values with CAD-derived mass properties -->
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="0.1" />
-      <inertia
-        ixx="0.0001"
-        ixy="0"
-        ixz="0"
-        iyy="0.0001"
-        iyz="0"
-        izz="0.0001" />
-    </inertial>
-
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/scissor.stl"
-          scale="1 1 1" />
-      </geometry>
-      <material name="scissor_material">
-        <color rgba="0.7 0.7 0.7 1" />
-      </material>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/scissor.stl"
-          scale="1 1 1" />
-      </geometry>
-    </collision>
-  </link>
-
-  <!-- Rigidly attach the scissor to the final wrist link -->
-  <joint name="scissor_fixed_joint" type="fixed">
-    <parent link="link_7" />
-    <child link="scissor_link" />
-    <origin xyz="0 0 0.165" rpy="0 0 0" />
-  </joint>
-  
-  
-  <link name="scissor_tcp"/>
-
-<joint name="scissor_tcp_fixed" type="fixed">
-  <parent link="scissor_link"/>
-  <child link="scissor_tcp"/>
-  <origin xyz="0 0 0" rpy="0 0 0"/>
-</joint>
-
-<link name="camera_tcp"/>
-
-<joint name="camera_tcp_fixed" type="fixed">
-  <parent link="scissor_link"/>
-  <child link="camera_tcp"/>
-  <origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
-</joint>
-
-
-
-</robot>
@@ -1,516 +0,0 @@
-<?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
ZhongyuWang-cart	cce375e4a5	test push	2026-07-07 17:08:33 +08:00
ZhengLiu-cart	377a676863	Update README.md	2026-06-14 04:17:40 +08:00
ZhengLiu-cart	9db480c474	Upload files to "img"	2026-06-14 04:13:40 +08:00
ZhengLiu-cart	3a8aecbf71	Update README.md add visualization	2026-06-09 17:28:57 +08:00
ZhengLiu-cart	7c8beaa3b9	Upload files to "kine_ctrl/visual/mjc_ik_test1"	2026-06-09 17:23:17 +08:00