@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout = 30000) public void testRead() { for (int i = 0; i < 10000; i++) { generateAppliedOrientation(); generateAppliedVelocity(); generateAppliedAcceleration(); fullRobotModel.update(randomTransformBodyToWorld, randomLinearVelocity, randomAngularVelocity, randomLinearAcceleration, randomAngularAcceleration); simulatedIMURawSensorReader.read(); rawSensors.getOrientation(actualIMUOrientation, IMU_INDEX); rawSensors.getAngularVelocity(actualAngularVelocity, IMU_INDEX); rawSensors.getAcceleration(actualLinearAcceleration, IMU_INDEX); generateExpectedOrientation(); generateExpectedAngularVelocity(); generateExpectedLinearAcceleration(); assertEqualsRotationMatrix(expectedIMUOrientation, actualIMUOrientation, 1e-3); assertEqualsVector(expectedAngularVelocityInIMUFrame, actualAngularVelocity, 1e-3); assertEqualsVector(expectedLinearAccelerationOfIMUInIMUFrame, actualLinearAcceleration, 1e-3); } }