ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add collision detection

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-28 22:41:35 +01:00
parent 86c40ee380
commit 9849466430
2 changed files with 41 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kine_ctrl/main.py
+3 -3
View File
@@ -43,11 +43,11 @@ def main():
# ----------- rm75 qp based kine ------------
robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75')
robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75', tcps=["scissor_tcp", "camera_tcp"])
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))
fp = robot_kine_qp.forward_kinematics(np.zeros(7),tool='scissor_tcp')
print(f'forward kine res = {fp}')
# ---------- rm75 official algorithm -----------
@@ -103,7 +103,7 @@ def main():
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}')
# robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
if d_p_ik < 0.01:
result[0][1] += 1