logger dynamic simulator

- add noise dynamic simulator - compute dxdt - create logger dynamic simulator
ami-iit · Jul 10, 2023 · c18efc5 · c18efc5
1 parent e0dc918
commit c18efc5
Show file tree

Hide file tree

Showing 10 changed files with 156 additions and 15 deletions.
diff --git a/+mystica/+intgr/@Integrator/Integrator.m b/+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','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)

diff --git a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m
@@ -56,8 +56,7 @@
                             'kBaum',obj.dxdtParam.baumgarteIntegralCoefficient,...
                             'model',input.model,...
                             'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,...
-                            'solverTechnique',obj.dxdtOpts.solverTechnique,...
-                            't',t);
+                            'solverTechnique',obj.dxdtOpts.solverTechnique);
                         dxdt = @(t,x) obj.mBodyVelAcc_0;
                     else
                         dxdt = @(t,x) input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(...

diff --git a/+mystica/+log/@LoggerDynRel/LoggerDynRel.m b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m
@@ -5,7 +5,11 @@
     properties
         mBodyTwist_0
         motorsAngVel
+        motorsAngVel_des
         motorsCurrent
+        motorsCurrentNoise
+        motorsCurrent_task
+        motorsCurrent_gravity
         jointsAngVel_PJ
         %mBodyAngVelStar_0
         mBodyWrenchExt_0
@@ -34,7 +38,11 @@
             [email protected]('model',input.model,'numberIterations',input.numberIterations)
             obj.mBodyTwist_0      = zeros(input.model.constants.mBodyTwist  ,obj.numberIterations);
             obj.motorsAngVel      = zeros(input.model.constants.motorsAngVel,obj.numberIterations);
+            obj.motorsAngVel_des  = zeros(input.model.constants.motorsAngVel,obj.numberIterations);
             obj.motorsCurrent     = zeros(input.model.constants.motorsAngVel,obj.numberIterations);
+            obj.motorsCurrent_gravity = zeros(input.model.constants.motorsAngVel,obj.numberIterations);
+            obj.motorsCurrent_task    = zeros(input.model.constants.motorsAngVel,obj.numberIterations);           
+            obj.motorsCurrentNoise    = zeros(input.model.constants.motorsAngVel,obj.numberIterations);           
             obj.jointsAngVel_PJ   = zeros(input.model.constants.jointsAngVel,obj.numberIterations);
             %obj.mBodyAngVelStar_0 = zeros(input.model.constants.mBodyAngVel ,obj.numberIterations);
             obj.mBodyWrenchExt_0      = zeros(input.model.constants.mBodyTwist,obj.numberIterations);
@@ -60,6 +68,10 @@ function store(obj,input)
                 input.controller
                 input.stgsDesiredShape
                 input.motorsCurrent
+                input.motorsCurrentNoise
+                input.motorsCurrent_task
+                input.motorsCurrent_gravity
+                input.motorsAngVel_des
             end
 
             obj.storeStateKinMBody('model',input.model,'controller',input.controller,'indexIteration',input.indexIteration,...
@@ -77,7 +89,12 @@ function store(obj,input)
 
             obj.mBodyTwist_0(   :,obj.indexIteration) = mBodyTwist;
             obj.motorsAngVel(   :,obj.indexIteration) = jointsAngVel(input.model.selector.indexes_motorsAngVel_from_jointsAngVel);
+            obj.motorsAngVel_des(:,obj.indexIteration) = input.motorsAngVel_des;
             obj.motorsCurrent(  :,obj.indexIteration) = input.motorsCurrent;
+            obj.motorsCurrentNoise(  :,obj.indexIteration) = input.motorsCurrentNoise;
+            obj.motorsCurrent_task(  :,obj.indexIteration) = input.motorsCurrent_task;
+            obj.motorsCurrent_gravity(  :,obj.indexIteration) = input.motorsCurrent_gravity;
+
             obj.jointsAngVel_PJ(:,obj.indexIteration) = jointsAngVel;
 
             % massMatrix mBodyTwAcc_0 = mBodyWrenchExt + mBodyWrenchJcF_0

diff --git a/+mystica/+noise/@NoiseDynRel/NoiseDynRel.m b/+mystica/+noise/@NoiseDynRel/NoiseDynRel.m
@@ -0,0 +1,77 @@
+classdef NoiseDynRel < handle
+    %NOISEKINREL Summary of this class goes here
+    %   Detailed explanation goes here
+
+    properties (SetAccess=protected,GetAccess=public)
+        stgsNoise
+        timeIncrement
+        kBaum
+        regTermDampPInv
+    end
+
+    methods
+        function [obj] = NoiseDynRel(input)
+            arguments
+                input.stgsNoise
+                input.controller_dt
+                input.kBaum
+                input.regTermDampPInv
+            end
+            obj.stgsNoise       = input.stgsNoise;
+            obj.timeIncrement   = input.controller_dt;
+            obj.kBaum           = input.kBaum;
+            obj.regTermDampPInv = input.regTermDampPInv;
+        end
+
+        function motorsCurrent = applyInputCompression(obj,input)
+            arguments
+                obj
+                input.motorsCurrent
+            end
+
+            if obj.stgsNoise.inputCompression.bool
+                mu = 0;
+                saturationValue     = obj.stgsNoise.inputCompression.maxValue;
+                probSaturationValue = obj.stgsNoise.inputCompression.probMaxValue/2;
+                noise = abs(mystica.utils.createBoundedGaussianNoise(size(input.motorsCurrent),mu,saturationValue,probSaturationValue));
+                motorsCurrent = input.motorsCurrent .* (1 - noise);
+            else
+                motorsCurrent = input.motorsCurrent;
+            end
+
+        end
+
+        function stateDynMBody = createStateDynMBodyNoise(obj,input)
+            arguments
+                obj
+                input.stateDynMBody
+            end
+            stateDynMBody = copy(input.stateDynMBody);
+        end
+
+        function stateDynMBody = applyEstimationError(obj,input)
+            arguments
+                obj
+                input.stateDynMBody
+                input.model
+            end
+
+            stateDynMBody = copy(input.stateDynMBody);
+
+            if obj.stgsNoise.errorStateEstimation.bool
+                mu = 0;
+                saturationValue     = obj.stgsNoise.errorStateEstimation.maxValue;
+                probSaturationValue = obj.stgsNoise.errorStateEstimation.probMaxValue;
+                motorsAngVelNoise   = mystica.utils.createBoundedGaussianNoise([input.model.constants.motorsAngVel 1],mu,saturationValue,probSaturationValue);
+                mBodyVelQuat_0 = stateDynMBody.get_mBodyVelQuat0_from_motorsAngVel('motorsAngVel',motorsAngVelNoise,...
+                    'model',input.model,'kBaum',obj.kBaum,'regTermDampPInv',obj.regTermDampPInv);
+                mBodyPosQuat_0 = stateDynMBody.mBodyPosQuat_0 + mBodyVelQuat_0 * obj.timeIncrement;
+                mBodyPosVel_0 = stateDynMBody.mBodyPosVel_0;
+                mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel) = mBodyPosQuat_0;
+                stateDynMBody.setMBodyPosVel( 'model',input.model,'mBodyPosVel_0' ,mBodyPosVel_0)
+
+            end
+
+        end
+    end
+end
diff --git a/+mystica/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m
@@ -54,6 +54,7 @@
     stgs.integrator.dxdtOpts.invOpts         = '';
     stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1;
     stgs.integrator.dxdtParam.feedbackJacobianConstraintsV = [0 0]; % [kp ki] -> dJc V + Jc dV = - kp Jc V - ki \int{Jc V dt}
+    stgs.integrator.dxdtParam.regTermDampPInv = 1e-6;
 
     stgs.integrator.statusTracker.workspacePrint.run        = 1;
     stgs.integrator.statusTracker.workspacePrint.frameRate  = 10;
@@ -62,7 +63,7 @@
     stgs.integrator.statusTracker.timeTrackerFile.baseName  = ['dynRel_',model.name]; %[char]
     stgs.integrator.statusTracker.limitMaximumTime          = stgs.integrator.limitMaximumTime;
 
-    %% Controller
+    %% Controller [TODO] remove stgs.controller
 
     stgs.controller.applyControlInput = true;
 
@@ -86,14 +87,21 @@
 
     stgs.controller.constraints.byPassModelLimits = 0;
 
+    stgs.controller.lowLevel.applyGravityCompensation = 1;
+    stgs.controller.lowLevel.timeStepUpdateGravityCompensation = 0;
+    stgs.controller.lowLevel.gainMotorVelocity = [0.05 0 0];  % (k1*P+k2*I+k3*D)
+    stgs.controller.lowLevel.boundCorrent = inf;
+    stgs.controller.highLevel.maxTimeStep = 0.01;
+    stgs.controller.highLevel.failApplyLastValue = 0;
+
     %% Noise
 
     stgs.noise.inputCompression.bool         = 0;
     stgs.noise.inputCompression.maxValue     = 0.2;
     stgs.noise.inputCompression.probMaxValue = 0.1;
 
     stgs.noise.errorStateEstimation.bool         = 0;
-    stgs.noise.errorStateEstimation.maxValue     = 1*stgs.controller.constraints.limitMotorVel;
+    stgs.noise.errorStateEstimation.maxValue     = 1;
     stgs.noise.errorStateEstimation.probMaxValue = 0.05;
 
     %% Visualization Settings
@@ -126,7 +134,7 @@
     stgs.visualizer.joint.sphere.dimMin         = model.linksAttributes{1}.linkDimension/6;
     stgs.visualizer.joint.sphere.dimMax         = model.linksAttributes{1}.linkDimension;
 
-    stgs.visualizer.desiredShape.fun.show          = 0;
+    stgs.visualizer.desiredShape.fun.show          = 1;
     stgs.visualizer.desiredShape.fun.edgeColor     = [0.5 0.7 0.9];
     stgs.visualizer.desiredShape.fun.faceColor     = [0.5 0.7 0.9];
     stgs.visualizer.desiredShape.fun.edgeAlpha     = 0.5;

diff --git a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m
@@ -50,7 +50,7 @@
     stgs.integrator.statusTracker.timeTrackerFile.baseName  = ['kinAbs_',model.name]; %[char]
     stgs.integrator.statusTracker.limitMaximumTime          = stgs.integrator.limitMaximumTime;
 
-    %% Controller
+    %% Controller [TODO] remove stgs.controller
 
     stgs.controller.applyControlInput = true;
 

diff --git a/+mystica/+stgs/getDefaultSettingsSimKinRel.m b/+mystica/+stgs/getDefaultSettingsSimKinRel.m
@@ -51,7 +51,7 @@
     stgs.integrator.statusTracker.timeTrackerFile.baseName  = ['kinRel_',model.name]; %[char]
     stgs.integrator.statusTracker.limitMaximumTime          = stgs.integrator.limitMaximumTime;
 
-    %% Controller
+    %% Controller [TODO] remove stgs.controller
 
     stgs.controller.applyControlInput = true;
 

diff --git a/+mystica/+viz/visualizeDynRel.m b/+mystica/+viz/visualizeDynRel.m
@@ -6,8 +6,7 @@
     end
     dataLiveStatistics{1} = struct('data',abs(input.data.errorOrientationNormals),'title','Error alignment normal vectors','ylabel','Angle $[deg]$');
     dataLiveStatistics{2} = struct('data',abs(input.data.errorPositionNormals),'title','Node position error','ylabel','Distance $[m]$');
-    dataLiveStatistics{3} = struct('data',abs(input.data.motorsAngVel),'title','Motor Angular velocity','ylabel','Velocity $[\frac{rad}{s}]$');
-    dataLiveStatistics{4} = struct('data',abs(input.data.nDoF),'title','degrees of freedom','ylabel','DoF $[]$');
+    dataLiveStatistics{3} = struct('data',abs(input.data.motorsCurrent),'title','Motor Current','ylabel','Current $[A]$');
     visual = mystica.viz.VisualizerMatlab('data',input.data,'stgs',input.stgs,'model',input.model,'dataLiveStatistics',dataLiveStatistics);
     visual.runVisualizer()
     visual.save()

diff --git a/+mystica/runSimDynRel.m b/+mystica/runSimDynRel.m
@@ -37,6 +37,13 @@
         'dt',stgs.integrator.maxTimeStep,...
         'stgsIntegrator',stgs.integrator);
 
