Vector2f LIP3D::requiredVelocity(const Vector2f& pos, float time)
{
const Array2f sinhkt(std::sinh(k.x() * time), std::sinh(k.y() * time));
const Array2f coshkt(std::cosh(k.x() * time), std::cosh(k.y() * time));
return k.array() * (pos.array() - position.array() * coshkt) / sinhkt;
}
Vector4f LIPStateEstimator::measure(SupportFoot supportFoot, const Vector2f& LIPOrigin) const
{
const Pose3f& supportFootToTorso = supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight;
const Quaternionf& torsoToWorld = theInertialData.orientation2D;
const Pose3f originToTorso = supportFootToTorso + Vector3f(LIPOrigin.x(), LIPOrigin.y(), 0.f);
const Vector3f comInOrigin = originToTorso.inverse() * theRobotModel.centerOfMass;
const Vector3f com = (torsoToWorld * originToTorso.rotation) * comInOrigin;
PLOT("module:ZmpWalkingEngine:LIPStateEstimator:Estimate:measuredComHeight", com.z());
const Vector2f accInWorld = (torsoToWorld * theInertialData.acc * 1000.f).head<2>();
const Vector2f zmp = com.head<2>().array() - (accInWorld.array() / LIP3D(LIPHeights).getK().square());
return (Vector4f() << com.head<2>(), zmp).finished();
}
