Skip to content

Commit

Permalink
Add angular contribution in the computation of the integral of Jc (#16)
Browse files Browse the repository at this point in the history
  • Loading branch information
@FabioBergonti
FabioBergonti authored Aug 8, 2022
1 parent 4f5f723 commit 154cd67
Show file tree
Hide file tree
Showing 7 changed files with 108 additions and 41 deletions.
18 changes: 18 additions & 0 deletions +mystica/+rbm/getQuatGivenRotm.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,3 +59,21 @@
quat = [qw;qx;qy;qz] / quaternionNorm;

end

%% alternative implementation
%
% function quat = getQuatGivenRotm2(rotm)
%
% qw = 0.5 * 1 * sqrt(rotm(1,1) + rotm(2,2) + rotm(3,3) + 1);
% qx = 0.5 * sign0(rotm(3,2) - rotm(2,3)) * sqrt(rotm(1,1) - rotm(2,2) - rotm(3,3) + 1);
% qy = 0.5 * sign0(rotm(1,3) - rotm(3,1)) * sqrt(rotm(2,2) - rotm(3,3) - rotm(1,1) + 1);
% qz = 0.5 * sign0(rotm(2,1) - rotm(1,2)) * sqrt(rotm(3,3) - rotm(1,1) - rotm(2,2) + 1);
%
% quat = [qw;qx;qy;qz];
%
% end
%
% function s = sign0(x)
% s = 2*(x>=0)-1;
% end

6 changes: 3 additions & 3 deletions +mystica/+rbm/getRotmGivenQuat.m
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
function rotm = getRotmGivenQuat(quat)

qw = quat(1);
qxyz = quat(2:4);

rotm = eye(3)+2*qw*mystica.utils.skew(qxyz)+2*mystica.utils.skew(qxyz)^2;

end
6 changes: 6 additions & 0 deletions +mystica/+rbm/invQuat.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
function quat_b_a = invQuat(quat_a_b)

qw = quat_a_b(1); qxyz = quat_a_b(2:4);
quat_b_a = [qw;-qxyz]/(norm(quat_a_b)^2);

end
19 changes: 19 additions & 0 deletions +mystica/+rbm/logQuat.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
function theta = logQuat(quat,input)
arguments
quat
input.selectQuat char {mustBeMember(input.selectQuat,{'positive','minDistance','normal'})} = 'normal'
end
qw = quat(1); qxyz = quat(2:4);
switch input.selectQuat
case 'normal'
normV = norm(qxyz,2)+eps;
theta = 2 * qxyz * atan2(normV,qw)/normV;
case 'positive'
quat = -quat*(qw<0)+quat*(qw>=0);
theta = mystica.rbm.logQuat(quat,'selectQuat','normal');
case 'minDistance'
theta1 = mystica.rbm.logQuat( quat,'selectQuat','normal'); n1 = norm(theta1,2);
theta2 = mystica.rbm.logQuat(-quat,'selectQuat','normal'); n2 = norm(theta2,2);
theta = theta1*(n1<=n2)+theta2*(n2<n1);
end
end
10 changes: 10 additions & 0 deletions +mystica/+rbm/multiplyQuat.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
function quat = multiplyQuat(quat1,quat2)

qw1 = quat1(1); qxyz1 = quat1(2:4);
qw2 = quat2(1); qxyz2 = quat2(2:4);

pw = qw1*qw2 - qxyz1'*qxyz2;
pxyz = qw1*qxyz2 + qw2*qxyz1 + cross(qxyz1,qxyz2);

quat = [pw;pxyz];
end
71 changes: 44 additions & 27 deletions +mystica/+state/@StateKinMBody/getJacobianConstraints.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,88 +5,105 @@ function getJacobianConstraints(obj,model)
obj mystica.state.StateKinMBody
model mystica.model.Model
end

Jc = {};
intJcV = {};
cnstr_lin = {};
cnstr_ang = {};

mBodyPosQuat_0_initial = casadi.SX.sym('x_initial',model.constants.mBodyPosQuat,1);

for i = transpose(model.indexesFixedLink(:))

sel_twist_i = model.linksAttributes{i}.selector.matrix_linkTwist_from_mBodyTwist;
linkPosQuat_0_initial = mBodyPosQuat_0_initial(model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);

linkPosQuat_0_initial = mBodyPosQuat_0_initial( model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);
linkPosQuat_0 = obj.csdSy.mBodyPosQuat_0(model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);

quat_bi_0 = mystica.rbm.invQuat(mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0_initial));
quat_0_b = mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0);
pos_0_bi = mystica.rbm.getPosGivenPosQuat( linkPosQuat_0_initial);
pos_0_b = mystica.rbm.getPosGivenPosQuat( linkPosQuat_0);

