[BUG] pose_jacobian_derivative() is not working properly

[juanjqo]

Bug description

• I updated the dqrobotics to the last version (19.10.0.1). However, the new changes break all my codes based on the dqrobotics. I am working on to do my codes compatibles with the last version. However, The pose_jacobian_derivative() method is not working properly.

Code

from dqrobotics.robots import KukaLw4Robot
import numpy as np
from dqrobotics.robot_modeling import DQ_Kinematics
robot = KukaLw4Robot.kinematics()
q = np.array([0,0,0,0,0,0,0])
q_dot = np.array([0,0,0,0,0,0,0])

J = robot.pose_jacobian(q)
J_dot = robot.pose_jacobian_derivative(q, q_dot)

print("shapes")
print(J.shape)
print(J_dot.shape)

Output

Traceback (most recent call last):
  File "/home/juanjqo/catkin_ws/src/my_dynamixel_poppy/src/poppeye_rosPython/Pendulum_robot/test_pendulum.py", line 28, in <module>
    J_dot = robot.pose_jacobian_derivative(q, q_dot)
TypeError: pose_jacobian_derivative(): incompatible function arguments. The following argument types are supported:
    1. (self: dqrobotics.robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[float64[m, 1]], arg1: numpy.ndarray[float64[m, 1]], arg2: int) -> numpy.ndarray[float64[m, n]]

Invoked with: <dqrobotics.robot_modeling.DQ_SerialManipulator object at 0x7fd56d40f570>, array([0, 0, 0, 0, 0, 0, 0]), array([0, 0, 0, 0, 0, 0, 0])

Process finished with exit code 1

Expected behavior

• I expect to compute correctly the pose jacobian derivative J_dot = robot.pose_jacobian_derivative(q, q_dot)

Expected output By using Matlab

 robot = KukaLwr4Robot.kinematics() % Btw, the name is different in Matlab. it is this expected?
 q = zeros(7,1);
 q_dot = [1 2 3 4 5 6 7]';
 J_dot = robot.pose_jacobian_derivative(q, q_dot)

J_dot =

   -4.0000   -1.0000   -4.0000    1.0000   -4.0000   -1.0000   -4.0000
    1.0000   -3.5000   -0.0000    2.0000    2.0000    0.5000   -1.0000
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
    0.5450    0.8400    0.7850    0.3200    1.1050    0.2750   -0.5450
   -2.2000    0.2350   -2.2000   -0.2350   -2.2000   -0.5450   -2.2000

Environment:

• OS: [Ubuntu 18.04]
• dqrobotics version 19.10.0.1
• Python version [3.6]
• MATLAB version [2018a]

Additional context

• Using "pose_jacobian_derivative(q, q_dot, to_joint)" works when _tojoint is lower than 7. Example:

from dqrobotics.robots import KukaLw4Robot
import numpy as np
from dqrobotics.robot_modeling import DQ_Kinematics
robot = KukaLw4Robot.kinematics()
q = np.array([0,0,0,0,0,0,0])
q_dot = np.array([1,2,3,4,5,6,7])

J = robot.pose_jacobian(q)
J_dot = robot.pose_jacobian_derivative(q, q_dot, 6)

print("shapes")
print(J.shape)
print(J_dot.shape)

Output:

shapes
(8, 7)
(8, 6)

[mmmarinho]

Hello, @juanjqo,

Thanks for the report.

That bugfix you mentioned was to make all the Python code zero-indexed as expected. Just some methods were one-indexed in prior development versions, but that was not intentional.

The signature jacobian_time_derivative(q, q_dot) doesn’t exist in Python yet and that’s where the error comes from. It has nothing to do with the indexing.

[juanjqo]

Hi @mmmarinho. Yes, it was what I thought. Thank you for your answer!

Cheers,

Juancho.

[mmmarinho]

@juanjqo

It should be working now.

About CPLEX, open a new issue and I can explain to you how to do it.

[juanjqo]

@mmmarinho thanks for the support!