Cost Functions to Specify Full-Body Motion and Multi-Goal Manipulation Tasks

While the problem of inverse kinematics on serial kinematic chains is well researched, solving motion tasks quickly on more complex robots remains an open problem. Examples include dual-arm manipulation, grasping with multi-finger hands, and full-body motion generation for humanoids. In this paper, we introduce an open-source software package for ROS and MoveIt! that solves inverse kinematics and motion tasks on robots with arbitrary kinematic trees. The underlying memetic algorithm integrates evolutionary optimization, particle swarm optimization, and gradient methods. The optimization respects joint limits, effectively avoids local minima, and achieves fast convergence to accurate solutions. More importantly, the overall motion goal is specified using a set of weighted sub-goals, providing great flexibility and control of secondary objectives. Several application examples demonstrate how to combine the predefined sub-goals to achieve complex motion tasks.