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

add urdf files.

Browse Source 
aligh the function parameter names of qp and rm methods
This commit is contained in:
LiuzhengSJ
2026-06-05 15:25:35 +01:00
parent aefc7bacd5
commit 2ca5033b46
14 changed files with 562 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

164
kine_ctrl/main.py
View File

@ -12,76 +12,18 @@ import time
from math import radians, degrees, pi, cos, sin
import numpy as np
def demo_position_control():
def main():
"""Demonstrate pure position control"""
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
# Create controller
robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
robot_mjk.start()
robot_mjk = MuJoCoPositionController()
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()
print("\n[Test 4] Return home\n")
robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
robot_mjk.wait_until_reached()
robot_mjk.print_state()
#---------------------------------------------------------------------------
joints = [10, 20, -30, -40, 50, 60, 70]
joints_rad = [radians(j) for j in joints] #radians(joints)
# target_position = [0.3, 0.2, 0.4]
# target_rpy = [0.0, 0.0, 3.14*0.25]
target_position = [0.17892041, 0.25274317, 0.83107248]
target_rpy = [0.78576018, 0.67554633, 1.86302226]
target_p = target_position + target_rpy
# target_p_rad = [radians(pos) for pos in target_position] + target_rpy
initial_guess = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] # [0.0, 20, -30, -40, 50, 60, 91] #
initial_guess_rad = [ radians(j) for j in initial_guess ]
tool_name = "scissor"
# ----------- rm75 qp based kine ------------
robot_kine_qp = kine_qp()
print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , 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}, 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}, 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)}')
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:
print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')
@ -92,110 +34,80 @@ def demo_position_control():
# 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])
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
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 ])
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
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.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
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
result = np.array([[0,0],[0,0]], dtype=np.int32) # to collect ik result qp_fk, qp_ik, rm_fk, rm_ik
solve_sum = 0
for i in range(10):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
joint_rand = np.random.uniform(ub, lb)
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_qp_p1 = robot_kine_qp.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
fk_rm_p1 = robot_kine_rm.forward_kinematics(q=(joint_rand*180/pi).tolist(), tool=tool_name)
fk_rm_p1 = robot_kine_rm.forward_kinematics(joint_angles=joint_rand.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)
d_fk = cal_pose_deviation(pose1=fk_rm_p1, pose2=fk_qp_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(ub/180*pi, lb/180*pi)
joint_rand_init = np.random.uniform(ub, lb)
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) )
ret_qp, q = robot_kine_qp.inverse_kinematics( target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
if ret_qp == 0:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
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
robot_mjk.send_command(joint_solution)
robot_mjk.send_command(q)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
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}')
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
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}')
ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
if ret_rm == 0:
fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
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'== fail in the rm ik, ret = {ret_rm}, q = {q}')
if success or ret == 0:
if ret_qp == 0 or ret_rm == 0:
solve_sum += 1
print(f'result is {result}')
print(f'solve_sum is {solve_sum}')
print(f'\nDone\n')
# try:
# while robot_mjk.viewer and robot_mjk.viewer.is_running():
# time.sleep(0.1)
# except KeyboardInterrupt:
# pass
robot_mjk.stop()
def main():
demo_position_control()
def cal_pose_deviation(pose1, pose2):
d_fk_p1 = np.array(pose1) - np.array(pose2)
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)
return d_fk