def test_kuka_tool_random(self):
bot = Kuka()
bot.tf_tool_tip = tool_tip_transform
N = 20
q_rand = np.random.rand(N, 6) * 2 * PI - PI
for qi in q_rand:
print(qi)
T1 = bot.fk(qi)
resi = bot.ik(T1)
if resi.success:
for q_sol in resi.solutions:
print(q_sol)
T2 = bot.fk(q_sol)
assert_almost_equal(T1, T2)
else:
# somethings is wrong, should be reachable
print(resi)
assert_almost_equal(qi, 0)
def test_kuka_random(self):
""" TODO some issues with the numerical accuracy of the ik?"""
bot = Kuka()
N = 20
q_rand = np.random.rand(N, 6) * 2 * PI - PI
for qi in q_rand:
print(qi)
T1 = bot.fk(qi)
resi = bot.ik(T1)
if resi.success:
for q_sol in resi.solutions:
print(q_sol)
T2 = bot.fk(q_sol)
assert_almost_equal(T1, T2, decimal=5)
else:
# somethings is wrong, should be reachable
print(resi)
assert_almost_equal(qi, 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)
# ======================================================
# Animate path and planning scene
# ======================================================
import matplotlib.pyplot as plt
from acrobotics.util import get_default_axes3d
fig, ax = get_default_axes3d([-0.8, 0.8], [-0.8, 0.8], [-0.2, 1.4])
ax.set_axis_off()
ax.view_init(elev=31, azim=-15)