Allow goals between current position and joint limit if the current limit is past the joint limit (PID)

[adeguet1]

Every time the PID is enabled, there should be a transition stage to move the joints within the joint limits. An alternative solution would be to allow goals within the current position the joint limit. This would prevent clamping when the system is manually moved outside the joint limits before the PID is enabled. The effect would be to only allow motion that would reduce the distance past the joint limit.