Don't "angle-wrap" non-continuous and prismatic joints

[philipp1234]

Some of the controllers try to incorrectly "angle-wrap" non-continuous joints.

• The problem is most obvious with prismatic joints. Linear positions should not be "wrapped around" like angles.
• The issue can also affect revolute joints with joint limits. If the goal position is less than 180° across the joint limit, the robot moves into the joint limit.
• When a robot is driven back under soft compliant control, the issue can cause the robot to suddenly hit out into the opposite direction.

The proposed code change seems to fix the issue for our use cases.