[BUG] The pose_jacobian() of DQ_HolonomicBase is trying to access link index 3

[marcos-pereira]

Bug description

• The method pose_jacobian() of DQ_HolonomicBase is giving a runtime error saying that it tried to access link index 3.

To Reproduce

Code

import numpy as np
from dqrobotics.robot_modeling import DQ_HolonomicBase

q = np.array([0.12,   0.0,    0.0])
holonomic_base = DQ_HolonomicBase()
J = holonomic_base.pose_jacobian(q)
print(J)

Output

Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
RuntimeError: Tried to access link index 3 which is unavailable.

Expected behavior

• The code should return a jacobian matrix as in the following matlab code

Matlab code

q = [0.12 0 0];
robot = DQ_HolonomicBase();
J_pose = robot.pose_jacobian(q)

Matlab output

J_pose =

         0         0         0
         0         0         0
         0         0         0
         0         0    0.5000
         0         0         0
    0.5000         0         0
         0    0.5000    0.0300
         0         0         0

Environment:

• OS: Ubuntu 18.04
• dqrobotics version 20.4.0.17
• Python version 3.6.9

Additional context

• Since the Python code depend on the C++ code, I believe there is a problem on line 64 of the DQ_HolonomicBase.cpp. I would guess it should be

if(to_link > 3 || to_link < 0)

because of the dimension of the configuration space of the holonomic base.

[mmmarinho]

Hello, @marcos-pereira,

I have a little bit too much going on to test this thoroughly but I don't want to leave you hanging.
I proposed a quick fix which I think will work, but could you test and let me know how it goes?

Kind regards,
Murilo

[marcos-pereira]

Hello @mmmarinho ,

Thank you for the quick fix.
I will test it as soon as the update is available.
By looking at the fix, it seems it should work :).

Best regards,
Marcos

[marcos-pereira]

Hi @mmmarinho ,

The fix with the DQ_HolonomicBase worked, but I found a really similar issue on the DQ_DifferentialDriveRobot. When calculating the Jacobian of the DifferentialDriveRobot, it tries to access link with index 3 because of line 50 in DQ_DifferentialDriveRobot.

Should I open a new issue?

EDIT: I opened issue #22 .

Best regards,
Marcos