add collision detection in workspace cal

2026-07-29 11:25:09 +01:00
parent 9849466430
commit 2713c54707
2 changed files with 18 additions and 43 deletions
@@ -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()