def test_Kuka_tool_robot(self):
bot = Kuka()
bot.tf_tool_tip = torch.tf_tool_tip
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi)
Tdesired = Tdesired @ torch.tf_tool_tip
assert_almost_equal(Tactual, Tdesired)
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)