def test_kuka_collision(self): bot = Kuka() q0 = [0, np.pi / 2, 0, 0, 0, 0] obj1 = ShapeSoup( [Box(0.2, 0.3, 0.5), Box(0.1, 0.3, 0.1)], [pose_x(0, 0.75, 0, 0.5), pose_x(0, 0.75, 0.5, 0.5)], ) obj2 = ShapeSoup( [Box(0.2, 0.3, 0.5), Box(0.1, 0.3, 0.1)], [pose_x(0, 0.3, -0.7, 0.5), pose_x(0, 0.75, 0.5, 0.5)], ) a1 = bot.is_in_collision(q0, obj1) assert a1 is True a2 = bot.is_in_collision(q0, obj2) assert a2 is False
scene = Scene([table, obstacle], [T_table, T_obs]) # ====================================================== # Linear interpolation path from start to goal # ====================================================== import numpy as np q_start = np.array([0.5, 1.5, -0.3, 0, 0, 0]) q_goal = np.array([2.5, 1.5, 0.3, 0, 0, 0]) q_path = np.linspace(q_start, q_goal, 10) # ====================================================== # Show which parts of the path are in collision # ====================================================== print([robot.is_in_collision(q, scene) for q in q_path]) # ====================================================== # Calculate forward and inverse kinematics # ====================================================== # forward kinematics are available by default T_fk = robot.fk([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) # inverse kinematics are implemented for specific robots ik_solution = robot.ik(T_fk) print(f"Inverse kinematics successful? {ik_solution.success}") for q in ik_solution.solutions: print(q) # ======================================================