KDL inverse kinematics

Based on the skeleton in ikkdl.cpp, implement the inverse velocity kinematics of a robot such that the end effector will follow an interactive marker in rviz. The skeleton provides:

• loading of the KDL::Tree from robot_description,
• extracting of the end effector link name
• creation of the corresponding linear KDL::Chain
• placing of the interactive marker at the end-effector (using forward kinematics) In order to correctly work, a robot with at least 6 DoF is required. To this end, load e.g. the PA10 robot description into your ROS workspace.

Do the following:

• Have a look at the loading of the KDL:Tree and the KDL::Chain.
• Compute the twist generating the correction motion to move the end-effector to the marker pose.
• From this twist (better a fraction of 0.1 of its magnitude) compute the corresponding joint velocities using the inverse Jacobian (KDL::ChainIkSolverVel_pinv).
• Check for instabilities close to singularities, e.g. at the border of the workspace.
• Replace KDL::ChainIkSolverVel_pinv with ChainIkSolverVel_pinv_nso. Check the behaviour in singularities for different values of eps.