From cee1a191ea7f39e0996be722c2429c010b15ded0 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Thu, 4 Jun 2026 17:11:02 +0100 Subject: [PATCH] compare the qp ik and rm ik --- kine_ctrl/main.py | 117 ++++++++++++++++++++++++++++++-------- kine_ctrl/rm75_kine_qp.py | 55 ++---------------- kine_ctrl/rm75_kine_rm.py | 5 +- 3 files changed, 102 insertions(+), 75 deletions(-) diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index c6319c1..38f37f4 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -1,4 +1,4 @@ -from casadi import print_operator + # conda activate coppeliasim # env fix, in terminal: fix_robotics_env.sh @@ -22,26 +22,11 @@ def demo_position_control(): # Create controller robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True) robot_mjk.start() - time.sleep(1) print("\n[Test 1] Move joint 1 to 45 degrees") robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0]) robot_mjk.wait_until_reached() robot_mjk.print_state() - time.sleep(0.5) - - # print("\n[Test 2] Move joint 2 to -30 degrees") - # robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0]) - # robot_mjk.wait_until_reached() - # robot_mjk.print_state() - # time.sleep(0.5) - # - # print("\n[Test 3] Move multiple joints simultaneously") - # robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0]) - # robot_mjk.wait_until_reached() - # robot_mjk.print_state() - # time.sleep(0.5) - print("\n[Test 4] Return home\n") robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0]) robot_mjk.wait_until_reached() @@ -69,24 +54,30 @@ def demo_position_control(): robot_kine_qp = kine_qp() print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}') - joint_solution, success, error = robot_kine_qp.inverse_kinematics( - target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad, - max_iter=500, debug=False, tool=tool_name ) + time0 = time.time() + for i in range(100): + joint_solution, success, error, ite = robot_kine_qp.inverse_kinematics( + target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad, + max_iter=500, debug=False, tool=tool_name ) + time1 = time.time() + print(f'used time by qp is {time1 - time0}') if success: - print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n') + print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}, iteration: {ite}\n') print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n') else: - print(f'solution: {joint_solution} success flag {success}, error {error}\n') - print( - f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)}\n') + print(f'solution: {joint_solution} success flag {success}, error {error}, iteration: {ite}\n') # ---------- rm75 official algorithm ----------- robot_kine_rm = kine_rm() print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}') - ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name) + time2 = time.time() + for i in range(100): + ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name) + time3 = time.time() + print(f'used time by rm is {time3 - time2}') print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}') if ret == 0: @@ -94,6 +85,84 @@ 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 ]) + 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) + print(f'the predefined joints are {joint_rand}') + + # -------------- fk ------------------ + fk_qp1 = robot_kine_qp.forward_kinematics(joint_rand.tolist(), tool=tool_name) + fk_qp_p1 = np.concatenate([fk_qp1['position'], fk_qp1['rpy']], axis=0) + + fk_rm_p1 = robot_kine_rm.forward_kinematics(q=(joint_rand*180/pi).tolist(), tool=tool_name) + + d_fk_p1 = np.array(fk_rm_p1) - np.array(fk_qp_p1) + for j in [3,4,5]: + while d_fk_p1[j] > pi: + d_fk_p1[j] -= 2*pi + while d_fk_p1[j] < -pi: + d_fk_p1[j] += -2*pi + d_fk = np.linalg.norm(d_fk_p1) + print(f'fk_qp_p1 = {fk_qp_p1}, fk_rm_p1 = {fk_rm_p1}, d_fk = {d_fk}\n') + + + # ----------- ik ---------------- + t_p = fk_rm_p1 + joint_rand_init = np.random.uniform(lower_limits, upper_limits) + 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) + print(f'joint_solution = {joint_solution}, success = {success}, error = {error}, ite = {ite}') + + if success: + fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name) + fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0) + d_p_ik = np.linalg.norm( np.array(fk_qp_p2) - np.array(t_p) ) + 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 + else: + fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name) + fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0) + d_p_ik = np.linalg.norm(np.array(fk_qp_p2) - np.array(t_p)) + print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}') + + ret, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], + initial_guess=(joint_rand_init*180/pi).tolist(), tool=tool_name) + if ret == 0: + fk_rm_p2 = robot_kine_rm.forward_kinematics(q=q, tool=tool_name) + d_p_ik = np.linalg.norm(np.array(fk_rm_p2) - np.array(t_p) ) + print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik}') + if d_p_ik < 0.01: + result[1][1] += 1 + else: + print(f'== fail in the rm ik, ret = {ret}y, q = {q}') + + print(f'result is {result}') + + diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index d8908c9..e3bc8a0 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -127,7 +127,7 @@ class KinematicsSolver(): polish=False ) - self.W = np.diag([1, 1, 1, 0.2, 0.2, 0.2]) + self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4]) def forward_kinematics(self, joint_angles, tool="ee"): @@ -179,7 +179,7 @@ class KinematicsSolver(): def inverse_kinematics(self, target_position, target_rpy=None, target_quat=None, initial_guess=None, - max_iter=500, tolerance=3e-3, debug=False, tool="ee"): + max_iter=500, tolerance=5e-3, debug=False, tool="ee"): """ Compute inverse kinematics using differential IK with multiple strategies. @@ -255,8 +255,8 @@ class KinematicsSolver(): error_SE3 = current_placement.actInv(target_placement) error_vec = pin.log(error_SE3).vector - print("\n initial error =", np.linalg.norm(error_vec)) - print(error_vec) + # print("\n initial error =", np.linalg.norm(error_vec)) + # print(error_vec) while iter_count < max_iter: # Compute forward kinematics @@ -342,40 +342,13 @@ class KinematicsSolver(): u=ub ) - - - print("iter", iter_count) - print("error", error_norm) - print("cond(H)", np.linalg.cond(H)) - - u, s, vh = np.linalg.svd(J_eff) - print("sv =", s) - - print("trans =", error_vec[:3]) - print("rot =", error_vec[3:]) - - - # Solve result = self.osqp_solver.solve() - print("OSQP status =", result.info.status) - print("dq =", result.x) - - if result.x is not None: - print("dq norm:", np.linalg.norm(result.x)) - if result.info.status != 'solved': break dq = result.x - pred_err = np.linalg.norm(error_vec) - pred_next = np.linalg.norm(error_vec - J_eff @ dq) - - print("predicted error:", pred_next) - - print(f'pred = {J_eff @ dq} and error_vec = {error_vec}') - if dq is None: break @@ -386,26 +359,10 @@ class KinematicsSolver(): prev_error = error_norm iter_count += 1 - print("target:", target_position, target_rpy) - - print("initial guess:", np.degrees(initial_guess)) - - fk0 = self.forward_kinematics(initial_guess) - print("fk guess:", fk0) - - print(result.info.status) - print(np.degrees(q)) - print(np.degrees(self.model.upperPositionLimit[:7])) - print(np.degrees(self.model.lowerPositionLimit[:7])) - if best_solution is not None: - print( - "converged", - error_norm, - ) - return best_solution, True, best_error + return best_solution, True, best_error, iter_count else: - return None, False, None + return q[:7].copy(), False, error_norm, iter_count # def invese_kinematics_velocity(self, target_position, target_rpy=None, # target_quat=None, initial_guess=None, tool="ee"): diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py index 47092ae..5bacf26 100644 --- a/kine_ctrl/rm75_kine_rm.py +++ b/kine_ctrl/rm75_kine_rm.py @@ -1,6 +1,7 @@ from Robotic_Arm.rm_robot_interface import * import numpy as np +import math class rm75_kine_api(): def __init__(self): @@ -26,11 +27,11 @@ class rm75_kine_api(): def cfg_limit(self): joint_max_limit = np.array([ 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159 - ]) * 57 + ]) * 180 / math.pi self.robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist()) joint_min_limit = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 - ]) * 57 + ]) * 180 / math.pi self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.tolist()) def cfg_work_frame(self , frame_name):