Skip to content

Commit cca29f7

Browse files
FabioBergontiFabioBergonti
authored
Merge pull request #2 from ami-iit/dynamic-simulator
Upload dynamic simulator maximal coordinates
2 parents abd63fc + 64ef032 commit cca29f7

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

42 files changed

+1469
-205
lines changed

‎+mystica/+controller/@ExampleDynRel/ExampleDynRel.m

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
classdef ExampleDynRel < mystica.controller.Base
2+
3+
properties (SetAccess=protected,GetAccess=public)
4+
motorsCurrent
5+
end
6+
7+
methods
8+
function obj = ExampleDynRel(input)
9+
arguments
10+
input.model
11+
input.stateDynMBody
12+
input.stgsController
13+
input.stgsDesiredShape
14+
input.time
15+
input.controller_dt
16+
end
17+
obj@mystica.controller.Base(...
18+
'model',input.model,...
19+
'state',input.stateDynMBody,...
20+
'stgsController',input.stgsController,...
21+
'stgsDesiredShape',input.stgsDesiredShape,...
22+
'time',input.time,...
23+
'controller_dt',input.controller_dt);
24+
obj.motorsCurrent = ones(input.model.constants.motorsAngVel,1);
25+
end
26+
function motorsCurrent = solve(obj,input)
27+
arguments
28+
obj
29+
input.time
30+
input.stateDynMBody
31+
input.model
32+
end
33+
motorsCurrent = rand(input.model.constants.motorsAngVel,1);
34+
obj.motorsCurrent = motorsCurrent;
35+
end
36+
end
37+
38+
end

‎+mystica/+intgr/@Integrator/Integrator.m

+32-5
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,10 @@
77
xf
88
dxdt
99
end
10+
properties (SetAccess=protected,GetAccess=protected)
11+
x
12+
t
13+
end
1014
properties (SetAccess=immutable,GetAccess=public)
1115
solverOpts
1216
dxdtOpts
@@ -34,6 +38,8 @@
3438
end
3539
obj.configureIntegrator(input);
3640
obj.xf = obj.xi;
41+
obj.x = [];
42+
obj.t = [];
3743
end
3844

3945
function xf = integrate(obj,input)
@@ -55,15 +61,17 @@
5561
I = casadi.integrator('I',obj.solverOpts.name,odeCsd,optsCsd);
5662
r = I('x0',obj.xi);
5763
obj.xf = full(r.xf);
64+
obj.t = [obj.ti obj.tf];
65+
obj.x = [obj.xi obj.xf];
5866
case 'function_handle'
5967
opts = odeset;
60-
list = {'AbsTol','RelTol'};
68+
list = {'AbsTol','RelTol','MaxStep'};
6169
for i = 1 : length(list)
6270
opts.(list{i}) = obj.getfield(obj.solverOpts,list{i});
6371
end
6472
ode = str2func(obj.solverOpts.name);
65-
[~,x] = ode(obj.dxdt,[obj.ti obj.tf],obj.xi,opts);
66-
obj.xf = transpose(x(end,:));
73+
[obj.t,obj.x] = ode(obj.dxdt,[obj.ti obj.tf],obj.xi,opts);
74+
obj.xf = transpose(obj.x(end,:));
6775
otherwise
6876
error('class(dxdt) not valid')
6977
end
@@ -72,7 +80,26 @@
7280
obj.createTimeTrackerFile()
7381
end
7482

75-
83+
function dxdt = get_dxdt(obj)
84+
arguments
85+
obj
86+
end
87+
if isempty(obj.x)
88+
dxdt = 0;
89+
elseif size(obj.x)==2
90+
dxdt = (obj.xf-obj.xi)/(obj.tf-obj.ti);
91+
else
92+
t = linspace(obj.t(1),obj.t(end),1e2);
93+
X = interp1(obj.t,obj.x,t);
94+
dt = t(2)-t(1);
95+
dxdt = zeros(size(X,1)-1,size(X,2));
96+
for i = 1 : size(dxdt,2)
97+
dxdt(:,i) = smooth(diff(X(:,i))/dt,'sgolay',4);
98+
end
99+
end
100+
dxdt = dxdt(end,:)';
101+
end
102+
76103
end
77104

78105
methods (Access = protected)
@@ -87,7 +114,7 @@ function printWorkspaceStatus(obj)
87114
if isempty(obj.statusTracker) == 0
88115
if obj.ratioTimePrintMax < floor(obj.tf*obj.statusTracker.workspacePrint.frameRate) && obj.statusTracker.workspacePrint.run
89116
obj.ratioTimePrintMax = floor(obj.tf*obj.statusTracker.workspacePrint.frameRate);
90-
fprintf('Integration Time: %.1f/%.0f\n',obj.tf,obj.statusTracker.limitMaximumTime);
117+
fprintf('Integration Time: %.2f/%.0f\n',obj.tf,obj.statusTracker.limitMaximumTime);
91118
end
92119
end
93120
end

