Pinocchio: Normalization of SpecialEuclidean and SpecialOrthogonal joint configuration where input has zero norm.

[gabrielebndn]

Normalization methods SpecialEuclidean and SpecialOrthogonal rely on Eigen's normalize() method. According to the method's doc, if the vector's norm is too small, then it is left unchanged. Because of this, if the input vector contains all zeroes at the place of quaternions/continuous joints, the output of Pinocchio's normalize is _not_ normalized.

This is unexpected behaviour. A possible solution would be normalizing to the neutral configuration for the specific joint where Eigen's normalization fails. Would you agree on this? I can handle it as soon as I have time.

Additionally, it would be helpful to add a method called isNormalized for joint configurations (and, more in general, isValid, checking normalization _and_ bounds) (for things like checking user's input and such).

[jcarpent]

This is unexpected behaviour. A possible solution would be normalizing to the neutral configuration for the specific joint where Eigen's normalization fails. Would you agree on this? I can handle it as soon as I have time.

It might be dangerous I would say, leading to some discontinuities, everything leading to the neutral element. I would prefer to avoid that, and I do think @jmirabel may provide his feedback on it too.

Additionally, it would be helpful to add a method called isNormalized for joint configurations (and, more in general, isValid, checking normalization _and_ bounds) (for things like checking user's input and such).

These new helper functions are more than welcome. Please go ahead.

[jmirabel]

FYI,

• the IsNormalized exists in hpp-pinocchio. Copy-paste may (or may not) be simpler than starting from scratch.
• I think the normalize was introduced to overcome rounding errors and the zero quaternion example is more likely to be a programming mistake than a rounding error. I think it is better to have one tool for each goal. That being said, I agree the method name may be misleading (as it is in Eigen).

[jmirabel]

We may decide to do as it is done in Eigen: state the small norm issue in the doc.

[jcarpent]

By the way, for the helpers, please add the prec arguments, similarly to isApprox for Pinocchio and Eigen.

[gabrielebndn]

Ok, so, based on your answers, I will

• Leave normalize as it is, but clarify its limitations in the doc
• Add isNormalized (with precision)
• Add isValidConfiguration (with precision for normalized, but hard check for the limits)

[jcarpent]

Fine with me. Thanks @gabrielebndn for handling this.

[jmirabel]

• Add isValidConfiguration (with precision for normalized, but hard check for the limits)

You'll have to pass the limits as arguments.

[gabrielebndn]

• Add isValidConfiguration (with precision for normalized, but hard check for the limits)

You'll have to pass the limits as arguments.

The idea is that there is no need to implement isValidConfiguration joint per joint. It can be done at the model level, and checked element-by-element. So I was thinking of using the internal lowerPositionLimit and upperPositionLimit. I can do it with custom limits too, of course.

[jcarpent]

Providing the two signatures would be nice indeed.

[jmirabel]

In which class do you want to implement the isValidConfiguration function ? Lie group classes do not have limits as member attributes.

[jcarpent]

The guess is to perform that at the model configuration level for the whole using the difference function.

[gabrielebndn]

In which class do you want to implement the isValidConfiguration function ? Lie group classes do not have limits as member attributes.

No class. I am not planning to implement it for Lie groups. Just a free function for the model. Basically (just a sketch):

bool isValidConfiguration(Model model,
                          VectorXd q,
                          VectorXd lowerBound,
                          VectorXd upperBound,
                          double prec=Eigen::NumTraits<double>::dummy_precision())
{
  if(!isNormalized(model, q, prec))
    return false;

  for(int i=0; i<model.nq; i++)
  {
    if(q[i] < lowerBound[i] || q[i] > upperBound[i])
      return false;
  }

  return true;
}

bool isValidConfiguration(Model model,
                          VectorXd q,
                          double prec=Eigen::NumTraits<double>::dummy_precision())
{
  return isValidConfiguration(model, q, model.lowerPositionLimit, model.upperPositionLimit, prec);
}

[jcarpent]

Please do it for Lie groups. It is just a matter of using Pinocchio::difference.

[florent-lamiraux]

We may decide to do as it is done in Eigen: state the small norm issue in the doc.

What about an assert, or an exception for those who never compile in debug mode ?

[gabrielebndn]

Please do it for Lie groups. It is just a matter of using Pinocchio::difference.

Not sure about it. First of all, the concept of bound for Lie Groups is dubious at best as far as I understand, second of all I am really not sure on how to implement it with quaternions (and it can be confusing for SO(2) too, what about \pi - \delta <= \theta <= \pi + \delta)? The truth is the bounds on the quaternion part of the configuration have never had a lot of sense and we only have them for ease of implementation. @jmirabel what do you think?

I think that for the bounds I'll stick to the element-by-element comparison performed at the model level. If you think it is an inadequate implementation, then I'll leave it to you, it was just a suggestion :slightly_smiling_face:

[jcarpent]

@gabrielebndn, you just need to use the correct operations, and pinocchio::difference will be transparent in your implementation. This is the only correct operator. In the case of quaternion parts, the limit is a bit above the one value for each component, and it won't affect the check. Test it. This is my only but main concern.

[jmirabel]

Not sure about it. First of all, the concept of bound for Lie Groups is dubious at best as far as I understand, second of all I am really not sure on how to implement it with quaternions (and it can be confusing for SO(2) too, what about \pi - \delta <= \theta <= \pi + \delta)? The truth is the bounds on the quaternion part of the configuration have never had a lot of sense and we only have them for ease of implementation. @jmirabel what do you think?

As you said, checking the bound of a unit quaternion or a unit complex is meaningless. Two useful things:

• a light check that ensure the coefficient are within [-1, 1], with a tiny epsilon.
• check the normalization.
  For both, the target is to ease debugging.

Please do it for Lie groups. It is just a matter of using Pinocchio::difference.

@jcarpent I think I don't understand what you mean. What do you want to do with difference ?

[jcarpent]

@jcarpent I think I don't understand what you mean. What do you want to do with difference ?

As simple as: pinocchio::difference(model,q,lb).array() >= 0.

[jcarpent]

This is exactly how it works in TSID and other related frameworks.

[jmirabel]

This won't work for quaternions.

[gabrielebndn]

This won't work for quaternions.

That's exactly my point.

[gabrielebndn]

Two useful things:

* a light check that ensure the coefficient are within [-1, 1], with a tiny epsilon.

* check the normalization.

@jmirabel, I'll just note that the second point implies the first one. If the quaternion is normalized, then its coefficients are certainly within [-1, 1], so no need to check it separately.

[jmirabel]

Yes, but this means you can safely do lb <= q <= ub, assuming lb and ub are properly set.

[gabrielebndn]

Yes, but this means you can safely do lb <= q <= ub, assuming lb and ub are properly set.

Of course, that was the idea

[jmirabel]

About the function name, I think isValidConfiguration is not precise enough. One may think you do collision checking too.
Not sure exactly of what function would suit it more:

• isConfigurationWithinBounds,
• isWithinBounds
• isConfigInConfigSpace

The unit-norm check being somehow equivalent to the standard joint bounds, I like the second one.

[jcarpent]

I would prefer to keep the original suggestion. The doc should be precise enough.
Your suggestions seem to be too long and not providing additional meaning to me.

[jmirabel]

isValidConfiguration isn't very short (and makes the code stutter).

From the developer perspective, I judge

pinocchio::isWithinBounds(model, config);

more readable than

pinocchio::isValidConfiguration(model, config);

[jcarpent]

There a two thing:
isValidConfiguration is calling isWithinBounds + isNormalized.

[gabrielebndn]

isValidConfiguration isn't very short (and makes the code stutter).

From the developer perspective, I judge

pinocchio::isWithinBounds(model, config);

more readable than

pinocchio::isValidConfiguration(model, config);

The idea was that isValidConfiguration would check that it is normalized _and_ within bounds. BTW, I am starting to reconsider the need of checking the bounds, after all it is a one-liner in C++ :

((q-lowerBound).array() >= 0).all() && ((upperBound-q).array() >= 0).all()

[jcarpent]

Yes, but you need to know Eigen.

[gabrielebndn]

There a two thing:
isValidConfiguration is calling isWithinBounds + isNormalized.

Exactly. Should I implement _both_ isWithinBounds and isValidConfiguration? Feels redundant, but costs me nothing

[gabrielebndn]

Otherwise, I can just implement isWithinBounds and let users do

isNormalized(q) && isWithinBounds(model, q);

Simple, clear, and doesn't need any advanced Eigen knowledge ;)

[jcarpent]

I'm not against it, and should be easy.

[gabrielebndn]

Perfect

[gabrielebndn]

Closed by #1324