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()