‎+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m

+84
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
classdef IntegratorDynRel < mystica.intgr.Integrator
2+
%INTEGRATOR Summary of this class goes here
3+
% Detailed explanation goes here
4+
5+
properties (SetAccess=protected,GetAccess=public)
6+
motorsCurrent %input
7+
mBodyPosVel_0 %x
8+
mBodyVelAcc_0 %dxdt = f(input,x)
9+
end
10+
properties (SetAccess=immutable,GetAccess=public)
11+
dt
12+
end
13+
14+
methods
15+
function obj = IntegratorDynRel(input)
16+
arguments
17+
input.stgsIntegrator struct;
18+
input.dt
19+
end
20+
obj@mystica.intgr.Integrator(...
21+
'dxdt' ,[],...
22+
'xi' ,[],...
23+
'ti' ,-input.dt,...
24+
'tf' ,0,...
25+
'stgsIntegrator',input.stgsIntegrator);
26+
obj.dt = input.dt;
27+
end
28+
29+
function xf = integrate(obj,input)
30+
arguments
31+
obj
32+
input.motorsCurrent
33+
input.stateDynMBody
34+
input.model
35+
end
36+
37+
obj.ti = obj.ti + obj.dt;
38+
obj.tf = obj.tf + obj.dt;
39+
40+
obj.motorsCurrent = input.motorsCurrent;
41+
obj.mBodyPosVel_0 = input.stateDynMBody.mBodyPosVel_0;
42+
43+
switch obj.solverOpts.name
44+
case {'rk','cvodes'}
45+
x = casadi.MX.sym('x',input.model.constants.mBodyPosVel,1);
46+
if obj.dxdtOpts.assumeConstant
47+
dxdt = casadi.Function('dxdt',{x},{input.stateDynMBody.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,obj.motorsCurrent)});
48+
else
49+
dxdt = casadi.Function('dxdt',{x},{input.stateDynMBody.csdFn.mBodyVelAcc_0(x,obj.motorsCurrent)});
50+
end
51+
case {'ode45','ode23','ode113','ode78','ode89','ode15s','ode23s','ode23t','ode23tb','ode15i',... %https://it.mathworks.com/help/matlab/math/choose-an-ode-solver.html
52+
'mystica.intgr.ode1','mystica.intgr.ode2','mystica.intgr.ode4'}
53+
if obj.dxdtOpts.assumeConstant
54+
obj.mBodyVelAcc_0 = input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(...
55+
'motorsCurrent',obj.motorsCurrent,...
56+
'kBaum',obj.dxdtParam.baumgarteIntegralCoefficient,...
57+
'model',input.model,...
58+
'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,...
59+
'solverTechnique',obj.dxdtOpts.solverTechnique);
60+
dxdt = @(t,x) obj.mBodyVelAcc_0;
61+
else
62+
dxdt = @(t,x) input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(...
63+
'motorsCurrent',obj.motorsCurrent,...
64+
'kBaum',obj.dxdtParam.baumgarteIntegralCoefficient,...
65+
'model',input.model,...
66+
'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,...
67+
'mBodyPosVel_0_warningOnlyIfNecessary',x,...
68+
'solverTechnique',obj.dxdtOpts.solverTechnique,...
69+
't',t);
70+
end
71+
otherwise
72+
error('not valid solver')
73+
end
74+
xf = obj.integrate@mystica.intgr.Integrator('dxdt',dxdt,'xi',obj.mBodyPosVel_0,'ti',obj.ti,'tf',obj.tf);
75+
76+
if obj.dxdtOpts.assumeConstant == 0
77+
% because the stateDynMBody (handle class) is updated inside the method get_mBodyVelAcc0_from_motorsCurrent
78+
input.stateDynMBody.setMBodyPosVel( 'model',input.model,'mBodyPosVel_0' ,obj.mBodyPosVel_0)
79+
end
80+
81+
end
82+
end
83+
84+
end

‎+mystica/+intgr/@IntegratorKinAbs/IntegratorKinAbs.m

+1-2
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,7 @@
5757
xf = obj.integrate@mystica.intgr.Integrator('dxdt',dxdt,'xi',obj.mBodyPosQuat_0,'ti',obj.ti,'tf',obj.tf);
5858

5959
if obj.dxdtOpts.assumeConstant == 0
60-
% because the stateKinMBody (handle class) is updated
61-
% inside the method get_mBodyVelQuat0_from_mBodyTwist0
60+
% because the stateKinMBody (handle class) is updated inside the method get_mBodyVelQuat0_from_mBodyTwist0
6261
input.stateKinMBody.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosQuat_0)
6362
end
6463

