cleaned up joint angle func