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

28
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)
return ret, [ q/180*math.pi for q in q_out]
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