def test_solve_ik_via_sampling(self):
arm = Panda()
waypoint = Dummy('Panda_waypoint')
configs = arm.solve_ik_via_sampling(
waypoint.get_position(), waypoint.get_orientation(), max_configs=5)
self.assertIsNotNone(configs)
self.assertEqual(len(configs), 5)
current_config = arm.get_joint_positions()
# Checks correct config (last)
arm.set_joint_positions(configs[-1])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks correct config (first)
arm.set_joint_positions(configs[0])
self.assertTrue(np.allclose(
arm.get_tip().get_pose(), waypoint.get_pose(), atol=0.001))
# Checks order
prev_config_dist = 0
for config in configs:
config_dist = sum(
[(c - f)**2 for c, f in zip(current_config, config)])
# This test requires that the metric scale for each joint remains at
# 1.0 in _getConfigDistance lua function
self.assertLessEqual(prev_config_dist, config_dist)
prev_config_dist = config_dist
pr.launch(SCENE_FILE, headless=False, responsive_ui=True)
pr.start()
agent = Panda()
starting_joint_positions = agent.get_joint_positions()
(x, y, z), q = agent.get_tip().get_position(), agent.get_tip().get_quaternion()
# Try solving via linearisation
new_joint_pos = agent.solve_ik_via_jacobian([x, y, z - 0.01], quaternion=q)
new_joint_pos = agent.solve_ik_via_jacobian([x, y, z - 0.05], quaternion=q)
new_joint_pos = agent.solve_ik_via_jacobian([x, y, z - 0.1], quaternion=q)
new_joint_pos = agent.solve_ik_via_jacobian([x, y, z - 0.2], quaternion=q)
# This will fail because the distance between start and goal is too far
try:
new_joint_pos = agent.solve_ik_via_jacobian([x, y, z - 0.4], quaternion=q)
except IKError:
# So let's swap to an alternative IK method...
# This returns 'max_configs' number of joint positions
input('Press key to run solve_ik_via_sampling...')
new_joint_pos = agent.solve_ik_via_sampling([x, y, z - 0.4],
quaternion=q)[0]
# Because the arm is in Forxe/Torque mode, we need to temporarily disable
# dynamics in order to instantaneously move joints.
agent.set_joint_positions(new_joint_pos, disable_dynamics=True)
input('Press any key to finish...')
pr.stop()
pr.shutdown()
