diff --git a/README.md b/README.md index 6201136..f4240f6 100644 --- a/README.md +++ b/README.md @@ -12,3 +12,21 @@ Key specifications: Next:\ Comparison with Realman official IK method. Embedded with current demo. + + +Comparison (04June2026): + +- With current dual arm joint limit, +``` +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%. + +- 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%. + diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index 38f37f4..cb2cd8e 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -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, diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index e3bc8a0..6057b17 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -92,10 +92,10 @@ class KinematicsSolver(): # Joint limits (radians) - expanded for better reachability self.lower_limits = np.array([ - -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 + -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159 ]) self.upper_limits = np.array([ - 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159 + 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159 ]) # Set joint limits in the model