From 12ead6a19128f3029fa6b12f9e7aad5f944758e9 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Fri, 3 Jul 2026 15:13:42 +0100 Subject: [PATCH] add workspace reachability evaluation file. --- .../workspace_comfortable/workspace_cal.py | 194 +++++++++--------- 1 file changed, 99 insertions(+), 95 deletions(-) diff --git a/kine_ctrl/workspace_comfortable/workspace_cal.py b/kine_ctrl/workspace_comfortable/workspace_cal.py index 756c22b..39f6be0 100644 --- a/kine_ctrl/workspace_comfortable/workspace_cal.py +++ b/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 -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 -# lb = -ub +# 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 +# +# +ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi +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) -Y_RANGE = (-0.6, 0.6) -Z_RANGE = (0.0, 1.00) +X_RANGE = (-0.25, 0.6) +Y_RANGE = (-0.25, 0.6) +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 -MAX_CONDITION_NUMBER = 80.0 -MIN_MANIPULABILITY_RATIO = 0.20 +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.30 -WEIGHT_JOINT_LIMIT = 0.30 -WEIGHT_MANIPULABILITY = 0.25 -WEIGHT_SINGULARITY = 0.15 +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 -# 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): - """ - 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 +# +# 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 # ============================================================ @@ -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): - """ - Convert joint vector to urdfpy FK config dictionary. - """ - return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)} +# def q_to_cfg(q): +# """ +# Convert joint vector to urdfpy FK config dictionary. +# """ +# return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)} -def fk_transform(robot, q): - """ - Forward kinematics from base_link to TCP_LINK. - - Returns - ------- - T : np.ndarray, shape (4, 4) - """ - cfg = q_to_cfg(q) - fk = robot.link_fk(cfg=cfg) - tcp_link = robot.link_map[TCP_LINK] - return fk[tcp_link] - - -def fk_position(robot, q): - T = fk_transform(robot, q) - return T[:3, 3] +# def fk_transform(robot, q): +# """ +# Forward kinematics from base_link to TCP_LINK. +# +# Returns +# ------- +# T : np.ndarray, shape (4, 4) +# """ +# cfg = q_to_cfg(q) +# fk = robot.link_fk(cfg=cfg) +# tcp_link = robot.link_map[TCP_LINK] +# return fk[tcp_link] +# +# +# def fk_position(robot, q): +# T = fk_transform(robot, q) +# 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). - - This measures TCP linear velocity sensitivity to joint velocities. + Convert joint vector to urdfpy FK config dictionary. """ - q = np.asarray(q, dtype=float) - n = len(q) + return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)} - J = np.zeros((3, n)) - p0 = fk_position(robot, q) - for i in range(n): - q_plus = q.copy() - q_minus = q.copy() +def fk_transform(robot, q): + """ + Forward kinematics from base_link to TCP_LINK. - q_plus[i] += eps - q_minus[i] -= eps - - p_plus = fk_position(robot, q_plus) - p_minus = fk_position(robot, q_minus) - - J[:, i] = (p_plus - p_minus) / (2.0 * eps) - - return J + Returns + ------- + T : np.ndarray, shape (4, 4) + """ + cfg = q_to_cfg(q) + fk = robot.link_fk(cfg=cfg) + tcp_link = robot.link_map[TCP_LINK] + return fk[tcp_link] 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: