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(); }