Skip to content

Commit

Permalink
Remove LinkState class (#12)
Browse files Browse the repository at this point in the history
  • Loading branch information
@FabioBergonti
FabioBergonti authored Jun 17, 2022
1 parent 552b896 commit 30b54a7
Show file tree
Hide file tree
Showing 10 changed files with 45 additions and 83 deletions.
31 changes: 18 additions & 13 deletions +mystica/+log/@Logger/Logger.m
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
classdef Logger < matlab.mixin.Copyable
%LOGGER Summary of this class goes here
% Detailed explanation goes here

properties (SetAccess=protected,GetAccess=public)
mBodyPosQuat_0
jointsAngEul_PJ
Expand Down Expand Up @@ -44,7 +44,7 @@
obj.errorPositionNormals = zeros(input.model.nLink,obj.numberIterations);
obj.intJcV = zeros(input.model.constants.nConstraints,obj.numberIterations);
end

function storeStateKinMBody(obj,input)
arguments
obj
Expand All @@ -59,37 +59,42 @@ function storeStateKinMBody(obj,input)
obj.time(:,obj.indexIteration) = input.time;
obj.nDoF(:,obj.indexIteration) = size(input.stateKinMBody.nullJc_mBodyTwist_0,2);
obj.mBodyPosQuat_0(:,obj.indexIteration) = input.stateKinMBody.mBodyPosQuat_0;

Zact = input.stateKinMBody.getZact('model',input.model);
if size(Zact,1) == size(Zact,2)
obj.detZact(:,obj.indexIteration) = det(Zact);
else
obj.detZact(:,obj.indexIteration) = 0;
end

obj.nCondZact( :,obj.indexIteration) = cond(Zact);
obj.nCondJcRed(:,obj.indexIteration) = cond(full(input.stateKinMBody.Jc(input.stateKinMBody.linIndRowJc,:)));
obj.sizeJcRed( :,obj.indexIteration) = size(input.stateKinMBody.Jc(input.stateKinMBody.linIndRowJc,:));

obj.errorOrientationNormals(:,obj.indexIteration) = 180/pi*real(full(input.controller.csdFn.mBodyErrorOrientationNormal(input.stateKinMBody.mBodyPosQuat_0,input.time)));
obj.errorPositionNormals(:,obj.indexIteration) = full(input.controller.csdFn.mBodyErrorPositionNormal(input.stateKinMBody.mBodyPosQuat_0,input.time));

obj.intJcV(:,obj.indexIteration) = input.stateKinMBody.getIntJcV;


rotm_0_b{input.model.nLink} = [];
for i = 1 : input.model.nLink
rotm_0_b{i} = mystica.rbm.getRotmGivenTform(input.stateKinMBody.get_link_tform_b('iLink',i,'model',input.model));
end

for j = 1 : input.model.nJoint
index = input.model.joints{j}.getJointConnectionDetails;
rotm_0_p = mystica.rbm.getRotmGivenTform(input.stateKinMBody.linksState{index.parent}.tform_0_b);
rotm_0_c = mystica.rbm.getRotmGivenTform(input.stateKinMBody.linksState{index.child }.tform_0_b);
rotm_0_p = rotm_0_b{index.parent};
rotm_0_c = rotm_0_b{index.child};
rotm_p_pj = mystica.rbm.getRotmGivenTform(input.model.linksAttributes{index.parent}.tform_b_j{index.cPointParent});
rotm_c_cj = mystica.rbm.getRotmGivenTform(input.model.linksAttributes{index.child }.tform_b_j{index.cPointChild });
rotm_pj_cj = rotm_p_pj'*rotm_0_p'*rotm_0_c*rotm_c_cj;
rzyx = mystica.rbm.getEulZYXGivenRotm(rotm_pj_cj);
obj.jointsAngEul_PJ(input.model.joints{j}.selector.indexes_jAngVel_from_jointsAngVel,obj.indexIteration) = [rzyx(3);rzyx(2);rzyx(1)];
obj.jointsAngRotZ_PJ(j,obj.indexIteration) = acos(mystica.utils.saturateSignal(rotm_pj_cj(1,1),1,-1));
end

end

function data = getDataSI(obj,input)
arguments
obj
Expand All @@ -101,8 +106,8 @@ function storeStateKinMBody(obj,input)
data.errorPositionNormals = data.errorPositionNormals./umc.length; %[m]
data.intJcV(input.model.selector.indexes_constrainedLinVel_from_JcV,:) = data.intJcV(input.model.selector.indexes_constrainedLinVel_from_JcV,:)./umc.length; %[m/s];
end

dataOut = merge(obj,dataIn,stgs);

end
end
5 changes: 5 additions & 0 deletions +mystica/+model/@LinkAttributes/LinkAttributes.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,9 @@
visual
selector
end
properties (Access={?mystica.model.Model})
tform_0_b
end

methods
function obj = LinkAttributes(input)
Expand All @@ -27,6 +30,7 @@
input.mass
input.inertiaTens_g_g = []
input.inertiaTens_b_b = []
input.tform_0_b
input.tform_b_j
input.tform_b_g
input.fixed
Expand All @@ -40,6 +44,7 @@
obj.index = input.index;
obj.joints = input.joints;
obj.mass = input.mass;
obj.tform_0_b = input.tform_0_b;
obj.tform_b_j = input.tform_b_j;
obj.tform_b_g = input.tform_b_g;
obj.fixed = input.fixed;
Expand Down
7 changes: 2 additions & 5 deletions +mystica/+model/@Model/Model.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,6 @@
linksAttributes
selector
end
properties (SetAccess=protected,GetAccess=protected)
linksState
end
properties (SetAccess=immutable,GetAccess=private)
graph
end
Expand Down Expand Up @@ -52,6 +49,7 @@
'joints' ,nJointArray(any(obj.graph.Edges.EndNodes == i,2)),...
'mass' ,cellLinks{i}.mass,...
'inertiaTens_g_g',cellLinks{i}.inertiaTens_g_g,...
'tform_0_b' ,cellLinks{i}.tform_0_b,...
'tform_b_j' ,cellLinks{i}.tform_b_j,...
'tform_b_g' ,cellLinks{i}.tform_b_g,...
'fixed' ,cellLinks{i}.fixed,...
Expand All @@ -62,7 +60,6 @@
'stlName' ,cellLinks{i}.stlName,...
'constants' ,obj.constants);

