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

compare the qp ik and rm ik with joint limits

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-04 21:55:35 +01:00
parent cee1a191ea
commit 48453fa5c8
3 changed files with 41 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
kine_ctrl/main.py
View File

@ -87,28 +87,31 @@ def demo_position_control():
# -------------- for comparison ----------------
print(f'in the comparison part')
#
# lower_limits = np.array([-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159])
# upper_limits = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
# result = np.array([[0, 0], [0, 0]])
#
# joint_rand = np.random.uniform(lower_limits, upper_limits)
#
# fk_qp = robot_kine_qp.forward_kinematics(joint_rand.tolist(), tool=tool_name)
# fk_qp_p = np.concatenate([fk_qp['position'], fk_qp['rpy']], axis=0)
# fk_rm_p = robot_kine_rm.forward_kinematics(q=(joint_rand*180/pi).tolist(), tool=tool_name)
# print(f'the fk diversion is { sum(abs( np.array(fk_rm_p) - np.array(fk_qp_p) )) }')
if True:
lower_limits = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159 ])
upper_limits = np.array([ 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159 ])
# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
lb = -ub
# lower_limits = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
# upper_limits = np.array([ 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159 ])
for i in range(7):
robot_kine_qp.model.lowerPositionLimit[i] = lb[i]/180*pi
robot_kine_qp.model.upperPositionLimit[i] = ub[i]/180*pi
robot_kine_rm.robot_kine_rm.rm_algo_set_joint_max_limit(ub)
robot_kine_rm.robot_kine_rm.rm_algo_set_joint_min_limit(lb)
result = np.array([[0,0],[0,0]], dtype=np.int32) # qp_fk, qp_ik, rm_fk, rm_ik
for i in range(1000):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(lower_limits, upper_limits)
joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
print(f'the predefined joints are {joint_rand}')
# -------------- fk ------------------
@ -129,7 +132,7 @@ def demo_position_control():
# ----------- ik ----------------
t_p = fk_rm_p1
joint_rand_init = np.random.uniform(lower_limits, upper_limits)
joint_rand_init = np.random.uniform(ub/180*pi, lb/180*pi)
print(f'the guess is {joint_rand_init}')
joint_solution, success, error, ite = robot_kine_qp.inverse_kinematics(
t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init,