Merge pull request #2 from ami-iit/dynamic-simulator

Upload dynamic simulator maximal coordinates

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

@@ -0,0 +1,38 @@
+classdef ExampleDynRel < mystica.controller.Base
+
+    properties (SetAccess=protected,GetAccess=public)
+        motorsCurrent
+    end
+
+    methods
+        function obj = ExampleDynRel(input)
+            arguments
+                input.model
+                input.stateDynMBody
+                input.stgsController
+                input.stgsDesiredShape
+                input.time
+                input.controller_dt
+            end
+            obj@mystica.controller.Base(...
+                'model',input.model,...
+                'state',input.stateDynMBody,...
+                'stgsController',input.stgsController,...
+                'stgsDesiredShape',input.stgsDesiredShape,...
+                'time',input.time,...
+                'controller_dt',input.controller_dt);
+            obj.motorsCurrent = ones(input.model.constants.motorsAngVel,1);
+        end
+        function motorsCurrent = solve(obj,input)
+            arguments
+                obj
+                input.time
+                input.stateDynMBody
+                input.model
+            end
+            motorsCurrent     = rand(input.model.constants.motorsAngVel,1);
+            obj.motorsCurrent = motorsCurrent;
+        end
+    end
+
+end

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

@@ -7,6 +7,10 @@
         xf
         dxdt
     end
+    properties (SetAccess=protected,GetAccess=protected)
+        x
+        t
+    end
     properties (SetAccess=immutable,GetAccess=public)
         solverOpts
         dxdtOpts
@@ -34,6 +38,8 @@
             end
             obj.configureIntegrator(input);
             obj.xf = obj.xi;
+            obj.x = [];
+            obj.t = [];
         end
 
         function xf = integrate(obj,input)
@@ -55,15 +61,17 @@
                     I = casadi.integrator('I',obj.solverOpts.name,odeCsd,optsCsd);
                     r           = I('x0',obj.xi);
                     obj.xf      = full(r.xf);
+                    obj.t = [obj.ti obj.tf];
+                    obj.x = [obj.xi obj.xf];
                 case 'function_handle'
                     opts = odeset;
-                    list = {'AbsTol','RelTol'};
+                    list = {'AbsTol','RelTol','MaxStep'};
                     for i = 1 : length(list)
                         opts.(list{i}) = obj.getfield(obj.solverOpts,list{i});
                     end
                     ode    = str2func(obj.solverOpts.name);
-                    [~,x]  = ode(obj.dxdt,[obj.ti obj.tf],obj.xi,opts);
-                    obj.xf = transpose(x(end,:));
+                    [obj.t,obj.x]  = ode(obj.dxdt,[obj.ti obj.tf],obj.xi,opts);
+                    obj.xf = transpose(obj.x(end,:));
                 otherwise
                     error('class(dxdt) not valid')
             end
@@ -72,7 +80,26 @@
             obj.createTimeTrackerFile()
         end
 
-
+        function dxdt = get_dxdt(obj)
+            arguments
+                obj
+            end
+            if isempty(obj.x)
+                dxdt = 0;
+            elseif size(obj.x)==2
+                dxdt = (obj.xf-obj.xi)/(obj.tf-obj.ti);
+            else
+                t = linspace(obj.t(1),obj.t(end),1e2);
+                X = interp1(obj.t,obj.x,t);
+                dt = t(2)-t(1);
+                dxdt = zeros(size(X,1)-1,size(X,2));
+                for i = 1 : size(dxdt,2)
+                    dxdt(:,i) = smooth(diff(X(:,i))/dt,'sgolay',4);
+                end
+            end
+            dxdt = dxdt(end,:)';
+        end
+
     end
 
     methods (Access = protected)
@@ -87,7 +114,7 @@ function printWorkspaceStatus(obj)
             if isempty(obj.statusTracker) == 0
                 if obj.ratioTimePrintMax < floor(obj.tf*obj.statusTracker.workspacePrint.frameRate) && obj.statusTracker.workspacePrint.run
                     obj.ratioTimePrintMax = floor(obj.tf*obj.statusTracker.workspacePrint.frameRate);
-                    fprintf('Integration Time: %.1f/%.0f\n',obj.tf,obj.statusTracker.limitMaximumTime);
+                    fprintf('Integration Time: %.2f/%.0f\n',obj.tf,obj.statusTracker.limitMaximumTime);
                 end
             end
         end

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

