ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add workspace reachability evaluation file.

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-03 15:13:42 +01:00
parent 319c1765bc
commit 12ead6a191
1 changed files with 99 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

194
kine_ctrl/workspace_comfortable/workspace_cal.py
View File

@ -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: