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

add collision detection in workspace cal

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-29 11:25:09 +01:00
parent 9849466430
commit 2713c54707
2 changed files with 18 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kine_ctrl/main.py
+4 -1
View File
@@ -47,8 +47,11 @@ def main():
robot_kine_qp.add_tool_frames(tools_in_ee)
robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
fp = robot_kine_qp.forward_kinematics(np.zeros(7),tool='scissor_tcp')
fp = robot_kine_qp.forward_kinematics(np.ones(7)*0.3,tool='scissor_tcp')
print(f'forward kine res = {fp}')
ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=fp[0:3], target_rpy=fp[3:6], initial_guess=np.zeros(7),
tool='scissor_tcp')
# ---------- rm75 official algorithm -----------
robot_kine_rm = kine_rm()