@@ -0,0 +1,84 @@
+classdef IntegratorDynRel < mystica.intgr.Integrator
+    %INTEGRATOR Summary of this class goes here
+    %   Detailed explanation goes here
+
+    properties (SetAccess=protected,GetAccess=public)
+        motorsCurrent %input
+        mBodyPosVel_0 %x
+        mBodyVelAcc_0 %dxdt = f(input,x)
+    end
+    properties (SetAccess=immutable,GetAccess=public)
+        dt
+    end
+
+    methods
+        function obj = IntegratorDynRel(input)
+            arguments
+                input.stgsIntegrator struct;
+                input.dt
+            end
+            obj@mystica.intgr.Integrator(...
+                'dxdt'          ,[],...
+                'xi'            ,[],...
+                'ti'            ,-input.dt,...
+                'tf'            ,0,...
+                'stgsIntegrator',input.stgsIntegrator);
+            obj.dt = input.dt;
+        end
+
+        function xf =  integrate(obj,input)
+            arguments
+                obj
+                input.motorsCurrent
+                input.stateDynMBody
+                input.model
+            end
+
+            obj.ti = obj.ti + obj.dt;
+            obj.tf = obj.tf + obj.dt;
+
+            obj.motorsCurrent = input.motorsCurrent;
+            obj.mBodyPosVel_0 = input.stateDynMBody.mBodyPosVel_0;
+
+            switch obj.solverOpts.name
+                case {'rk','cvodes'}
+                    x    = casadi.MX.sym('x',input.model.constants.mBodyPosVel,1);
+                    if obj.dxdtOpts.assumeConstant
+                        dxdt = casadi.Function('dxdt',{x},{input.stateDynMBody.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,obj.motorsCurrent)});
+                    else
+                        dxdt = casadi.Function('dxdt',{x},{input.stateDynMBody.csdFn.mBodyVelAcc_0(x,obj.motorsCurrent)});
+                    end
+                case {'ode45','ode23','ode113','ode78','ode89','ode15s','ode23s','ode23t','ode23tb','ode15i',... %https://it.mathworks.com/help/matlab/math/choose-an-ode-solver.html
+                        'mystica.intgr.ode1','mystica.intgr.ode2','mystica.intgr.ode4'} 
+                    if obj.dxdtOpts.assumeConstant
+                        obj.mBodyVelAcc_0 = input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(...
+                            'motorsCurrent',obj.motorsCurrent,...
+                            'kBaum',obj.dxdtParam.baumgarteIntegralCoefficient,...
+                            'model',input.model,...
+                            'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,...
+                            'solverTechnique',obj.dxdtOpts.solverTechnique);
+                        dxdt = @(t,x) obj.mBodyVelAcc_0;
+                    else
+                        dxdt = @(t,x) input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(...
+                            'motorsCurrent',obj.motorsCurrent,...
+                            'kBaum',obj.dxdtParam.baumgarteIntegralCoefficient,...
+                            'model',input.model,...
+                            'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,...
+                            'mBodyPosVel_0_warningOnlyIfNecessary',x,...
+                            'solverTechnique',obj.dxdtOpts.solverTechnique,...
+                            't',t);
+                    end
+                otherwise
+                    error('not valid solver')
+            end
+            xf = obj.integrate@mystica.intgr.Integrator('dxdt',dxdt,'xi',obj.mBodyPosVel_0,'ti',obj.ti,'tf',obj.tf);
+
+            if obj.dxdtOpts.assumeConstant == 0
+                % because the stateDynMBody (handle class) is updated inside the method get_mBodyVelAcc0_from_motorsCurrent
+                input.stateDynMBody.setMBodyPosVel( 'model',input.model,'mBodyPosVel_0' ,obj.mBodyPosVel_0)
+            end
+
+        end
+    end
+
+end

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

@@ -57,8 +57,7 @@
             xf = obj.integrate@mystica.intgr.Integrator('dxdt',dxdt,'xi',obj.mBodyPosQuat_0,'ti',obj.ti,'tf',obj.tf);
 
             if obj.dxdtOpts.assumeConstant == 0
-                % because the stateKinMBody (handle class) is updated
-                % inside the method get_mBodyVelQuat0_from_mBodyTwist0
+                % because the stateKinMBody (handle class) is updated inside the method get_mBodyVelQuat0_from_mBodyTwist0
                 input.stateKinMBody.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosQuat_0)
             end
 

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

