Numerical issue when computing gradient w.r.t. quaternion using casadi

[modu-robotics]

Bug description

I am trying to solve an optimization problem using pinocchio+casadi, where my cost function is a scalar valued function including pinocchio::quaternion::log3 operator. When casadi attempts to evaluate gradient of my cost function at zero configuration, the gradient has NAN values at quaternion components. I assume this is numerical stability issue rather than a bug, since the gradient has a value at small non zero quaternion. Here is a minimal example to reproduce this:

  auto quat_casadi = casadi::SX::sym("quat", 4);  // qx, qy, qz, qw

  // eigen quaternion eats scalar first
  Eigen::Quaternion<casadi::SX> quat(quat_casadi(3), quat_casadi(0),
                                     quat_casadi(1), quat_casadi(2));
  auto so3 = pinocchio::quaternion::log3(quat);
  auto so3_casadi = casadi::SX::sym("so3", 3);
  pinocchio::casadi::copy(so3, so3_casadi);
  auto cost = casadi::SX::dot(so3_casadi, so3_casadi);

  auto grad_func =
      casadi::Function("grad", {quat_casadi}, {gradient(cost, quat_casadi)},
                       {"quat_in"}, {"grad_out"});

  auto out = grad_func(casadi::DMDict{
      {"quat_in", std::vector<double>{0, 0, 0, 1}}}); // evaluate gradient with a zero quaternion

  std::cout << out << std::endl; // This reports {"grad_out": [nan, nan, nan, 0]}

  auto out = grad_func(casadi::DMDict{
      {"quat_in", std::vector<double>{0, 0, 0.0499792, 0.9987503}}}); // evaluate gradient with a small yaw rotation (0.1 rad)
  std::cout << out << std::endl; // This reports {"grad_out": [0, 0, 0.4005, -1.66625e-05]}

System

• OS: Mac OS
• Pinocchio version: source install from pinocchio3-preview

[modu-robotics]

I tried to update pinocchio3-preview branch's pinocchio::quaternion::log3 function to match master branch's pinocchio::quaternion::log3 function and it resolves the issue.
Is there any plan to update this function?
If authors have no bandwidth for it, I am happy to make a PR.

[jcarpent]

Thanks for the report. Please, integrate the change in the branch. Thanks in advanced.