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

35
kine_ctrl/main.py
View File

@ -22,12 +22,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([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([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
lb = -ub
tool_name = "scissor"
@ -48,6 +48,17 @@ def main():
robot_kine_rm.add_tool_frames(tools_in_ee)
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
# ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
# initial_guess=[0.1] * 7, tool="no_tool")
#
# print(f'ret_rm = {ret_rm}, q = {q}')
# pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
# print(f'pose = {pose}')
#
# time.sleep(5)
# -------------- for comparison ----------------
@ -59,7 +70,7 @@ def main():
solve_sum = 0
for i in range(10):
for i in range(1000):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub, lb)
print(f'the predefined joints are {joint_rand}')
@ -83,27 +94,27 @@ def main():
if ret_qp == 0:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
print(f'---- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
if d_p_ik < 0.01:
result[0][1] += 1
robot_mjk.send_command(q)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
# robot_mjk.send_command(q)
# robot_mjk.wait_until_reached()
# robot_mjk.print_state()
else:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
print(f'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
if ret_rm == 0:
fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
if d_p_ik < 0.01:
result[1][1] += 1
else:
print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}')
print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
if ret_qp == 0 or ret_rm == 0:
solve_sum += 1

26
kine_ctrl/rm75_kine_rm.py
View File

@ -7,7 +7,7 @@ class rm75_kine_api():
def __init__(self):
# ---------- rm75 official algorithm -----------
print(f'------- the realman official kinematic initialising -------')
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
# Initialize the robotic arm model and sensor type in the algorithm
self.robot_kine_rm = Algo(arm_model, force_type)
@ -120,14 +120,36 @@ class rm75_kine_api():
self.work_name = work
self.cfg_work_frame(work)
target = target_position + target_rpy
target = list(target_position) + list(target_rpy)
if initial_guess is not None:
q_ref = [ 180/math.pi * ig for ig in initial_guess ]
else:
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
# print(f'the arm angle is ret = {ret}, and phi = {phi}')
params = rm_inverse_kinematics_params_t(q_ref,
target, 1)
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
pose_dis = cal_pose_deviation(pose_fk, target)
# print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
#
# print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
if ret < 0:
return ret, [ q/180*math.pi for q in q_out]
elif pose_dis < 0.01:
return ret, [ q/180*math.pi for q in q_out]
else:
return -10, [ q/180*math.pi for q in q_out]
def cal_pose_deviation(pose1, pose2):
d_fk_p1 = np.array(pose1) - np.array(pose2)
for j in [3, 4, 5]:
while d_fk_p1[j] > math.pi:
d_fk_p1[j] -= 2 * math.pi
while d_fk_p1[j] < -math.pi:
d_fk_p1[j] += 2 * math.pi
d_fk = np.linalg.norm(d_fk_p1)
return d_fk

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: