IKFast
Using IkFast on a UsarsimROS Robot
IKFast is an inverse kinematics solver that can be configured as a plugin for the ROS arm_navigation node. IKFast is an analytic solver and is capable of finding multiple solutions for the same position.
Instructions for setting up ikfast on ROS can be found here.
Note that the ikfast KR60_arm_navigation node should have two .cpp source files: [robot_name]_[manip_name]_ikfast_solver.cpp (generated by ikfast.py), and [robot_name]_[manip_name]_ikfast_plugin.cpp (copied from an existing file by create_ikfast_plugin.py).
Caveats
IKFast will fail to generate solver code if any joint axis is not aligned along one of the coordinate axes. Make sure that you modify the urdf file so that all joint axes are perpendicular and axis-aligned (the KR60-nojt-perp-*.xml urdf files can be used for the KR60).
The IKFast arm_navigation plugin code (generated from arm_kinematics_tools/templates/ikfast61_plugin_template.cxx) also suffers from some issues, many of which can be resolved by using the patched version of KR60_KR60Arm_ikfast_plugin.cpp found in this repository, rather than the generated plugin.
getClosestSolution()
Current behavior
Since ikfast often finds multiple solutions to a request, the plugin needs a way to choose which solution to return to the planner. A getClosestSolution() function is implemented in <robot_name>_<group_name>_ikfast_plugin.cpp, which finds the solution closest (in terms of joint angles) to the seeded state (the start state, in most cases). However, this function will never be called by an external solver.
The code that calls getClosestSolution() is only present in the searchPositionIK() functions, which are not called by the arm_navigation node in a move_arm request--getPositionIK() is used instead. This function simply iterates through the list of solutions and picks the first one that is within joint limits.
Patch
The patched KR60_KR60Arm_ikfast_plugin.cpp file adds a new getClosestSolution() function that allows for the solutions list to be pruned as individual IK solutions are found to be outside of joint limits. The behavior of the patched plugin's version of getPositionIK() calls getClosestSolution() repeatedly, removing the result from the list until a solution is found within joint limits (effectively performing a selection sort).
harmonize()
Current behavior
The harmonize() function in the ikfast plugin forces solution joint angles into the interval (-2π, 2π). However, potential solutions are not harmonized when they are checked to see if they are within joint limits, so solutions may be discarded even if they are valid.
Patch
The patched harmonize() function forces angles into the interval (-π, π), since USARSim has poor handling of joint limits outside of this range. This also ensures that the getClosestSolution() function performs properly, since joint angles are represented unambiguously. Also, potential solutions are harmonized before checking joint limits.
Further problems
It is possible (likely!) that there are additional problems with the ikfast arm_navigation plugin. It is currently unknown if these and other problems persist in the MoveIt! implementation of the solver.