Problem with other URDF model with fixed joint and iiwa14 URDF model

[yuying334]

Hello,
There are some problems with the testing using URDF model like iiwa14. The jacobian matrix has significantly error using symbolic method comparing with using iDytree and with geometricjacobian in Matlab. Could you please add the literature for calculating jacobian that you refer to. And is there is fixed joint in urdf model, it can not work properly in file accountForFixedjoint. Thank you and looking forward to your response.

[traversaro]

Hello @yuying334, can you provide the code you using to compare the results of urdf2casadi-matlab, iDynTree and geometricjacobian ? Thanks!

[singhbal-baljinder]

@yuying334 Unfortunately the fixed joint case is not supported for now , see #5. I will change the documentation to make it more explicit.

[yuying334]

Hello, I just change the file testJacobian.m to compare the results of urdf2casadi-matlab, iDynTree and geometricjacobian

%% Choose a urdf model
close all
location_tests_folder = 'D:\urdf2casadi-matlab-master\urdf2casadi-matlab-master';
project_path=genpath(location_tests_folder);
addpath(project_path);
kuka_urdf = [location_tests_folder,'/URDFs/kr30_ha-identified.urdf'];
twoLink_urdf = [location_tests_folder,'/URDFs/twoLinks.urdf'];
kuka_kr210 = [location_tests_folder,'/URDFs/kuka_kr210.urdf'];
% dual_arm = [location_tests_folder,'/URDFs/model_left.urdf'];
iCub_r_leg = [location_tests_folder,'/URDFs/iCub_r_leg.urdf'];
iiwa14 = [location_tests_folder,'/URDFs/iiwa14.urdf'];
iCub_genova= [location_tests_folder,'/URDFs/iCubGenova9.urdf'];

%% Input urdf file to acquire robot structure
robot=importrobot('iiwa14.urdf');
robot.DataFormat='column';
robotURDFModel = iiwa14;
casadi_path='D:/casadi-windows-matlabR2016a-v3.5.5';
addpath(casadi_path)
urdf2casadi_path='D:/urdf2casadi-matlab-master';
addpath(urdf2casadi_path)
setenv("PATH",strcat("D:\conda\conda\envs\project-urdf2casadi\Library\bin", ";", getenv("PATH")));
addpath("D:\conda\conda\envs\project-urdf2casadi\Library\mex")

%% Generate functions
% Fix location folder to store the generated c and .mex files
location_generated_functions = [location_tests_folder,'/automaticallyGeneratedFunctions'];
opts.geneate_c_code = true;
opts.location_generated_fucntion = location_generated_functions;
opts.FrameVelocityRepresentation = "MIXED_REPRESENTATION";
[J_symb,X,XForce,S,O_X_ee] = urdf2casadi.Dynamics.auxiliarySymbolicDynamicsFunctions.createSpatialTransformsFunction(robotURDFModel,opts);

% IDynTree variables
mdlLoader = iDynTree.ModelCalibrationHelper();
mdlLoader.loadModelFromFile(robotURDFModel);
kinDynComp = iDynTree.KinDynComputations();
kinDynComp.loadRobotModel(mdlLoader.model());

% Set the body-fixed frame (left-trivialized velocity) representation
kinDynComp.setFrameVelocityRepresentation(iDynTree.MIXED_REPRESENTATION);
model_iDyn = kinDynComp.model();

nrOfJoints = model_iDyn.getNrOfJoints();
q_idyn = iDynTree.VectorDynSize(nrOfJoints);
dq_idyn = iDynTree.VectorDynSize(nrOfJoints);
grav = iDynTree.Vector3();
J_idyn = iDynTree.MatrixDynSize();


% Constants
gravityModulus = 9.80665;
baseFrame = 0;
eeFrame = 6;
nrOfTests = 10;
J_iDyn_list = zeros(6,nrOfJoints,nrOfTests);
J_symb_list = zeros(6,nrOfJoints,nrOfTests);
J_matlab_list= zeros(6,nrOfJoints,nrOfTests);
e_jac_DS = zeros(1,nrOfTests);
e_jac_GS = zeros(1,nrOfTests);
for i = 1:nrOfTests
    
    jointPos = rand(nrOfJoints,1);
    
    % Compute jacobian with iDynTree
    q_idyn.fromMatlab(jointPos);
    dq_idyn.fromMatlab(zeros(nrOfJoints,1));
    grav.fromMatlab([0;0;-gravityModulus]);

    kinDynComp.setRobotState(q_idyn, dq_idyn, grav);
    kinDynComp.getRelativeJacobian(baseFrame, eeFrame, J_idyn);
    J_tmp = J_idyn.toMatlab();
    
    % Featherstone's notation considers first the angular part
    J_iDyn_list(4:6,:,i) = J_tmp(1:3,:);
    J_iDyn_list(1:3,:,i) = J_tmp(4:6,:);

    % Compute Jacobian with symbolic function
    J_symb_list(:,:,i) = full(J_symb(jointPos));
    J_matlab_list(:,:,i) = full(geometricJacobian(robot,jointPos,robot.BodyNames{1,end}));
    
    % Error in the norm of the difference of the Jacobians
    e_jac_DS(:,i)  = norm(J_symb_list(:,:,i) - J_iDyn_list(:,:,i));
    e_jac_GS(:,i)  = norm(J_iDyn_list(:,:,i) - J_matlab_list(:,:,i));

