Toolchanger offset causing collision with planned configurations

[yijiangh]

• [x] settle on a way we compare two frames (should specify point distance and angle distance) (1mm for origin, 0.0025 for radians compare angle between axis)
• [x] settle on a way we compare two configurations (prismatic joint should have a different value than the revolute joint) (0.0025 (0.001) for rev joint values, 1 mm for prismatic joint)
• [x] joint flip (prismatic joint, 0.05 meter, revolute joint 10 degrees)
• [ ] use the same 0.05 for prismatic joint, rev 10 deg for free motion resolutions
• [x] why free motion's first trajectory point disagree with the given start_conf

[yck011522]

Joint Flip and Jump is pretty much the same thing, and both have the same limits: 0.05 meter and 10 degrees (0.174533rad)