dqrobotics/cpp

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

maripaf opened this issue · 1 comments

maripaf commented

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.