def test_franka_panda(n_attempts): from ikfast_franka_panda import get_fk, get_ik, get_dof, get_free_dof print('*****************\n franka_panda ikfast_pybind test') n_jts = get_dof() n_free_jts = get_free_dof() # assert n_jts == 6 and n_free_jts == 1 print('franka_panda: \nn_jts: {}, n_free_jts: {}'.format( n_jts, n_free_jts)) feasible_ranges = { 'robot_joint_1': { 'lower': -2.8973, 'upper': 2.8973 }, 'robot_joint_2': { 'lower': -1.7628, 'upper': 1.7628 }, 'robot_joint_3': { 'lower': -2.8973, 'upper': 2.8973 }, 'robot_joint_4': { 'lower': -3.0718, 'upper': -0.0698 }, 'robot_joint_5': { 'lower': -2.8973, 'upper': 2.8973 }, 'robot_joint_6': { 'lower': -0.0175, 'upper': 3.7525 }, 'robot_joint_7': { 'lower': -2.8973, 'upper': 2.8973 }, } pos, rot = get_fk([0] * n_jts) assert not get_ik(pos, rot, []) assert not get_ik(pos, rot, [1, 1]) assert not get_ik(pos, rot, [1, 1, 1, 1]) print("Testing random configurations...") for _ in range(n_attempts): q = np.random.rand(n_jts) for i, jt_name in enumerate(feasible_ranges.keys()): q[i] = q[i] * (feasible_ranges[jt_name]['upper'] - feasible_ranges[jt_name]['lower']) + \ feasible_ranges[jt_name]['lower'] check_q(get_fk, get_ik, q, feasible_ranges, free_joint_ids=[6]) print("Done!")
def compute_ik(ee2base: np.ndarray) -> List[List[float]]: """Compute the inverse kinematics of franka panda robot. This function is provided to help do the inverse kinematics calculation. The output of this function is deterministic. It will return a list of solutions for the given cartesian pose. In practice, some solutions are not physically-plausible due to self collision. So in this homework, you may need to choose the free_joint_value and which solution to use by yourself. References: ikfast_pybind: ikfast_pybind is a python binding generation library for the analytic kinematics engine IKfast. Please visit here for more information: https://pypi.org/project/ikfast-pybind/ ikfast: ikfast is a powerful inverse kinematics solver provided within Rosen Diankov’s OpenRAVE motion planning software. Diankov, R. (2010). Automated construction of robotic manipulation programs. Args: ee2base: transformation from end effector to base Returns: A list of possible IK solutions when the last joint value is set as free_joint_value """ try: import ikfast_franka_panda as panda except ImportError: print("Please install ikfast_pybind before using this function") print("Install: pip3 install ikfast-pybind") raise ImportError link72ee = np.array([[0.7071068, -0.7071068, 0, 0], [0.7071068, 0.7071068, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) link7_pose = ee2base @ link72ee pos = link7_pose[:3, 3] rot = link7_pose[:3, :3] return panda.get_ik(pos, rot, [0.785])