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

Example for using qp based ik with urdf, and realman official ik

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-22 16:19:24 +01:00
parent 58452bce90
commit f1846ffe1e
1 changed files with 26 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
kine_ctrl/main.py
View File

@ -12,18 +12,41 @@ import time
from math import radians, degrees, pi, cos, sin from math import radians, degrees, pi, cos, sin
import numpy as np import numpy as np
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
tools_in_ee = {
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
}
# joint limit
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
# lb = -ub
tool_name = "scissor"
def main(): def main():
"""Demonstrate pure position control""" """Demonstrate pure position control"""
# Create controller # Create controller
robot_mjk = MuJoCoPositionController() robot_mjk = MuJoCoPositionController()
tool_name = "scissor"
# ----------- rm75 qp based kine ------------ # ----------- rm75 qp based kine ------------
robot_kine_qp = kine_qp() robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
robot_kine_qp.add_tool_frames(tools_in_ee)
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
# ---------- rm75 official algorithm ----------- # ---------- rm75 official algorithm -----------
robot_kine_rm = kine_rm() robot_kine_rm = kine_rm()
robot_kine_rm.add_tool_frames(tools_in_ee)
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
@ -32,16 +55,6 @@ def main():
if True: if True:
ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])/180*pi
lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])/180*pi
# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
# lb = -ub
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
result = np.array([[0,0],[0,0]], dtype=np.int32) # to collect ik result 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 solve_sum = 0
@ -95,7 +108,7 @@ def main():
if ret_qp == 0 or ret_rm == 0: if ret_qp == 0 or ret_rm == 0:
solve_sum += 1 solve_sum += 1
print(f'result is {result}') print(f'results with qp and rm for ik are {result}')
print(f'solve_sum is {solve_sum}') print(f'solve_sum is {solve_sum}')