[ROS2] Make MoveIt Servo more robust to singularities

[amalnanavati]

As of the above #119, MoveIt Servo works well for local motions, but for large motions like moving from the home configuration to an at-mouth position, it doesn't work well and often gets stuck in a singularity. If we want to enable farther-distance teleoperations like that, we may have to do one or more of the following:

1. Implement dampened Jacobians in MoveIt Servo: http://robotics.caltech.edu/~jwb/courses/ME115/handouts/damped.pdf . This should mostly be the same as the inverse jacobian method, expect with a slight non-Cartesian motion at singularities. In theory, this should eliminate singularities altogether.
2. Allow for the IK solution to violate a strictly cartesian motion. For example, with the Jaco controller, if moving the fork towards you requires changing the yaw, it will do so.
3. Allow a more intuitive interface for getting out of singularities (than joint control). With the Jaco controller, it is the home button. Perhaps we can expose users to a similarly intuitive motion to reset the arm to some configuration?