issue with fixed joints in the computeKinematics() function

[VModugno]

Hi guys,
I'm currently working with urdf2casadi-matlab and I have encountered an issue with the computeKinematics() function when I set some joints in the urdf as fixed. In particular, I get the error on line 34 ( X{i}{1,j} = eye(smds.NB) ) and I managed to fix it by changing this line with X{i}{1,j} = eye(6). In this way the code run but I'm not sure about the correctness of the change. I believe it is just an error about the dimension of the matrix X{i}{1,j} which should not depend on the number of active joints but should be always a 6x6 identity matrix. I would have wanted to compare the resulting kinematic with idyntree but I'm having some trouble defining which are the active joints in the object kinDynComp (which is the variable containing the idyntree object inside the computeMassMatrixIDynTree() function inside urdf2casadi-matlab). If you want I can run some test I just need to know how to set the active joints in the idyntree object.
thank you for your help

[singhbal-baljinder]

Thanks for pointing out this issue @VModugno. Indeed the line you mentioned has a bug:

urdf2casadi-matlab/Utils/computeKinematics.m

Line 34 in c91d729

X{i}{1,j} = eye(smds.NB);

As you correctly noticed, it should be X{i}{1,j} = eye(6);. As for the fixed joints I have started implementing it in:

urdf2casadi-matlab/Utils/modelExtractionFunctions/accountForFixedJoint.m

Lines 1 to 65 in c91d729

	function smds = accountForFixedJoint(smds)
	%Accorporate bodies connected by `fixed` joints
	% Find indeces of fixed joints
	fixedJoints = find(ismember(smds.jtype,'fixed'));
	%% Save fixed joint and corresponding link name
	smds.fixed.jointName = smds.jointName(fixedJoints);
	smds.fixed.linkName = smds.linkName(fixedJoints);
	smds.fixed.linkInertia = smds.I(fixedJoints);
	smds.fixed.XTree = smds.Xtree(fixedJoints);
	if ~isempty(fixedJoints)
	% For each joint j accorporate the link j in link j-1
	for j = smds.NB:-1:1
	if strcmp(smds.jtype{j}, 'fixed')
	% Update spatial inertia matrix of bodies connected by fixed joints
	% Sum the spatial inertia of the body connected by the fixed joint by
	% first expressing the inestia in the same reference frame
	% Exclude the first joint from the count:
	% If the joint connecting the first link with the base link is fixed
	% its inertia is added to the base link, which for now is condered
	% fixed.
	if j>1
	smds.I{1,j-1} = smds.I{1,j-1} + (smds.Xtree{j}.')*smds.I{1,j}*smds.Xtree{j};
	end
	if j<smds.NB
	% Update the transform considering that the joint is fixed
	smds.Xtree{j+1} = smds.Xtree{j+1}*smds.Xtree{j};
	% Update parents list. Since we are removing the fixed joint
	% link we decrease by one the parents.
	smds.parent(j+1:end) = smds.parent(j+1:end)-1;
	end
	end
	end
	%% Remove link corresponding to fixed joints
	% Update the final number of considered joints
	smds.NB = smds.NB - max(size(fixedJoints));
	% Delete from the list of parents the entry of the body with index
	% fixedJoint(j), even if it connects with the base link
	smds.parent(fixedJoints) = [];
	% Delete body at fixedJoints(j) index from body list
	smds.I(fixedJoints) = [];
	% The masses should have already been added in the spatial inertia
	smds.mass(fixedJoints) = [];
	smds.com(fixedJoints) = [];
	smds.rpy(fixedJoints) = [];
	% Delete fixed joint from joint list
	smds.Xtree(fixedJoints) = [];
	smds.linkName(fixedJoints) = [];
	smds.jointName(fixedJoints) = [];
	% Remove fixed joints from list of joint types and axis
	smds.jtype(fixedJoints) = [];
	smds.jaxis(fixedJoints) = [];
	end
	end

I have only tested it with this URDF https://github.com/robotology/urdf2casadi-matlab/blob/master/URDFs/iCub_r_leg.urdf and it seemed to work. As for how to check the number of non fixed joints in iDynTree it should be sufficient to use the following command: kinDynComp.model().getNrOfJoints(). Alternatively, if for example you have a model with only single degree of freedom joints, you can use: kinDynComp.model().getNrOfDOFs().