testWakeUp method

void testWakeUp()

Implementation

void testWakeUp(){
  if(linearVelocity.testZero() || angularVelocity.testZero() || position.testDiff(sleepPosition) || orientation.testDiff(sleepOrientation)) awake(); // awake the body
}