bool BaseOfSupport::update(const Eigen::VectorXd& xi_k) {
    // Get feet corners
    Eigen::MatrixXd feetCorners;
    feetCorners = _stepController->getContact2DCoordinates();
    // Compute the bounding box of the current support configuration
    Eigen::Matrix2d minMaxBoundingBox;
    computeBoundingBox(feetCorners, minMaxBoundingBox);
    // Update right-hand side of constraints
    buildb(minMaxBoundingBox);
    buildf();
    // Matrices in the preview window s.t.
    // A*X <= fbar - B*xi_k
    // Therefore we can prebuild A, fbar and B which are not time-dependent
    buildfbar(_f);
    _rhs = _fbar - _B*xi_k;
    return true;
}
void SkelSmoothTerm::build()
{
    buildA();
    buildb();
}
// only need to update b
void ARAPTerm::update()
{
    buildb();
}
void ARAPTerm::build()
{
    initRotation();
    buildA();
    buildb();
}