Problems using DQ_CooperativeDualTaskSpace

[juanjqo]

Hi @mmmarinho,

I have problems with the absolute pose jacobian matrix using DQ_CooperativeDualTaskSpace. Specifically, the matrix dimensions are wrong.

Using the Matlab example in Python:

from dqrobotics import *
from dqrobotics.robots         import KukaLw4Robot
from dqrobotics.robot_modeling import DQ_Kinematics
from dqrobotics.robot_modeling import DQ_CooperativeDualTaskSpace
from numpy import hstack, vstack
import numpy as np

theta = np.random.rand(7,1)

kuka1 = KukaLw4Robot.kinematics()
frame1 = 1 + DQ.E*0.5*DQ([0,-0.4,0,0])
kuka1.set_base_frame(frame1)
kuka1.set_reference_frame(frame1)

kuka2 = KukaLw4Robot.kinematics()
frame2 = 1+DQ.E*0.5*DQ([0, 0.4,0,0])
kuka2.set_base_frame(frame2)
kuka2.set_reference_frame(frame2)
two_arms = DQ_CooperativeDualTaskSpace(kuka1, kuka2)

q1_start = np.array([-1.6965, 2.2620, 1.5708, 1.4451, -0.4398, 0.0628, 0])
q2_start = np.array([1.6336, -0.8168, 1.5708, 1.5080, -0.2513, 0, 0])

q = hstack((q1_start, q2_start))

two_arms.absolute_pose_jacobian(q)
print(two_arms.absolute_pose_jacobian(q).shape)
print(two_arms.relative_pose_jacobian(q).shape)

Returns:

(8, 49)
(8, 14)

Process finished with exit code 0

This is, the absolute Jacobian matrix is 8x49 and the relative Jacobian matrix is 8x14.

The same minimal example using matlab:

    clear all
    clc
    close all

    %% Basic definitions for the two-arm system
    kuka1 = KukaLwr4Robot.kinematics();    
    frame1 = 1 + DQ.E*0.5*DQ([0,-0.4,0,0]);
    kuka1.set_base_frame(frame1);
    kuka1.set_reference_frame(frame1);

    kuka2 = KukaLwr4Robot.kinematics();
    frame2 = 1+DQ.E*0.5*DQ([0, 0.4,0,0]);
    kuka2.set_base_frame(frame2);
    kuka2.set_reference_frame(frame2);
    two_arms = DQ_CooperativeDualTaskSpace(kuka1, kuka2);

    %% Initial conditions
    q1_start = [-1.6965, 2.2620, 1.5708, 1.4451, -0.4398, 0.0628, 0]';
    q2_start = [1.6336, -0.8168, 1.5708, 1.5080, -0.2513, 0, 0]';
    q = [q1_start; q2_start];
    size(two_arms.absolute_pose_jacobian(q))
    size( two_arms.relative_pose_jacobian(q))

Returns:

ans =
     8    14
ans =
     8    14

A temporal solution for this example is :

     Ja_aux = two_arms.absolute_pose_jacobian(q)
     Ja = Ja_aux[0:8, 0:14]

Cheers,

Juancho

[mmmarinho]

Hello, @juanjqo!

Something is wrong in the C++ code. I think I've identified the problem and I'll fix it later today.

[mmmarinho]

Should be fixed in fa061d5bae.

Output is now:

(8, 14)
(8, 14)

[juanjqo]

@mmmarinho, Thanks!

I have a question about updates. When I update the dqrobotics for Python via pip3, the dqrobotics for C++ is updated as well automatically? Or I have to update the C++ version using apt-get commands.

[mmmarinho]

Hello, @juanjqo,

They are isolated, you have to separately update the C++ version.

One thing to take into account is that the development PPA is being built daily, which means that it might take one day for you to be able to get the updated C++ code via the PPA.

The Python package is built after every commit, so you can get the update within 5min or so.

[juanjqo]

I see. Thanks!