Clean treeikpossolver_online

[bit-pirate]

• Get rid of commented out code
• Get rid of duplicated functions (normal + *_it)
• Discuss a patch for the KDL version

[bit-pirate]

This is related to https://github.com/pal-robotics/reem_teleop/issues/18.

[bit-pirate]

First, some information (also for myself) to shed some light on the mess inside the IK solver:

• The *_it versions have been added, because the first version didn't do iterations.
• A low pass filter for smoothing the joint position output has been added.
• Some logic has been added to recognise small end effector motions, in which higher smoothing is applied (to avoid shaking of the robot due to noisy input)
  • This is probably obsolete when using Hilario's filter for the motion tracking input.
  • It's also currently deactivated (unintentionally as it looks like ...)
• The commented-out code is the result of various tests and commiting before cleaning that up.

[bit-pirate]

After spending some time getting into my own code again, I came up with this agenda to clean and simplify the IK solver:

• Merge iterating and normal versions (i.e. normal version = iterating version with 1 iteration)
• Remove filtering and small-motion-checking or - if really needed - move it one level higher up into the piece of code calling the ik solver (i.e. tree_kinematics.cpp)
• Cartesian velocity limiting
  • checking one time at the start should be enough, since the largest twist occurs in the first iteration
  • move it out off the ik solver as well (i.e. into tree_kinematics.cpp)

Any objections or comments?

[adolfo-rt]

Changes description seem sensible.