Changed some math for meeting with Dr. Taylor
Merge branch 'master' of https://github.com/jhu-saw/sawConstraintController