From 0bed2f87f4973da32a6b9a5c5190c97ce38fcfe9 Mon Sep 17 00:00:00 2001
From: LiuzhengSJ <zhengliu@sjtu.edu.cn>
Date: Wed, 3 Jun 2026 21:14:34 +0100
Subject: [PATCH] wrong, not converged

---
 kine_ctrl/rm75_kine_qp.py | 38 ++++++++++++++++++++++++--------------
 1 file changed, 24 insertions(+), 14 deletions(-)

diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py
index e1e921e..ce5e9d0 100644
--- a/kine_ctrl/rm75_kine_qp.py
+++ b/kine_ctrl/rm75_kine_qp.py
@@ -232,6 +232,7 @@ class KinematicsSolver():
                 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
@@ -246,7 +247,7 @@ class KinematicsSolver():
                 self.data,
                 q,
                 ee_frame_id,
-                pin.ReferenceFrame.LOCAL
+                pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
             )
 
             pin.forwardKinematics(self.model, self.data, q)
@@ -276,13 +277,9 @@ class KinematicsSolver():
                 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
+                    if error_norm < best_error:
+                        best_error = error_norm
+                        best_solution = q[:7].copy()
                     break
 
                 # Check if error is increasing (diverging)
@@ -296,19 +293,28 @@ class KinematicsSolver():
                     self.model,
                     self.data,
                     ee_frame_id,
-                    pin.ReferenceFrame.LOCAL
+                    pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
                 )
 
                 # =========================
                 # QP-based IK
                 # =========================
+                w_posture = 0.0
 
-                H = J.T @ self.W @ J
+                J_eff = J # -pin.Jlog6(error_SE3) @ 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.T @ self.W @ error_vec
+                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
@@ -359,13 +365,18 @@ class KinematicsSolver():
 
                 dq = result.x
 
-                print(f'pred = {J @ dq} and error_vec = {error_vec}')
+                pred_err = np.linalg.norm(error_vec)
+                pred_next = np.linalg.norm(error_vec - J_eff @ dq)
+
+                print("predicted error:", pred_next)
+
+                print(f'pred = {J_eff @ dq} and error_vec = {error_vec}')
 
                 if dq is None:
                     break
 
                 # Apply joint limits with scaling
-                alpha = 0.5
+                alpha = 0.2
                 q = pin.integrate(self.model, q, alpha * dq)
 
                 prev_error = error_norm
@@ -388,7 +399,6 @@ class KinematicsSolver():
             print(
                 "converged",
                 error_norm,
-                position_error
             )
             return best_solution, True, best_error
         else: