The pose_jacobian method of DQ_holonomicBase class returns a wrong Jacobian [BUG]

Bug description

Hi @mmmarinho!, I think that the same bug 62 (currently fixed in Matlab) is present in Python and C++. The pose_jacobian method of DQ_holonomicBase class returns a wrong Jacobian. Specifically, in the C++ implementation, the line 86 is wrong. Current: const double j73 = 0.25(xc - ys), but the right is: const double j73 = 0.25(-xc - ys).

Code

import numpy as np
from dqrobotics.robot_modeling import DQ_HolonomicBase

q = np.array([0.0003,   -0.0003,    0.0158])
holonomic_base = DQ_HolonomicBase()
J = holonomic_base.pose_jacobian(q, 2)
print(J)

Output

[[ 0.00000000e+00  0.00000000e+00 -3.94995891e-03]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  4.99984398e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 4.99984398e-01  3.94995891e-03 -7.55901535e-05]
 [-3.94995891e-03  4.99984398e-01  7.55901535e-05]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]]

Process finished with exit code 0

Expected behavior

I expect the same result of the implementation of Jhol in equation 4.11 of the Bruno dissertation.

Expected output

import numpy as np
from math import sin, cos

def my_holonomic_jacobian(q):

    x = q[0]
    y = q[1]
    phi = q[2]

    s = sin(phi / 2)
    c = cos(phi / 2)
    j71 = -0.5 * s
    j62 = -j71
    j13 = -j62
    j72 = 0.5 * c
    j61 = j72
    j43 = j61

    j63 = 0.25 * (-x * s + y * c)
    j73 = 0.25 * (-x * c - y * s)
    J = np.array([[0, 0, j13],
                  [0, 0, 0],
                  [0, 0, 0],
                  [0, 0, j43],
                  [0, 0, 0],
                  [j61, j62, j63],
                  [j71, j72, j73],
                  [0, 0, 0]])
    return J

q = np.array([0.0003,   -0.0003,    0.0158])
J = my_holonomic_jacobian(q)
print(J)

[[ 0.00000000e+00  0.00000000e+00 -3.94995891e-03]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  4.99984398e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 4.99984398e-01  3.94995891e-03 -7.55901535e-05]
 [-3.94995891e-03  4.99984398e-01 -7.44051658e-05]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00]]

Process finished with exit code 0

Environment:

OS: Ubuntu 18.04
dqrobotics version: dqrobotics-20.4.0.17
Python version 3.6

Cheers!

Juancho.

dqrobotics / python

The pose_jacobian method of DQ_holonomicBase class returns a wrong Jacobian [BUG] #19