void InertiaSensorFilter::update(OrientationData& orientationData,
const InertiaSensorData& theInertiaSensorData,
const SensorData& theSensorData,
const RobotModel& theRobotModel,
const FrameInfo& theFrameInfo,
const MotionInfo& theMotionInfo,
const WalkingEngineOutput& theWalkingEngineOutput)
{
// MODIFY("module:InertiaSensorFilter:parameters", p);
//
// DECLARE_PLOT("module:InertiaSensorFilter:expectedGyroX");
// DECLARE_PLOT("module:InertiaSensorFilter:gyroX");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedGyroY");
// DECLARE_PLOT("module:InertiaSensorFilter:gyroY");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedAccX");
// DECLARE_PLOT("module:InertiaSensorFilter:accX");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedAccY");
// DECLARE_PLOT("module:InertiaSensorFilter:accY");
// DECLARE_PLOT("module:InertiaSensorFilter:expectedAccZ");
// DECLARE_PLOT("module:InertiaSensorFilter:accZ");
// check whether the filter shall be reset
if(!lastTime || theFrameInfo.time <= lastTime)
{
if(theFrameInfo.time == lastTime)
return; // weird log file replaying?
x = State<>();
cov = p.processCov;
lastLeftFoot = lastRightFoot = Pose3D();
lastTime = theFrameInfo.time - (unsigned int)(theFrameInfo.cycleTime * 1000.f);
}
// get foot positions
const Pose3D& leftFoot(theRobotModel.limbs[MassCalibration::footLeft]);
const Pose3D& rightFoot(theRobotModel.limbs[MassCalibration::footRight]);
const Pose3D leftFootInvert(leftFoot.invert());
const Pose3D rightFootInvert(rightFoot.invert());
// calculate rotation and position offset using the robot model (joint data)
const Pose3D leftOffset(lastLeftFoot.translation.z != 0.f ? Pose3D(lastLeftFoot).conc(leftFootInvert) : Pose3D());
const Pose3D rightOffset(lastRightFoot.translation.z != 0.f ? Pose3D(lastRightFoot).conc(rightFootInvert) : Pose3D());
// detect the foot that is on ground
bool useLeft = true;
if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x != 0)
useLeft = theWalkingEngineOutput.speed.translation.x > 0 ?
(leftOffset.translation.x > rightOffset.translation.x) :
(leftOffset.translation.x < rightOffset.translation.x);
else
{
Pose3D left(x.rotation);
Pose3D right(x.rotation);
left.conc(leftFoot);
right.conc(rightFoot);
useLeft = left.translation.z < right.translation.z;
}
// calculate velocity
Vector3<> calcVelocity, lastCalcVelocity;
float timeScale = 1.f / (float(theFrameInfo.time - lastTime) * 0.001f);
calcVelocity = useLeft ? leftOffset.translation : rightOffset.translation;
calcVelocity *= timeScale * 0.001f; // => m/s
// update the filter
timeScale = float(theFrameInfo.time - lastTime) * 0.001f;
predict(theInertiaSensorData.gyro.x != InertiaSensorData::off ?
RotationMatrix(Vector3<>(theInertiaSensorData.gyro.x * timeScale, theInertiaSensorData.gyro.y * timeScale, 0)) :
(useLeft ? leftOffset.rotation : rightOffset.rotation));
// insert calculated rotation
if(theInertiaSensorData.acc.x != InertiaSensorData::off)
safeRawAngle = Vector2<>(theSensorData.data[SensorData::angleX], theSensorData.data[SensorData::angleY]);
if((theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::sitDownKeeper)) &&
fabs(safeRawAngle.x) < p.calculatedAccLimit.x && fabs(safeRawAngle.y) < p.calculatedAccLimit.y)
{
const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
RotationMatrix calculatedRotation(Vector3<>(
atan2f(usedRotation.c1.z, usedRotation.c2.z),
atan2f(-usedRotation.c0.z, usedRotation.c2.z), 0.f));
Vector3<> accGravOnly(calculatedRotation.c0.z, calculatedRotation.c1.z, calculatedRotation.c2.z);
accGravOnly *= -9.80665f;
readingUpdate(accGravOnly);
}
else // insert acceleration sensor values
{
if(theInertiaSensorData.acc.x != InertiaSensorData::off)
readingUpdate(theInertiaSensorData.acc);
}
// fill the representation
orientationData.orientation = Vector2<>(
atan2f(x.rotation.c1.z, x.rotation.c2.z),
atan2f(-x.rotation.c0.z, x.rotation.c2.z));
// this removes any kind of z-rotation from internal rotation
if(orientationData.orientation.squareAbs() < 0.04f * 0.04f)
x.rotation = RotationMatrix(Vector3<>(orientationData.orientation.x, orientationData.orientation.y, 0.f));
orientationData.velocity = calcVelocity;
// store some data for the next iteration
lastLeftFoot = leftFoot;
lastRightFoot = rightFoot;
lastTime = theFrameInfo.time;
// plots
// PLOT("module:InertiaSensorFilter:orientationX", orientationData.orientation.x);
// PLOT("module:InertiaSensorFilter:orientationY", orientationData.orientation.y);
// PLOT("module:InertiaSensorFilter:velocityX", orientationData.velocity.x);
// PLOT("module:InertiaSensorFilter:velocityY", orientationData.velocity.y);
// PLOT("module:InertiaSensorFilter:velocityZ", orientationData.velocity.z);
}
void InertialDataFilter::update(InertialData& inertialData)
{
DECLARE_PLOT("module:InertialDataFilter:expectedAccX");
DECLARE_PLOT("module:InertialDataFilter:accX");
DECLARE_PLOT("module:InertialDataFilter:expectedAccY");
DECLARE_PLOT("module:InertialDataFilter:accY");
DECLARE_PLOT("module:InertialDataFilter:expectedAccZ");
DECLARE_PLOT("module:InertialDataFilter:accZ");
// check whether the filter shall be reset
if(!lastTime || theFrameInfo.time <= lastTime)
{
if(theFrameInfo.time == lastTime)
return; // weird log file replaying?
reset();
}
if(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
{
reset();
}
// get foot positions
Pose3f leftFoot = theRobotModel.limbs[Limbs::footLeft];
Pose3f rightFoot = theRobotModel.limbs[Limbs::footRight];
leftFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
rightFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
const Pose3f leftFootInvert(leftFoot.inverse());
const Pose3f rightFootInvert(rightFoot.inverse());
// calculate rotation and position offset using the robot model (joint data)
const Pose3f leftOffset(lastLeftFoot.translation.z() != 0.f ? Pose3f(lastLeftFoot).conc(leftFootInvert) : Pose3f());
const Pose3f rightOffset(lastRightFoot.translation.z() != 0.f ? Pose3f(lastRightFoot).conc(rightFootInvert) : Pose3f());
// detect the foot that is on ground
bool useLeft = true;
if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x() != 0)
{
useLeft = theWalkingEngineOutput.speed.translation.x() > 0 ?
(leftOffset.translation.x() > rightOffset.translation.x()) :
(leftOffset.translation.x() < rightOffset.translation.x());
}
else
{
Pose3f left(mean.rotation);
Pose3f right(mean.rotation);
left.conc(leftFoot);
right.conc(rightFoot);
useLeft = left.translation.z() < right.translation.z();
}
// update the filter
const float timeScale = theFrameInfo.cycleTime;
predict(RotationMatrix::fromEulerAngles(theInertialSensorData.gyro.x() * timeScale,
theInertialSensorData.gyro.y() * timeScale, 0));
// insert calculated rotation
safeRawAngle = theInertialSensorData.angle.head<2>().cast<float>();
bool useFeet = true;
MODIFY("module:InertialDataFilter:useFeet", useFeet);
if(useFeet &&
(theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::standHigh)) &&
std::abs(safeRawAngle.x()) < calculatedAccLimit.x() && std::abs(safeRawAngle.y()) < calculatedAccLimit.y())
{
const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
Vector3f accGravOnly(usedRotation.col(0).z(), usedRotation.col(1).z(), usedRotation.col(2).z());
accGravOnly *= Constants::g_1000;
readingUpdate(accGravOnly);
}
else // insert acceleration sensor values
readingUpdate(theInertialSensorData.acc);
// fill the representation
inertialData.angle = Vector2a(std::atan2(mean.rotation.col(1).z(), mean.rotation.col(2).z()), std::atan2(-mean.rotation.col(0).z(), mean.rotation.col(2).z()));
inertialData.acc = theInertialSensorData.acc;
inertialData.gyro = theInertialSensorData.gyro;
inertialData.orientation = Rotation::removeZRotation(Quaternionf(mean.rotation));
// this keeps the rotation matrix orthogonal
mean.rotation.normalize();
// store some data for the next iteration
lastLeftFoot = leftFoot;
lastRightFoot = rightFoot;
lastTime = theFrameInfo.time;
// plots
Vector3f angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(inertialData.orientation));
PLOT("module:InertialDataFilter:angleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:angleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:angleZ", toDegrees(angleAxisVec.z()));
PLOT("module:InertialDataFilter:unfilteredAngleX", theInertialSensorData.angle.x().toDegrees());
PLOT("module:InertialDataFilter:unfilteredAngleY", theInertialSensorData.angle.y().toDegrees());
angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(mean.rotation));
PLOT("module:InertialDataFilter:interlanAngleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:interlanAngleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:interlanAngleZ", toDegrees(angleAxisVec.z()));
}
