Fixed orientations calculation in the imu_joint_controller node