From e06e48f21b8440ecf106ea9bcdb562eae130140e Mon Sep 17 00:00:00 2001
From: LiuzhengSJ <zhengliu@sjtu.edu.cn>
Date: Mon, 6 Jul 2026 11:10:35 +0100
Subject: [PATCH] ik success bug identified.

---
 kine_ctrl/main.py                             | 17 +++++----
 kine_ctrl/rm75_kine_rm.py                     |  3 +-
 .../workspace_comfortable/workspace_cal.py    | 38 ++++++++++---------
 3 files changed, 32 insertions(+), 26 deletions(-)

diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py
index 3a2a1e0..26bf307 100644
--- a/kine_ctrl/main.py
+++ b/kine_ctrl/main.py
@@ -48,14 +48,15 @@ def main():
     robot_kine_rm.add_tool_frames(tools_in_ee)
     robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
-    # ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
-    #                                              initial_guess=[0.1] * 7, tool="no_tool")
-    #
-    # print(f'ret_rm = {ret_rm}, q = {q}')
-    # pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
-    # print(f'pose = {pose}')
-    #
-    # time.sleep(5)
+    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool="no_tool")
+
+    print(f'ret_rm = {ret_rm}, q = {q}')
+    pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
+    print(f'pose = {pose}')
+    print('-'*100)
+
+    time.sleep(5)
 
 
 
diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py
index 54853b4..eb175ee 100644
--- a/kine_ctrl/rm75_kine_rm.py
+++ b/kine_ctrl/rm75_kine_rm.py
@@ -137,7 +137,8 @@ class rm75_kine_api():
         # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
         #
         # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
-        if ret < 0:
+        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+        if int(ret) < 0:
             return ret, [ q/180*math.pi for q in q_out]
         elif pose_dis < 0.01:
             return ret, [ q/180*math.pi for q in q_out]
diff --git a/kine_ctrl/workspace_comfortable/workspace_cal.py b/kine_ctrl/workspace_comfortable/workspace_cal.py
index cd7f99c..c287012 100644
--- a/kine_ctrl/workspace_comfortable/workspace_cal.py
+++ b/kine_ctrl/workspace_comfortable/workspace_cal.py
@@ -47,6 +47,27 @@ from Robotic_Arm.rm_robot_interface import *
 import time
 from math import radians, degrees, pi, cos, sin
 
+# Cartesian workspace grid, in meters.
+# Adjust according to your robot placement and task.
+X_RANGE = (-0.6, 0.6)
+Y_RANGE = (-0.6, 0.6)
+Z_RANGE = (0.0, 0.8)
+
+GRID_RESOLUTION = 0.025  # 5 cm. Use 0.02 for finer but slower.
+
+# Comfort thresholds
+MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
+MAX_CONDITION_NUMBER = 150.0
+MIN_MANIPULABILITY_RATIO = 0.10
+
+# Scoring weights
+WEIGHT_IK_SUCCESS = 0.70
+WEIGHT_JOINT_LIMIT = 0.10
+WEIGHT_MANIPULABILITY = 0.1
+WEIGHT_SINGULARITY = 0.1
+
+
+
 
 #   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
 #   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
@@ -98,24 +119,7 @@ JOINT_NAMES = [
     "joint_7",
 ]
 
-# Cartesian workspace grid, in meters.
-# Adjust according to your robot placement and task.
-X_RANGE = (-0.6, 0.6)
-Y_RANGE = (-0.6, 0.6)
-Z_RANGE = (0.0, 0.8)
 
-GRID_RESOLUTION = 0.025  # 5 cm. Use 0.02 for finer but slower.
-
-# Comfort thresholds
-MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
-MAX_CONDITION_NUMBER = 150.0
-MIN_MANIPULABILITY_RATIO = 0.10
-
-# Scoring weights
-WEIGHT_IK_SUCCESS = 0.70
-WEIGHT_JOINT_LIMIT = 0.10
-WEIGHT_MANIPULABILITY = 0.1
-WEIGHT_SINGULARITY = 0.1
 
 # Numerical Jacobian settings
 JACOBIAN_EPS = 1e-5