Added rpy_offset to deal with initial orientation of different haptic devices