end
% Plot error
figure(1)
plot(e_jac_DS'); title('Error: norm(J_{symb}  - J_{iDyn} )')
figure(2)
plot(e_jac_GS'); title('Error: norm(J_{symb}  - J_{Geometric} )')

[singhbal-baljinder]

@yuying334 Regarding the mismatch in the Jacobian computation the first thing that comes to my mind is to be sure to be using the same reference frame with respect to the Jacobian is computed. The notation I am using in this project and the algorithms come from the following sources:

• https://github.com/traversaro/traversaro-phd-thesis (https://github.com/traversaro/traversaro-phd-thesis/releases);
• https://research.tue.nl/en/publications/multibody-dynamics-notation-version-2;
• https://link.springer.com/book/10.1007/978-1-4899-7560-7.

I hope this can help a little in the debugging.

[yuying334]

Thank you for your support. I will cheack it out

[yuying334]

Hi, I still confused about the spatial motion transform from link i to any link j in its subtree: X{i}{1,j}, why it is a 6 by 6 matrix but not Homogeneous transformation martix namely 4 by 4 matrix? Thank you in advance.

[traversaro]

To which specific equation are you referring to?

[yuying334]

I can not finde the equation in the sources paper, I just want to figure out your code, for example, the function to calculate Kinematics in file "computeKinematics.m" with following code:
`function [X,XForce,S,Xup, v, a] = computeKinematics (smds, q, qd, qdd, g)
%Compute forward kinematics transformations, link velocities and accelerations
% Specificcaly compute:
% *link i spatial velocity(the left trivialized velocity in Traversaro's notation), acceleation in LOCAL body i coordinates
% *motion coordinate transform
% from each link i parent lambda(i) and link i (i_X_(lambda(i))=
% X_up{i});
% *spatial motion transform from link i to any link j in its subtree: X{i}{1,j}. Notice that the
% transform from base to link i is simply X{1}{1,i}*Xup{1}
% *force coordinate transform from body i to its predecessor(XForce{i})
% *Joint motion subspace matrix S (referred as K in Springer Handbook of Robotics(2016) in Chapter 6.3)

% Import necessary functions
import urdf2casadi.Utils.Spatial.jcalc
import urdf2casadi.Utils.Spatial.crm

%Gravity
a_grav = [0;0;0;g(1);g(2);g(3)];

% smds.NB: The number of links excluding the base, which for now is considered
% fix(= number of Joints)
for i = 1:smds.NB
[ XJ, S{i} ] = jcalc(smds.jaxis{i}, smds.jtype{i}, q(i) );
vJ = S{i}qd(i);
Xup{i} = XJ * smds.Xtree{i};
if smds.parent(i) == 0
v{i} = vJ;
a{i} = Xup{i}(-a_grav) + S{i}*qdd(i);
else
v{i} = Xup{i}*v{smds.parent(i)} + vJ;
a{i} = Xup{i}*a{smds.parent(i)} + S{i}*qdd(i) + crm(v{i})*vJ;
end
end
for i = 1:smds.NB
X{i}{1,i} = eye(6);
XForce{i}{1,i} = eye(6);
for j = i+1:smds.NB
X{i}{1,j} = eye(6);
for k = i+1:j
X{i}{1,j} = Xup{k}*X{i}{1,j};
end
XForce{j}{1,i} = X{i}{1,j}.';
end
end`
Here if i did not missunderstand the meaning of X{i}{1,j}, it is spatial motion transformation matrix, right? As i know, the spatial motion transformation that used widely is Homogeneous transformation martix with rotation and translation, namely a 4 by 4 matrix but not a 6 by 6 matrix in the file? could you please tell me why it is a 6 by 6 matrix? Thank you

[singhbal-baljinder]

Hi, I still confused about the spatial motion transform from link i to any link j in its subtree: X{i}{1,j}, why it is a 6 by 6 matrix but not Homogeneous transformation martix namely 4 by 4 matrix? Thank you in advance.

@yuying334 This project uses the Spatial(6D) Vectors formalism, the one described in the third reference linked in #23 (comment). More material on this topic can be found in http://royfeatherstone.org/teaching/index.html.