geometry_msgs::Twist overload of setFromDiffIK
Fix secondary task function signature and other minor fixes