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

[marcos-pereira]

Hi @mmmarinho , although this issue is similar to issue #20 , I decided to open it because it is related to another file.

Bug description

• The method pose_jacobian() of DQ_DifferentialDriveRobot 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_DifferentialDriveRobot
q = np.array([0.12,   0.0,    0.0])
diff_robot = DQ_DifferentialDriveRobot(1,1)
J = diff_robot.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 unnavailable.

Expected behavior

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

Matlab code

q = [0.12 0 0];
robot = DQ_DifferentialDriveRobot(1,1);
J_pose = robot.pose_jacobian(q)

Matlab output

J_pose =

         0         0
         0         0
         0         0
    0.5000   -0.5000
         0         0
    0.2500    0.2500
    0.0300   -0.0300
         0         0

Environment:

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

Additional context

• Since the Python code depend on the C++ code, I believe there is a problem between lines 50 to 52 of 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.

[mmmarinho]

Thanks, @marcos-pereira,

Let me know how it goes.

[marcos-pereira]

Hi @mmmarinho , now when running

import numpy as np
from dqrobotics.robot_modeling import DQ_DifferentialDriveRobot
q = np.array([0.12,   0.0,    0.0])
diff_robot = DQ_DifferentialDriveRobot(1,1)
J = diff_robot.pose_jacobian(q)
print(J)

it throws a segmentation fault on the pose_jacobian line.
Looking at the DQ_DifferentialDriveRobot.cpp I don't know exactly what is the problem. Maybe on line 52 when adding +1, but I'm not sure. Just a guess: the Matlab version returns a two column Jacobian matrix, hence, the J.block() on line 52 should be (0,0,3,2), in other words, there is no need for the +1.
However I can't confirm that only by looking at the DQ_DifferentialDriveRobot.m and I also don't know how the Eigen block() method works.

[mmmarinho]

Hello, @marcos-pereira !

Thanks for testing.

This robot is a bit different from the other ones so I need to fix the implementation. I’ll propose another fix tomorrow when I have access to my computer.

Cheers,
Murilo

[mmmarinho]

Hello, @marcos-pereira,

Could you check if it is working now? Thanks in advance.

[marcos-pereira]

Hi, @mmmarinho ,

Thanks for the fix, but it is still not working.
When running:

import numpy as np
from dqrobotics.robot_modeling import DQ_DifferentialDriveRobot
q = np.array([0.12,   0.0,    0.0])
diff_robot = DQ_DifferentialDriveRobot(1,1)
J = diff_robot.pose_jacobian(q)

It strangely throws the runtime error:

Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
RuntimeError: DQ_DifferentialDriveRobot::pose_jacobian(q,to_link) only accepts to_link in {0,1}

I checked the DQ_DifferentialDriveRobot.cpp. It seems to be correct, but somehow it always enters the if of line 50. I also tried calculating pose_jacobian(q,1) or with different to_link values, but the runtime error always happens.