From f10152fc2986138ce5c3a23fa280bda946e017b9 Mon Sep 17 00:00:00 2001
From: LiuzhengSJ <zhengliu@sjtu.edu.cn>
Date: Thu, 4 Jun 2026 14:31:29 +0100
Subject: [PATCH] correct the w_posture

---
 kine_ctrl/main.py         | 11 +++++++----
 kine_ctrl/rm75_kine_qp.py | 21 ++++++++++++++-------
 2 files changed, 21 insertions(+), 11 deletions(-)

diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py
index 554700b..c6319c1 100644
--- a/kine_ctrl/main.py
+++ b/kine_ctrl/main.py
@@ -61,7 +61,7 @@ def demo_position_control():
     target_rpy = [0.78576018, 0.67554633, 1.86302226]
     target_p = target_position + target_rpy
     # target_p_rad = [radians(pos) for pos in target_position] + target_rpy
-    initial_guess = [11, 20, -30, -40, 50, 60, 71] # [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] #
+    initial_guess = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] # [0.0, 20, -30, -40, 50, 60, 91] #
     initial_guess_rad = [ radians(j) for j in initial_guess ]
     tool_name = "scissor"
 
@@ -71,12 +71,15 @@ def demo_position_control():
 
     joint_solution, success, error = robot_kine_qp.inverse_kinematics(
             target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
-            max_iter=500, debug=False, tool=tool_name
-        )
+            max_iter=500, debug=False, tool=tool_name )
 
-    print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
     if success:
+        print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
         print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
+    else:
+        print(f'solution: {joint_solution} success flag {success}, error {error}\n')
+        print(
+            f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)}\n')
 
 
 
diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py
index b1b7796..d8908c9 100644
--- a/kine_ctrl/rm75_kine_qp.py
+++ b/kine_ctrl/rm75_kine_qp.py
@@ -255,7 +255,7 @@ class KinematicsSolver():
             error_SE3 = current_placement.actInv(target_placement)
             error_vec = pin.log(error_SE3).vector
 
-            print("initial error =", np.linalg.norm(error_vec))
+            print("\n initial error =", np.linalg.norm(error_vec))
             print(error_vec)
 
             while iter_count < max_iter:
@@ -295,7 +295,7 @@ class KinematicsSolver():
                 # =========================
                 # QP-based IK
                 # =========================
-                w_posture = 0.0
+                w_posture = 0.0001
 
                 J_eff = pin.Jlog6(error_SE3) @ J #J #
 
@@ -348,6 +348,14 @@ class KinematicsSolver():
                 print("error", error_norm)
                 print("cond(H)", np.linalg.cond(H))
 
+                u, s, vh = np.linalg.svd(J_eff)
+                print("sv =", s)
+
+                print("trans =", error_vec[:3])
+                print("rot   =", error_vec[3:])
+
+
+
                 # Solve
                 result = self.osqp_solver.solve()
                 print("OSQP status =", result.info.status)
@@ -385,11 +393,10 @@ class KinematicsSolver():
                 fk0 = self.forward_kinematics(initial_guess)
                 print("fk guess:", fk0)
 
-                print("initial error norm:", error_norm)
-
-                print(iter_count,
-                      error_norm,
-                      result.info.status)
+                print(result.info.status)
+                print(np.degrees(q))
+                print(np.degrees(self.model.upperPositionLimit[:7]))
+                print(np.degrees(self.model.lowerPositionLimit[:7]))
 
         if best_solution is not None:
             print(