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

add ik q_solved refinement function, to make it as close as possible to last_q.

Browse Source 
update the rm75_kine_rm.py to included a series of arm-angle for inverse kinematics calculation.
This commit is contained in:
LiuzhengSJ
2026-07-30 15:28:14 +01:00
parent ace5dea9a2
commit e001f2e1f1
3 changed files with 474 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kine_ctrl/rm75_kine_rm.py
+29 -5
View File
@@ -102,7 +102,7 @@ class rm75_kine_api():
return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles] , flag=flag)
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work"):
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work", step_arm_angle = 15.0):
'''
:param target_position: list of position values, m
:param target_rpy: list of rpy values, rad
@@ -126,10 +126,34 @@ class rm75_kine_api():
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, phi0 = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
params = rm_inverse_kinematics_params_t(q_ref, target, 1)
step_arm_angle = step_arm_angle
offsets = [0.0]
arm_angle = step_arm_angle
while arm_angle <= 180.0:
offsets += [arm_angle, -arm_angle]
arm_angle += step_arm_angle
best_ret, best_q_out, best_dis = -1, None, None
for offset in offsets:
phi = ((phi0 + offset + 180.0) % 360.0) - 180.0
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
if int(ret) != 0:
if best_q_out is None:
best_ret, best_q_out = ret, q_out
continue
p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
pose_dis = cal_pose_deviation(p_fk, target)
if pose_dis < 0.01:
# success in ik calculation
return ret, [q / 180 * math.pi for q in q_out]
if best_dis is None or pose_dis < best_dis:
best_ret, best_q_out, best_dis = -10, q_out, pose_dis
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)