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-13 14:54:33 +01:00
parent 7050c93c84
commit 58e84c6a33
21 changed files with 97 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
kine_ctrl/main.py
View File

@ -71,7 +71,7 @@ def main():
solve_sum = 0
for i in range(1000):
for i in range(10):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub, lb)
print(f'the predefined joints are {joint_rand}')
@ -96,6 +96,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)
if d_p_ik < 0.01:
result[0][1] += 1