Error when integrating SE3 with the same vector given as input and output

[duburcqa]

I don't think it would impede performance much since quaternions are fixed-size but you probably know better than me.

[jcarpent]

Before digging further, could you provide a reproducible issue example?
This is worth adding as a unit test to enhance the Pinocchio library.

[duburcqa]

You can clearly see that without copying the original quaternion in a temporary the chack that is done later after update the quaternion to determine if the sign has been reversed (const Scalar dot_product = res_quat.dot(quat);) becomes useless as it always returns exactly 1.0 because res_quat and quat are precisely pointing to the same memory space :)

[jcarpent]

Could you add such an example then in https://github.com/stack-of-tasks/pinocchio/blob/devel/unittest/joint-configurations.cpp? It will allow to not reproduce such bugs in the future.

[jcarpent]

Thanks in advance @duburcqa for your help.

[duburcqa]

The test here is already checking this. Just that random sampling one configuration is not a sufficient to trigger the bug. I suggest modifying the test to check many random configuration to be generic. What do you think ?

[duburcqa]

I will see if I can identify exactly when the issue is occurring, but I have no reproduction script for now.

[jcarpent]

This is fine for me to add the randomization of the test to be sure to output a failure case.
Thanks a lot @duburcqa.

[duburcqa]

I will have a look at it on sunday I think. I'm quite busy by then.

[jcarpent]

@duburcqa Do you have any idea when do you plan to finish this PR?

[duburcqa]

I will do it tomorrow. Sorry I have been quite busy lately.

[jcarpent]

No problem, thanks a lot for the short notice.

[duburcqa]

Hi, sorry for the delay. Now I have time to write a unit test. I found two ways to trigger the issue. I don't know which one you prefer to write a unit test. The random one is clearly the most convenient to implement as it is more generic.

  typedef double Scalar;
  typedef typename LieGroup<JointModelFreeFlyer>::type LieGroupType;
  typedef typename JointModelFreeFlyer::ConfigVector_t ConfigVector_t;
  typedef typename JointModelFreeFlyer::TangentVector_t TangentVector_t;

  // First approach
  JointModelFreeFlyer jmodel;
  ConfigVector_t const Ones(ConfigVector_t::Ones(jmodel.nq()));
  for (int i = 0; i <= 50; ++i) {
    ConfigVector_t qTest(ConfigVector_t::Random(jmodel.nq()));
    TangentVector_t qTest_dot(TangentVector_t::Random(jmodel.nv()));
    ConfigVector_t qResult(ConfigVector_t::Random(jmodel.nq()));
    qTest = LieGroupType().randomConfiguration(-Ones, Ones);
    ConfigVector_t const qTestCopy = qTest;
    LieGroupType().integrate(qTest, qTest_dot, qTest);
    assert(qTestCopy.dot(qTest) > 0.0);
  }

  // Second approach
  JointModelFreeFlyer jmodel;
  ConfigVector_t qTest(
      (ConfigVector_t() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished());
  TangentVector_t qTest_dot(
      (TangentVector_t() << 0.0, 0.0, 0.0, 0.0, 0.0, 4.0).finished());
  ConfigVector_t const qTestCopy = qTest;
  LieGroupType().integrate(qTest, qTest_dot, qTest);
  assert(qTestCopy.dot(qTest) > 0.0);

[duburcqa]

I updated the unit tests. It fails as expected without the proposed patch. I had to increase the tolerance because it was now failing otherwise due to running evaluating the tests 50 times with different samples now.

[jcarpent]

@duburcqa Thanks for finishing this PR.
The PR looks to me, even if I'm surprised by the fact you need to largely increase the tolerance. This should be investigated further in the future.

[duburcqa]

I'm also concerned about the relaxed tolerance thresholds. I could eventually find some time to investigate this issue.

[jcarpent]

I've shortly investigated the issue of this PR, and it is, in fact, solved by a more advanced formula present in Pinocchio 3x.

[duburcqa]

Perfect ! Glad to hear it