add collision detection in workspace cal

kine_ctrl/main.py

@@ -47,8 +47,11 @@ def main():
     robot_kine_qp.add_tool_frames(tools_in_ee)
     robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
-    fp = robot_kine_qp.forward_kinematics(np.zeros(7),tool='scissor_tcp')
+    fp = robot_kine_qp.forward_kinematics(np.ones(7)*0.3,tool='scissor_tcp')
     print(f'forward kine res = {fp}')
+    ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=fp[0:3], target_rpy=fp[3:6], initial_guess=np.zeros(7),
+                                     tool='scissor_tcp')
+
 
     #   ----------      rm75 official algorithm         -----------
     robot_kine_rm = kine_rm()

kine_ctrl/workspace_comfortable/workspace_cal.py

@@ -3,7 +3,7 @@ RM75-B comfortable workspace evaluator.
 
 You provide:
     - URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
-    - your own IK solver inside solve_ik_user()
+    - your own IK solver inside solve_ik()
 
 This script computes:
     - IK success rate
@@ -57,9 +57,13 @@ GRID_RESOLUTION = 0.05  # 5 cm. Use 0.02 for finer but slower.
 
 num_orientations =  120
 
-tool_name = "v_minis"
+tool_name = "scissor"
 
-output_csv = "rm75b_comfort_workspace"  + tool_name + ".csv"
+
+
+URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-SCI.urdf'
+
+output_csv = "workspace"  + tool_name + URDF_PATH.split('/')[-1].split('.')[0] + ".csv"
 
 
 # Comfort thresholds
@@ -95,7 +99,6 @@ ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
 lb = -ub
 
 
-URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
 MESH_DIR = str(Path(URDF_PATH).parent)
 
 #  -----------      rm75 qp based kine          ------------
@@ -164,40 +167,12 @@ def make_task_orientations(num_orientations=num_orientations, seed=1):
 
     return orientations
 
-#
-# def euler_angles_to_rotation_matrix(rx, ry, rz):
-#     """
-#     Official RM convention:
-#         R = Rz @ Ry @ Rx
-#     This matches scipy:
-#         Rotation.from_euler("xyz", [rx, ry, rz]).as_matrix()
-#     """
-#     Rx = np.array([
-#         [1, 0, 0],
-#         [0, np.cos(rx), -np.sin(rx)],
-#         [0, np.sin(rx),  np.cos(rx)]
-#     ])
-#
-#     Ry = np.array([
-#         [ np.cos(ry), 0, np.sin(ry)],
-#         [0,           1, 0],
-#         [-np.sin(ry), 0, np.cos(ry)]
-#     ])
-#
-#     Rz = np.array([
-#         [np.cos(rz), -np.sin(rz), 0],
-#         [np.sin(rz),  np.cos(rz), 0],
-#         [0,           0,          1]
-#     ])
-#
-#     return Rz @ Ry @ Rx
-
 
 # ============================================================
-# 3. YOUR IK FUNCTION GOES HERE
+# 3.  IK FUNCTION GOES HERE
 # ============================================================
 
-def solve_ik_user(target_position, target_rotation):
+def solve_ik(target_position, target_rotation):
     """
     Replace this function with your own IK solver.
 
@@ -229,23 +204,20 @@ def solve_ik_user(target_position, target_rotation):
         [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
     """
 
-    # ========================================================
-    # INSERT YOUR IK CODE HERE
-    # ========================================================
     initial_guess = [0.1] * 7
 
     ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250)
     # print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
     if ret_qp == 0:
-        return q
+        if not robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True):
+            return q
 
 
     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
+        if not robot_kine_qp.collision_detect(q, stop_at_first_collision=True, verbose=True):
+            return q
 
 
     return None
@@ -564,7 +536,7 @@ def evaluate_workspace():
             attempted += 1
 
 
-            ik_result = solve_ik_user(point, rpy)
+            ik_result = solve_ik(point, rpy)
 
             # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
             candidate_solutions = normalize_ik_solutions(ik_result)