ActivationModelQuadraticBarrier does not respect upper and lower bounds when one of the bounds is np.inf

[akhilsathuluri]

I was trying to use the ActivationModelQuadraticBarrier as a way to prevent some links in my robot to not collide with the ground. So when the ub=np.inf and the lb=0 are set in the ActivationBounds, then the non inf bound is not respected.

I found that this is due to the implementation of beta here:

if (beta >= Scalar(0) && beta <= Scalar(1.)) {
  VectorXs m = Scalar(0.5) * (lb + ub);
  VectorXs d = Scalar(0.5) * (ub - lb);
  lb = m - beta * d;
  ub = m + beta * d;
 } else {
  beta = Scalar(1.);
}

this operation eats up the non inf bound when one of the bounds is inf. A possible suggestion to prevent this could be:

if (beta >= Scalar(0) && beta <= Scalar(1.)) {
  for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
    if (lb(i)!=(-std::numeric_limits<Scalar>::max()) && ub(i)!=(std::numeric_limits<Scalar>::max())){
      double m = Scalar(0.5) * (lb(i) + ub(i));
      double d = Scalar(0.5) * (ub(i) - lb(i));
      lb(i) = m - beta * d;
      ub(i) = m + beta * d;
    }
  } 
} else {
  beta = Scalar(1.);
}

@traversaro: Correct me if Im missing anything.

[traversaro]

@traversaro: Correct me if Im missing anything.

I am a bit confused by the ub(i)!=(std::numeric_limits<Scalar>::max()) part. std::numeric_limits<Scalar>::max is the maximum finite number that can be represented by Scalar, for some reason np.inf it is mapped to that and not to proper infinite values?

[akhilsathuluri]

No you're right. I skipped some context there.
A relevant snippet from the original code:

ActivationBoundsTpl(const VectorXs& lower, const VectorXs& upper, const Scalar b = (Scalar)1.)
      : lb(lower), ub(upper), beta(b) {
    if (lb.size() != ub.size()) {
      throw_pretty("Invalid argument: "
                   << "The lower and upper bounds don't have the same dimension (lb,ub dimensions equal to " +
                          std::to_string(lb.size()) + "," + std::to_string(ub.size()) + ", respectively)");
    }
    if (beta < Scalar(0) || beta > Scalar(1.)) {
      throw_pretty("Invalid argument: "
                   << "The range of beta is between 0 and 1");
    }
    using std::isfinite;
    for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
      if (isfinite(lb(i)) && isfinite(ub(i))) {
        if (lb(i) - ub(i) > 0) {
          throw_pretty("Invalid argument: "
                       << "The lower and upper bounds are badly defined; ub has to be bigger / equals to lb");
        }
      }
      // Assign the maximum value for infinity/nan values
      if (!isfinite(lb(i))) {
        lb(i) = -std::numeric_limits<Scalar>::max();
      }
      if (!isfinite(ub(i))) {
        ub(i) = std::numeric_limits<Scalar>::max();
      }
    }

    if (beta >= Scalar(0) && beta <= Scalar(1.)) {
      VectorXs m = Scalar(0.5) * (lb + ub);
      VectorXs d = Scalar(0.5) * (ub - lb);
      lb = m - beta * d;
      ub = m + beta * d;
    } else {
      beta = Scalar(1.);
    }

So just before the beta computation, the np.nans and np.infs are converted to the max Scalars. So thats the reason for checking the max Scalar instead of np.inf. So the mapping is infact done explicitly as in attached code. I guess this clarifies the confusion partly.

[traversaro]

Ahh, I see, that is more clear.

[cmastalli]

We have designated this function to work with maximum values instead of infinity ones.

A better workaround to support both will be to use the maximum value when it is detected an infinity one in the bounds. The sign will depend on the infinity sign as well.

Is this clear enough?

[cmastalli]

@akhilsathuluri -- Could you help us by submitting a PR introducing your solution?

Please also consider a unit test to check this condition.

[akhilsathuluri]

Please see #1191 for a PR fixing the issue.
Update: #1198