Fixed an error with the conversion from quaternion to rpy. 

@airballking is asssigned to this.