correct the rm official ik issue.

out of workspace ik calculation may return ret = 0.
in this version, the fk verification is done for double check its success.

kine_ctrl/main.py

@@ -22,12 +22,12 @@ tools_in_ee = {
 }
 
 # joint limit
-ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
+# 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
+# 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
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
-# lb = -ub
+lb = -ub
 
 tool_name = "scissor"
 
@@ -48,6 +48,17 @@ 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)
+
+
+
 
 
 #   --------------  for comparison   ----------------
@@ -59,7 +70,7 @@ def main():
 
         solve_sum = 0
 
-        for i in range(10):
+        for i in range(1000):
             print(f'\n-------------- in i = {i} ----------------')
             joint_rand = np.random.uniform(ub, lb)
             print(f'the predefined joints are {joint_rand}')
@@ -83,27 +94,27 @@ def main():
             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}')
+                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.send_command(q)
-                robot_mjk.wait_until_reached()
+                # robot_mjk.wait_until_reached()
-                robot_mjk.print_state()
+                # 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}')
+                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}')
+                print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
                 if d_p_ik < 0.01:
                     result[1][1] += 1
             else:
-                print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
+                print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
 
             if ret_qp == 0 or ret_rm == 0:
                 solve_sum += 1

kine_ctrl/rm75_kine_rm.py

@@ -7,7 +7,7 @@ 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
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 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)
@@ -120,14 +120,36 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = target_position + target_rpy
+        target = list(target_position) + list(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)
+        # print(f'the arm angle is ret = {ret}, and phi = {phi}')
         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)
+        pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
+        pose_dis = cal_pose_deviation(pose_fk, target)
+
+        # 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:
             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]
+        else:
+            return -10, [ q/180*math.pi for q in q_out]
+
+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] > math.pi:
+            d_fk_p1[j] -= 2 * math.pi
+        while d_fk_p1[j] < -math.pi:
+            d_fk_p1[j] += 2 * math.pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk

kine_ctrl/workspace_comfortable/workspace_cal.py

@@ -100,11 +100,11 @@ JOINT_NAMES = [
 
 # Cartesian workspace grid, in meters.
 # Adjust according to your robot placement and task.
-X_RANGE = (-0.25, 0.6)
+X_RANGE = (-0.6, 0.6)
-Y_RANGE = (-0.25, 0.6)
+Y_RANGE = (-0.6, 0.6)
-Z_RANGE = (0.1, 0.6)
+Z_RANGE = (0.0, 0.8)
 
-GRID_RESOLUTION = 0.1  # 5 cm. Use 0.02 for finer but slower.
+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
@@ -126,7 +126,7 @@ JACOBIAN_EPS = 1e-5
 # 2. TASK ORIENTATION SAMPLING
 # ============================================================
 
-def make_task_orientations(num_orientations=60, seed=1):
+def make_task_orientations(num_orientations=200, seed=1):
     """
     Random orientation sampling using RM's Euler convention:
 
@@ -229,6 +229,8 @@ def solve_ik_user(target_position, target_rotation):
     ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
     # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
     if ret_rm == 0:
+        pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
+        # print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}')
         return q
 
 
@@ -548,19 +550,20 @@ def evaluate_workspace():
             attempted += 1
 
             # print(f"\n - target point: {point}, target orientation: {rpy}")
-
+            rpy = [1.2022060487764064, -1.0097962261845583, -0.6518417572686532]
             ik_result = solve_ik_user(point, rpy)
+
+            # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
             candidate_solutions = normalize_ik_solutions(ik_result)
 
-
-
-
             if len(candidate_solutions) == 0:
                 continue
 
             evaluated_solutions = []
 
             for q in candidate_solutions:
+                # pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name)
+                # print(f'the fk of q is {pose}\n')
                 metrics = evaluate_single_solution(robot, q, lower, upper)
                 # print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
                 if metrics is not None: