[BUG] Logarithm of a unit dual quaternion is returning `nan`

Bug description

Similar to #25 : When running a kinematic controller, after the system stabilizes with the error close to zero (1-invariant error), the logarithm of the dual quaternion error is returning "nan".

To Reproduce

Running the following code you will see that the running stops after some iterations, when the logarithm becomes "nan".

Code

#include <Eigen/Dense>
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <iostream>
#include <cmath>

using namespace DQ_robotics;

int main()
{
    // robot definition
    Matrix<double, 4, 7> kuka_dh;
    kuka_dh << 0, 0, 0, 0, 0, 0, 0, // theta
               0.3105, 0, 0.4, 0, 0.39, 0, 0, // d
               0, 0, 0, 0, 0, 0, 0, // a
               0, M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, M_PI_2, -M_PI_2; // alpha
    DQ_SerialManipulator kuka(kuka_dh,"modified");

    int num_dof = kuka.get_dim_configuration_space();
    VectorXd q(num_dof), q_dot(num_dof);
    q << 0., 0.5, 0., -1.2, 0., 0., 0.;
    q_dot << 0, 0, 0, 0, 0, 0, 0;

    Eigen::MatrixXd J = kuka.pose_jacobian(q);
    Eigen::MatrixXd N(8,num_dof);
    DQ x = kuka.fkm(q);

    VectorXd q_desired(num_dof);
    q_desired << 0., 0.5, 0.5, -1.2, 0., 0., 0.;
    DQ xd = kuka.fkm(q_desired);

    DQ x_til = x.conj()*xd;

    double k = 5;
    double sampling_time = 0.005;
    int i = 0;

    // initialize rotation angle
    DQ test;

    while (i < 2000) {

        ++i;

        x = kuka.fkm(q);
        J = kuka.pose_jacobian(q);

        // invariant error
        x_til = x.conj()*xd;

        // check log
        test = log(x_til);
        if (std::isnan(test.q(0))) {
            std::cout << test << std::endl;
            return 1;
        }

        // first order kinematic control
        N = haminus8(xd)*C8()*J;
        q_dot = pinv(N)*(k*vec8(1-x_til));
        q = sampling_time*q_dot + q;

        // error
        std::cout << (1 - x.conj()*xd).vec8().norm() << std::endl;
    }

    // exit program
    return 0;
}

Output

nan nani nanj nank +E( 0 0i -2.26944e-09j 0k )

Expected behavior

I expected the rotation angle to be zero.

Expected output

0 0i 0j 0k +E( 0 0i -2.26944e-09j 0k )

Environment:

OS: Ubuntu 16.04
dqrobotics version 0.1.0

Additional Information:

the log function in C++ is using the acos function instead of rotation_angle.

dqrobotics / cpp

[BUG] Logarithm of a unit dual quaternion is returning `nan` #26