M = sel_twist_i;
N = [linkPosQuat_0_initial(1:model.constants.linkPos);sparse(model.constants.linkAngVel,1)];
N = [pos_0_b-pos_0_bi;mystica.rbm.logQuat(mystica.rbm.multiplyQuat(quat_0_b,quat_bi_0),'selectQuat','minDistance')];

Jc{ end+1} = M;
intJcV{ end+1} = N;
cnstr_lin{end+1} = [1 1 1 0 0 0]';
cnstr_ang{end+1} = [0 0 0 1 1 1]';
end

for i = 1 : model.nJoint
index = model.joints{i}.getJointConnectionDetails();

tform_0_p = obj.get_link_tform_b('iLink',index.parent,'model',model,'mBodyPosQuat_0',obj.csdSy.mBodyPosQuat_0);
tform_0_c = obj.get_link_tform_b('iLink',index.child ,'model',model,'mBodyPosQuat_0',obj.csdSy.mBodyPosQuat_0);
tform_p_pj = model.linksAttributes{index.parent}.tform_b_j{index.cPointParent};
tform_c_cj = model.linksAttributes{index.child }.tform_b_j{index.cPointChild };

tform_0_pj = tform_0_p*tform_p_pj;
tform_0_cj = tform_0_c*tform_c_cj;

quat_0_p = mystica.rbm.getQuatGivenPosQuat(obj.csdSy.mBodyPosQuat_0(model.linksAttributes{index.parent}.selector.indexes_linkPosQuat_from_mBodyPosQuat));
quat_0_c = mystica.rbm.getQuatGivenPosQuat(obj.csdSy.mBodyPosQuat_0(model.linksAttributes{index.child }.selector.indexes_linkPosQuat_from_mBodyPosQuat));

quat_pj_0 = mystica.rbm.invQuat(mystica.rbm.multiplyQuat(quat_0_p,mystica.rbm.getQuatGivenTform(tform_p_pj)));
quat_0_cj = mystica.rbm.multiplyQuat(quat_0_c,mystica.rbm.getQuatGivenTform(tform_c_cj));
quat_pj_cj = mystica.rbm.multiplyQuat(quat_pj_0,quat_0_cj);

quat_cj0_pj0 = mystica.rbm.invQuat(mystica.rbm.getQuatGivenTform(model.joints{i}.tform_pj0_cj0));

if strcmp(...
model.joints{i}.type,'spherical') || ...
...strcmp(model.joints{i}.type,'universal') || ... %toBeDone! see https://github.com/ami-iit/element_morphing-cover-design/issues/86
strcmp(model.joints{i}.type,'revolute') || ...
strcmp(model.joints{i}.type,'fixed')

rotm_0_p = mystica.rbm.getRotmGivenTform(tform_0_p);
rotm_0_c = mystica.rbm.getRotmGivenTform(tform_0_c);
pos_p_pj = mystica.rbm.getPosGivenTform(tform_p_pj);
pos_c_cj = mystica.rbm.getPosGivenTform(tform_c_cj);
sel_twist_p = model.linksAttributes{index.parent}.selector.matrix_linkTwist_from_mBodyTwist;
sel_twist_c = model.linksAttributes{index.child }.selector.matrix_linkTwist_from_mBodyTwist;