@@ -59,8 +59,7 @@
             xf = obj.integrate@mystica.intgr.Integrator('dxdt',dxdt,'xi',obj.mBodyPosQuat_0,'ti',obj.ti,'tf',obj.tf);
 
             if obj.dxdtOpts.assumeConstant == 0
-                % because the stateKinMBody (handle class) is updated
-                % inside the method get_mBodyVelQuat0_from_motorsAngVel
+                % because the stateKinMBody (handle class) is updated inside the method get_mBodyVelQuat0_from_motorsAngVel
                 input.stateKinMBody.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosQuat_0)
             end
 

+mystica/+intgr/ode1.m

@@ -0,0 +1,26 @@
+function [tout,yout] = ode1(F,T,y0,opts)
+
+    t0 = T(1);
+    tf = T(end);
+    if isempty(opts.MaxStep)
+        h = (tf-t0)/10;
+    else
+        h = (tf-t0)/round((tf-t0)/opts.MaxStep);
+    end
+    time = t0 : h : tf;
+    y = y0(:)';
+    yout = zeros(length(time),length(y));
+    tout = zeros(length(time),1);
+
+    yout(1,:) = y;
+    tout(1,:) = t0;
+
+    for i = 2 : length(time)
+        t = time(i);
+        s1 = F(t,y); s1 = s1(:)';
+        y = y + h*s1;
+        yout(i,:) = y;
+        tout(i,:) = t;
+    end
+
+end

+mystica/+intgr/ode2.m

@@ -0,0 +1,26 @@
+function [tout,yout] = ode2(F,T,y0,opts)
+
+    t0 = T(1);
+    tf = T(end);
+    if isempty(opts.MaxStep)
+        h = (tf-t0)/10;
+    else
+        h = (tf-t0)/round((tf-t0)/opts.MaxStep);
+    end
+    time = t0 : h : tf;
+    y = y0(:)';
+    yout = zeros(length(time),length(y));
+    tout = zeros(length(time),1);
+
+    yout(1,:) = y;
+    tout(1,:) = t0;
+
+    for i = 2 : length(time)
+        t = time(i);
+        s1 = F(t,y);             s1 = s1(:)';
+        s2 = F(t+h/2, y+h*s1/2); s2 = s2(:)';
+        y = y + h*s2;
+        yout(i,:) = y;
+        tout(i,:) = t;
+    end
+end

+mystica/+intgr/ode4.m

@@ -0,0 +1,28 @@
+function [tout,yout] = ode4(F,T,y0,opts)
+
+    t0 = T(1);
+    tf = T(end);
+    if isempty(opts.MaxStep)
+        h = (tf-t0)/10;
+    else
+        h = (tf-t0)/round((tf-t0)/opts.MaxStep);
+    end
+    time = t0 : h : tf;
+    y = y0(:)';
+    yout = zeros(length(time),length(y));
+    tout = zeros(length(time),1);
+
+    yout(1,:) = y;
+    tout(1,:) = t0;
+
+    for i = 2 : length(time)
+        t = time(i);
+        s1 = F(t,y);             s1=s1(:)';
+        s2 = F(t+h/2, y+h*s1/2); s2=s2(:)';
+        s3 = F(t+h/2, y+h*s2/2); s3=s3(:)';
+        s4 = F(t+h, y+h*s3);     s4=s4(:)';
+        y = y + h*(s1 + 2*s2 + 2*s3 + s4)/6;
+        yout(i,:) = y;
+        tout(i,:) = t;
+    end
+end

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

@@ -107,6 +107,18 @@ function storeStateKinMBody(obj,input)
             data.intJcV(input.model.selector.indexes_constrainedLinVel_from_JcV,:) = data.intJcV(input.model.selector.indexes_constrainedLinVel_from_JcV,:)./umc.length; %[m/s];
         end
 
+        function data = deleteLoggedIndexes(obj,input)
+            arguments
+                obj
+                input.indexes
+            end
+            names = fieldnames(obj); % extract names of features
+            data = obj.copy();
+            for i = 1:length(names)
+                data.(names{i})(:,input.indexes) = [];
+            end
+        end
+
         dataOut = merge(obj,dataIn,stgs);
 
     end