‎+mystica/+intgr/@IntegratorKinRel/IntegratorKinRel.m

+1-2
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,7 @@
5959
xf = obj.integrate@mystica.intgr.Integrator('dxdt',dxdt,'xi',obj.mBodyPosQuat_0,'ti',obj.ti,'tf',obj.tf);
6060

6161
if obj.dxdtOpts.assumeConstant == 0
62-
% because the stateKinMBody (handle class) is updated
63-
% inside the method get_mBodyVelQuat0_from_motorsAngVel
62+
% because the stateKinMBody (handle class) is updated inside the method get_mBodyVelQuat0_from_motorsAngVel
6463
input.stateKinMBody.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosQuat_0)
6564
end
6665

‎+mystica/+intgr/ode1.m

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
function [tout,yout] = ode1(F,T,y0,opts)
2+
3+
t0 = T(1);
4+
tf = T(end);
5+
if isempty(opts.MaxStep)
6+
h = (tf-t0)/10;
7+
else
8+
h = (tf-t0)/round((tf-t0)/opts.MaxStep);
9+
end
10+
time = t0 : h : tf;
11+
y = y0(:)';
12+
yout = zeros(length(time),length(y));
13+
tout = zeros(length(time),1);
14+
15+
yout(1,:) = y;
16+
tout(1,:) = t0;
17+
18+
for i = 2 : length(time)
19+
t = time(i);
20+
s1 = F(t,y); s1 = s1(:)';
21+
y = y + h*s1;
22+
yout(i,:) = y;
23+
tout(i,:) = t;
24+
end
25+
26+
end

‎+mystica/+intgr/ode2.m

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
function [tout,yout] = ode2(F,T,y0,opts)
2+
3+
t0 = T(1);
4+
tf = T(end);
5+
if isempty(opts.MaxStep)
6+
h = (tf-t0)/10;
7+
else
8+
h = (tf-t0)/round((tf-t0)/opts.MaxStep);
9+
end
10+
time = t0 : h : tf;
11+
y = y0(:)';
12+
yout = zeros(length(time),length(y));
13+
tout = zeros(length(time),1);
14+
15+
yout(1,:) = y;
16+
tout(1,:) = t0;
17+
18+
for i = 2 : length(time)
19+
t = time(i);
20+
s1 = F(t,y); s1 = s1(:)';
21+
s2 = F(t+h/2, y+h*s1/2); s2 = s2(:)';
22+
y = y + h*s2;
23+
yout(i,:) = y;
24+
tout(i,:) = t;
25+
end
26+
end

‎+mystica/+intgr/ode4.m

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
function [tout,yout] = ode4(F,T,y0,opts)
2+
3+
t0 = T(1);
4+
tf = T(end);
5+
if isempty(opts.MaxStep)
6+
h = (tf-t0)/10;
7+
else
8+
h = (tf-t0)/round((tf-t0)/opts.MaxStep);
9+
end
10+
time = t0 : h : tf;
11+
y = y0(:)';
12+
yout = zeros(length(time),length(y));
13+
tout = zeros(length(time),1);
14+
15+
yout(1,:) = y;
16+
tout(1,:) = t0;
17+
18+
for i = 2 : length(time)
19+
t = time(i);
20+
s1 = F(t,y); s1=s1(:)';
21+
s2 = F(t+h/2, y+h*s1/2); s2=s2(:)';
22+
s3 = F(t+h/2, y+h*s2/2); s3=s3(:)';
23+
s4 = F(t+h, y+h*s3); s4=s4(:)';
24+
y = y + h*(s1 + 2*s2 + 2*s3 + s4)/6;
25+
yout(i,:) = y;
26+
tout(i,:) = t;
27+
end
28+
end

‎+mystica/+log/@Logger/Logger.m

+12
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,18 @@ function storeStateKinMBody(obj,input)
107107
data.intJcV(input.model.selector.indexes_constrainedLinVel_from_JcV,:) = data.intJcV(input.model.selector.indexes_constrainedLinVel_from_JcV,:)./umc.length; %[m/s];
108108
end
109109

110+
function data = deleteLoggedIndexes(obj,input)
111+
arguments
112+
obj
113+
input.indexes
114+
end
115+
names = fieldnames(obj); % extract names of features
116+
data = obj.copy();
117+
for i = 1:length(names)
118+
data.(names{i})(:,input.indexes) = [];
119+
end
120+
end
121+
110122
dataOut = merge(obj,dataIn,stgs);
111123

112124
end

0 commit comments

Comments
 (0)