Wrong Holonomic Jacobian using DQ_HolonomicBase [BUG]

[juanjqo]

Bug description

• The method pose_jacobian from DQ_HolonomicBase returns a wrong Jacobian.

Code

from dqrobotics import*
from dqrobotics.robot_modeling import DQ_Kinematics
from dqrobotics.robot_modeling import DQ_HolonomicBase
import numpy as np

holonomic_base = DQ_HolonomicBase()

q = np.array([0.2, 0.3, 0.1], np.float64)
J = holonomic_base.pose_jacobian(q, 3)
print(J)
print(J.shape)

Output

[[ 0.         -0.02498958         nan]
 [ 0.          0.                 nan]
 [ 0.          0.                 nan]
 [ 0.          0.49937513         nan]
 [ 0.          0.                 nan]
 [ 0.02498958  0.07240731         nan]
 [ 0.49937513  0.04618908         nan]
 [ 0.          0.                 nan]]
(8, 3)

Process finished with exit code 0

Expected behavior

• The correct holonomic Jacobian.

Expected output

%By using Matlab
 holonomic_base = DQ_HolonomicBase()
q = [0.2 0.3 0.1]
J = holonomic_base.pose_jacobian(q)

J =

         0         0   -0.0250
         0         0         0
         0         0         0
         0         0    0.4994
         0         0         0
    0.4994    0.0250    0.0724
   -0.0250    0.4994    0.0462
         0         0         0

Environment:

• OS: Ubuntu 18.04

• dqrobotics version: Version: 0.1.1.42

• Python version: Python 3.6.8

• MATLAB version: 2018a