Difference() behavior not consistent between joint types

[stephane-caron]

As of Pinocchio 2.6.4, pin.difference doesn't behave the same for different joint types when infinity is involved:

Free flyers:

>>> import pinocchio as pin
>>> model = pin.Model()
>>> model.addJoint(0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "root_joint")
1
>>> model.upperPositionLimit
array([1.79769313e+308, 1.79769313e+308, 1.79769313e+308, 1.79769313e+308,
       1.79769313e+308, 1.79769313e+308, 1.79769313e+308])
>>> pin.difference(model, pin.neutral(model), model.upperPositionLimit)
array([nan, nan, nan, nan, nan, nan])

Revolute joints:

>>> import pinocchio as pin
>>> model = pin.Model()
>>> model.addJoint(0, pin.JointModelRZ(), pin.SE3.Identity(), "foo")
1
>>> model.upperPositionLimit
array([1.79769313e+308])
>>> pin.difference(model, pin.neutral(model), model.upperPositionLimit)
array([1.79769313e+308])

Expected behavior:

I think the second behavior is the right one, as it is expected that "+infinity - finite = +infinity". (Also, that's the one that would trivialize computing joint limits in IK, which is my current use case 😉)

[cmastalli]

I believe it should report inf for undefined limits. Moreover, the result of difference should be also inf.

[jcarpent]

If you have an unbounded joint, then you should instead use the unbounded joint type, which is done for that.
The default value for the joint limits is +inf: the results you obtained come from internal computations.
We cannot guarantee a fixed output, as it really depends on the compiler in use.

I would say that it is the role of the end-user to properly use the correct input and output of the difference operations (which act on Lie groups).

[jcarpent]

For the case of FreeFlyer, the bounds on the rotation part have no meaning in general. The NaN comes from there.

[stephane-caron]

For the case of FreeFlyer, the bounds on the rotation part have no meaning in general. The NaN comes from there.

The bounds in translation can make sense though (e.g. a free-floating robot in a box, or the payload of a cable-driven parallel robot). Right now translation bounds are also nan-ed in free flyers.

If you have an unbounded joint, then you should instead use the unbounded joint type, which is done for that.

Just tried it out, however it behaves differently from the two others:

Unbounded revolute joints:

>>> import pinocchio as pin
>>> model = pin.Model()
>>> model.addJoint(0, pin.JointModelRUBZ(), pin.SE3.Identity(), "foo")
1
>>> model.upperPositionLimit
array([1.79769313e+308, 1.79769313e+308])
>>> pin.difference(model, pin.neutral(model), model.upperPositionLimit)
array([0.])

I agree that the error here rather lies with upperPositionLimit not being a valid configuration.

The default value for the joint limits is +inf: the results you obtained come from internal computations.

From model.hxx it seems the default is max float rather than +inf:

    const VectorXs min_config = VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
    const VectorXs max_config = VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());

I get your point that std::numeric_limits<Scalar>::infinity() will only work on architectures where std::numeric_limits<Scalar>::has_infinity == true is true, while max float will work everywhere. But is there a Pinocchio target architecture where std::numeric_limits<Scalar>::has_infinity == false? 🤔

[jcarpent]

The same for RBUZ joint types, there should be no bound on it. So, the questions are:

• (i) should we make the difference robust against limits bounds (max() value)?
• (ii) or should we instead use adequate values for LIe group joint limits?

I'm in favour of (ii) as the first one will induce a lowering of performances and make the function hardly differentiable for autodiff tools.

[stephane-caron]

OK, I see the stakes better. I agree it would make sense to have well-formed configurations as a pre-requisite for difference. Then the next thing to do would be to document this assumption, e.g. with a @note stating that the function's behavior is unspecified otherwise. (Ideally the type system could actually check this assumption for us, rather than delegating the task to end users.)

By (ii), do you mean changing the type of position limits to be something more structured than a configuration vector?

The use case I have in mind are joint position limits in a velocity-based IK. Currently it relies on

Delta_q_max = pin.difference(configuration.model, q_act, q_max)

And then some ad-hoc parsing on NaNs/big floats to figure out which tangent coordinates are not proper limits. One way to get rid of this would be to add something like a JointModel.hasLimits() function, so that we can easily construct the slice of tangent coordinates to look at.

[jcarpent]

Good idea to add the notion of hasConfigurationLimits() for each joint.
@fabinsch Could you handle this feature and extend the documentation accordingly?

[jcarpent]

About this notion of limits, a FreeFlyer will have a bound on the translation but not on the rotation.

[stephane-caron]

One way to get rid of this would be to add something like a JointModel.hasLimits() function, so that we can easily construct the slice of tangent coordinates to look at.

I should have made it explicit, but what I have in mind are runtime limits, i.e. whether the user/URDF specified a limit explicitly.

[jcarpent]

I think checking if the value is a max would do the job then.

[stephane-caron]

That works with configurations, the question is how to do the same with tangent vectors?

slice = [joint.idx_v for joint in model.joints if joint.has_runtime_configuration_limits]

Exposing the joint's min_config and max_config would do the job 👍

[jcarpent]

This is in the Model, no?

[jcarpent]

I don't understand your approach in fact. @stephane-caron Do you think is the right one?

[jcarpent]

What we can do for sure, is to set the real bounds on the configuration for non-Euclidian manifolds.

[jcarpent]

@stephane-caron To move forward in the current issue, I would suggest you clamp the limit values in the set [-1e20, 1e20] for instance to get correct output from difference.

[stephane-caron]

Yep, that's in essence how joint limits are implemented in Pink v0.5. What's not great about this approach is that in relies on how Pinocchio is implemented, rather than on its API.

To keep things in scope, let me spin a separate issue on this point: #1758

From my side, the initial point of this issue about the API for difference has been addressed #1753 and we could close it. 👌

[jcarpent]

What's not great about this approach is that in relies on how Pinocchio is implemented, rather than on its API.

I don't understand this point.

[stephane-caron]

Arh, sorry you meant clamping the inputs to difference to get correct outputs, while I was looking at clamping its outputs.

Clamping the inputs is not sufficient in itself because difference is not meant to handle out-of-manifold rotation coordinates. For example:

>>> model = pin.Model()
>>> model.addJoint(0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "root_joint")
1
>>> pin.difference(model, pin.neutral(model), np.array([1e20] * 7))
array([9618.35346861, 9618.35346861, 9618.35346861,    0.        ,
          0.        ,    0.        ])

But if we make sure unbounded rotations have proper coordinates, then we get correct outputs for the other coordinates:

>>> pin.difference(model, pin.neutral(model), np.array([1e20]*3 + [0, 0, 0, 1]))
array([1.e+20, 1.e+20, 1.e+20, 0.e+00, 0.e+00, 0.e+00])

No worries re. my issue which found a pleasant solution in stephane-caron/pink#12. I hope our exchange here was not too unstructured and somehow useful feedback for Pinocchio 😃

[jcarpent]

We need to fix in Pinocchio the proper value for the bounds limit for unbounded joints.
We will do that with the neutral element of the given joint type.