+    noise = mystica.noise.NoiseDynRel(...
+        'stgsNoise',stgs.noise,...
+        'controller_dt',stgs.integrator.maxTimeStep,...
+        'kBaum',stgs.integrator.dxdtParam.baumgarteIntegralCoefficient,...
+        'regTermDampPInv',stgs.integrator.dxdtParam.regTermDampPInv);
+    stateDynNoise = noise.createStateDynMBodyNoise('stateDynMBody',stateDyn);
+
     data  = mystica.log.LoggerDynRel(...
         'model',model,...
         'stateDynMBody',stateDyn,...
@@ -47,14 +54,20 @@
 
     for k = kVec
         % Controller
-        motorsCurrent = contr.solve('stateDynMBody',stateDyn,'time',tout(k),'model',model) * stgs.controller.applyControlInput;
+        motorsCurrent = contr.solve('stateDynMBody',stateDynNoise,'time',tout(k),'model',model,'mBodyVelAcc',intgr.get_dxdt()) * stgs.controller.applyControlInput;
+        motorsCurrentNoise = noise.applyInputCompression('motorsCurrent',motorsCurrent);
         % Integrator
-        mBodyPosVel_0 = intgr.integrate('stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,'model',model);
+        mBodyPosVel_0 = intgr.integrate('stateDynMBody',stateDyn,'motorsCurrent',motorsCurrentNoise,'model',model);
         % Logger
         data.store('indexIteration',k,'time',tout(k),'model',model,'controller',contr,'stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,...
+            'motorsAngVel_des',contr.motorsAngVel,...
+            'motorsCurrent_gravity',contr.motorsCurrent_gravity,...
+            'motorsCurrent_task',contr.motorsCurrent_task,...
+            'motorsCurrentNoise',motorsCurrentNoise,...
             'stgsDesiredShape',stgs.desiredShape)
         % New State
         stateDyn.setMBodyPosVel('mBodyPosVel_0',mBodyPosVel_0,'model',model);
+        stateDynNoise = noise.applyEstimationError('model',model,'stateDynMBody',stateDyn);
     end
 
     stats = mp.getPerformance();

diff --git a/README.md b/README.md
@@ -23,14 +23,15 @@ The _maximal_ representation consists of a set of _non-minimum_ variables 𝐪 c
 ### Table of content
 
 - [:hammer: Dependencies](#hammer-dependencies)
-- [:floppy_disk: Installation](#floppy_disk-installation)
+- [:floppy\_disk: Installation](#floppy_disk-installation)
 - [:rocket: Usage](#rocket-usage)
 - [:gear: Contributing](#gear-contributing)
 
 
 ## :hammer: Dependencies
 
 - [`matlab`](https://mathworks.com/)
+- [`matlab Curve Fitting Toolbox`](https://ch.mathworks.com/products/curvefitting.html) for [`smooth`](https://ch.mathworks.com/help/curvefit/smooth.html) function
 - a matlab [supported-compilers](https://mathworks.com/support/requirements/supported-compilers.html) for MEX-file compilation
 
 Other requisites are: