diff --git a/+mystica/+controller/@ExampleDynRel/ExampleDynRel.m b/+mystica/+controller/@ExampleDynRel/ExampleDynRel.m new file mode 100644 index 0000000..b10d90a --- /dev/null +++ b/+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 diff --git a/+mystica/+intgr/@Integrator/Integrator.m b/+mystica/+intgr/@Integrator/Integrator.m index 988a9cf..a94e4cf 100644 --- 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'}; + 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 diff --git a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m new file mode 100644 index 0000000..1fcf291 --- /dev/null +++ b/+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 diff --git a/+mystica/+intgr/@IntegratorKinAbs/IntegratorKinAbs.m b/+mystica/+intgr/@IntegratorKinAbs/IntegratorKinAbs.m index 9327bc8..f73569f 100644 --- a/+mystica/+intgr/@IntegratorKinAbs/IntegratorKinAbs.m +++ b/+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 diff --git a/+mystica/+intgr/@IntegratorKinRel/IntegratorKinRel.m b/+mystica/+intgr/@IntegratorKinRel/IntegratorKinRel.m index e69c807..cba7bd5 100644 --- a/+mystica/+intgr/@IntegratorKinRel/IntegratorKinRel.m +++ b/+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 diff --git a/+mystica/+intgr/ode1.m b/+mystica/+intgr/ode1.m new file mode 100644 index 0000000..d37b84d --- /dev/null +++ b/+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 diff --git a/+mystica/+intgr/ode2.m b/+mystica/+intgr/ode2.m new file mode 100644 index 0000000..25fe8d3 --- /dev/null +++ b/+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 diff --git a/+mystica/+intgr/ode4.m b/+mystica/+intgr/ode4.m new file mode 100644 index 0000000..9150233 --- /dev/null +++ b/+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 diff --git a/+mystica/+log/@Logger/Logger.m b/+mystica/+log/@Logger/Logger.m index 8a5d79a..14421dc 100644 --- a/+mystica/+log/@Logger/Logger.m +++ b/+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 diff --git a/+mystica/+log/@LoggerDynRel/LoggerDynRel.m b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m new file mode 100644 index 0000000..2a4a1fb --- /dev/null +++ b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m @@ -0,0 +1,143 @@ +classdef LoggerDynRel < mystica.log.Logger + %LOGGERKINABS Summary of this class goes here + % Detailed explanation goes here + + properties + mBodyTwist_0 + motorsAngVel + motorsAngVel_des + motorsCurrent + motorsCurrentNoise + motorsCurrent_task + motorsCurrent_gravity + jointsAngVel_PJ + %mBodyAngVelStar_0 + mBodyWrenchExt_0 + mBodyWrenchGra_0 + mBodyWrenchCor_0 + mBodyWrenchFri_0 + mBodyWrenchInp_0 + mBodyWrenchJcF_0 + mBodyTwAcc_0 + jointsWrenchConstr_PJ + end + properties (SetAccess=immutable, GetAccess=protected) + stgsIntegration + end + + methods + function obj = LoggerDynRel(input) + %LOGGERKINABS Construct an instance of this class + % Detailed explanation goes here + arguments + input.model + input.stateDynMBody + input.numberIterations + input.stgsIntegrator + end + obj@mystica.log.Logger('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); + obj.mBodyWrenchGra_0 = zeros(input.model.constants.mBodyTwist,obj.numberIterations); + obj.mBodyWrenchCor_0 = zeros(input.model.constants.mBodyTwist,obj.numberIterations); + obj.mBodyWrenchFri_0 = zeros(input.model.constants.mBodyTwist,obj.numberIterations); + obj.mBodyWrenchInp_0 = zeros(input.model.constants.mBodyTwist,obj.numberIterations); + obj.mBodyWrenchJcF_0 = zeros(input.model.constants.mBodyTwist,obj.numberIterations); + obj.mBodyTwAcc_0 = zeros(input.model.constants.mBodyTwist,obj.numberIterations); + obj.jointsWrenchConstr_PJ = zeros(input.model.constants.nConstraints,obj.numberIterations); + + obj.stgsIntegration.dxdtOpts = input.stgsIntegrator.dxdtOpts; + obj.stgsIntegration.dxdtParam = input.stgsIntegrator.dxdtParam; + end + + function store(obj,input) + arguments + obj + input.time + input.model + input.indexIteration + input.stateDynMBody + 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,... + 'stateKinMBody',input.stateDynMBody,'stgsDesiredShape',input.stgsDesiredShape,'time',input.time); + + mBodyTwist = input.stateDynMBody.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); + jointsAngVel = input.stateDynMBody.referenceConversion.from_mBodyTwist0_2_jointsAngVelPJ * mBodyTwist; + + [mBodyVelAcc_0,jointsWrenchConstr] = input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(... + 'motorsCurrent',input.motorsCurrent,... + 'kBaum',obj.stgsIntegration.dxdtParam.baumgarteIntegralCoefficient,... + 'model',input.model,... + 'kFeedbackJcV',obj.stgsIntegration.dxdtParam.feedbackJacobianConstraintsV,... + 'solverTechnique',obj.stgsIntegration.dxdtOpts.solverTechnique); + + 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 + % + % mBodyWrenchExt = - wrenchGravity - wrenchCoriolis + wrenchFriction + wrenchInput + obj.mBodyWrenchExt_0(:,obj.indexIteration) = full(mystica_stateDyn('mBodyWrenchExt_0',input.stateDynMBody.mBodyPosVel_0,input.motorsCurrent)); + obj.mBodyWrenchGra_0(:,obj.indexIteration) = full(mystica_stateDyn('mBodyWrenchGra_0',input.stateDynMBody.mBodyPosQuat_0)); + obj.mBodyWrenchCor_0(:,obj.indexIteration) = full(mystica_stateDyn('mBodyWrenchCor_0',input.stateDynMBody.mBodyPosVel_0)); + obj.mBodyWrenchFri_0(:,obj.indexIteration) = full(mystica_stateDyn('mBodyWrenchFri_0',input.stateDynMBody.mBodyPosVel_0)); + obj.mBodyWrenchInp_0(:,obj.indexIteration) = full(mystica_stateDyn('mBodyWrenchInp_0',input.stateDynMBody.mBodyPosQuat_0,input.motorsCurrent)); + % mBodyWrenchJcF_0 = Jc' F + obj.mBodyWrenchJcF_0(:,obj.indexIteration) = input.stateDynMBody.Jc'*jointsWrenchConstr; + + obj.mBodyTwAcc_0(:,obj.indexIteration) = mBodyVelAcc_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); + obj.jointsWrenchConstr_PJ(:,obj.indexIteration) = jointsWrenchConstr; + + end + + function data = getDataSI(obj,input) + arguments + obj + input.model + end + umc = input.model.unitMeas.converter; + data = obj.getDataSI@mystica.log.Logger('model',input.model); + data.mBodyTwist_0(input.model.selector.indexes_mBodyLinVel_from_mBodyTwist,:) = data.mBodyTwist_0(input.model.selector.indexes_mBodyLinVel_from_mBodyTwist,:)./(umc.length); %[m/s] + data.mBodyTwAcc_0(input.model.selector.indexes_mBodyLinVel_from_mBodyTwist,:) = data.mBodyTwAcc_0(input.model.selector.indexes_mBodyLinVel_from_mBodyTwist,:)./(umc.length); %[m/s^2] + data.mBodyWrenchExt_0 = convertWrench(data.mBodyWrenchExt_0,input.model,umc); %[N][Nm] + data.mBodyWrenchGra_0 = convertWrench(data.mBodyWrenchGra_0,input.model,umc); %[N][Nm] + data.mBodyWrenchCor_0 = convertWrench(data.mBodyWrenchCor_0,input.model,umc); %[N][Nm] + data.mBodyWrenchFri_0 = convertWrench(data.mBodyWrenchFri_0,input.model,umc); %[N][Nm] + data.mBodyWrenchInp_0 = convertWrench(data.mBodyWrenchInp_0,input.model,umc); %[N][Nm] + data.mBodyWrenchJcF_0 = convertWrench(data.mBodyWrenchJcF_0,input.model,umc); %[N][Nm] + + data.jointsWrenchConstr_PJ(input.model.selector.indexes_constrainedLinVel_from_JcV,:) = data.jointsWrenchConstr_PJ(input.model.selector.indexes_constrainedLinVel_from_JcV,:)./(umc.mass*umc.length); %[N] + data.jointsWrenchConstr_PJ(input.model.selector.indexes_constrainedAngVel_from_JcV,:) = data.jointsWrenchConstr_PJ(input.model.selector.indexes_constrainedAngVel_from_JcV,:)./(umc.mass*umc.length.^2); %[Nm] + + function W = convertWrench(W,model,umc) + W(model.selector.indexes_mBodyLinVel_from_mBodyTwist,:) = W(model.selector.indexes_mBodyLinVel_from_mBodyTwist,:)./(umc.mass*umc.length); %[N] + W(model.selector.indexes_mBodyAngVel_from_mBodyTwist,:) = W(model.selector.indexes_mBodyAngVel_from_mBodyTwist,:)./(umc.mass*umc.length.^2); %[Nm] + end + + end + + end +end diff --git a/+mystica/+model/@Model/Model.m b/+mystica/+model/@Model/Model.m index 6c15bea..037104a 100644 --- a/+mystica/+model/@Model/Model.m +++ b/+mystica/+model/@Model/Model.m @@ -118,14 +118,14 @@ obj.configure(); end - function updateSelectorConstrainedDirections(obj,input) + function appendSelectorConstrainedDirections(obj,input) arguments obj input.indexes_ang = [] input.indexes_lin = [] end - obj.selector.indexes_constrainedAngVel_from_JcV = [obj.selector.indexes_constrainedAngVel_from_JcV ; input.indexes_ang(:)]; - obj.selector.indexes_constrainedLinVel_from_JcV = [obj.selector.indexes_constrainedLinVel_from_JcV ; input.indexes_lin(:)]; + obj.selector.indexes_constrainedAngVel_from_JcV = unique([obj.selector.indexes_constrainedAngVel_from_JcV ; input.indexes_ang(:)]); + obj.selector.indexes_constrainedLinVel_from_JcV = unique([obj.selector.indexes_constrainedLinVel_from_JcV ; input.indexes_lin(:)]); end function jointIndex = getJointIndex(obj,parentIndex,childIndex) diff --git a/+mystica/+noise/@NoiseDynRel/NoiseDynRel.m b/+mystica/+noise/@NoiseDynRel/NoiseDynRel.m new file mode 100644 index 0000000..4b392d7 --- /dev/null +++ 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/+rbm/getQuatGivenRotm.m b/+mystica/+rbm/getQuatGivenRotm.m index ef9c71c..2857486 100644 --- a/+mystica/+rbm/getQuatGivenRotm.m +++ b/+mystica/+rbm/getQuatGivenRotm.m @@ -59,3 +59,21 @@ quat = [qw;qx;qy;qz] / quaternionNorm; end + +%% alternative implementation +% +% function quat = getQuatGivenRotm2(rotm) +% +% qw = 0.5 * 1 * sqrt(rotm(1,1) + rotm(2,2) + rotm(3,3) + 1); +% qx = 0.5 * sign0(rotm(3,2) - rotm(2,3)) * sqrt(rotm(1,1) - rotm(2,2) - rotm(3,3) + 1); +% qy = 0.5 * sign0(rotm(1,3) - rotm(3,1)) * sqrt(rotm(2,2) - rotm(3,3) - rotm(1,1) + 1); +% qz = 0.5 * sign0(rotm(2,1) - rotm(1,2)) * sqrt(rotm(3,3) - rotm(1,1) - rotm(2,2) + 1); +% +% quat = [qw;qx;qy;qz]; +% +% end +% +% function s = sign0(x) +% s = 2*(x>=0)-1; +% end + diff --git a/+mystica/+rbm/getRotmGivenQuat.m b/+mystica/+rbm/getRotmGivenQuat.m index 7c38a33..0041d10 100644 --- a/+mystica/+rbm/getRotmGivenQuat.m +++ b/+mystica/+rbm/getRotmGivenQuat.m @@ -1,8 +1,8 @@ function rotm = getRotmGivenQuat(quat) - + qw = quat(1); qxyz = quat(2:4); - + rotm = eye(3)+2*qw*mystica.utils.skew(qxyz)+2*mystica.utils.skew(qxyz)^2; - + end diff --git a/+mystica/+rbm/invQuat.m b/+mystica/+rbm/invQuat.m new file mode 100644 index 0000000..5d0b5a7 --- /dev/null +++ b/+mystica/+rbm/invQuat.m @@ -0,0 +1,6 @@ +function quat_b_a = invQuat(quat_a_b) + + qw = quat_a_b(1); qxyz = quat_a_b(2:4); + quat_b_a = [qw;-qxyz]/(norm(quat_a_b)^2); + +end diff --git a/+mystica/+rbm/logQuat.m b/+mystica/+rbm/logQuat.m new file mode 100644 index 0000000..1fd98b1 --- /dev/null +++ b/+mystica/+rbm/logQuat.m @@ -0,0 +1,19 @@ +function theta = logQuat(quat,input) + arguments + quat + input.selectQuat char {mustBeMember(input.selectQuat,{'positive','minDistance','normal'})} = 'normal' + end + qw = quat(1); qxyz = quat(2:4); + switch input.selectQuat + case 'normal' + normV = norm(qxyz,2)+eps; + theta = 2 * qxyz * atan2(normV,qw)/normV; + case 'positive' + quat = -quat*(qw<0)+quat*(qw>=0); + theta = mystica.rbm.logQuat(quat,'selectQuat','normal'); + case 'minDistance' + theta1 = mystica.rbm.logQuat( quat,'selectQuat','normal'); n1 = norm(theta1,2); + theta2 = mystica.rbm.logQuat(-quat,'selectQuat','normal'); n2 = norm(theta2,2); + theta = theta1*(n1<=n2)+theta2*(n2 dV + x = dV; + mBodyPosVel = opti.parameter(model.constants.mBodyPosVel ,1); + motorsCurrent = opti.parameter(model.constants.motorsAngVel,1); + % + mBodyPosQuat = mBodyPosVel(model.selector.indexes_mBodyPosQuat_from_mBodyPosVel); + V = mBodyPosVel(model.selector.indexes_mBodyTwist_from_mBodyPosVel); % mBodyTwist_0 -> V + M = obj.csdFn.massMatrix(mBodyPosQuat); % massMatrix -> M + W = obj.csdFn.mBodyWrenchExt_0(mBodyPosVel,motorsCurrent); % mBodyWrenchExt_0 -> W + Jc = obj.csdFn.Jc(mBodyPosQuat); + dJc = obj.csdFn.dJc(mBodyPosVel); + intJcV = obj.csdFn.intJcV(mBodyPosQuat,obj.mBodyPosQuat_0_initial); + % Cost Function & Constraint + E = (Jc*dV+dJc*V)+(kpFeedbackJcV*Jc*V)+(kiFeedbackJcV*intJcV); + %D = M*dV-W-Jc'*f; + opti.minimize(0.5*dV'*M*dV-dV'*W); + opti.subject_to(E==0); + optFun = opti.to_function('mBodyVelAcc',{mBodyPosVel,motorsCurrent},{x}); + % + X = optFun(mBodyPosVel,motorsCurrent); + mBodyVelQuat = obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(mBodyPosQuat,V,stgsIntegrator.dxdtParam.baumgarteIntegralCoefficient); + mBodyTwAcc_0 = X; + mBodyVelAcc_0 = [mBodyVelQuat ; mBodyTwAcc_0]; + % + obj.csdFn.mBodyVelAcc_0 = casadi.Function('mBodyVelAcc_0',... + {mBodyPosVel,motorsCurrent},... + {mBodyVelAcc_0},... + {'mBodyPosVel','motorsCurrent'},... + {'mBodyVelAcc_0'}); + + % alternative for computing [dV] + obj.opti = opti; + obj.optiVar.mBodyPosVel = mBodyPosVel; + obj.optiVar.motorsCurrent = motorsCurrent; + obj.optiVar.X = X; + +end diff --git a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m new file mode 100644 index 0000000..3ff94ce --- /dev/null +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -0,0 +1,110 @@ +function [mBodyVelAcc_0,varargout] = get_mBodyVelAcc0_from_motorsCurrent(obj,input,stgs) + arguments + obj + input.motorsCurrent (:,1) + input.model mystica.model.Model + input.kBaum (1,1) + input.mBodyPosVel_0_warningOnlyIfNecessary (:,1) + input.kFeedbackJcV + input.t = inf + stgs.solverTechnique char {mustBeMember(stgs.solverTechnique,{'inv','opt'})} + end + + %---------------------------------------------------------------------% + % Problem: + % { M dV = W + Jc'f + % { Jc dV + dJc V = 0 + % + % - Compute dV and f + % - Compute `mBodyVelAcc0` and `jointsWrenchConstr_PJ` + %---------------------------------------------------------------------% + + switch stgs.solverTechnique + case 'inv' + + %-----------------------------------------------------------------% + % Inverse + %-----------------------------------------------------------------% + + if isfield(input,'mBodyPosVel_0_warningOnlyIfNecessary') + obj.setMBodyPosVel('mBodyPosVel_0',input.mBodyPosVel_0_warningOnlyIfNecessary,'model',input.model) + end + + kpFeedbackJcV = input.kFeedbackJcV(1); + kiFeedbackJcV = input.kFeedbackJcV(2); + + mBodyTwist_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); + mBodyVelQuat_0 = sparse(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); + + Jc = obj.Jc( obj.linIndRowJc,:); + dJc = sparse(mystica_stateDyn('dJc',obj.mBodyPosVel_0)); + dJc = dJc(obj.linIndRowJc,:); + intJcV = obj.getIntJcV; + intJcV = intJcV( obj.linIndRowJc,:); + V = mBodyTwist_0; + M = sparse(mystica_stateDyn('massMatrix',obj.mBodyPosQuat_0)); % massMatrix -> M + W = sparse(mystica_stateDyn('mBodyWrenchExt_0',obj.mBodyPosVel_0,input.motorsCurrent)); % mBodyWrenchExt_0 -> W + invMJcT = mystica.utils.scaling(M)*((M*mystica.utils.scaling(M))\Jc'); + invMW = mystica.utils.scaling(M)*((M*mystica.utils.scaling(M))\W); + JcInvMJcT = Jc*invMJcT; + % Jc dV + dJc V = fdbkJc = - kp Jc V - ki intJcV + fdbkJc = -kpFeedbackJcV*Jc*V -kiFeedbackJcV*intJcV; + f = mystica.utils.scaling(JcInvMJcT)*((JcInvMJcT*mystica.utils.scaling(JcInvMJcT))\(-Jc*invMW-dJc*V+fdbkJc)); % jointsWrenchConstr_PJ -> f + dV = invMJcT*f + invMW; % mBodyTwAcc_0 -> dV + + jointsWrenchConstr_PJ = zeros(input.model.constants.nConstraints,1); + jointsWrenchConstr_PJ(obj.linIndRowJc,1) = f; + mBodyTwAcc_0 = dV; + + mBodyVelAcc_0 = [mBodyVelQuat_0 ; mBodyTwAcc_0]; + + case 'opt' + + %-----------------------------------------------------------------% + % Optimization (CasADi) + %-----------------------------------------------------------------% + + if isfield(input,'mBodyPosVel_0_warningOnlyIfNecessary') + % 'reduced' method to avoid the computation of nullspace + obj.setMBodyPosVel('mBodyPosVel_0',input.mBodyPosVel_0_warningOnlyIfNecessary,'model',input.model,'method','reduced') + end + + if 0 + mBodyVelAcc_0 = obj.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,input.motorsCurrent); + mBodyTwAcc_0 = mBodyVelAcc_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); + else + obj.opti.set_value(obj.optiVar.mBodyPosVel,obj.mBodyPosVel_0); + obj.opti.set_value(obj.optiVar.motorsCurrent,input.motorsCurrent); + sol = obj.opti.solve(); + X = sol.value(obj.optiVar.X); + + mBodyTwist_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); + mBodyVelQuat_0 = sparse(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); + + mBodyTwAcc_0 = X(1:input.model.constants.mBodyTwist,1); + mBodyVelAcc_0 = [mBodyVelQuat_0 ; mBodyTwAcc_0]; + end + + mBodyVelAcc_0 = full(mBodyVelAcc_0); + + if nargout > 1 + M = sparse(mystica_stateDyn('massMatrix',obj.mBodyPosQuat_0)); % massMatrix -> M + W = sparse(mystica_stateDyn('mBodyWrenchExt_0',obj.mBodyPosVel_0,input.motorsCurrent)); % mBodyWrenchExt_0 -> W + jointsWrenchConstr_PJ = pinv(full(obj.Jc'))*(M*mBodyTwAcc_0-W); + jointsWrenchConstr_PJ = full(jointsWrenchConstr_PJ); + mBodyWrenchExt_0 = full(W); + end + + end + + %---------------------------------------------------------------------% + % Output + + if nargout > 1 + varargout{1} = jointsWrenchConstr_PJ; + end + if nargout > 2 + varargout{2} = mBodyWrenchExt_0; + end + +end diff --git a/+mystica/+state/@StateKinMBody/StateKinMBody.m b/+mystica/+state/@StateKinMBody/StateKinMBody.m index 83704d6..1722c6d 100644 --- a/+mystica/+state/@StateKinMBody/StateKinMBody.m +++ b/+mystica/+state/@StateKinMBody/StateKinMBody.m @@ -37,8 +37,8 @@ 'decompositionMethod' ,obj.stgs.nullSpace.decompositionMethod,... 'rankRevealingMethod' ,obj.stgs.nullSpace.rankRevealingMethod,... 'toleranceRankRevealing',obj.stgs.nullSpace.toleranceRankRevealing); - % generate MEX file containing casadi funtions - obj.generateMEX(); + % generate MEX file containing (kin) casadi funtions + obj.generateMEX_kin(); % Evaluate initial state obj.setMBodyPosQuat('mBodyPosQuat_0',input.mBodyPosQuat_0,'model',input.model) obj.mBodyPosQuat_0_initial = obj.mBodyPosQuat_0; @@ -55,6 +55,30 @@ function clearProperties(obj) end function generateMEX(obj) + obj.generateMEX_kin(); + 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) + jointsAngVel_PJ = get_jointsAngVelPJ_from_motorsAngVel(obj,input); + Zact = getZact(obj,input); + setMBodyPosQuat(obj,input) + end + + methods (Access=private) + function generateMEX_kin(obj) nameMEX = 'mystica_stateKin'; opts = struct('main', true,'mex', true); initial_path = pwd; @@ -81,24 +105,6 @@ function generateMEX(obj) movefile([nameMEX,'.c'],[nameMEX,'_old.c']) cd(initial_path) 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) - jointsAngVel_PJ = get_jointsAngVelPJ_from_motorsAngVel(obj,input); - Zact = getZact(obj,input); - setMBodyPosQuat(obj,input) end methods (Access=protected) diff --git a/+mystica/+state/@StateKinMBody/getJacobianConstraints.m b/+mystica/+state/@StateKinMBody/getJacobianConstraints.m index b2188ff..0fe9f8f 100644 --- a/+mystica/+state/@StateKinMBody/getJacobianConstraints.m +++ b/+mystica/+state/@StateKinMBody/getJacobianConstraints.m @@ -5,88 +5,105 @@ function getJacobianConstraints(obj,model) obj mystica.state.StateKinMBody model mystica.model.Model end - + Jc = {}; intJcV = {}; cnstr_lin = {}; cnstr_ang = {}; - + mBodyPosQuat_0_initial = casadi.SX.sym('x_initial',model.constants.mBodyPosQuat,1); - + for i = transpose(model.indexesFixedLink(:)) - + sel_twist_i = model.linksAttributes{i}.selector.matrix_linkTwist_from_mBodyTwist; - linkPosQuat_0_initial = mBodyPosQuat_0_initial(model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat); - + linkPosQuat_0_initial = mBodyPosQuat_0_initial( model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat); + linkPosQuat_0 = obj.csdSy.mBodyPosQuat_0(model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat); + + quat_bi_0 = mystica.rbm.invQuat(mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0_initial)); + quat_0_b = mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0); + pos_0_bi = mystica.rbm.getPosGivenPosQuat( linkPosQuat_0_initial); + pos_0_b = mystica.rbm.getPosGivenPosQuat( linkPosQuat_0); + M = sel_twist_i; - N = [linkPosQuat_0_initial(1:model.constants.linkPos);sparse(model.constants.linkAngVel,1)]; - + N = [pos_0_b-pos_0_bi;mystica.rbm.logQuat(mystica.rbm.multiplyQuat(quat_0_b,quat_bi_0),'selectQuat','minDistance')]; + Jc{ end+1} = M; intJcV{ end+1} = N; cnstr_lin{end+1} = [1 1 1 0 0 0]'; cnstr_ang{end+1} = [0 0 0 1 1 1]'; end - + for i = 1 : model.nJoint index = model.joints{i}.getJointConnectionDetails(); - + 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 }; - + tform_0_pj = tform_0_p*tform_p_pj; + tform_0_cj = tform_0_c*tform_c_cj; + + quat_0_p = mystica.rbm.getQuatGivenPosQuat(obj.csdSy.mBodyPosQuat_0(model.linksAttributes{index.parent}.selector.indexes_linkPosQuat_from_mBodyPosQuat)); + quat_0_c = mystica.rbm.getQuatGivenPosQuat(obj.csdSy.mBodyPosQuat_0(model.linksAttributes{index.child }.selector.indexes_linkPosQuat_from_mBodyPosQuat)); + + quat_pj_0 = mystica.rbm.invQuat(mystica.rbm.multiplyQuat(quat_0_p,mystica.rbm.getQuatGivenTform(tform_p_pj))); + quat_0_cj = mystica.rbm.multiplyQuat(quat_0_c,mystica.rbm.getQuatGivenTform(tform_c_cj)); + quat_pj_cj = mystica.rbm.multiplyQuat(quat_pj_0,quat_0_cj); + + quat_cj0_pj0 = mystica.rbm.invQuat(mystica.rbm.getQuatGivenTform(model.joints{i}.tform_pj0_cj0)); + if strcmp(... model.joints{i}.type,'spherical') || ... ...strcmp(model.joints{i}.type,'universal') || ... %toBeDone! see https://github.com/ami-iit/element_morphing-cover-design/issues/86 strcmp(model.joints{i}.type,'revolute') || ... strcmp(model.joints{i}.type,'fixed') - + rotm_0_p = mystica.rbm.getRotmGivenTform(tform_0_p); rotm_0_c = mystica.rbm.getRotmGivenTform(tform_0_c); pos_p_pj = mystica.rbm.getPosGivenTform(tform_p_pj); pos_c_cj = mystica.rbm.getPosGivenTform(tform_c_cj); sel_twist_p = model.linksAttributes{index.parent}.selector.matrix_linkTwist_from_mBodyTwist; sel_twist_c = model.linksAttributes{index.child }.selector.matrix_linkTwist_from_mBodyTwist; - + M = transpose(rotm_0_p)*[eye(3) -mystica.utils.skew( rotm_0_p * pos_p_pj )]*sel_twist_p - ... transpose(rotm_0_p)*[eye(3) -mystica.utils.skew( rotm_0_c * pos_c_cj )]*sel_twist_c; - - N = transpose(rotm_0_p)*(mystica.rbm.getPosGivenTform(tform_0_p*tform_p_pj)-mystica.rbm.getPosGivenTform(tform_0_c*tform_c_cj)); - + + N = transpose(rotm_0_p)*(mystica.rbm.getPosGivenTform(tform_0_pj)-mystica.rbm.getPosGivenTform(tform_0_cj)); + Jc{ end+1} = M; intJcV{ end+1} = N; cnstr_lin{end+1} = ones( size(M,1),1); cnstr_ang{end+1} = zeros(size(M,1),1); - + end if ...strcmp(model.joints{i}.type,'universal') || ... %toBeDone! see https://github.com/ami-iit/element_morphing-cover-design/issues/86 strcmp(model.joints{i}.type,'revolute') || ... strcmp(model.joints{i}.type,'fixed') - + constrainedDirections = diag(not(model.joints{i}.axesRotation)); constrainedDirections = constrainedDirections(sum(constrainedDirections,2)==1,:)*1; - + rotm_pj_0 = transpose(mystica.rbm.getRotmGivenTform(tform_0_p*tform_p_pj)); sel_angVel_c = model.linksAttributes{index.child }.selector.matrix_linkAngVel_from_mBodyTwist; sel_angVel_p = model.linksAttributes{index.parent}.selector.matrix_linkAngVel_from_mBodyTwist; - + M = constrainedDirections*rotm_pj_0*(sel_angVel_c - sel_angVel_p); - N = sparse(size(constrainedDirections,1),1); - + N = constrainedDirections*mystica.rbm.logQuat(mystica.rbm.multiplyQuat(quat_pj_cj,quat_cj0_pj0),'selectQuat','minDistance'); + Jc{ end+1} = M; intJcV{ end+1} = N; cnstr_lin{end+1} = zeros(size(M,1),1); cnstr_ang{end+1} = ones( size(M,1),1); - + end end - + obj.csdSy.Jc = vertcat(Jc{:}); obj.csdFn.Jc = casadi.Function('Jc',{obj.csdSy.mBodyPosQuat_0},{obj.csdSy.Jc}); - + obj.csdSy.intJcV = vertcat(intJcV{:}); obj.csdFn.intJcV = casadi.Function('intJcV',{obj.csdSy.mBodyPosQuat_0,mBodyPosQuat_0_initial},{obj.csdSy.intJcV}); - - model.updateSelectorConstrainedDirections('indexes_ang',find(vertcat(cnstr_ang{:})),'indexes_lin',find(vertcat(cnstr_lin{:}))) - + + model.appendSelectorConstrainedDirections('indexes_ang',find(vertcat(cnstr_ang{:})),'indexes_lin',find(vertcat(cnstr_lin{:}))) + end diff --git a/+mystica/+state/@StateKinMBody/getKinematicQuantities.m b/+mystica/+state/@StateKinMBody/getKinematicQuantities.m index f6a4e4e..d938d0f 100644 --- a/+mystica/+state/@StateKinMBody/getKinematicQuantities.m +++ b/+mystica/+state/@StateKinMBody/getKinematicQuantities.m @@ -40,7 +40,7 @@ function getKinematicQuantities(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('get_mBodyVelQuat_from_mBodyTwist',{obj.csdSy.mBodyPosQuat_0,mBodyTwist_0,kBaum},{reshape(mBodyVelQuat0_csdFn_matrix(... + obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0 = casadi.Function('get_mBodyVelQuat0_from_mBodyTwist0',{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)}); diff --git a/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m b/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m index 4c2be13..30deecb 100644 --- a/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m +++ b/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m @@ -17,7 +17,7 @@ end 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)); + mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',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 diff --git a/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m b/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m index 7077727..b4e5b34 100644 --- a/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m +++ b/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m @@ -17,7 +17,7 @@ mBodyTwist_0 = obj.nullJc_mBodyTwist_0 * invZact * input.motorsAngVel; 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)); + mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',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 diff --git a/+mystica/+state/@StateKinMBody/setMBodyPosQuat.m b/+mystica/+state/@StateKinMBody/setMBodyPosQuat.m index 7b3d798..a68a450 100644 --- a/+mystica/+state/@StateKinMBody/setMBodyPosQuat.m +++ b/+mystica/+state/@StateKinMBody/setMBodyPosQuat.m @@ -3,13 +3,20 @@ function setMBodyPosQuat(obj,input) obj mystica.state.StateKinMBody input.mBodyPosQuat_0 (:,1) input.model mystica.model.Model + input.method = 'full' end - + obj.mBodyPosQuat_0 = input.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; - + switch input.method + case 'full' + 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; + case 'reduced' + obj.nullJc_mBodyTwist_0 = NaN; + obj.nullJc_jointsAngVel_PJ = NaN; + obj.linIndRowJc = NaN; + end end diff --git a/+mystica/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m new file mode 100644 index 0000000..db0f078 --- /dev/null +++ b/+mystica/+stgs/getDefaultSettingsSimDynRel.m @@ -0,0 +1,163 @@ +function stgs = getDefaultSettingsSimDynRel(model,input) + arguments + model + input.startFile = '' + input.stgs_integrator_limitMaximumTime (1,1) double = 5 + end + + strTime = [datestr(now,'yyyy-mm-dd'),'_h',datestr(now,'HH-MM')]; + + stgs.unitMeas = model.unitMeas; + stgs.startFile = input.startFile; + + %% Desired shape + + stgs.desiredShape.fun = @(x,y) 0.005*(5*cos(10*x-2)+5*cos(10*x-20*y)); %[m] + stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x-t/8e1,y-t/8e1); %[m] + stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x,y,t)-stgs.desiredShape.fun(0,0,t); %[m] + stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x/stgs.unitMeas.converter.length,y/stgs.unitMeas.converter.length,t)*stgs.unitMeas.converter.length; %[m]*(umc.length) + stgs.desiredShape.invertNormals = 1; + + %% Saving & Logger + + stgs.saving.workspace.run = 1; + stgs.saving.workspace.name = ['dynRel_',model.name,'_',strTime,'.mat']; + stgs.saving.workspace.clearCasadi = 0; + + %% model Settings + + stgs.model.friction.viscous.apply = 1; + stgs.model.friction.coulomb.apply = 1; + stgs.model.friction.coulomb.smoothSign.method = 'sigmoid'; + stgs.model.friction.coulomb.smoothSign.settlingTime = 5*pi/180; %[rad] + stgs.model.friction.coulomb.smoothSign.toleranceBands = 0.02; %[0...1] + + %% StateDyn Settings + + stgs.stateDyn.nullSpace.decompositionMethod = 'qrFull'; + stgs.stateDyn.nullSpace.rankRevealingMethod = 'limitErrorNullSpace'; + stgs.stateDyn.nullSpace.toleranceRankRevealing = [1e-8]; + + %% Integration Settings + + stgs.integrator.maxTimeStep = 1e-2; + stgs.integrator.limitMaximumTime = input.stgs_integrator_limitMaximumTime; + + stgs.integrator.solverOpts.name = 'ode45'; + stgs.integrator.solverOpts.RelTol = 1e-3; + stgs.integrator.solverOpts.AbsTol = 1e-6; + stgs.integrator.solverOpts.MaxStep = []; + + stgs.integrator.dxdtOpts.assumeConstant = 0; + stgs.integrator.dxdtOpts.solverTechnique = 'inv'; %used only in case of ode. possible values: 'inv'(inverse)/'opt'(optimization problem, see https://github.com/ami-iit/element_morphing-cover-design/pull/197) + stgs.integrator.dxdtOpts.optOpts.name = 'osqp'; + 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; + stgs.integrator.statusTracker.timeTrackerFile.run = 0; + stgs.integrator.statusTracker.timeTrackerFile.frameRate = 100; %[Hz] + stgs.integrator.statusTracker.timeTrackerFile.baseName = ['dynRel_',model.name]; %[char] + stgs.integrator.statusTracker.limitMaximumTime = stgs.integrator.limitMaximumTime; + + %% Controller + + stgs.controller.applyControlInput = true; + stgs.controller.casadi.optimizationType = 'conic'; + stgs.controller.casadi.solver = 'osqp'; + stgs.controller.costFunction.weightTaskOrientation = 1; + stgs.controller.costFunction.weightTaskMinVariation = 0; + stgs.controller.costFunction.gainLinkAngVelStarAligned = 30; + stgs.controller.costFunction.gainLinkAngVelStarOpposite = 100; + stgs.controller.costFunction.useFeedForwardTermLinkAngVelStar = 1; + + %% 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.noise.errorStateEstimation.probMaxValue = 0.05; + + %% Visualization Settings + + stgs.visualizer.run = 1; + stgs.visualizer.frameRate = 20; + stgs.visualizer.limitMaximumTime = stgs.integrator.limitMaximumTime; + + stgs.visualizer.statusTracker.workspacePrint.run = 0; + stgs.visualizer.statusTracker.workspacePrint.frameRate = 10; + + stgs.visualizer.origin.dimCSYS = model.linksAttributes{1}.linkDimension/5; + stgs.visualizer.mBody.bodyCSYS.show = 0; + stgs.visualizer.mBody.bodyCSYS.dim = model.linksAttributes{1}.linkDimension/10; + stgs.visualizer.mBody.jointCSYS.show = 0; + stgs.visualizer.mBody.jointCSYS.dim = model.linksAttributes{1}.linkDimension/10; + + stgs.visualizer.joint.cone.show = 0; + stgs.visualizer.joint.cone.angleIn = 45*pi/180; + stgs.visualizer.joint.cone.angleDe = 10*pi/180; + stgs.visualizer.joint.cone.color = 'g'; + stgs.visualizer.joint.cone.faceAlpha = 0.1; + stgs.visualizer.joint.cone.dim = model.linksAttributes{1}.linkDimension/6; + + stgs.visualizer.joint.sphere.show = 0; + stgs.visualizer.joint.sphere.colorBodyFrame = 0; + stgs.visualizer.joint.sphere.showNAct = 0; + stgs.visualizer.joint.sphere.colorNAct = [1 1 1]; + stgs.visualizer.joint.sphere.faceAlpha = 0.5; + stgs.visualizer.joint.sphere.dimMin = model.linksAttributes{1}.linkDimension/6; + stgs.visualizer.joint.sphere.dimMax = model.linksAttributes{1}.linkDimension; + + 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; + stgs.visualizer.desiredShape.fun.faceAlpha = 0.1; + stgs.visualizer.desiredShape.fun.update = 1; + stgs.visualizer.desiredShape.normal.show = 0; + stgs.visualizer.desiredShape.normal.color = 'b'; + stgs.visualizer.desiredShape.normal.linewidth = 3; + stgs.visualizer.desiredShape.normal.dim = model.linksAttributes{1}.linkDimension/10; + stgs.visualizer.desiredShape.points.show = 0; + stgs.visualizer.desiredShape.points.color = 'k'; + stgs.visualizer.desiredShape.points.colorFace = 'k'; + stgs.visualizer.desiredShape.points.markerSize = 10; + stgs.visualizer.desiredShape.points.markerSym = 'o'; + + stgs.visualizer.figure.backgroundColor = 'white'; + stgs.visualizer.figure.windowState = 'maximized'; + stgs.visualizer.figure.position = [617 157 741 782]; + stgs.visualizer.figure.showAxis = 1; + + stgs.visualizer.livePerformances.run = 1; + stgs.visualizer.livePerformances.prctileValues = [10 90]; + + stgs.visualizer.cameraView.mBodySimulation.values = [230 40]; + stgs.visualizer.cameraView.initialRotation.run = 0; + stgs.visualizer.cameraView.initialRotation.durationTotal = 3; + stgs.visualizer.cameraView.initialRotation.pause.start = 1; + stgs.visualizer.cameraView.initialRotation.pause.end = 1; + stgs.visualizer.cameraView.initialRotation.values = [0,0]; + stgs.visualizer.cameraView.finalRotation.run = 0; + stgs.visualizer.cameraView.finalRotation.durationTotal = 5; + stgs.visualizer.cameraView.finalRotation.pause.start = 1; + stgs.visualizer.cameraView.finalRotation.pause.end = 1; + stgs.visualizer.cameraView.finalRotation.values = [90,0]; + + stgs.visualizer.background = {}; + + stgs.visualizer.gif.save = 0; + stgs.visualizer.gif.name = ['dynRel_',model.name,'_',strTime,'.gif']; + stgs.visualizer.gif.compressionRatio = 2; + + stgs.visualizer.video.save = 0; + stgs.visualizer.video.name = ['dynRel_',model.name,'_',strTime,'.mp4']; + stgs.visualizer.video.quality = 5; + +end diff --git a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m index 15eb2a1..2f478ea 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m @@ -12,7 +12,7 @@ %% Desired shape - stgs.desiredShape.fun = @(x,y) -1*( ((x-0.15)*2).^2+((y+0.15)*2).^2); %[m] + stgs.desiredShape.fun = @(x,y) -1*(x.^2+y.^2)*5; %[m] stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x,y); %[m] stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x,y,t)-stgs.desiredShape.fun(0,0,t); %[m] stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x/stgs.unitMeas.converter.length,y/stgs.unitMeas.converter.length,t)*stgs.unitMeas.converter.length; %[m]*(umc.length) @@ -35,11 +35,12 @@ stgs.integrator.maxTimeStep = 1e-2; stgs.integrator.limitMaximumTime = input.stgs_integrator_limitMaximumTime; - stgs.integrator.solverOpts.name = 'ode45'; - stgs.integrator.solverOpts.RelTol = 1e-3; - stgs.integrator.solverOpts.AbsTol = 1e-6; + stgs.integrator.solverOpts.name = 'ode45'; + stgs.integrator.solverOpts.RelTol = 1e-3; + stgs.integrator.solverOpts.AbsTol = 1e-6; + stgs.integrator.solverOpts.MaxStep = []; - stgs.integrator.dxdtOpts.assumeConstant = 1; + stgs.integrator.dxdtOpts.assumeConstant = 0; stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1; stgs.integrator.statusTracker.workspacePrint.run = 1; @@ -51,17 +52,15 @@ %% Controller + stgs.controller.applyControlInput = true; stgs.controller.casadi.optimizationType = 'conic'; stgs.controller.casadi.solver = 'osqp'; - stgs.controller.costFunction.weightTaskOrientation = 1; stgs.controller.costFunction.weightTaskMinVariation = 0; - stgs.controller.costFunction.gainLinkAngVelStarAligned = 30; stgs.controller.costFunction.gainLinkAngVelStarOpposite = 100; stgs.controller.costFunction.useFeedForwardTermLinkAngVelStar = 1; - stgs.controller.constraints.eq2inep = 0; %% Visualization Settings diff --git a/+mystica/+stgs/getDefaultSettingsSimKinRel.m b/+mystica/+stgs/getDefaultSettingsSimKinRel.m index 99c1827..c829c7a 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinRel.m @@ -35,11 +35,12 @@ stgs.integrator.maxTimeStep = 1e-2; stgs.integrator.limitMaximumTime = input.stgs_integrator_limitMaximumTime; - stgs.integrator.solverOpts.name = 'ode45'; - stgs.integrator.solverOpts.RelTol = 1e-3; - stgs.integrator.solverOpts.AbsTol = 1e-6; + stgs.integrator.solverOpts.name = 'ode45'; + stgs.integrator.solverOpts.RelTol = 1e-3; + stgs.integrator.solverOpts.AbsTol = 1e-6; + stgs.integrator.solverOpts.MaxStep = []; - stgs.integrator.dxdtOpts.assumeConstant = 1; + stgs.integrator.dxdtOpts.assumeConstant = 0; stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1; stgs.integrator.dxdtParam.regTermDampPInv = 1e-6; @@ -52,26 +53,15 @@ %% Controller + stgs.controller.applyControlInput = true; stgs.controller.casadi.optimizationType = 'conic'; stgs.controller.casadi.solver = 'osqp'; - - stgs.controller.regTermDampPInv = 1e-6; - stgs.controller.costFunction.weightTaskOrientation = 1; stgs.controller.costFunction.weightTaskMinVariation = 0; - stgs.controller.costFunction.weightTaskMinOptiVar = 0; - stgs.controller.costFunction.gainLinkAngVelStarAligned = 30; stgs.controller.costFunction.gainLinkAngVelStarOpposite = 100; stgs.controller.costFunction.useFeedForwardTermLinkAngVelStar = 1; - stgs.controller.constraints.eq2inep = 0; - stgs.controller.constraints.limitPassiveAngVel = 5*pi/180; % "controller" limit (there is also the model limit) - stgs.controller.constraints.limitMotorVel = 5*pi/180; % "controller" limit (there is also the model limit) - stgs.controller.constraints.limitRoM = 50*pi/180; % "controller" limit (there is also the model limit) - - stgs.controller.constraints.byPassModelLimits = 0; - %% Noise stgs.noise.inputCompression.bool = 0; @@ -79,7 +69,7 @@ stgs.noise.inputCompression.probMaxValue = 0.1; stgs.noise.errorStateEstimation.bool = 0; - stgs.noise.errorStateEstimation.maxValue = 1*stgs.controller.constraints.limitMotorVel; + stgs.noise.errorStateEstimation.maxValue = 0.1; stgs.noise.errorStateEstimation.probMaxValue = 0.05; %% Visualization Settings diff --git a/+mystica/+utils/@NullSpace/NullSpace.m b/+mystica/+utils/@NullSpace/NullSpace.m index e174a9b..ab0e366 100644 --- a/+mystica/+utils/@NullSpace/NullSpace.m +++ b/+mystica/+utils/@NullSpace/NullSpace.m @@ -1,5 +1,5 @@ classdef NullSpace < handle - + properties (SetAccess = protected, GetAccess = public) A Z @@ -10,13 +10,15 @@ end properties (SetAccess = protected, GetAccess = protected) linIndRow + pinvAred + pinvA V end - + methods function obj = NullSpace(input) arguments - input.decompositionMethod char = 'svd' + input.decompositionMethod char {mustBeMember(input.decompositionMethod,{'svd','qrFull','qrSparse'})} = 'svd' input.rankRevealingMethod char = 'limitSingularValue_tolAuto' input.toleranceRankRevealing double = [] input.A double = [] @@ -24,12 +26,12 @@ obj.stgs.decompositionMethod = input.decompositionMethod; obj.stgs.rankRevealingMethod = input.rankRevealingMethod; obj.stgs.toleranceRankRevealing = input.toleranceRankRevealing; - + if isempty(input.A) == 0 obj.compute(input.A) end end - + function Z = compute(obj,A) switch obj.stgs.decompositionMethod case 'qrFull' @@ -44,7 +46,7 @@ end Z = obj.Z; end - + function linIndRow = getLinIndRow(obj) switch obj.stgs.decompositionMethod case 'svd' @@ -53,28 +55,65 @@ end linIndRow = obj.linIndRow; end - + + function pinvAred = getPinvAred(obj) + switch obj.stgs.decompositionMethod + case 'svd' + [Q,R,P] = qr(transpose(obj.A)); + obj.pinvAred = obj.computePinvQR(Q,R,P); + end + pinvAred = obj.pinvAred; + end + + function pinvA = getPinvA(obj) + switch obj.stgs.decompositionMethod + case {'qrFull','qrSparse'} + [U,S,V] = svd(obj.A,0); + obj.pinvA = obj.computePinvSVD(U,S,V); + end + pinvA = obj.pinvA; + end + end - + methods (Access=protected) getRank(obj) - + function qr(obj) - [Q,R,p] = qr(transpose(obj.A),'vector'); + [Q,R,P] = qr(transpose(obj.A)); + [p,~] = find(P); p=p'; obj.singValues = full(abs(diag(R))); obj.V = full(Q); obj.getRank(); obj.Z = obj.V(:,(obj.rank+1):end); obj.linIndRow = sort(p(1:obj.rank)); + % + obj.pinvAred = obj.computePinvQR(Q,R,P); end - + function svd(obj) - [~,S,obj.V] = svd(obj.A,0); + [U,S,obj.V] = svd(obj.A,0); obj.singValues = diag(S); obj.getRank(); obj.Z = obj.V(:,(obj.rank+1):end); + % + obj.pinvA = obj.computePinvSVD(U,S,obj.V); + end + + function pinvAred = computePinvQR(obj,Q,R,P) + Q1 = Q(:,1:obj.rank); + R1 = R(1:obj.rank,1:obj.rank); + P1 = P(:,1:obj.rank); P1(sum(P1,2)==0,:)=[]; + pinvAred = Q1*((R1')\(inv(P1))); + end + + function pinvA = computePinvSVD(obj,U,S,V) + Ur = U(:,1:obj.rank); + Sr = S(1:obj.rank,1:obj.rank); invSr = diag(1./diag(Sr)); + Vr = V(:,1:obj.rank); + pinvA = Vr * invSr * Ur'; end - + end - + end diff --git a/+mystica/+utils/MeasurePerformance.m b/+mystica/+utils/MeasurePerformance.m new file mode 100644 index 0000000..4ce1532 --- /dev/null +++ b/+mystica/+utils/MeasurePerformance.m @@ -0,0 +1,18 @@ +classdef MeasurePerformance + properties (Access=protected) + i_tic + t0_cpu + end + + methods + function obj = MeasurePerformance() + obj.i_tic = tic; + obj.t0_cpu = cputime; + end + + function stats = getPerformance(obj) + stats.cputime = cputime - obj.t0_cpu; + stats.timer = toc(obj.i_tic); + end + end +end diff --git a/+mystica/+utils/getCommitHash.m b/+mystica/+utils/getCommitHash.m new file mode 100644 index 0000000..0536f46 --- /dev/null +++ b/+mystica/+utils/getCommitHash.m @@ -0,0 +1,11 @@ +function s = getCommitHash + initialPath = pwd; + cd(fileparts(mfilename('fullpath'))) + [~,msg]=system('git status'); + if ~contains(msg,'nothing to commit, working tree clean') + warning('Local modification!') + end + [~,hash] = system('git rev-parse HEAD'); + s = ['mystica: https://github.com/ami-iit/mystica/commit/',hash(1:end-1)]; + cd(initialPath) +end diff --git a/+mystica/+utils/skewVee.m b/+mystica/+utils/skewVee.m index 7fbebc0..2651f1f 100644 --- a/+mystica/+utils/skewVee.m +++ b/+mystica/+utils/skewVee.m @@ -1,19 +1,16 @@ function x = skewVee(X) -% function x = skewVee(X) + % function x = skewVee(X) X_skew = (X - X.')/2; -% X = [ 0 X(1,2) X(1,3); -% -X(1,2) 0 X(2,3); -% -X(1,3) X(2,3) 0 ]; + % X = [ 0 X(1,2) X(1,3); + % -X(1,2) 0 X(2,3); + % -X(1,3) X(2,3) 0 ]; -% X = [ 0 -x(3) x(2); -% x(3) 0 -x(1); -% -x(2) x(1) 0 ]; + % X = [ 0 -x(3) x(2); + % x(3) 0 -x(1); + % -x(2) x(1) 0 ]; - - x = [-X_skew(2,3);... - X_skew(1,3);... - -X_skew(1,2)]; + x = [-X_skew(2,3);X_skew(1,3);-X_skew(1,2)]; end diff --git a/+mystica/+utils/smoothSign.m b/+mystica/+utils/smoothSign.m new file mode 100644 index 0000000..2ef560c --- /dev/null +++ b/+mystica/+utils/smoothSign.m @@ -0,0 +1,19 @@ +function y = smoothSign(x,input) + arguments + x + input.method char {mustBeMember(input.method,{'sign','sigmoid','atan'})} = 'sign'; + input.settlingTime double = 1; + input.toleranceBands double = 0.02; + end + h = 1-input.toleranceBands; + switch input.method + case 'sign' + y = sign(x); + case 'sigmoid' + ts = -log((1-h)/(1+h)); % ts is the settling time of the unitary sigmoid function + y = (1./(1+exp(-x*ts/input.settlingTime))-0.5)*2; + case 'atan' + ts = tan(h*pi/2); % ts is the settling time of the unitary atan function + y = atan(x*ts/input.settlingTime)/(pi/2); + end +end diff --git a/+mystica/+viz/@VisualizerMatlab/VisualizerMatlab.m b/+mystica/+viz/@VisualizerMatlab/VisualizerMatlab.m index cafac6d..31858fc 100644 --- a/+mystica/+viz/@VisualizerMatlab/VisualizerMatlab.m +++ b/+mystica/+viz/@VisualizerMatlab/VisualizerMatlab.m @@ -62,7 +62,7 @@ ylim(ylim) ZlimTemp = zlim; %zlim([ min([ZlimTemp(1) obj.figureMatrixLimits(3,1)]) , max([ZlimTemp(2) obj.figureMatrixLimits(3,2)]) ]) - zlim([obj.figureMatrixLimits(3,1) obj.figureMatrixLimits(3,2)]) + %zlim([obj.figureMatrixLimits(3,1) obj.figureMatrixLimits(3,2)]) view(obj.stgsVisualizer.cameraView.mBodySimulation.values) camlight('headlight'); diff --git a/+mystica/+viz/visualizeDynRel.m b/+mystica/+viz/visualizeDynRel.m new file mode 100644 index 0000000..d76a480 --- /dev/null +++ b/+mystica/+viz/visualizeDynRel.m @@ -0,0 +1,13 @@ +function visual = visualizeDynRel(input) + arguments + input.model + input.data + input.stgs + 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.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() +end diff --git a/+mystica/+viz/visualizeKinAbs.m b/+mystica/+viz/visualizeKinAbs.m index 94a1786..54335a9 100644 --- a/+mystica/+viz/visualizeKinAbs.m +++ b/+mystica/+viz/visualizeKinAbs.m @@ -6,7 +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.jointsAngVel_PJ),'title','Joint Angular velocity','ylabel','Velocity $[\frac{rad}{s}]$'); + dataLiveStatistics{3} = struct('data',abs(input.data.jointsAngVel_PJ*180/pi),'title','Joint Angular velocity','ylabel','Velocity $[\frac{deg}{s}]$'); dataLiveStatistics{4} = struct('data',abs(input.data.nDoF),'title','degrees of freedom','ylabel','DoF $[]$'); visual = mystica.viz.VisualizerMatlab('data',input.data,'stgs',input.stgs,'model',input.model,'dataLiveStatistics',dataLiveStatistics); visual.runVisualizer() diff --git a/+mystica/runSimDynRel.m b/+mystica/runSimDynRel.m index 85b9255..2b7a42e 100644 --- a/+mystica/runSimDynRel.m +++ b/+mystica/runSimDynRel.m @@ -1,4 +1,4 @@ -function [data,stateDyn] = runSimDynRel(input) +function [data,stateDyn,stats] = runSimDynRel(input) arguments input.stgs struct input.model mystica.model.Model @@ -6,6 +6,8 @@ input.nameControllerClass char end + mp = mystica.utils.MeasurePerformance(); + stgs = input.stgs; model = input.model; ClassController = str2func(input.nameControllerClass); @@ -20,7 +22,8 @@ 'mBodyPosQuat_0',input.mBodyPosQuat_0,... 'mBodyTwist_0',zeros(model.constants.mBodyTwist,1),... 'stgsIntegrator',stgs.integrator,... - 'stgsStateDynMBody',stgs.stateDyn); + 'stgsStateDynMBody',stgs.stateDyn,... + 'stgsModel',stgs.model); contr = ClassController(... 'model',model,... @@ -34,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,... @@ -44,21 +54,27 @@ for k = kVec % Controller - motorsCurrent = contr.solve('stateDynMBody',stateDyn,'time',tout(k),'model',model); + 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); - stateDyn.setMBodyPosQuat('mBodyPosQuat_0',stateDyn.mBodyPosQuat_0,'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); - stateDyn.setMBodyPosQuat('mBodyPosQuat_0',stateDyn.mBodyPosQuat_0,'model',model); + stateDynNoise = noise.applyEstimationError('model',model,'stateDynMBody',stateDyn); end + stats = mp.getPerformance(); + %% Saving Workspace - clear ans k kVec motorsCurrent mBodyPosQuat_0 tout dataLiveStatistics + clear ans k kVec motorsCurrent mBodyPosQuat_0 tout mp if stgs.saving.workspace.run if stgs.saving.workspace.clearCasadi diff --git a/+mystica/runSimKinAbs.m b/+mystica/runSimKinAbs.m index b2f0bad..1a72e84 100644 --- a/+mystica/runSimKinAbs.m +++ b/+mystica/runSimKinAbs.m @@ -1,4 +1,4 @@ -function [data,stateKin] = runSimKinAbs(input) +function [data,stateKin,stats] = runSimKinAbs(input) arguments input.stgs struct input.model mystica.model.Model @@ -6,6 +6,8 @@ input.nameControllerClass char end + mp = mystica.utils.MeasurePerformance(); + stgs = input.stgs; model = input.model; ClassController = str2func(input.nameControllerClass); @@ -40,7 +42,7 @@ for k = kVec % Controller - mBodyTwist_0 = contr.solve('time',tout(k),'stateKinMBody',stateKin,'model',model); + mBodyTwist_0 = contr.solve('time',tout(k),'stateKinMBody',stateKin,'model',model) * stgs.controller.applyControlInput; % Integrator mBodyPosQuat_0 = intgr.integrate('stateKinMBody',stateKin,'mBodyTwist_0',mBodyTwist_0,'model',model); % Logger @@ -49,9 +51,11 @@ stateKin.setMBodyPosQuat('mBodyPosQuat_0',mBodyPosQuat_0,'model',model); end + stats = mp.getPerformance(); + %% Saving Workspace - clear ans k kVec mBodyTwist_0 mBodyPosQuat_0 tout + clear ans k kVec mBodyTwist_0 mBodyPosQuat_0 tout mp if stgs.saving.workspace.run if stgs.saving.workspace.clearCasadi diff --git a/+mystica/runSimKinRel.m b/+mystica/runSimKinRel.m index c3d09c9..96feebd 100644 --- a/+mystica/runSimKinRel.m +++ b/+mystica/runSimKinRel.m @@ -1,4 +1,4 @@ -function [data,stateKin] = runSimKinRel(input) +function [data,stateKin,stats] = runSimKinRel(input) arguments input.stgs struct input.model mystica.model.Model @@ -6,6 +6,8 @@ input.nameControllerClass char end + mp = mystica.utils.MeasurePerformance(); + stgs = input.stgs; model = input.model; ClassController = str2func(input.nameControllerClass); @@ -47,7 +49,7 @@ for k = kVec % Controller - motorsAngVel = contr.solve('stateKinMBody',stateKinNoise,'time',tout(k),'model',model); + motorsAngVel = contr.solve('stateKinMBody',stateKinNoise,'time',tout(k),'model',model) * stgs.controller.applyControlInput; motorsAngVelNoise = noise.applyInputCompression('motorsAngVel',motorsAngVel); % Integrator mBodyPosQuat_0 = intgr.integrate('stateKinMBody',stateKin,'motorsAngVel',motorsAngVelNoise,'model',model); @@ -59,9 +61,11 @@ stateKinNoise = noise.applyEstimationError('model',model,'stateKinMBody',stateKin); end + stats = mp.getPerformance(); + %% Saving Workspace - clear ans k kVec motorsAngVel motorsAngVelNoise mBodyPosQuat_0 tout dataLiveStatistics + clear ans k kVec motorsAngVel motorsAngVelNoise mBodyPosQuat_0 tout mp if stgs.saving.workspace.run if stgs.saving.workspace.clearCasadi diff --git a/README.md b/README.md index 2b04595..9c0083b 100644 --- a/README.md +++ b/README.md @@ -7,23 +7,23 @@ **Multi bodY SimulaTor maxImal CoordinAtes** -**mystica** is a matlab library to simulate the kinematics of multibody systems. -**mystica** computes the system evolution modeling the problem using a state definition that we define _maximal_. -The _maximal_ representation consists of a set of _non-minimum_ variables 𝐪 complemented by holonomic constraint g(𝐪) = 0. +**mystica** is a MATLAB library to simulate the kinematics and dynamics of multibody systems. +**mystica** computes the system evolution by modeling the problem using a state definition that we define _maximal_. +The _maximal_ representation consists of a set of _non-minimum_ variables 𝐪 complemented by the holonomic constraint g(𝐪) = 0. ---

