ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

correct the rm official ik issue.

Browse Source 
out of workspace ik calculation may return ret = 0.
in this version, the fk verification is done for double check its success.
This commit is contained in:
LiuzhengSJ
2026-07-03 20:13:05 +01:00
parent 12ead6a191
commit fb414078f1
3 changed files with 60 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
kine_ctrl/workspace_comfortable/workspace_cal.py
View File

@ -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)
Y_RANGE = (-0.25, 0.6)
Z_RANGE = (0.1, 0.6)
X_RANGE = (-0.6, 0.6)
Y_RANGE = (-0.6, 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: