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

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-04 17:11:02 +01:00
parent f10152fc29
commit cee1a191ea
3 changed files with 102 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
kine_ctrl/main.py
View File

@ -1,4 +1,4 @@
from casadi import print_operator
# conda activate coppeliasim # conda activate coppeliasim
# env fix, in terminal: fix_robotics_env.sh # env fix, in terminal: fix_robotics_env.sh
@ -22,26 +22,11 @@ def demo_position_control():
# Create controller # Create controller
robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True) robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
robot_mjk.start() robot_mjk.start()
time.sleep(1)
print("\n[Test 1] Move joint 1 to 45 degrees") print("\n[Test 1] Move joint 1 to 45 degrees")
robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0]) robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached() robot_mjk.wait_until_reached()
robot_mjk.print_state() 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") print("\n[Test 4] Return home\n")
robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0]) robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached() robot_mjk.wait_until_reached()
@ -69,24 +54,30 @@ def demo_position_control():
robot_kine_qp = kine_qp() robot_kine_qp = kine_qp()
print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}') 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( time0 = time.time()
target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad, for i in range(100):
max_iter=500, debug=False, tool=tool_name ) 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: 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') print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
else: else:
print(f'solution: {joint_solution} success flag {success}, error {error}\n') print(f'solution: {joint_solution} success flag {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')
# ---------- rm75 official algorithm ----------- # ---------- rm75 official algorithm -----------
robot_kine_rm = kine_rm() robot_kine_rm = kine_rm()
print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}') 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]}') print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
if ret == 0: 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}')

55
kine_ctrl/rm75_kine_qp.py
View File

@ -127,7 +127,7 @@ class KinematicsSolver():
polish=False 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"): def forward_kinematics(self, joint_angles, tool="ee"):
@ -179,7 +179,7 @@ class KinematicsSolver():
def inverse_kinematics(self, target_position, target_rpy=None, def inverse_kinematics(self, target_position, target_rpy=None,
target_quat=None, initial_guess=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. Compute inverse kinematics using differential IK with multiple strategies.
@ -255,8 +255,8 @@ class KinematicsSolver():
error_SE3 = current_placement.actInv(target_placement) error_SE3 = current_placement.actInv(target_placement)
error_vec = pin.log(error_SE3).vector error_vec = pin.log(error_SE3).vector
print("\n initial error =", np.linalg.norm(error_vec)) # print("\n initial error =", np.linalg.norm(error_vec))
print(error_vec) # print(error_vec)
while iter_count < max_iter: while iter_count < max_iter:
# Compute forward kinematics # Compute forward kinematics
@ -342,40 +342,13 @@ class KinematicsSolver():
u=ub 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 # Solve
result = self.osqp_solver.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': if result.info.status != 'solved':
break break
dq = result.x 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: if dq is None:
break break
@ -386,26 +359,10 @@ class KinematicsSolver():
prev_error = error_norm prev_error = error_norm
iter_count += 1 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: if best_solution is not None:
print( return best_solution, True, best_error, iter_count
"converged",
error_norm,
)
return best_solution, True, best_error
else: else:
return None, False, None return q[:7].copy(), False, error_norm, iter_count
# def invese_kinematics_velocity(self, target_position, target_rpy=None, # def invese_kinematics_velocity(self, target_position, target_rpy=None,
# target_quat=None, initial_guess=None, tool="ee"): # target_quat=None, initial_guess=None, tool="ee"):

5
kine_ctrl/rm75_kine_rm.py
View File

@ -1,6 +1,7 @@
from Robotic_Arm.rm_robot_interface import * from Robotic_Arm.rm_robot_interface import *
import numpy as np import numpy as np
import math
class rm75_kine_api(): class rm75_kine_api():
def __init__(self): def __init__(self):
@ -26,11 +27,11 @@ class rm75_kine_api():
def cfg_limit(self): def cfg_limit(self):
joint_max_limit = np.array([ joint_max_limit = 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, 3.14159
]) * 57 ]) * 180 / math.pi
self.robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist()) self.robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist())
joint_min_limit = np.array([ joint_min_limit = 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, -3.14159
]) * 57 ]) * 180 / math.pi
self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.tolist()) self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.tolist())
def cfg_work_frame(self , frame_name): def cfg_work_frame(self , frame_name):