ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki 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

46
kine_ctrl/rm75_kine_rm.py
View File

@ -12,6 +12,8 @@ class rm75_kine_api():
# Initialize the robotic arm model and sensor type in the algorithm
self.robot_kine_rm = Algo(arm_model, force_type)
self.cfg_j_limit()
self.tool_frames = {
'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.144, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
@ -24,15 +26,18 @@ class rm75_kine_api():
self.tool_name = "ee"
self.work_name = "work"
def cfg_limit(self):
joint_max_limit = np.array([
3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
]) * 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
]) * 180 / math.pi
self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.tolist())
def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
if max_j is None:
max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
if min_j is None:
min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 ])
if rad_flag:
self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
else:
self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
def cfg_work_frame(self , frame_name):
self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
@ -46,10 +51,11 @@ class rm75_kine_api():
def get_tool_frame(self):
return self.robot_kine_rm.rm_algo_get_curr_toolframe()
def forward_kinematics(self, q, flag = 1 , tool="ee", work="work"):
def forward_kinematics(self, joint_angles, flag = 1 , tool="ee", work="work"):
'''
:param q: list of joint values, in degree
flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
:param joint_angles: list of joint values, in rad
:param flag: 0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
:param return: [x,y,z,rx,ry,rz], m & rad
'''
if tool != self.tool_name:
self.tool_name = tool
@ -58,9 +64,19 @@ class rm75_kine_api():
self.work_name = work
self.cfg_work_frame(work)
return self.robot_kine_rm.rm_algo_forward_kinematics(joint=q, flag=flag)
return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles] , flag=flag)
def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"):
'''
:param target_position: list of position values, m
:param target_rpy: list of rpy values, rad
:param initial_guess: initial guess of angles, rad
:param tool: tool name, refer to self.tool_frames
:param work: work name, refer to self.work_frames
return ret: state of ik calculation, 0:success, -2: out of workspace
[q_]: the ik calculated angles for joints, rad
'''
if tool != self.tool_name:
self.tool_name = tool
self.cfg_tool_frame(tool)
@ -71,11 +87,11 @@ class rm75_kine_api():
target = target_position + target_rpy
if initial_guess is not None:
q_ref = initial_guess
q_ref = [ 180/math.pi * ig for ig in initial_guess ]
else:
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
params = rm_inverse_kinematics_params_t(q_ref,
target, 1)
ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
return ret, q_out
return ret, [ q/180*math.pi for q in q_out]