Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Evaluate state (Kin) quantities exploiting CasADi code-generation #10

Merged
merged 2 commits into from
Jun 17, 2022
Merged

Evaluate state (Kin) quantities exploiting CasADi code-generation #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ffb52b9
implement codegen
FabioBergonti Jun 17, 2022
f238c59
fix
FabioBergonti Jun 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 17 additions & 1 deletion +mystica/+state/@StateKinMBody/StateKinMBody.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,9 @@
'decompositionMethod' ,obj.stgs.nullSpace.decompositionMethod,...
'rankRevealingMethod' ,obj.stgs.nullSpace.rankRevealingMethod,...
'toleranceRankRevealing',obj.stgs.nullSpace.toleranceRankRevealing);
% generate MEX file containing casadi funtions
obj.generateMEX();
% Evaluate initial state
obj.setMBodyPosQuat('mBodyPosQuat_0',input.mBodyPosQuat_0,'model',input.model)
obj.mBodyPosQuat_0_initial = obj.mBodyPosQuat_0;
input.model.constants.setNumberConstraints(size(obj.Jc,1)); % Note: @mystica.model.Model and @Constants are two handle classes! We are modifying model.constants in the main file
Expand All @@ -56,7 +59,20 @@ function clearProperties(obj)
end

function intJcV = getIntJcV(obj)
intJcV = full(obj.csdFn.intJcV(obj.mBodyPosQuat_0,obj.mBodyPosQuat_0_initial));
intJcV = full(mystica_stateKin('intJcV',obj.mBodyPosQuat_0,obj.mBodyPosQuat_0_initial)); % obj.csdFn.intJcV(obj.mBodyPosQuat_0,obj.mBodyPosQuat_0_initial)
end

function generateMEX(obj)
opts = struct('main', true,'mex', true);
C = casadi.CodeGenerator('mystica_stateKin.c',opts);
C.add(obj.csdFn.Jc);
C.add(obj.csdFn.intJcV);
C.add(obj.csdFn.rC_from_jointsAngVelPJ_2_jointsAngVel0);
C.add(obj.csdFn.rC_from_mBodyTwist0_2_jointsAngVelPJ);
C.add(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0)
C.generate();
mex mystica_stateKin.c -largeArrayDims
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For curiosity :)
Once the function mystica_stateKin is generated, does it live in the workspace and be called in every part of the code?

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, is it computationally expensive to generate it?
I might understand why you don't check if the function already exists.
To check if the content of the function changed, you have to generate it again :D

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I generate the MEX files in the current folder. So, until you do not change the current directory, they can be called everywhere

delete('mystica_stateKin.c')
end

mBodyVelQuat = get_mBodyVelQuat0_from_mBodyTwist0(obj,input);
Expand Down
6 changes: 3 additions & 3 deletions +mystica/+state/@StateKinMBody/getReferenceConversion.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,10 @@ function getReferenceConversion(obj,model)
end

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

obj.csdSy.rC_from_jointsAngVelPJ_2_jointsAngVel0 = mystica.utils.getCasadiMatrixWithStructuredZeros(blkdiag(rC_from_jointsAngVelPJ_2_jointsAngVel0{:}),'casadiType','SX');
obj.csdFn.rC_from_jointsAngVelPJ_2_jointsAngVel0 = casadi.Function('rC_0_PJ',{obj.csdSy.mBodyPosQuat_0},{obj.csdSy.rC_from_jointsAngVelPJ_2_jointsAngVel0});
obj.csdFn.rC_from_jointsAngVelPJ_2_jointsAngVel0 = casadi.Function('rC_from_jointsAngVelPJ_2_jointsAngVel0',{obj.csdSy.mBodyPosQuat_0},{obj.csdSy.rC_from_jointsAngVelPJ_2_jointsAngVel0});

%% get_mBodyVelQuat0_from_mBodyTwist0