obj.linksState{i} = mystica.state.LinkState('tform',cellLinks{i}.tform_0_b);
obj.indexBase = (cellLinks{i}.baseLink == 1) * i + (cellLinks{i}.baseLink == 0) * obj.indexBase;

if cellLinks{i}.fixed
Expand Down Expand Up @@ -140,7 +137,7 @@ function updateSelectorConstrainedDirections(obj,input)
function mBodyPosQuat_0 = getMBodyPosQuatRestConfiguration(obj)
mBodyPosQuat_0 = zeros(obj.constants.mBodyPosQuat,1);
for i = 1 : obj.nLink
mBodyPosQuat_0(obj.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat) = mystica.rbm.getPosQuatGivenTform(obj.linksState{i}.tform_0_b);
mBodyPosQuat_0(obj.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat) = mystica.rbm.getPosQuatGivenTform(obj.linksAttributes{i}.tform_0_b);
end
end

Expand Down
4 changes: 2 additions & 2 deletions +mystica/+model/@Model/setMBodyRestConfiguration.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,9 @@ function setMBodyRestConfiguration(obj)
tform_jCPknown_jCPunknown = obj.joints{indexJoint}.tform_pj0_cj0*(indexLinkParent==indexKnown) + ...
mystica.rbm.getTformInv(obj.joints{indexJoint}.tform_pj0_cj0)*(indexLinkChild==indexKnown);

tform_0_bUnknown = obj.linksState{indexKnown}.tform_0_b * obj.linksAttributes{indexKnown}.tform_b_j{indexCPknown} * tform_jCPknown_jCPunknown * mystica.rbm.getTformInv(obj.linksAttributes{indexUnknown}.tform_b_j{indexCPunknown});
tform_0_bUnknown = obj.linksAttributes{indexKnown}.tform_0_b * obj.linksAttributes{indexKnown}.tform_b_j{indexCPknown} * tform_jCPknown_jCPunknown * mystica.rbm.getTformInv(obj.linksAttributes{indexUnknown}.tform_b_j{indexCPunknown});

obj.linksState{indexUnknown}.setLinkStateGivenLinkTform(tform_0_bUnknown);
obj.linksAttributes{indexUnknown}.tform_0_b = tform_0_bUnknown;

end

Expand Down
37 changes: 0 additions & 37 deletions +mystica/+state/@LinkState/LinkState.m
View file

This file was deleted.

24 changes: 13 additions & 11 deletions +mystica/+state/@StateKinMBody/StateKinMBody.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
nullJc_jointsAngVel_PJ
linIndRowJc
referenceConversion
linksState
csdFn
stgs
end
Expand All @@ -32,12 +31,8 @@
obj.csdSy.mBodyPosQuat_0 = casadi.SX.sym('x',input.model.constants.mBodyPosQuat,1);

% links initialization
obj.linksState{input.model.nLink} = {};
for i = 1 : input.model.nLink
obj.linksState{i} = mystica.state.LinkState('csdMBodyPosQuat',obj.csdSy.mBodyPosQuat_0,'indexesLinkPosQuat',input.model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);
end
obj.getJacobianConstraints(input.model)
obj.getReferenceConversion(input.model)
obj.getKinematicQuantities(input.model)
obj.nullEvaluator = mystica.utils.NullSpace(...
'decompositionMethod' ,obj.stgs.nullSpace.decompositionMethod,...
'rankRevealingMethod' ,obj.stgs.nullSpace.rankRevealingMethod,...
Expand All @@ -53,9 +48,6 @@
function clearProperties(obj)
obj.csdFn = [];
obj.csdSy = [];
for i = 1 : length(obj.linksState)
obj.linksState{i}.clearCasadi
end
end

function intJcV = getIntJcV(obj)
Expand All @@ -75,6 +67,17 @@ function generateMEX(obj)
delete('mystica_stateKin.c')
end

function tform_b = get_link_tform_b(obj,input)
arguments
obj
input.iLink (1,1) double
input.model mystica.model.Model
input.mBodyPosQuat_0 = obj.mBodyPosQuat_0
end
linkPosQuat_0 = input.mBodyPosQuat_0(input.model.linksAttributes{input.iLink}.selector.indexes_linkPosQuat_from_mBodyPosQuat);
tform_b = mystica.rbm.getTformGivenPosQuat(linkPosQuat_0);
end

mBodyVelQuat = get_mBodyVelQuat0_from_mBodyTwist0(obj,input);
mBodyVelQuat = get_mBodyVelQuat0_from_motorsAngVel(obj,input);
mBodyTwist = get_mBodyTwist0_from_motorsAngVel(obj,input)
Expand All @@ -84,9 +87,8 @@ function generateMEX(obj)
end

methods (Access=protected)
getReferenceConversion(obj,model)
getKinematicQuantities(obj,model)
getJacobianConstraints(obj,model)
updateLinkState(obj,model)
end

end
4 changes: 2 additions & 2 deletions +mystica/+state/@StateKinMBody/getJacobianConstraints.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@ function getJacobianConstraints(obj,model)
for i = 1 : model.nJoint
index = model.joints{i}.getJointConnectionDetails();

tform_0_p = obj.linksState{index.parent}.csdFn.tform_0_b(obj.csdSy.mBodyPosQuat_0);
tform_0_c = obj.linksState{index.child }.csdFn.tform_0_b(obj.csdSy.mBodyPosQuat_0);
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 };

Expand Down
6 changes: 3 additions & 3 deletions ...e/@StateKinMBody/getReferenceConversion.m → ...e/@StateKinMBody/getKinematicQuantities.m
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
function getReferenceConversion(obj,model)
%GETREFERENCECONVERSION Summary of this function goes here
function getKinematicQuantities(obj,model)
%getKinematicQuantities Summary of this function goes here
% Detailed explanation goes here
arguments
obj mystica.state.StateKinMBody
Expand All @@ -16,7 +16,7 @@ function getReferenceConversion(obj,model)
sel_angVel_c = model.linksAttributes{index.child }.selector.matrix_linkAngVel_from_mBodyTwist;
sel_angVel_p = model.linksAttributes{index.parent}.selector.matrix_linkAngVel_from_mBodyTwist;

rotm_p_0 = transpose(mystica.rbm.getRotmGivenTform(obj.linksState{index.parent}.csdFn.tform_0_b(obj.csdSy.mBodyPosQuat_0)));
rotm_p_0 = transpose(mystica.rbm.getRotmGivenTform(obj.get_link_tform_b('iLink',index.parent,'model',model,'mBodyPosQuat_0',obj.csdSy.mBodyPosQuat_0)));
rotm_pj_p = transpose(mystica.rbm.getRotmGivenTform(model.linksAttributes{index.parent}.tform_b_j{index.cPointParent}));

rC_from_mBodyTwist0_2_jointsAngVelPJ{j} = rotm_pj_p * rotm_p_0 * (sel_angVel_c - sel_angVel_p);
Expand Down
1 change: 0 additions & 1 deletion +mystica/+state/@StateKinMBody/setMBodyPosQuat.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@ function setMBodyPosQuat(obj,input)
end

obj.mBodyPosQuat_0 = input.mBodyPosQuat_0;
obj.updateLinkState(input.model)
obj.Jc = sparse(mystica_stateKin('Jc',obj.mBodyPosQuat_0)); % obj.csdFn.Jc(obj.mBodyPosQuat_0)
obj.referenceConversion.from_mBodyTwist0_2_jointsAngVelPJ = sparse(mystica_stateKin('rC_from_mBodyTwist0_2_jointsAngVelPJ',obj.mBodyPosQuat_0)); % obj.csdFn.rC_from_mBodyTwist0_2_jointsAngVelPJ(obj.mBodyPosQuat_0)
obj.nullJc_mBodyTwist_0 = obj.nullEvaluator.compute(obj.Jc);
Expand Down
9 changes: 0 additions & 9 deletions +mystica/+state/@StateKinMBody/updateLinkState.m
View file

This file was deleted.

0 comments on commit 30b54a7

Please sign in to comment.