@Before public void setUp() throws Exception { transformIMUToJoint.setRotation(jointToIMURotation); transformIMUToJoint.setTranslation(jointToIMUOffset); transformJointToIMU.setAndInvert(transformIMUToJoint); imuFrame = fullRobotModel.createOffsetFrame(fullRobotModel.getBodyLink().getParentJoint(), transformIMUToJoint, "imuFrame"); Vector3D linearAcceleration = new Vector3D(0.0, 0.0, GRAVITY); Vector3D angularAcceleration = new Vector3D(); ReferenceFrame rootBodyFrame = fullRobotModel.getElevatorFrame(); SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBodyFrame, ReferenceFrame.getWorldFrame(), rootBodyFrame, angularAcceleration, linearAcceleration); simulatedIMURawSensorReader = new PerfectSimulatedIMURawSensorReader(rawSensors, IMU_INDEX, rigidBody, imuFrame, fullRobotModel.getElevator(), rootAcceleration); simulatedIMURawSensorReader.initialize(); }
private void generateExpectedLinearAcceleration() { FrameVector3D centerAppliedAccelerationPart = new FrameVector3D(randomLinearAcceleration); FrameVector3D centerCoriolisAccelerationPart = new FrameVector3D(bodyFrame); centerCoriolisAccelerationPart.cross(randomAngularVelocity, randomLinearVelocity); FrameVector3D gravitationalAccelerationPart = new FrameVector3D(fullRobotModel.getWorldFrame()); gravitationalAccelerationPart.setZ(GRAVITY); gravitationalAccelerationPart.changeFrame(bodyFrame); FrameVector3D centripedalAccelerationPart = new FrameVector3D(bodyFrame); centripedalAccelerationPart.cross(randomAngularVelocity, jointToIMUOffset); centripedalAccelerationPart.cross(randomAngularVelocity, centripedalAccelerationPart); FrameVector3D angularAccelerationPart = new FrameVector3D(bodyFrame); angularAccelerationPart.cross(randomAngularAcceleration, jointToIMUOffset); expectedLinearAccelerationOfIMUInIMUFrame.set(centerAppliedAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(centerCoriolisAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(gravitationalAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(centripedalAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(angularAccelerationPart); transformJointToIMU.transform(expectedLinearAccelerationOfIMUInIMUFrame); }
@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); } }
public void update(RigidBodyTransform transformBodyToWorld, FrameVector3D linearVelocity, FrameVector3D angularVelocity, FrameVector3D linearAcceleration, FrameVector3D angularAcceleration) { // Update Body Pose rootJoint.setJointConfiguration(transformBodyToWorld); // TODO correct? updateFrames(); // Update Body Velocity Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocity, linearVelocity); rootJoint.setJointTwist(bodyTwist); // Update Body Acceleration SpatialAcceleration accelerationOfChestWithRespectToWorld = new SpatialAcceleration(bodyFrame, worldFrame, bodyFrame, angularAcceleration, linearAcceleration); accelerationOfChestWithRespectToWorld.setBaseFrame(getElevatorFrame()); rootJoint.setJointAcceleration(accelerationOfChestWithRespectToWorld); updateFrames(); }