Expand All @@ -40,7 +40,7 @@ function getReferenceConversion(obj,model)
linkVelQuat_0 = casadi.Function('f',{linkPosQuat_0,linkTwist_0},{[mystica.rbm.getPosGivenTwist(linkTwist_0);...
mystica.rbm.getDotQuatGivenAngVel0(mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0),mystica.rbm.getAngVelGivenTwist(linkTwist_0),kBaum)]});
mBodyVelQuat0_csdFn_matrix = linkVelQuat_0.map(model.nLink);
obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0 = casadi.Function('f',{obj.csdSy.mBodyPosQuat_0,mBodyTwist_0,kBaum},{reshape(mBodyVelQuat0_csdFn_matrix(...
obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0 = casadi.Function('get_mBodyVelQuat_from_mBodyTwist',{obj.csdSy.mBodyPosQuat_0,mBodyTwist_0,kBaum},{reshape(mBodyVelQuat0_csdFn_matrix(...
reshape(obj.csdSy.mBodyPosQuat_0,model.constants.linkPosQuat,model.nLink),...
reshape(mBodyTwist_0,model.constants.linkTwist,model.nLink)),model.constants.mBodyPosQuat,1)});

Expand Down
12 changes: 8 additions & 4 deletions +mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
input.kBaum (1,1)
input.mBodyPosQuat_0_warningOnlyIfNecessary (:,1)
end

% if `input.mBodyPosQuat_0` is set, the state should be updated.
% However this is time consuming. For the aim of this function it's not
% necessary to compute all the quantity state dependent.
Expand All @@ -15,7 +15,11 @@
obj.mBodyPosQuat_0 = input.mBodyPosQuat_0_warningOnlyIfNecessary;
%obj.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',input.mBodyPosQuat_0)
end

mBodyVelQuat_0 = full(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,input.mBodyTwist_0,input.kBaum));


if isnumeric(obj.mBodyPosQuat_0) && isnumeric(input.mBodyTwist_0) && isnumeric(input.kBaum)
mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat_from_mBodyTwist',obj.mBodyPosQuat_0,input.mBodyTwist_0,input.kBaum));
else
mBodyVelQuat_0 = full(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,input.mBodyTwist_0,input.kBaum));
end

end
14 changes: 9 additions & 5 deletions +mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,19 @@
input.regTermDampPInv (1,1)
input.mBodyPosQuat_0_warningOnlyIfNecessary (:,1)
end

if isfield(input,'mBodyPosQuat_0_warningOnlyIfNecessary')
obj.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',input.mBodyPosQuat_0_warningOnlyIfNecessary)
end

Zact = obj.getZact('model',input.model);
invZact = mystica.utils.pinvDamped(Zact,input.regTermDampPInv);
mBodyTwist_0 = obj.nullJc_mBodyTwist_0 * invZact * input.motorsAngVel;

mBodyVelQuat_0 = full(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));


if isnumeric(obj.mBodyPosQuat_0) && isnumeric(mBodyTwist_0) && isnumeric(input.kBaum)
mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat_from_mBodyTwist',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
else
mBodyVelQuat_0 = full(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
end

end
4 changes: 2 additions & 2 deletions +mystica/+state/@StateKinMBody/setMBodyPosQuat.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@ function setMBodyPosQuat(obj,input)

obj.mBodyPosQuat_0 = input.mBodyPosQuat_0;
obj.updateLinkState(input.model)
obj.Jc = sparse(obj.csdFn.Jc(obj.mBodyPosQuat_0));
obj.referenceConversion.from_mBodyTwist0_2_jointsAngVelPJ = sparse(obj.csdFn.rC_from_mBodyTwist0_2_jointsAngVelPJ(obj.mBodyPosQuat_0));
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);
obj.nullJc_jointsAngVel_PJ = obj.referenceConversion.from_mBodyTwist0_2_jointsAngVelPJ * obj.nullJc_mBodyTwist_0;
obj.linIndRowJc = obj.nullEvaluator.getLinIndRow;
Expand Down
12 changes: 12 additions & 0 deletions +mystica/+utils/deleteGeneratedMEX.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
function deleteGeneratedMEX()
delFnc('mystica_stateKin')
end
function delFnc(name)
d = dir([name,'.mex*']);
name_full = d.name;
if exist(name_full,'file')==3
delete(name_full)
fprintf('%s deleted\n',name_full)
end
end

1 change: 1 addition & 0 deletions +mystica/runSimDynRel.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@
%% Saving Workspace

clear ans k kVec motorsCurrent mBodyPosQuat_0 tout dataLiveStatistics
mystica.utils.deleteGeneratedMEX
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does it also work without parenthesis?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In matlab yes :)


if stgs.saving.workspace.run
if stgs.saving.workspace.clearCasadi
Expand Down
1 change: 1 addition & 0 deletions +mystica/runSimKinAbs.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@
%% Saving Workspace

clear ans k kVec mBodyTwist_0 mBodyPosQuat_0 tout
mystica.utils.deleteGeneratedMEX

if stgs.saving.workspace.run
if stgs.saving.workspace.clearCasadi
Expand Down
1 change: 1 addition & 0 deletions +mystica/runSimKinRel.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@
%% Saving Workspace

clear ans k kVec motorsAngVel motorsAngVelNoise mBodyPosQuat_0 tout dataLiveStatistics
mystica.utils.deleteGeneratedMEX

if stgs.saving.workspace.run
if stgs.saving.workspace.clearCasadi
Expand Down
2 changes: 1 addition & 1 deletion examples/kinematicSimulationRel.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,5 +34,5 @@
% visualize data

if stgs.visualizer.run
mystica.viz.visualizeKinRel('model',model,'data',data,'stgs',stgs)
mystica.viz.visualizeKinRel('model',model,'data',data,'stgs',stgs);
end