add workspace reachability evaluation file.

kine_ctrl/workspace_comfortable/workspace_cal.py

@@ -58,12 +58,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 = "no_tool"
 
@@ -100,35 +100,33 @@ JOINT_NAMES = [
 
 # Cartesian workspace grid, in meters.
 # Adjust according to your robot placement and task.
-X_RANGE = (-0.6, 0.6)
+X_RANGE = (-0.25, 0.6)
-Y_RANGE = (-0.6, 0.6)
+Y_RANGE = (-0.25, 0.6)
-Z_RANGE = (0.0, 1.00)
+Z_RANGE = (0.1, 0.6)
 
-GRID_RESOLUTION = 0.075  # 5 cm. Use 0.02 for finer but slower.
+GRID_RESOLUTION = 0.1  # 5 cm. Use 0.02 for finer but slower.
 
 # Comfort thresholds
-MIN_JOINT_MARGIN = 0.15       # 15% away from joint limits
+MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
-MAX_CONDITION_NUMBER = 80.0
+MAX_CONDITION_NUMBER = 150.0
-MIN_MANIPULABILITY_RATIO = 0.20
+MIN_MANIPULABILITY_RATIO = 0.10
 
 # Scoring weights
-WEIGHT_IK_SUCCESS = 0.30
+WEIGHT_IK_SUCCESS = 0.70
-WEIGHT_JOINT_LIMIT = 0.30
+WEIGHT_JOINT_LIMIT = 0.10
-WEIGHT_MANIPULABILITY = 0.25
+WEIGHT_MANIPULABILITY = 0.1
-WEIGHT_SINGULARITY = 0.15
+WEIGHT_SINGULARITY = 0.1
 
 # Numerical Jacobian settings
 JACOBIAN_EPS = 1e-5
 
-# If your IK returns multiple solutions, set this True.
-IK_RETURNS_MULTIPLE_SOLUTIONS = False
 
 
 # ============================================================
 # 2. TASK ORIENTATION SAMPLING
 # ============================================================
 
-def make_task_orientations(num_orientations=200, seed=1):
+def make_task_orientations(num_orientations=60, seed=1):
     """
     Random orientation sampling using RM's Euler convention:
 
@@ -152,33 +150,33 @@ def make_task_orientations(num_orientations=200, seed=1):
 
     return orientations
 
-
+#
-def euler_angles_to_rotation_matrix(rx, ry, rz):
+# def euler_angles_to_rotation_matrix(rx, ry, rz):
-    """
+#     """
-    Official RM convention:
+#     Official RM convention:
-        R = Rz @ Ry @ Rx
+#         R = Rz @ Ry @ Rx
-    This matches scipy:
+#     This matches scipy:
-        Rotation.from_euler("xyz", [rx, ry, rz]).as_matrix()
+#         Rotation.from_euler("xyz", [rx, ry, rz]).as_matrix()
-    """
+#     """
-    Rx = np.array([
+#     Rx = np.array([
-        [1, 0, 0],
+#         [1, 0, 0],
-        [0, np.cos(rx), -np.sin(rx)],
+#         [0, np.cos(rx), -np.sin(rx)],
-        [0, np.sin(rx),  np.cos(rx)]
+#         [0, np.sin(rx),  np.cos(rx)]
-    ])
+#     ])
-
+#
-    Ry = np.array([
+#     Ry = np.array([
-        [ np.cos(ry), 0, np.sin(ry)],
+#         [ np.cos(ry), 0, np.sin(ry)],
-        [0,           1, 0],
+#         [0,           1, 0],
-        [-np.sin(ry), 0, np.cos(ry)]
+#         [-np.sin(ry), 0, np.cos(ry)]
-    ])
+#     ])
-
+#
-    Rz = np.array([
+#     Rz = np.array([
-        [np.cos(rz), -np.sin(rz), 0],
+#         [np.cos(rz), -np.sin(rz), 0],
-        [np.sin(rz),  np.cos(rz), 0],
+#         [np.sin(rz),  np.cos(rz), 0],
-        [0,           0,          1]
+#         [0,           0,          1]
-    ])
+#     ])
-
+#
-    return Rz @ Ry @ Rx
+#     return Rz @ Ry @ Rx
 
 
 # ============================================================
@@ -223,13 +221,17 @@ def solve_ik_user(target_position, target_rotation):
     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
 
+
     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:
         return q
 
+
     return None
 
 
@@ -262,30 +264,30 @@ def load_robot_and_limits(urdf_path):
     return robot, lower, upper
 
 
-def q_to_cfg(q):
+# def q_to_cfg(q):
-    """
+#     """
-    Convert joint vector to urdfpy FK config dictionary.
+#     Convert joint vector to urdfpy FK config dictionary.
-    """
+#     """
-    return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
+#     return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
 
 
-def fk_transform(robot, q):
+# def fk_transform(robot, q):
-    """
+#     """
-    Forward kinematics from base_link to TCP_LINK.
+#     Forward kinematics from base_link to TCP_LINK.
-
+#
-    Returns
+#     Returns
-    -------
+#     -------
-    T : np.ndarray, shape (4, 4)
+#     T : np.ndarray, shape (4, 4)
-    """
+#     """
-    cfg = q_to_cfg(q)
+#     cfg = q_to_cfg(q)
-    fk = robot.link_fk(cfg=cfg)
+#     fk = robot.link_fk(cfg=cfg)
-    tcp_link = robot.link_map[TCP_LINK]
+#     tcp_link = robot.link_map[TCP_LINK]
-    return fk[tcp_link]
+#     return fk[tcp_link]
-
+#
-
+#
-def fk_position(robot, q):
+# def fk_position(robot, q):
-    T = fk_transform(robot, q)
+#     T = fk_transform(robot, q)
-    return T[:3, 3]
+#     return T[:3, 3]
 
 
 # ============================================================
@@ -328,31 +330,25 @@ def joint_margin(q, lower, upper):
     return float(np.min(margin))
 
 
-def numerical_position_jacobian(robot, q, eps=1e-5):
+def q_to_cfg(q):
     """
-    Numerical translational Jacobian, shape (3, 7).
+    Convert joint vector to urdfpy FK config dictionary.
-
-    This measures TCP linear velocity sensitivity to joint velocities.
     """
-    q = np.asarray(q, dtype=float)
+    return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
-    n = len(q)
 
-    J = np.zeros((3, n))
-    p0 = fk_position(robot, q)
 
-    for i in range(n):
+def fk_transform(robot, q):
-        q_plus = q.copy()
+    """
-        q_minus = q.copy()
+    Forward kinematics from base_link to TCP_LINK.
 
-        q_plus[i] += eps
+    Returns
-        q_minus[i] -= eps
+    -------
-
+    T : np.ndarray, shape (4, 4)
-        p_plus = fk_position(robot, q_plus)
+    """
-        p_minus = fk_position(robot, q_minus)
+    cfg = q_to_cfg(q)
-
+    fk = robot.link_fk(cfg=cfg)
-        J[:, i] = (p_plus - p_minus) / (2.0 * eps)
+    tcp_link = robot.link_map[TCP_LINK]
-
+    return fk[tcp_link]
-    return J
 
 
 def numerical_geometric_jacobian(robot, q, eps=1e-5):
@@ -453,15 +449,18 @@ def singularity_score(condition_number):
 
 def normalize_ik_solutions(ik_result):
     """
-    Convert IK return into a list of q vectors.
+    Your IK returns:
+        - None if failed
+        - one list/array of 7 joint values if successful
     """
     if ik_result is None:
         return []
 
-    if isinstance(ik_result, list) or isinstance(ik_result, tuple):
-        return [np.asarray(q, dtype=float).reshape(-1) for q in ik_result]
-
     q = np.asarray(ik_result, dtype=float).reshape(-1)
+
+    if q.shape[0] != 7:
+        return []
+
     return [q]
 
 
@@ -544,14 +543,18 @@ def evaluate_workspace():
         attempted = 0
         ik_success_count = 0
 
+
         for rpy in orientations:
             attempted += 1
 
-
+            # print(f"\n - target point: {point}, target orientation: {rpy}")
 
             ik_result = solve_ik_user(point, rpy)
             candidate_solutions = normalize_ik_solutions(ik_result)
 
+
+
+
             if len(candidate_solutions) == 0:
                 continue
 
@@ -559,6 +562,7 @@ def evaluate_workspace():
 
             for q in candidate_solutions:
                 metrics = evaluate_single_solution(robot, q, lower, upper)
+                # print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
                 if metrics is not None:
                     evaluated_solutions.append(metrics)
 
@@ -577,7 +581,7 @@ def evaluate_workspace():
 
             point_solution_metrics.append(best)
             all_valid_solution_metrics.append(best)
-
+        print(f'this position+all orientations, the point_solution_metrics = {point_solution_metrics}')
         ik_success_rate = ik_success_count / attempted if attempted > 0 else 0.0
 
         if len(point_solution_metrics) == 0: