Revision history [back]

I wound up solving this problem. The fix was simple, though I'm not too familiar with the mathematical reasoning behind why this solution works, other than the non-commutativity of matrix multiplication. I needed to change

current_quaternion = last_quaternion * tf::createQuaternionFromRPY(roll, pitch, yaw);

to

current_quaternion = tf::createQuaternionFromRPY(roll, pitch, yaw) * last_quaternion;