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