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
def test_Kuka_on_rail_robot(self):
bot = KukaOnRail()
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi[1:])
Tdesired = pose_x(PI / 2, 0, 0, qi[0]) @ Tdesired
assert_almost_equal(Tactual, Tdesired)
def test_PlanarArm_base(self):
robot1.tf_base = pose_x(1.5, 0.3, 0.5, 1.2)
q_test = self.generate_random_configurations(robot1)
for qi in q_test:
Tactual = robot1.fk(qi)
Tdesired = fki.fk_PlanarArm(qi, robot1.links)
Tdesired = robot1.tf_base @ Tdesired
assert_almost_equal(Tactual, Tdesired)
def test_Kuka_base_robot(self):
bot = Kuka()
bot.tf_base = pose_x(0.5, 0.1, 0.2, 0.3)
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi)
Tdesired = bot.tf_base @ Tdesired
assert_almost_equal(Tactual, Tdesired)
def test_sw_random_other_base(self):
bot = SphericalWrist()
bot.tf_base = pose_x(1.5, 0.3, 0.5, 1.2)
N = 20
q_rand = np.random.rand(N, 3) * 2 * PI - PI
for qi in q_rand:
T1 = bot.fk(qi)
resi = bot.ik(T1)
for q_sol in resi.solutions:
R2 = bot.fk(q_sol)[:3, :3]
assert_almost_equal(T1[:3, :3], R2)
def check_random_collision():
tf1 = pose_x(uniform(-2, 2), uniform(0, 3), 0, 0)
tf2 = pose_x(uniform(-2, 2), 0, uniform(0, 3), 0)
return box1.is_in_collision(tf1, box2, tf2)
def test_pose_x(self):
A = pose_x(0.6, 1, 2, 3)
assert_almost_equal(A[0, :3], np.array([1, 0, 0]))
assert_almost_equal(A[:3, 0], np.array([1, 0, 0]))
assert_almost_equal(A[:3, 3], np.array([1, 2, 3]))
assert_almost_equal(A[3, :], np.array([0, 0, 0, 1]))