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)
# ======================================================