:warning: REPOSITORY UNDER DEVELOPMENT :warning: -
The libraries implemented in this repository are still experimental and we cannot guarantee stable API +
The libraries implemented in this repository are still experimental, and we cannot guarantee a stable API.

--- -### Table of content +### Table of Contents - [:hammer: Dependencies](#hammer-dependencies) -- [:floppy_disk: Installation](#floppy_disk-installation) +- [:floppy\_disk: Installation](#floppy_disk-installation) - [:rocket: Usage](#rocket-usage) - [:gear: Contributing](#gear-contributing) - [Citing this work](#citing-this-work) @@ -32,69 +32,72 @@ The _maximal_ representation consists of a set of _non-minimum_ variables 𝐪 c ## :hammer: Dependencies -- [`matlab`](https://mathworks.com/) -- a matlab [supported-compilers](https://mathworks.com/support/requirements/supported-compilers.html) for MEX-file compilation +- [`MATLAB`](https://mathworks.com/) +- [`MATLAB Curve Fitting Toolbox`](https://ch.mathworks.com/products/curvefitting.html) for the [`smooth`](https://ch.mathworks.com/help/curvefit/smooth.html) function +- A MATLAB [supported compiler](https://mathworks.com/support/requirements/supported-compilers.html) for MEX-file compilation Other requisites are: -- [`casadi`](https://web.casadi.org/) +- [`CasADi`](https://web.casadi.org/) - [`yamlmatlab`](https://github.com/ewiger/yamlmatlab) -Both [`casadi`](https://web.casadi.org/) and [`yamlmatlab`](https://github.com/ewiger/yamlmatlab) are downloaded and configured in section [Installation](#floppy_disk-installation). +Both [`CasADi`](https://web.casadi.org/) and [`yamlmatlab`](https://github.com/ewiger/yamlmatlab) are downloaded and configured in the [Installation](#floppy_disk-installation) section. ## :floppy_disk: Installation 1. Clone the repo: -``` cmd +```cmd git clone https://github.com/ami-iit/mystica.git cd mystica ``` -2. Run in **matlab** the function [`install()`](install.m). -``` matlab +2. Run the [`install()`](install.m) function in **MATLAB**. + +```matlab install() ``` -The function [`install()`](install.m) downloads [`mambaforge`](https://github.com/conda-forge/miniforge#mambaforge). [`mambaforge`](https://github.com/conda-forge/miniforge#mambaforge) is a package manager that downloads and configures our dependencies in conda enviroment called `mystica`. +The [`install()`](install.m) function downloads [`mambaforge`](https://github.com/conda-forge/miniforge#mambaforge). [`mambaforge`](https://github.com/conda-forge/miniforge#mambaforge) is a package manager that downloads and configures our dependencies in a conda environment called `mystica`.
If you already have mambaforge - If you already have mambaforge configured, you can call install() function defining mambaforge_prefix value: + If you already have mambaforge configured, you can call the install() function by defining the mambaforge_prefix value: 
install('mambaforge_prefix',<your mambaforge path prefix>)
⚠️ **Known issue** -For some versions of Ubuntu and Matlab, Matlab is not able to load the required libraries like CasADi (see [issue](https://github.com/ami-iit/mystica/issues/6)). A possible workaround is to launch `matlab` with LD_PRELOAD +For some versions of Ubuntu and MATLAB, MATLAB is not able to load the required libraries like CasADi (see [issue](https://github.com/ami-iit/mystica/issues/6)). A possible workaround is to launch `matlab` with LD_PRELOAD: ``` LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab ``` ## :rocket: Usage -Before every usage remember to run the autogenerated matlab file called `deps/setup.m` to setup the `MATLABPATH`. +Before every usage, remember to run the autogenerated MATLAB file called `deps/setup.m` to set up the `MATLABPATH`. + +Alternatively, you can launch MATLAB from the terminal after activating the conda environment called `mystica`: -Alternatively, you can launch matlab from terminal after having activated the conda enviroment called `mystica`: -``` cmd +```cmd conda activate mystica matlab ``` -Here a snippet of a code to simulate the kinematics of a 4 bar linkage mechanism: +Here's a snippet of code to simulate the kinematics of a 4-bar linkage mechanism: -``` matlab +```matlab model = mystica.model.getModel4BarLinkage(); stgs = mystica.stgs.getDefaultSettingsSimKinRel(model); -data = mystica.runSimKinRel('model',model,'stgs',stgs,'mBodyPosQuat_0',model.getMBodyPosQuatRestConfiguration,'nameControllerClass','mystica.controller.ExampleKinRel'); -mystica.viz.visualizeKinRel('model',model,'data',data,'stgs',stgs) +data = mystica.runSimKinRel('model', model, 'stgs', stgs, 'mBodyPosQuat_0', model.getMBodyPosQuatRestConfiguration, 'nameControllerClass', 'mystica.controller.ExampleKinRel'); +mystica.viz.visualizeKinRel('model', model, 'data', data, 'stgs', stgs) ``` Other examples can be found in the [examples directory](examples). -If you want to go deep into the code, we prepared [this document](docs/nomenclature.md) as an entry point to explain and describe our variables. +If you want to delve deeper into the code, we have prepared [this document](docs/nomenclature.md) as an entry point to explain and describe our variables. ## :gear: Contributing -**mystica** is an open-source project, and is thus built with your contributions. We strongly encourage you to open an issue with your feature request. Once the issue has been opened, you can also proceed with a pull-request :rocket: +**mystica** is an open-source project, and is thus built with your contributions. We strongly encourage you to open an issue with your feature request. Once the issue has been opened, you can also proceed with a pull-request. :rocket: ## Citing this work diff --git a/install.m b/install.m index 58a8041..7400716 100644 --- a/install.m +++ b/install.m @@ -1,7 +1,8 @@ function install(input) arguments - input.mambaforge_prefix char = '' - input.env_name char = 'mystica' + input.mambaforge_prefix char = '' + input.env_name char = 'mystica' + input.install_casadi_via_mamba logical = true end % function created inspired by https://github.com/robotology/robotology-superbuild/blob/master/scripts/install_robotology_packages.m @@ -10,6 +11,7 @@ function install(input) setup_script = fullfile(mystica_fullpath,'deps','setup.m'); matlab_path_env = ''; + matlab_folders_to_be_installed = {}; if exist(install_prefix,'dir') fprintf('Directory %s already exists.\n', install_prefix); @@ -31,7 +33,13 @@ function install(input) % Install all the packages via conda fprintf('Installing packages\n'); - system(sprintf('"%s" create -n "%s" -y -c conda-forge -c robotology casadi-matlab-bindings=3.5.5.2 "libblas=*=*openblas"\n', mamba_full_path,input.env_name)); + if input.install_casadi_via_mamba + system(sprintf('"%s" create -n "%s" -y -c conda-forge -c robotology casadi-matlab-bindings=3.5.5.2 "libblas=*=*openblas"\n', mamba_full_path,input.env_name)); + else + system(sprintf('"%s" create -n "%s" -y -c conda-forge -c robotology \n', mamba_full_path,input.env_name)); + casadi_full_path = fullfile(install_prefix,'casadi'); + download_casadi_binaries(casadi_full_path) + end % see discussion https://github.com/ami-iit/element_morphing-cover-design/issues/215#issuecomment-1081515249 to understand why we added "libblas=*=*openblas" fprintf('Installation of packages completed\n'); @@ -43,35 +51,32 @@ function install(input) %% Configure GitHub repositories - fprintf('Installing GitHub repositories\n'); + fprintf('Cloning GitHub repositories\n') clone_git_repository('https://github.com/ewiger/yamlmatlab.git','deps') - matlab_path_env = strcat( fullfile(install_prefix,'yamlmatlab') , env_sep , matlab_path_env ); - system(sprintf('"%s" env config vars set MATLABPATH="%s" -p "%s"',mamba_full_path,matlab_path_env,env_full_path)); - fprintf('Installation of GitHub repositories completed\n'); + fprintf('Cloning GitHub repositories completed\n') - %% Addpath mystica directory + %% Installing matlab directories - fprintf('Installing mystica root folder\n') - matlab_path_env = strcat( mystica_fullpath , env_sep , matlab_path_env ); - system(sprintf('"%s" env config vars set MATLABPATH="%s" -p "%s"',mamba_full_path,matlab_path_env,env_full_path)); - fprintf('Installing mystica root folder completed\n') + % yamlmatlab + matlab_folders_to_be_installed{end+1} = fullfile(install_prefix,'yamlmatlab'); - %% Addpath mystica/meshes directory + % mystica root folder + matlab_folders_to_be_installed{end+1} = mystica_fullpath; - fprintf('Installing mystica meshes folder\n') - mystica_meshes_fullpath = fullfile(mystica_fullpath,'meshes'); - matlab_path_env = strcat( mystica_meshes_fullpath , env_sep , matlab_path_env ); - system(sprintf('"%s" env config vars set MATLABPATH="%s" -p "%s"',mamba_full_path,matlab_path_env,env_full_path)); - fprintf('Installing mystica meshes folder completed\n') + % mystica meshes + matlab_folders_to_be_installed{end+1} = fullfile(mystica_fullpath,'meshes'); - %% Create csdMEX folder + % csdMEX + matlab_folders_to_be_installed{end+1} = fullfile(mystica_fullpath,'deps','csdMEX'); mkdir(matlab_folders_to_be_installed{end}) - fprintf('Installing mystica csdMEX folder\n') - mystica_csdMEX_fullpath = fullfile(mystica_fullpath,'deps','csdMEX'); - mkdir(mystica_csdMEX_fullpath) - matlab_path_env = strcat( mystica_csdMEX_fullpath , env_sep , matlab_path_env ); - system(sprintf('"%s" env config vars set MATLABPATH="%s" -p "%s"',mamba_full_path,matlab_path_env,env_full_path)); - fprintf('Installing mystica meshes folder completed\n') + % casadi binaries + if ~input.install_casadi_via_mamba + matlab_folders_to_be_installed{end+1} = casadi_full_path; + end + + for i = 1 : length(matlab_folders_to_be_installed) + matlab_path_env = install_matlab_folder(matlab_folders_to_be_installed{i},matlab_path_env,mamba_full_path,env_full_path); + end %% Creation of setup.m @@ -84,32 +89,23 @@ function install(input) fprintf(setupID,' env_sep = ":";\n'); fprintf(setupID,'end\n'); fprintf(setupID,'\n'); - fprintf(setupID,'%% Install prefix (hardcoded at generation time)\n'); - fprintf(setupID,'pckgs_install_prefix = "%s";\n', pckgs_install_prefix); - fprintf(setupID,'install_prefix = "%s";\n', install_prefix); - fprintf(setupID,'mystica_fullpath = "%s";\n', mystica_fullpath); - fprintf(setupID,'mystica_meshes_fullpath = "%s";\n', mystica_meshes_fullpath); - fprintf(setupID,'mystica_csdMEX_fullpath = "%s";\n', mystica_csdMEX_fullpath); + fprintf(setupID,'%% Configure `matlab_folders_to_be_installed` (hardcoded at generation time)\n'); + for i = 1 : length(matlab_folders_to_be_installed) + fprintf(setupID,'matlab_folders_to_be_installed{%i} = "%s";\n',i,matlab_folders_to_be_installed{i}); + end + fprintf(setupID,'%% Configure `pckgs_install_prefix` (hardcoded at generation time)\n'); + fprintf(setupID,'pckgs_install_prefix = "%s";\n', pckgs_install_prefix); fprintf(setupID,'\n'); + fprintf(setupID,'%% Install `matlab_folders_to_be_installed`\n'); + for i = 1 : length(matlab_folders_to_be_installed) + fprintf(setupID,'addpath(matlab_folders_to_be_installed{%i});\n',i); + end fprintf(setupID,'%% AddPath packages installed with conda\n'); fprintf(setupID,'addpath(fullfile(pckgs_install_prefix,"mex"));\n'); fprintf(setupID,'\n'); - fprintf(setupID,'%% AddPath github repositories\n'); - fprintf(setupID,'addpath(fullfile(install_prefix,"yamlmatlab"));\n'); - fprintf(setupID,'\n'); - fprintf(setupID,'%% AddPath mystica\n'); - fprintf(setupID,'addpath(mystica_fullpath);\n'); - fprintf(setupID,'\n'); - fprintf(setupID,'%% AddPath mystica meshes\n'); - fprintf(setupID,'addpath(mystica_meshes_fullpath);\n'); - fprintf(setupID,'\n'); - fprintf(setupID,'%% AddPath mystica csdMEX\n'); - fprintf(setupID,'addpath(mystica_csdMEX_fullpath);\n'); - fprintf(setupID,'\n'); fprintf(setupID,'%% Add to the env:"PATH" the directory with the packages installed with conda\n'); fprintf(setupID,'setenv("PATH",strcat(fullfile(pckgs_install_prefix,"bin"), env_sep, getenv("PATH")));\n'); fclose( setupID); - fprintf('packages are successfully installed!\n'); fprintf('Please run %s before using the packages,\n',setup_script) fprintf('or activate the conda enviroment %s and open matlab from that terminal.\n',input.env_name); @@ -133,6 +129,35 @@ function clone_git_repository(repository_url,install_prefix) end end +function matlab_path_env = install_matlab_folder(name,matlab_path_env,mamba_full_path,env_full_path) + if ispc + env_sep = ";"; + else + env_sep = ":"; + end + fprintf('Installing %s\n',name) + matlab_path_env = strcat( name , env_sep , matlab_path_env ); + system(sprintf('"%s" env config vars set MATLABPATH="%s" -p "%s"',mamba_full_path,matlab_path_env,env_full_path)); +end + +function download_casadi_binaries(casadi_full_path) + if ismac + websave('casadi','https://github.com/casadi/casadi/releases/download/3.5.5/casadi-osx-matlabR2015a-v3.5.5.tar.gz') + untar('casadi.gz',casadi_full_path) + delete('casadi.gz') + elseif isunix + websave('casadi','https://github.com/casadi/casadi/releases/download/3.5.5/casadi-linux-matlabR2014b-v3.5.5.tar.gz') + untar('casadi.gz',casadi_full_path) + delete('casadi.gz') + elseif ispc + websave('casadi','https://github.com/casadi/casadi/releases/download/3.5.5/casadi-windows-matlabR2016a-v3.5.5.zip') + unzip('casadi.zip',casadi_full_path) + delete('casadi.zip') + else + error('Platform not supported') + end +end + function [mamba_full_path,env_full_path] = configure_mambaforge(mambaforge_prefix,install_prefix,env_name) if ispc