From 6c8a335e1db48d0584b6523770485c0488daa057 Mon Sep 17 00:00:00 2001
From: LiuzhengSJ <zhengliu@sjtu.edu.cn>
Date: Mon, 22 Jun 2026 13:29:58 +0100
Subject: [PATCH] start to adjust to ready-to-use class

---
 kine_ctrl/fix_robotics_env.sh |  6 ++++
 kine_ctrl/main.py             |  8 ++---
 kine_ctrl/rm75_kine_qp.py     | 63 ++++++++++-------------------------
 kine_ctrl/rm75_kine_rm.py     |  2 +-
 4 files changed, 28 insertions(+), 51 deletions(-)
 create mode 100755 kine_ctrl/fix_robotics_env.sh

diff --git a/kine_ctrl/fix_robotics_env.sh b/kine_ctrl/fix_robotics_env.sh
new file mode 100755
index 0000000..48fda83
--- /dev/null
+++ b/kine_ctrl/fix_robotics_env.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+echo "Fixing robotics environment..."
+conda activate coppeliasim
+export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
+pip install osqp==0.6.2.post8 --force-reinstall
+python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"
diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py
index 71da75f..77ef3a1 100644
--- a/kine_ctrl/main.py
+++ b/kine_ctrl/main.py
@@ -32,11 +32,11 @@ def main():
 
     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([150.0, 110.0, 170.0,  130, 175.0, 125.0,  179.0])/180*pi
+        lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0,  -179.0])/180*pi
 
-        ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
-        lb = -ub
+        # 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)
 
diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py
index 6e78fd6..9e11d37 100644
--- a/kine_ctrl/rm75_kine_qp.py
+++ b/kine_ctrl/rm75_kine_qp.py
@@ -8,6 +8,7 @@ import osqp
 from scipy import sparse
 from math import radians, degrees, pi, cos, sin
 import time
+import threading
 
 
 
@@ -24,61 +25,18 @@ class KinematicsSolver():
         # -------------------------------------------------
         # 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
-            )
-        )
-
-
+        self.add_frame(frame_name="ee", position=[0.0, 0.0, 0.0], rotationXYZ=[0.0, 0.0, 0.0])
         # -------------------------------------------------
         # 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
-            )
-        )
-
+        self.add_frame(frame_name="scissor", position=[0.0, 0.0, 0.19], rotationXYZ=[0.0, 0.0, 0.0])
         # -------------------------------------------------
         # 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
-            )
-        )
-
+        self.add_frame(frame_name="camera",position=[0.05, 0.02, 0.10], rotationXYZ=[-pi*0.5, 0.0, -pi*0.5])
         # -------------------------------------------------
         # Store tool frame IDs
         # -------------------------------------------------
-
         self.tool_frames = {
             "scissor": self.model.getFrameId("scissor"),
             "camera": self.model.getFrameId("camera"),
@@ -118,6 +76,19 @@ class KinematicsSolver():
 
         self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
 
+    def add_frame(self,frame_name, position, rotationXYZ):
+        '''
+        :param frame_name:  str
+        :param position: [x, y, z] target position (meters)
+        :param rotationXYZ: [x, y, z] target rotation (rad)
+        '''
+        camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0],  rotationXYZ[1],  rotationXYZ[2] )
+        camera_offset = pin.SE3(
+            camera_rotation,
+            np.array(position)
+        )
+        self.model.addFrame(  pin.Frame(  frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
+
     def 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]
diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py
index 218590c..9fd3790 100644
--- a/kine_ctrl/rm75_kine_rm.py
+++ b/kine_ctrl/rm75_kine_rm.py
@@ -16,7 +16,7 @@ class rm75_kine_api():
 
         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),
+            'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.19, 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 = {