def test_get_linear_path_and_step(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
path = arm.get_linear_path(
waypoint.get_position(), waypoint.get_orientation())
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(np.allclose(
arm.get_tip().get_position(), waypoint.get_position(), atol=0.01))
def test_get_linear_path_and_step(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(
np.allclose(mobile.get_position()[:2],
waypoint.get_position()[:2],
atol=0.001))