M = transpose(rotm_0_p)*[eye(3) -mystica.utils.skew( rotm_0_p * pos_p_pj )]*sel_twist_p - ...
transpose(rotm_0_p)*[eye(3) -mystica.utils.skew( rotm_0_c * pos_c_cj )]*sel_twist_c;
N = transpose(rotm_0_p)*(mystica.rbm.getPosGivenTform(tform_0_p*tform_p_pj)-mystica.rbm.getPosGivenTform(tform_0_c*tform_c_cj));

N = transpose(rotm_0_p)*(mystica.rbm.getPosGivenTform(tform_0_pj)-mystica.rbm.getPosGivenTform(tform_0_cj));

Jc{ end+1} = M;
intJcV{ end+1} = N;
cnstr_lin{end+1} = ones( size(M,1),1);
cnstr_ang{end+1} = zeros(size(M,1),1);

end
if ...strcmp(model.joints{i}.type,'universal') || ... %toBeDone! see https://github.com/ami-iit/element_morphing-cover-design/issues/86
strcmp(model.joints{i}.type,'revolute') || ...
strcmp(model.joints{i}.type,'fixed')

constrainedDirections = diag(not(model.joints{i}.axesRotation));
constrainedDirections = constrainedDirections(sum(constrainedDirections,2)==1,:)*1;

rotm_pj_0 = transpose(mystica.rbm.getRotmGivenTform(tform_0_p*tform_p_pj));
sel_angVel_c = model.linksAttributes{index.child }.selector.matrix_linkAngVel_from_mBodyTwist;
sel_angVel_p = model.linksAttributes{index.parent}.selector.matrix_linkAngVel_from_mBodyTwist;

M = constrainedDirections*rotm_pj_0*(sel_angVel_c - sel_angVel_p);
N = sparse(size(constrainedDirections,1),1);
N = constrainedDirections*mystica.rbm.logQuat(mystica.rbm.multiplyQuat(quat_pj_cj,quat_cj0_pj0),'selectQuat','minDistance');

Jc{ end+1} = M;
intJcV{ end+1} = N;
cnstr_lin{end+1} = zeros(size(M,1),1);
cnstr_ang{end+1} = ones( size(M,1),1);

end
end

obj.csdSy.Jc = vertcat(Jc{:});
obj.csdFn.Jc = casadi.Function('Jc',{obj.csdSy.mBodyPosQuat_0},{obj.csdSy.Jc});

obj.csdSy.intJcV = vertcat(intJcV{:});
obj.csdFn.intJcV = casadi.Function('intJcV',{obj.csdSy.mBodyPosQuat_0,mBodyPosQuat_0_initial},{obj.csdSy.intJcV});

model.appendSelectorConstrainedDirections('indexes_ang',find(vertcat(cnstr_ang{:})),'indexes_lin',find(vertcat(cnstr_lin{:})))

end
19 changes: 8 additions & 11 deletions +mystica/+utils/skewVee.m
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,16 @@
function x = skewVee(X)
% function x = skewVee(X)
% function x = skewVee(X)

X_skew = (X - X.')/2;

% X = [ 0 X(1,2) X(1,3);
% -X(1,2) 0 X(2,3);
% -X(1,3) X(2,3) 0 ];
% X = [ 0 X(1,2) X(1,3);
% -X(1,2) 0 X(2,3);
% -X(1,3) X(2,3) 0 ];

% X = [ 0 -x(3) x(2);
% x(3) 0 -x(1);
% -x(2) x(1) 0 ];
% X = [ 0 -x(3) x(2);
% x(3) 0 -x(1);
% -x(2) x(1) 0 ];


x = [-X_skew(2,3);...
X_skew(1,3);...
-X_skew(1,2)];
x = [-X_skew(2,3);X_skew(1,3);-X_skew(1,2)];

end

0 comments on commit 154cd67

Please sign in to comment.