diff --git a/README.md b/README.md index f4240f6..25195ab 100644 --- a/README.md +++ b/README.md @@ -14,19 +14,21 @@ Comparison with Realman official IK method. Embedded with current demo. -Comparison (04June2026): +### Comparison (05June2026): - With current dual arm joint limit, ``` -ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])\ +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]) ``` -the success rates for qp-based ik and realman Algo ik are 45% and 23%. +the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\ +At least one solver works out the ik, rate = **74%**. - With realman-75 physical joint limit, ``` ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0]) lb = -ub ``` -the success rates for qp-based ik and realman Algo ik are 76% and 51%. +the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\ +At least one solver works out the ik, rate = **84%**. diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index cb2cd8e..372615f 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -107,9 +107,9 @@ def demo_position_control(): result = np.array([[0,0],[0,0]], dtype=np.int32) # qp_fk, qp_ik, rm_fk, rm_ik + solve_sum = 0 - - for i in range(1000): + for i in range(2000): print(f'\n-------------- in i = {i} ----------------') joint_rand = np.random.uniform(ub/180*pi, lb/180*pi) print(f'the predefined joints are {joint_rand}') @@ -135,8 +135,8 @@ def demo_position_control(): 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, - max_iter=500, debug=False, tool=tool_name) + t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, + max_iter=500, debug=False, tool=tool_name) print(f'joint_solution = {joint_solution}, success = {success}, error = {error}, ite = {ite}') if success: @@ -163,7 +163,11 @@ def demo_position_control(): else: print(f'== fail in the rm ik, ret = {ret}y, q = {q}') + if success or ret == 0: + solve_sum += 1 + print(f'result is {result}') + print(f'solve_sum is {solve_sum}')