From c9e6a73fd68ccb663ff7ede15e80ea514dfb54a9 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Mon, 2 May 2022 13:30:19 +0200 Subject: [PATCH 01/19] implement dynamic simulator --- .../@ExampleDynRel/ExampleDynRel.m | 38 ++++ .../@IntegratorDynRel/IntegratorDynRel.m | 84 ++++++++ +mystica/+log/@LoggerDynRel/LoggerDynRel.m | 129 ++++++++++++ .../+state/@StateDynMBody/StateDynMBody.m | 67 ++++++ .../@StateDynMBody/getDynamicQuantities.m | 196 ++++++++++++++++++ .../get_mBodyVelAcc0_from_motorsCurrent.m | 91 ++++++++ +mystica/+stgs/getDefaultSettingsSimDynRel.m | 164 +++++++++++++++ +mystica/+viz/visualizeDynRel.m | 14 ++ 8 files changed, 783 insertions(+) create mode 100644 +mystica/+controller/@ExampleDynRel/ExampleDynRel.m create mode 100644 +mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m create mode 100644 +mystica/+log/@LoggerDynRel/LoggerDynRel.m create mode 100644 +mystica/+state/@StateDynMBody/StateDynMBody.m create mode 100644 +mystica/+state/@StateDynMBody/getDynamicQuantities.m create mode 100644 +mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m create mode 100644 +mystica/+stgs/getDefaultSettingsSimDynRel.m create mode 100644 +mystica/+viz/visualizeDynRel.m 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/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m new file mode 100644 index 0000000..61f356a --- /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 + 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); + 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/+log/@LoggerDynRel/LoggerDynRel.m b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m new file mode 100644 index 0000000..5eac278 --- /dev/null +++ b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m @@ -0,0 +1,129 @@ +classdef LoggerDynRel < mystica.log.Logger + %LOGGERKINABS Summary of this class goes here + % Detailed explanation goes here + + properties + mBodyTwist_0 + motorsAngVel + motorsCurrent + 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.motorsCurrent = 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 + 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.motorsCurrent( :,obj.indexIteration) = input.motorsCurrent; + obj.jointsAngVel_PJ(:,obj.indexIteration) = jointsAngVel; + + % massMatrix mBodyTwAcc_0 = mBodyWrenchExt + mBodyWrenchJcF_0 + % + % mBodyWrenchExt = - wrenchGravity - wrenchCoriolis + wrenchFriction + wrenchInput + + mBodyWrench_0 = input.stateDynMBody.csdFn.mBodyWrench_0('mBodyPosVel_0',input.stateDynMBody.mBodyPosVel_0,'motorsCurrent',input.motorsCurrent); + + obj.mBodyWrenchExt_0(:,obj.indexIteration) = full(mBodyWrench_0.ext); + obj.mBodyWrenchGra_0(:,obj.indexIteration) = full(mBodyWrench_0.coriolis); + obj.mBodyWrenchCor_0(:,obj.indexIteration) = full(mBodyWrench_0.gravity); + obj.mBodyWrenchFri_0(:,obj.indexIteration) = full(mBodyWrench_0.friction); + obj.mBodyWrenchInp_0(:,obj.indexIteration) = full(mBodyWrench_0.input); + % 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/+state/@StateDynMBody/StateDynMBody.m b/+mystica/+state/@StateDynMBody/StateDynMBody.m new file mode 100644 index 0000000..941a5a7 --- /dev/null +++ b/+mystica/+state/@StateDynMBody/StateDynMBody.m @@ -0,0 +1,67 @@ +classdef StateDynMBody < mystica.state.StateKinMBody + %STATEDYNMBODY Summary of this class goes here + % Detailed explanation goes here + + properties (SetAccess=protected,GetAccess=public) + mBodyPosVel_0 + end + properties (SetAccess=protected,GetAccess=protected) + opti = [] + optiVar + end + + methods + function obj = StateDynMBody(input) + arguments + input.model + input.mBodyPosQuat_0 + input.mBodyTwist_0 + input.stgsStateDynMBody + input.stgsIntegrator + end + + obj@mystica.state.StateKinMBody('model',input.model,... + 'mBodyPosQuat_0',input.mBodyPosQuat_0,... + 'stgsStateKinMBody',input.stgsStateDynMBody) + + % Definition Casadi Variables + obj.csdSy.motorsCurrent = casadi.SX.sym('I',input.model.constants.motorsAngVel,1); + obj.csdSy.mBodyPosVel_0 = casadi.SX.sym('chi',input.model.constants.mBodyPosVel,1); + + obj.getDynamicQuantities(input.model,input.stgsIntegrator); + + obj.setMBodyPosVel('mBodyPosVel_0',[input.mBodyPosQuat_0;input.mBodyTwist_0],'model',input.model) + obj.setMBodyPosQuat('mBodyPosQuat_0',obj.mBodyPosQuat_0,'model',input.model); + + end + [mBodyVelAcc_0,varargout] = get_mBodyVelAcc0_from_motorsCurrent(obj,input,stgs) + + function setMBodyPosVel(obj,input) + arguments + obj mystica.state.StateDynMBody + input.mBodyPosVel_0 (:,1) + input.model mystica.model.Model + end + + obj.mBodyPosVel_0 = input.mBodyPosVel_0; + + switch 'reduceComputationOfQuantities_mBodyPosQuat_Depedent' + case 'computeQuantities_mBodyPosQuat_Depedent' + obj.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel)) + case 'reduceComputationOfQuantities_mBodyPosQuat_Depedent' + % remove obj.setMBodyPosQuat to reduce computational time (see https://github.com/ami-iit/element_morphing-cover-design/commit/804e87f9b8d9dc144e09a0fc51e1e79b38e38a3a) + obj.mBodyPosQuat_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel); + obj.Jc = []; + obj.referenceConversion.from_mBodyTwist0_2_jointsAngVelPJ = []; + obj.nullJc_mBodyTwist_0 = []; + obj.nullJc_jointsAngVel_PJ = []; + obj.linIndRowJc = []; + end + + end + + end + methods (Access=protected) + getDynamicQuantities(obj,model,stgsIntegrator) + end +end diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m new file mode 100644 index 0000000..efbaeb2 --- /dev/null +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -0,0 +1,196 @@ +function getDynamicQuantities(obj,model,stgsIntegrator) + + %% Compute Casadi Symbolic Variables + + obj.csdSy.mBodyPosQuat_0 = obj.csdSy.mBodyPosVel_0(model.selector.indexes_mBodyPosQuat_from_mBodyPosVel); + obj.csdSy.mBodyTwist_0 = obj.csdSy.mBodyPosVel_0(model.selector.indexes_mBodyTwist_from_mBodyPosVel); + obj.csdSy.mBodyVelQuat_0 = obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.csdSy.mBodyPosQuat_0,obj.csdSy.mBodyTwist_0,stgsIntegrator.dxdtParam.baumgarteIntegralCoefficient); + + obj.csdSy.Jc = obj.csdFn.Jc(obj.csdSy.mBodyPosQuat_0); + obj.csdSy.rC_from_mBodyTwist0_2_jointsAngVelPJ = obj.csdFn.rC_from_mBodyTwist0_2_jointsAngVelPJ( obj.csdSy.mBodyPosQuat_0); + obj.csdSy.rC_from_jointsAngVelPJ_2_jointsAngVel0 = obj.csdFn.rC_from_jointsAngVelPJ_2_jointsAngVel0(obj.csdSy.mBodyPosQuat_0); + + obj.csdSy.dJc = reshape(jacobian(obj.csdSy.Jc,obj.csdSy.mBodyPosQuat_0)*obj.csdSy.mBodyVelQuat_0,size(obj.csdSy.Jc)); + + jointsAngVelPJ = obj.csdSy.rC_from_mBodyTwist0_2_jointsAngVelPJ * obj.csdSy.mBodyTwist_0; + + %% Joint | frictionActAngVel, frictionPasAngVel, Ki, Kdw + + Ki{ model.nJoint} = []; + Kdw{model.nJoint} = []; + frictionActAngVel{model.nJoint} = []; + frictionPasAngVel{model.nJoint} = []; + + for j = 1 : model.nJoint + jAngVel_pj_cj = jointsAngVelPJ(model.joints{j}.selector.indexes_jAngVel_from_jointsAngVel ); + boolJointActuated = model.joints{j}.axesActuated; + boolJointPassive = ~model.joints{j}.axesActuated & model.joints{j}.axesRotation; + if any(boolJointActuated) + indexTemp = find(boolJointActuated); + actAxesAngVelPJ = jAngVel_pj_cj(indexTemp); + eff = model.joints{j}.transmission.efficiency(indexTemp,indexTemp); + gamma = model.joints{j}.transmission.gearRatio(indexTemp,indexTemp); + Kt = model.joints{j}.coefficients.motorTorque(indexTemp,indexTemp); + Kc = model.joints{j}.coefficients.coulombFriction(indexTemp,indexTemp); + Kv = model.joints{j}.coefficients.viscousFriction(indexTemp,indexTemp); + Jm = model.joints{j}.inertiaTensMotorRot_j_g(indexTemp,indexTemp); + % friction actuated axis of rotation + frictionActAngVel{j} = - (Kv * gamma.^2 * eff * actAxesAngVelPJ + Kc * gamma * eff * sign(actAxesAngVelPJ)); + Ki{j} = eff * gamma * Kt; + Kdw{j} = Jm * gamma.^2 * eff; + end + if any(boolJointPassive) + indexTemp = find(boolJointPassive); + pasAxesAngVelPJ = jAngVel_pj_cj(indexTemp); + Kc = model.joints{j}.coefficients.coulombFriction(indexTemp,indexTemp); + Kv = model.joints{j}.coefficients.viscousFriction(indexTemp,indexTemp); + % friction passive axis of rotation (actuated joints + passive joints) + frictionPasAngVel{j} = - (Kv * pasAxesAngVelPJ + Kc * sign(pasAxesAngVelPJ)); + end + end + + Ki = sparse(blkdiag(Ki{:})); + Kdw = sparse(blkdiag(Kdw{:})); + frictionActAngVel = vertcat(frictionActAngVel{:}); + frictionPasAngVel = vertcat(frictionPasAngVel{:}); + + %% Link | massMatrixWithoutMotorInertia, wrenchGravity, wrenchCoriolis, B + + massMatrixWithoutMotorInertia{model.nLink} = []; + wrenchGravity{ model.nLink} = []; + wrenchCoriolis{model.nLink} = []; + B{2*model.nLink} = []; + B(:)={sparse(model.constants.linkPos,model.constants.jointsAngVel)}; + + for i = 1 : model.nLink + mass = model.linksAttributes{i}.mass; + rotm_0_b = mystica.rbm.getRotmGivenTform(obj.linksState{i}.csdFn.tform_0_b(obj.csdSy.mBodyPosQuat_0)); + pos_b_g = mystica.rbm.getPosGivenTform(model.linksAttributes{i}.tform_b_g); + inertiaTens_0_b = rotm_0_b * model.linksAttributes{i}.inertiaTens_b_b * transpose(rotm_0_b); + sel_twist = model.linksAttributes{i}.selector.matrix_linkTwist_from_mBodyTwist; + angVel_0_b = obj.csdSy.mBodyTwist_0(model.linksAttributes{i}.selector.indexes_linkAngVel_from_mBodyTwist); + + massMatrixWithoutMotorInertia{i} = ... + [eye(model.constants.linkPos)*mass, -mass*mystica.utils.skew(rotm_0_b*pos_b_g);... + mass*mystica.utils.skew(rotm_0_b*pos_b_g), inertiaTens_0_b]*sel_twist; + + wrenchGravity{ i} = -mass*[model.constants.gravity ; mystica.utils.skew(rotm_0_b*pos_b_g)*model.constants.gravity]; + wrenchCoriolis{i} = [mass*mystica.utils.skew(angVel_0_b)^2*rotm_0_b*pos_b_g; mystica.utils.skew(angVel_0_b)*inertiaTens_0_b*angVel_0_b]; + + for j = model.linksAttributes{i}.joints(:)' + signF = - (model.joints{j}.linkParent == i) + (model.joints{j}.linkChild == i); + B{2*i} = B{2*i} + signF*model.joints{j}.selector.matrix_jAngVel_from_jointsAngVel; + end + + end + + massMatrixWithoutMotorInertia = vertcat(massMatrixWithoutMotorInertia{:}); + wrenchGravity = vertcat(wrenchGravity{ :}); + wrenchCoriolis = vertcat(wrenchCoriolis{:}); + B = vertcat(B{:}); + + %% massMatrix, massMatrixMotorInertia, dJc + + rC_w_V = obj.csdSy.rC_from_mBodyTwist0_2_jointsAngVelPJ; + rC_0_PJ = obj.csdSy.rC_from_jointsAngVelPJ_2_jointsAngVel0; + sel_wAct = model.selector.matrix_motorsAngVel_from_jointsAngVel; + sel_wPas = model.selector.matrix_passiveAngVel_from_jointsAngVel; + + massMatrixMotorInertia = B * rC_0_PJ * sel_wAct' * Kdw * sel_wAct * rC_w_V; + + massMatrix = massMatrixWithoutMotorInertia + massMatrixMotorInertia; + + obj.csdFn.dJc = casadi.Function('dJc',{obj.csdSy.mBodyPosVel_0 },{obj.csdSy.dJc}); + obj.csdFn.massMatrix = casadi.Function('M',{ obj.csdSy.mBodyPosQuat_0},{massMatrix}); + obj.csdFn.massMatrixMotorInertia = casadi.Function('Mmi',{obj.csdSy.mBodyPosQuat_0},{massMatrixMotorInertia}); + + %% mBodyWrenchExt_0, mBodyWrenchCor_0, mBodyWrenchGra_0, mBodyWrenchFri_0, mBodyWrenchInp_0 + + wrenchCurrent = B * rC_0_PJ * sel_wAct' * Ki * obj.csdSy.motorsCurrent; + + wrenchFriction = B * rC_0_PJ * sel_wAct' * frictionActAngVel; + if ~isempty(frictionPasAngVel) + wrenchFriction = wrenchFriction + B * rC_0_PJ * sel_wPas' * frictionPasAngVel; + end + + % M*dV + wrenchCoriolis + wrenchGravity = B * motorCurrent + friction + Jc'f + % + % mBodyWrenchExt = B * motorCurrent + friction - wrenchCoriolis - wrenchGravity + % + % M*dV = Jc'f + mBodyWrenchExt + + wrenchTotal = -wrenchGravity-wrenchCoriolis+wrenchFriction+wrenchCurrent; + + obj.csdFn.mBodyWrenchExt_0 = casadi.Function('wE',{ obj.csdSy.mBodyPosVel_0,obj.csdSy.motorsCurrent},{wrenchTotal}); + obj.csdFn.mBodyWrenchGra_0 = casadi.Function('wEg',{obj.csdSy.mBodyPosQuat_0},{wrenchGravity}); + + obj.csdFn.mBodyWrench_0 = casadi.Function('mBodyWrench_0',... + {obj.csdSy.mBodyPosVel_0,obj.csdSy.motorsCurrent},... %input + {wrenchTotal,wrenchCoriolis,wrenchGravity,wrenchFriction,wrenchCurrent},... %output + {'mBodyPosVel_0','motorsCurrent'},... %label input + {'ext','coriolis','gravity','friction','input'}); %label output + + obj.csdFn.from_motorsCurrent_2_mBodyWrenchCur = casadi.Function('B',{obj.csdSy.mBodyPosQuat_0},{B * rC_0_PJ * sel_wAct' * Ki}); + + %% mBodyVelAcc0 + + kpFeedbackJcV = stgsIntegrator.dxdtParam.feedbackJacobianConstraintsV(1); + kiFeedbackJcV = stgsIntegrator.dxdtParam.feedbackJacobianConstraintsV(2); + + switch stgsIntegrator.dxdtOpts.optOpts.name + case 'osqp' + opti = casadi.Opti('conic'); + p_opts = struct('expand',true,'error_on_fail',false); + s_opts = struct(); + opti.solver(stgsIntegrator.dxdtOpts.optOpts.name,p_opts,s_opts); + case 'ipopt' + opti = casadi.Opti(); + p_opts = struct('expand',true,'error_on_fail',false,'print_time',false); + s_opts = struct('print_level',0); + opti.solver(stgsIntegrator.dxdtOpts.optOpts.name,p_opts,s_opts); + otherwise + error('solver not valid') + end + + % scaling opti variable + k_dV = ones(model.constants.mBodyTwist,1); + for i = 1 : model.nLink + k_dV(model.linksAttributes{i}.selector.indexes_linkLinVel_from_mBodyTwist) = (model.linksAttributes{i}.linkDimension); + k_dV(model.linksAttributes{i}.selector.indexes_linkAngVel_from_mBodyTwist) = 1; + end + k_f = ones(model.constants.nConstraints,1); + k_f(model.selector.indexes_constrainedAngVel_from_JcV) = (model.linksAttributes{1}.mass*model.linksAttributes{1}.linkDimension^2); + k_f(model.selector.indexes_constrainedLinVel_from_JcV) = (model.linksAttributes{1}.mass*model.linksAttributes{1}.linkDimension); + % Variable definitions + dV = opti.variable(model.constants.mBodyTwist ,1).*k_dV; % mBodyTwAcc_0 -> dV + f = opti.variable(model.constants.nConstraints,1).*k_f; % jointsWrenchConstr_pj -> f + x = [dV;f]; + 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); + opti.minimize(E'*E); + opti.subject_to(M*dV==W+Jc'*f); + 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(1:model.constants.mBodyTwist,1); + jointsWrenchConstr_PJ = X(model.constants.mBodyTwist+1:end,1); + mBodyVelAcc_0 = [mBodyVelQuat ; mBodyTwAcc_0]; + % + obj.csdFn.mBodyVelAcc_0 = casadi.Function('mBodyVelAcc_0',... + {mBodyPosVel,motorsCurrent},... + {mBodyVelAcc_0,jointsWrenchConstr_PJ},... + {'mBodyPosVel','motorsCurrent'},... + {'mBodyVelAcc_0','jointsWrenchConstr_PJ'}); + +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..222b003 --- /dev/null +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -0,0 +1,91 @@ +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` + %---------------------------------------------------------------------% + + if isfield(input,'mBodyPosVel_0_warningOnlyIfNecessary') + obj.setMBodyPosVel('mBodyPosVel_0',input.mBodyPosVel_0_warningOnlyIfNecessary,'model',input.model) + end + + switch stgs.solverTechnique + case 'inv' + + %-----------------------------------------------------------------% + % Inverse + %-----------------------------------------------------------------% + + obj.setMBodyPosQuat('mBodyPosQuat_0',obj.mBodyPosQuat_0,'model',input.model); + + kpFeedbackJcV = input.kFeedbackJcV(1); + kiFeedbackJcV = input.kFeedbackJcV(2); + + mBodyTwist_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); + mBodyVelQuat_0 = sparse(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); + + Jc = obj.Jc( obj.linIndRowJc,:); + dJc = sparse(obj.csdFn.dJc(obj.mBodyPosVel_0)); + dJc = dJc(obj.linIndRowJc,:); + intJcV = obj.getIntJcV; + intJcV = intJcV( obj.linIndRowJc,:); + V = mBodyTwist_0; + M = sparse(obj.csdFn.massMatrix(obj.mBodyPosQuat_0)); % massMatrix -> M + W = sparse(obj.csdFn.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) + %-----------------------------------------------------------------% + + [mBodyVelAcc_0,jointsWrenchConstr_PJ] = obj.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,input.motorsCurrent); + + mBodyVelAcc_0 = full(mBodyVelAcc_0); + jointsWrenchConstr_PJ = full(jointsWrenchConstr_PJ); + + end + + %---------------------------------------------------------------------% + % Output + + if nargout > 1 + varargout{1} = jointsWrenchConstr_PJ; + end + if nargout > 2 + varargout{2} = mBodyWrenchExt_0; + end + + k = 1e3; + if input.t*k-floor(input.t*k)<1e-2 + disp(input.t) + end + +end diff --git a/+mystica/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m new file mode 100644 index 0000000..9f73664 --- /dev/null +++ b/+mystica/+stgs/getDefaultSettingsSimDynRel.m @@ -0,0 +1,164 @@ +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)); + stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x-t/8e1,y-t/8e1); + stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x,y,t)-stgs.desiredShape.fun(0,0,t); + stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x/stgs.unitMeas.converter.length,y/stgs.unitMeas.converter.length,t)*stgs.unitMeas.converter.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; + + %% StateKin 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.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.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.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; + 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.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 = 0; + 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/+viz/visualizeDynRel.m b/+mystica/+viz/visualizeDynRel.m new file mode 100644 index 0000000..2a22333 --- /dev/null +++ b/+mystica/+viz/visualizeDynRel.m @@ -0,0 +1,14 @@ +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.motorsAngVel),'title','Motor Angular velocity','ylabel','Velocity $[\frac{rad}{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() + visual.save() +end From fcc51bd34c6293aa1a9890e7d43ffd5d1cc70679 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Mon, 13 Jun 2022 17:03:22 +0200 Subject: [PATCH 02/19] general cleanup - clarify UM - getCommitHash - fix bug logger - create `stgs.controller.applyControlInput` - add `deleteLoggedIndexes` --- .../@IntegratorDynRel/IntegratorDynRel.m | 20 +++++++++---------- +mystica/+log/@Logger/Logger.m | 12 +++++++++++ +mystica/+log/@LoggerDynRel/LoggerDynRel.m | 4 ++-- +mystica/+stgs/getDefaultSettingsSimDynRel.m | 10 ++++++---- +mystica/+stgs/getDefaultSettingsSimKinAbs.m | 6 ++++-- +mystica/+stgs/getDefaultSettingsSimKinRel.m | 4 +++- +mystica/+utils/getCommitHash.m | 11 ++++++++++ .../+viz/@VisualizerMatlab/VisualizerMatlab.m | 2 +- +mystica/+viz/visualizeKinAbs.m | 2 +- +mystica/runSimDynRel.m | 2 +- +mystica/runSimKinAbs.m | 2 +- +mystica/runSimKinRel.m | 2 +- 12 files changed, 53 insertions(+), 24 deletions(-) create mode 100644 +mystica/+utils/getCommitHash.m diff --git a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m index 61f356a..c9be127 100644 --- a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m +++ b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m @@ -1,7 +1,7 @@ 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 @@ -10,7 +10,7 @@ properties (SetAccess=immutable,GetAccess=public) dt end - + methods function obj = IntegratorDynRel(input) arguments @@ -25,7 +25,7 @@ 'stgsIntegrator',input.stgsIntegrator); obj.dt = input.dt; end - + function xf = integrate(obj,input) arguments obj @@ -33,13 +33,13 @@ 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); @@ -70,15 +70,15 @@ 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/+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 index 5eac278..70ac6d9 100644 --- a/+mystica/+log/@LoggerDynRel/LoggerDynRel.m +++ b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m @@ -87,8 +87,8 @@ function store(obj,input) mBodyWrench_0 = input.stateDynMBody.csdFn.mBodyWrench_0('mBodyPosVel_0',input.stateDynMBody.mBodyPosVel_0,'motorsCurrent',input.motorsCurrent); obj.mBodyWrenchExt_0(:,obj.indexIteration) = full(mBodyWrench_0.ext); - obj.mBodyWrenchGra_0(:,obj.indexIteration) = full(mBodyWrench_0.coriolis); - obj.mBodyWrenchCor_0(:,obj.indexIteration) = full(mBodyWrench_0.gravity); + obj.mBodyWrenchGra_0(:,obj.indexIteration) = full(mBodyWrench_0.gravity); + obj.mBodyWrenchCor_0(:,obj.indexIteration) = full(mBodyWrench_0.coriolis); obj.mBodyWrenchFri_0(:,obj.indexIteration) = full(mBodyWrench_0.friction); obj.mBodyWrenchInp_0(:,obj.indexIteration) = full(mBodyWrench_0.input); % mBodyWrenchJcF_0 = Jc' F diff --git a/+mystica/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m index 9f73664..c3d39e4 100644 --- a/+mystica/+stgs/getDefaultSettingsSimDynRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimDynRel.m @@ -12,10 +12,10 @@ %% Desired shape - stgs.desiredShape.fun = @(x,y) 0.005*(5*cos(10*x-2)+5*cos(10*x-20*y)); - stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x-t/8e1,y-t/8e1); - stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x,y,t)-stgs.desiredShape.fun(0,0,t); - stgs.desiredShape.fun = @(x,y,t) stgs.desiredShape.fun(x/stgs.unitMeas.converter.length,y/stgs.unitMeas.converter.length,t)*stgs.unitMeas.converter.length; + 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 @@ -55,6 +55,8 @@ %% Controller + stgs.controller.applyControlInput = true; + stgs.controller.casadi.optimizationType = 'conic'; stgs.controller.casadi.solver = 'osqp'; diff --git a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m index 15eb2a1..0957ec1 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) @@ -39,7 +39,7 @@ stgs.integrator.solverOpts.RelTol = 1e-3; stgs.integrator.solverOpts.AbsTol = 1e-6; - stgs.integrator.dxdtOpts.assumeConstant = 1; + stgs.integrator.dxdtOpts.assumeConstant = 0; stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1; stgs.integrator.statusTracker.workspacePrint.run = 1; @@ -51,6 +51,8 @@ %% Controller + stgs.controller.applyControlInput = true; + stgs.controller.casadi.optimizationType = 'conic'; stgs.controller.casadi.solver = 'osqp'; diff --git a/+mystica/+stgs/getDefaultSettingsSimKinRel.m b/+mystica/+stgs/getDefaultSettingsSimKinRel.m index 99c1827..ec6b29c 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinRel.m @@ -39,7 +39,7 @@ stgs.integrator.solverOpts.RelTol = 1e-3; stgs.integrator.solverOpts.AbsTol = 1e-6; - stgs.integrator.dxdtOpts.assumeConstant = 1; + stgs.integrator.dxdtOpts.assumeConstant = 0; stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1; stgs.integrator.dxdtParam.regTermDampPInv = 1e-6; @@ -52,6 +52,8 @@ %% Controller + stgs.controller.applyControlInput = true; + stgs.controller.casadi.optimizationType = 'conic'; stgs.controller.casadi.solver = 'osqp'; 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/+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/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..70ffe4d 100644 --- a/+mystica/runSimDynRel.m +++ b/+mystica/runSimDynRel.m @@ -44,7 +44,7 @@ for k = kVec % Controller - motorsCurrent = contr.solve('stateDynMBody',stateDyn,'time',tout(k),'model',model); + motorsCurrent = contr.solve('stateDynMBody',stateDyn,'time',tout(k),'model',model) * stgs.controller.applyControlInput; % Integrator mBodyPosVel_0 = intgr.integrate('stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,'model',model); stateDyn.setMBodyPosQuat('mBodyPosQuat_0',stateDyn.mBodyPosQuat_0,'model',model); diff --git a/+mystica/runSimKinAbs.m b/+mystica/runSimKinAbs.m index b2f0bad..98ea73c 100644 --- a/+mystica/runSimKinAbs.m +++ b/+mystica/runSimKinAbs.m @@ -40,7 +40,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 diff --git a/+mystica/runSimKinRel.m b/+mystica/runSimKinRel.m index c3d09c9..9561d6c 100644 --- a/+mystica/runSimKinRel.m +++ b/+mystica/runSimKinRel.m @@ -47,7 +47,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); From e2e6a21a11db39e23a7894581ea1585c286bb7c3 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Thu, 16 Jun 2022 14:48:24 +0200 Subject: [PATCH 03/19] implement opti without to_function --- .../@IntegratorDynRel/IntegratorDynRel.m | 9 +++++--- +mystica/+model/@Model/Model.m | 6 ++--- .../@StateDynMBody/getDynamicQuantities.m | 7 ++++++ .../get_mBodyVelAcc0_from_motorsCurrent.m | 23 ++++++++++++++----- .../@StateKinMBody/getJacobianConstraints.m | 2 +- +mystica/runSimDynRel.m | 1 + 6 files changed, 35 insertions(+), 13 deletions(-) diff --git a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m index c9be127..0062c95 100644 --- a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m +++ b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m @@ -55,7 +55,8 @@ 'kBaum',obj.dxdtParam.baumgarteIntegralCoefficient,... 'model',input.model,... 'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,... - 'solverTechnique',obj.dxdtOpts.solverTechnique); + 'solverTechnique',obj.dxdtOpts.solverTechnique,... + 't',t); dxdt = @(t,x) obj.mBodyVelAcc_0; else dxdt = @(t,x) input.stateDynMBody.get_mBodyVelAcc0_from_motorsCurrent(... @@ -64,7 +65,8 @@ 'model',input.model,... 'kFeedbackJcV',obj.dxdtParam.feedbackJacobianConstraintsV,... 'mBodyPosVel_0_warningOnlyIfNecessary',x,... - 'solverTechnique',obj.dxdtOpts.solverTechnique); + 'solverTechnique',obj.dxdtOpts.solverTechnique,... + 't',t); end otherwise error('not valid solver') @@ -74,7 +76,8 @@ 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) + input.stateDynMBody.setMBodyPosVel( 'model',input.model,'mBodyPosVel_0' ,obj.mBodyPosVel_0) + input.stateDynMBody.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel)); 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/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index efbaeb2..c4b890e 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -179,6 +179,7 @@ function getDynamicQuantities(obj,model,stgsIntegrator) E = (Jc*dV+dJc*V)+(kpFeedbackJcV*Jc*V)+(kiFeedbackJcV*intJcV); opti.minimize(E'*E); opti.subject_to(M*dV==W+Jc'*f); + opti.subject_to(E==0); optFun = opti.to_function('mBodyVelAcc',{mBodyPosVel,motorsCurrent},{x}); % X = optFun(mBodyPosVel,motorsCurrent); @@ -193,4 +194,10 @@ function getDynamicQuantities(obj,model,stgsIntegrator) {'mBodyPosVel','motorsCurrent'},... {'mBodyVelAcc_0','jointsWrenchConstr_PJ'}); + % alternative for computing [dV;f] + 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 index 222b003..d3300df 100644 --- a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -66,7 +66,23 @@ % Optimization (CasADi) %-----------------------------------------------------------------% - [mBodyVelAcc_0,jointsWrenchConstr_PJ] = obj.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,input.motorsCurrent); + if 0 + [mBodyVelAcc_0,jointsWrenchConstr_PJ] = obj.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,input.motorsCurrent); + 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(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); + + mBodyTwAcc_0 = X(1:input.model.constants.mBodyTwist,1); + jointsWrenchConstr_PJ = X(input.model.constants.mBodyTwist+1:end,1); + mBodyVelAcc_0 = [mBodyVelQuat_0 ; mBodyTwAcc_0]; + + obj.setMBodyPosQuat('mBodyPosQuat_0',obj.mBodyPosQuat_0,'model',input.model); + end mBodyVelAcc_0 = full(mBodyVelAcc_0); jointsWrenchConstr_PJ = full(jointsWrenchConstr_PJ); @@ -83,9 +99,4 @@ varargout{2} = mBodyWrenchExt_0; end - k = 1e3; - if input.t*k-floor(input.t*k)<1e-2 - disp(input.t) - end - end diff --git a/+mystica/+state/@StateKinMBody/getJacobianConstraints.m b/+mystica/+state/@StateKinMBody/getJacobianConstraints.m index b2188ff..87eb1c3 100644 --- a/+mystica/+state/@StateKinMBody/getJacobianConstraints.m +++ b/+mystica/+state/@StateKinMBody/getJacobianConstraints.m @@ -87,6 +87,6 @@ function getJacobianConstraints(obj,model) 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/runSimDynRel.m b/+mystica/runSimDynRel.m index 70ffe4d..1f53b20 100644 --- a/+mystica/runSimDynRel.m +++ b/+mystica/runSimDynRel.m @@ -47,6 +47,7 @@ motorsCurrent = contr.solve('stateDynMBody',stateDyn,'time',tout(k),'model',model) * stgs.controller.applyControlInput; % Integrator mBodyPosVel_0 = intgr.integrate('stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,'model',model); + stateDyn.setMBodyPosVel('mBodyPosVel_0',mBodyPosVel_0,'model',model); stateDyn.setMBodyPosQuat('mBodyPosQuat_0',stateDyn.mBodyPosQuat_0,'model',model); % Logger data.store('indexIteration',k,'time',tout(k),'model',model,'controller',contr,'stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,... From d341b11164d89609619c8356434e586f6713297c Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Fri, 17 Jun 2022 17:37:30 +0200 Subject: [PATCH 04/19] Follow PR#12 --- +mystica/+state/@StateDynMBody/getDynamicQuantities.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index c4b890e..e4ed759 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -64,7 +64,7 @@ function getDynamicQuantities(obj,model,stgsIntegrator) for i = 1 : model.nLink mass = model.linksAttributes{i}.mass; - rotm_0_b = mystica.rbm.getRotmGivenTform(obj.linksState{i}.csdFn.tform_0_b(obj.csdSy.mBodyPosQuat_0)); + rotm_0_b = mystica.rbm.getRotmGivenTform(obj.get_link_tform_b('iLink',i,'model',model,'mBodyPosQuat_0',obj.csdSy.mBodyPosQuat_0)); pos_b_g = mystica.rbm.getPosGivenTform(model.linksAttributes{i}.tform_b_g); inertiaTens_0_b = rotm_0_b * model.linksAttributes{i}.inertiaTens_b_b * transpose(rotm_0_b); sel_twist = model.linksAttributes{i}.selector.matrix_linkTwist_from_mBodyTwist; From d1dd60097a3ffb4a03167c8e7bab58ac3602db21 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Fri, 17 Jun 2022 17:48:05 +0200 Subject: [PATCH 05/19] Follow PR #10 --- .../@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m | 4 ++-- +mystica/+state/@StateKinMBody/getKinematicQuantities.m | 2 +- .../@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m | 2 +- .../@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m index d3300df..792e52d 100644 --- a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -36,7 +36,7 @@ kiFeedbackJcV = input.kFeedbackJcV(2); mBodyTwist_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); - mBodyVelQuat_0 = sparse(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); + mBodyVelQuat_0 = sparse(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); Jc = obj.Jc( obj.linIndRowJc,:); dJc = sparse(obj.csdFn.dJc(obj.mBodyPosVel_0)); @@ -75,7 +75,7 @@ X = sol.value(obj.optiVar.X); mBodyTwist_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel); - mBodyVelQuat_0 = sparse(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); + 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); jointsWrenchConstr_PJ = X(input.model.constants.mBodyTwist+1:end,1); 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 From 3a7a8181ce79fa115dbfdddd03768fb94fb8e6fc Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Fri, 17 Jun 2022 19:12:55 +0200 Subject: [PATCH 06/19] merge setMBodyPosQuat and PosVel add 'reduced' for avoiding nullspace computation fix --- .../+intgr/@IntegratorDynRel/IntegratorDynRel.m | 2 -- +mystica/+state/@StateDynMBody/StateDynMBody.m | 17 ++--------------- .../get_mBodyVelAcc0_from_motorsCurrent.m | 15 ++++++++------- .../+state/@StateKinMBody/setMBodyPosQuat.m | 17 ++++++++++++----- +mystica/runSimDynRel.m | 3 --- 5 files changed, 22 insertions(+), 32 deletions(-) diff --git a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m index 0062c95..2347116 100644 --- a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m +++ b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m @@ -77,10 +77,8 @@ % 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) - input.stateDynMBody.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel)); end - end end diff --git a/+mystica/+state/@StateDynMBody/StateDynMBody.m b/+mystica/+state/@StateDynMBody/StateDynMBody.m index 941a5a7..440549c 100644 --- a/+mystica/+state/@StateDynMBody/StateDynMBody.m +++ b/+mystica/+state/@StateDynMBody/StateDynMBody.m @@ -29,9 +29,7 @@ obj.csdSy.mBodyPosVel_0 = casadi.SX.sym('chi',input.model.constants.mBodyPosVel,1); obj.getDynamicQuantities(input.model,input.stgsIntegrator); - obj.setMBodyPosVel('mBodyPosVel_0',[input.mBodyPosQuat_0;input.mBodyTwist_0],'model',input.model) - obj.setMBodyPosQuat('mBodyPosQuat_0',obj.mBodyPosQuat_0,'model',input.model); end [mBodyVelAcc_0,varargout] = get_mBodyVelAcc0_from_motorsCurrent(obj,input,stgs) @@ -41,22 +39,11 @@ function setMBodyPosVel(obj,input) obj mystica.state.StateDynMBody input.mBodyPosVel_0 (:,1) input.model mystica.model.Model + input.method = 'full' end obj.mBodyPosVel_0 = input.mBodyPosVel_0; - - switch 'reduceComputationOfQuantities_mBodyPosQuat_Depedent' - case 'computeQuantities_mBodyPosQuat_Depedent' - obj.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel)) - case 'reduceComputationOfQuantities_mBodyPosQuat_Depedent' - % remove obj.setMBodyPosQuat to reduce computational time (see https://github.com/ami-iit/element_morphing-cover-design/commit/804e87f9b8d9dc144e09a0fc51e1e79b38e38a3a) - obj.mBodyPosQuat_0 = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel); - obj.Jc = []; - obj.referenceConversion.from_mBodyTwist0_2_jointsAngVelPJ = []; - obj.nullJc_mBodyTwist_0 = []; - obj.nullJc_jointsAngVel_PJ = []; - obj.linIndRowJc = []; - end + obj.setMBodyPosQuat('model',input.model,'mBodyPosQuat_0',obj.mBodyPosVel_0(input.model.selector.indexes_mBodyPosQuat_from_mBodyPosVel),'method',input.method) end diff --git a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m index 792e52d..bacddba 100644 --- a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -19,10 +19,6 @@ % - Compute `mBodyVelAcc0` and `jointsWrenchConstr_PJ` %---------------------------------------------------------------------% - if isfield(input,'mBodyPosVel_0_warningOnlyIfNecessary') - obj.setMBodyPosVel('mBodyPosVel_0',input.mBodyPosVel_0_warningOnlyIfNecessary,'model',input.model) - end - switch stgs.solverTechnique case 'inv' @@ -30,7 +26,9 @@ % Inverse %-----------------------------------------------------------------% - obj.setMBodyPosQuat('mBodyPosQuat_0',obj.mBodyPosQuat_0,'model',input.model); + 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); @@ -66,6 +64,11 @@ % 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,jointsWrenchConstr_PJ] = obj.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,input.motorsCurrent); else @@ -80,8 +83,6 @@ mBodyTwAcc_0 = X(1:input.model.constants.mBodyTwist,1); jointsWrenchConstr_PJ = X(input.model.constants.mBodyTwist+1:end,1); mBodyVelAcc_0 = [mBodyVelQuat_0 ; mBodyTwAcc_0]; - - obj.setMBodyPosQuat('mBodyPosQuat_0',obj.mBodyPosQuat_0,'model',input.model); end mBodyVelAcc_0 = full(mBodyVelAcc_0); 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/runSimDynRel.m b/+mystica/runSimDynRel.m index 1f53b20..aa76f71 100644 --- a/+mystica/runSimDynRel.m +++ b/+mystica/runSimDynRel.m @@ -47,14 +47,11 @@ motorsCurrent = contr.solve('stateDynMBody',stateDyn,'time',tout(k),'model',model) * stgs.controller.applyControlInput; % Integrator mBodyPosVel_0 = intgr.integrate('stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,'model',model); - stateDyn.setMBodyPosVel('mBodyPosVel_0',mBodyPosVel_0,'model',model); - stateDyn.setMBodyPosQuat('mBodyPosQuat_0',stateDyn.mBodyPosQuat_0,'model',model); % Logger data.store('indexIteration',k,'time',tout(k),'model',model,'controller',contr,'stateDynMBody',stateDyn,'motorsCurrent',motorsCurrent,... 'stgsDesiredShape',stgs.desiredShape) % New State stateDyn.setMBodyPosVel('mBodyPosVel_0',mBodyPosVel_0,'model',model); - stateDyn.setMBodyPosQuat('mBodyPosQuat_0',stateDyn.mBodyPosQuat_0,'model',model); end %% Saving Workspace From e3585e3c4223c176768c295e04a0442c05b9c358 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Sat, 18 Jun 2022 12:04:01 +0200 Subject: [PATCH 07/19] implement code-generation sim-dyn fix --- +mystica/+log/@LoggerDynRel/LoggerDynRel.m | 10 ++-- .../+state/@StateDynMBody/StateDynMBody.m | 42 +++++++++++++++++ .../@StateDynMBody/getDynamicQuantities.m | 13 ++++-- .../get_mBodyVelAcc0_from_motorsCurrent.m | 6 +-- .../+state/@StateKinMBody/StateKinMBody.m | 46 +++++++++++-------- 5 files changed, 84 insertions(+), 33 deletions(-) diff --git a/+mystica/+log/@LoggerDynRel/LoggerDynRel.m b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m index 70ac6d9..4cd2a86 100644 --- a/+mystica/+log/@LoggerDynRel/LoggerDynRel.m +++ b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m @@ -86,11 +86,11 @@ function store(obj,input) mBodyWrench_0 = input.stateDynMBody.csdFn.mBodyWrench_0('mBodyPosVel_0',input.stateDynMBody.mBodyPosVel_0,'motorsCurrent',input.motorsCurrent); - obj.mBodyWrenchExt_0(:,obj.indexIteration) = full(mBodyWrench_0.ext); - obj.mBodyWrenchGra_0(:,obj.indexIteration) = full(mBodyWrench_0.gravity); - obj.mBodyWrenchCor_0(:,obj.indexIteration) = full(mBodyWrench_0.coriolis); - obj.mBodyWrenchFri_0(:,obj.indexIteration) = full(mBodyWrench_0.friction); - obj.mBodyWrenchInp_0(:,obj.indexIteration) = full(mBodyWrench_0.input); + 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; diff --git a/+mystica/+state/@StateDynMBody/StateDynMBody.m b/+mystica/+state/@StateDynMBody/StateDynMBody.m index 440549c..734ec92 100644 --- a/+mystica/+state/@StateDynMBody/StateDynMBody.m +++ b/+mystica/+state/@StateDynMBody/StateDynMBody.m @@ -29,6 +29,9 @@ obj.csdSy.mBodyPosVel_0 = casadi.SX.sym('chi',input.model.constants.mBodyPosVel,1); obj.getDynamicQuantities(input.model,input.stgsIntegrator); + % generate MEX file containing (dyn) casadi funtions + obj.generateMEX_dyn(); + % Evaluate initial state obj.setMBodyPosVel('mBodyPosVel_0',[input.mBodyPosQuat_0;input.mBodyTwist_0],'model',input.model) end @@ -47,8 +50,47 @@ function setMBodyPosVel(obj,input) end + function generateMEX(obj) + obj.generateMEX@mystica.state.StateKinMBody(); + obj.generateMEX_dyn(); + end + end methods (Access=protected) getDynamicQuantities(obj,model,stgsIntegrator) end + methods (Access=private) + function generateMEX_dyn(obj) + nameMEX = 'mystica_stateDyn'; + opts = struct('main', true,'mex', true); + initial_path = pwd; + cd(fullfile(mystica.utils.getMysticaFullPath,'deps','csdMEX')); + C = casadi.CodeGenerator([nameMEX,'.c'],opts); + C.add(obj.csdFn.dJc); + C.add(obj.csdFn.massMatrix); + C.add(obj.csdFn.massMatrixMotorInertia); + C.add(obj.csdFn.mBodyWrenchExt_0); + C.add(obj.csdFn.mBodyWrenchGra_0); + C.add(obj.csdFn.mBodyWrenchCor_0); + C.add(obj.csdFn.mBodyWrenchFri_0); + C.add(obj.csdFn.mBodyWrenchInp_0); + C.add(obj.csdFn.from_motorsCurrent_2_mBodyWrenchInp0); + %C.add(obj.csdFn.mBodyVelAcc_0); osqp cannot be code-generated + C.generate(); + fileID = fopen([nameMEX,'.c'] ,'r'); new_code = fscanf(fileID,'%s'); fclose(fileID); + if exist([nameMEX,'_old.c'],'file') + fileID = fopen([nameMEX,'_old.c'],'r'); old_code = fscanf(fileID,'%s'); fclose(fileID); + else + old_code = ''; + end + if ~strcmp(new_code,old_code) || isempty(dir([nameMEX,'.mex*'])) + fprintf('generating %s.mex\n',nameMEX) + mex([nameMEX,'.c'],'-largeArrayDims') + else + fprintf('%s.mex already exists\n',nameMEX) + end + movefile([nameMEX,'.c'],[nameMEX,'_old.c']) + cd(initial_path) + end + end end diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index e4ed759..693fdac 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -101,8 +101,8 @@ function getDynamicQuantities(obj,model,stgsIntegrator) massMatrix = massMatrixWithoutMotorInertia + massMatrixMotorInertia; obj.csdFn.dJc = casadi.Function('dJc',{obj.csdSy.mBodyPosVel_0 },{obj.csdSy.dJc}); - obj.csdFn.massMatrix = casadi.Function('M',{ obj.csdSy.mBodyPosQuat_0},{massMatrix}); - obj.csdFn.massMatrixMotorInertia = casadi.Function('Mmi',{obj.csdSy.mBodyPosQuat_0},{massMatrixMotorInertia}); + obj.csdFn.massMatrix = casadi.Function('massMatrix',{ obj.csdSy.mBodyPosQuat_0},{massMatrix}); + obj.csdFn.massMatrixMotorInertia = casadi.Function('massMatrixMotorInertia',{obj.csdSy.mBodyPosQuat_0},{massMatrixMotorInertia}); %% mBodyWrenchExt_0, mBodyWrenchCor_0, mBodyWrenchGra_0, mBodyWrenchFri_0, mBodyWrenchInp_0 @@ -121,8 +121,11 @@ function getDynamicQuantities(obj,model,stgsIntegrator) wrenchTotal = -wrenchGravity-wrenchCoriolis+wrenchFriction+wrenchCurrent; - obj.csdFn.mBodyWrenchExt_0 = casadi.Function('wE',{ obj.csdSy.mBodyPosVel_0,obj.csdSy.motorsCurrent},{wrenchTotal}); - obj.csdFn.mBodyWrenchGra_0 = casadi.Function('wEg',{obj.csdSy.mBodyPosQuat_0},{wrenchGravity}); + obj.csdFn.mBodyWrenchExt_0 = casadi.Function('mBodyWrenchExt_0',{obj.csdSy.mBodyPosVel_0,obj.csdSy.motorsCurrent},{wrenchTotal},{'mBodyPosVel_0','motorsCurrent'},{'mBodyWrenchExt_0'}); + obj.csdFn.mBodyWrenchGra_0 = casadi.Function('mBodyWrenchGra_0',{obj.csdSy.mBodyPosQuat_0},{wrenchGravity},{'mBodyPosQuat_0'},{'mBodyWrenchGra_0'}); + obj.csdFn.mBodyWrenchCor_0 = casadi.Function('mBodyWrenchCor_0',{obj.csdSy.mBodyPosVel_0},{wrenchCoriolis},{'mBodyPosVel_0'},{'mBodyWrenchCor_0'}); + obj.csdFn.mBodyWrenchFri_0 = casadi.Function('mBodyWrenchFri_0',{obj.csdSy.mBodyPosVel_0},{wrenchFriction},{'mBodyPosVel_0'},{'mBodyWrenchFri_0'}); + obj.csdFn.mBodyWrenchInp_0 = casadi.Function('mBodyWrenchInp_0',{obj.csdSy.mBodyPosQuat_0,obj.csdSy.motorsCurrent},{wrenchCurrent},{'mBodyPosQuat_0','motorsCurrent'},{'mBodyWrenchInp_0'}); obj.csdFn.mBodyWrench_0 = casadi.Function('mBodyWrench_0',... {obj.csdSy.mBodyPosVel_0,obj.csdSy.motorsCurrent},... %input @@ -130,7 +133,7 @@ function getDynamicQuantities(obj,model,stgsIntegrator) {'mBodyPosVel_0','motorsCurrent'},... %label input {'ext','coriolis','gravity','friction','input'}); %label output - obj.csdFn.from_motorsCurrent_2_mBodyWrenchCur = casadi.Function('B',{obj.csdSy.mBodyPosQuat_0},{B * rC_0_PJ * sel_wAct' * Ki}); + obj.csdFn.from_motorsCurrent_2_mBodyWrenchInp0 = casadi.Function('from_motorsCurrent_2_mBodyWrenchInp0',{obj.csdSy.mBodyPosQuat_0},{B * rC_0_PJ * sel_wAct' * Ki}); %% mBodyVelAcc0 diff --git a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m index bacddba..9604a18 100644 --- a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -37,13 +37,13 @@ mBodyVelQuat_0 = sparse(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum)); Jc = obj.Jc( obj.linIndRowJc,:); - dJc = sparse(obj.csdFn.dJc(obj.mBodyPosVel_0)); + dJc = sparse(mystica_stateDyn('dJc',obj.mBodyPosVel_0)); dJc = dJc(obj.linIndRowJc,:); intJcV = obj.getIntJcV; intJcV = intJcV( obj.linIndRowJc,:); V = mBodyTwist_0; - M = sparse(obj.csdFn.massMatrix(obj.mBodyPosQuat_0)); % massMatrix -> M - W = sparse(obj.csdFn.mBodyWrenchExt_0(obj.mBodyPosVel_0,input.motorsCurrent)); % mBodyWrenchExt_0 -> W + 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; 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) From 6d0b6cd48e27ccbd70b849b1e709678920a31d1d Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Tue, 5 Jul 2022 14:37:17 +0200 Subject: [PATCH 08/19] document integrator --- +mystica/+intgr/@Integrator/Integrator.m | 2 +- +mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m | 3 +-- +mystica/+intgr/@IntegratorKinAbs/IntegratorKinAbs.m | 3 +-- +mystica/+intgr/@IntegratorKinRel/IntegratorKinRel.m | 3 +-- +mystica/+log/@LoggerDynRel/LoggerDynRel.m | 3 --- +mystica/+state/@StateDynMBody/getDynamicQuantities.m | 2 +- 6 files changed, 5 insertions(+), 11 deletions(-) diff --git a/+mystica/+intgr/@Integrator/Integrator.m b/+mystica/+intgr/@Integrator/Integrator.m index 988a9cf..da358e4 100644 --- a/+mystica/+intgr/@Integrator/Integrator.m +++ b/+mystica/+intgr/@Integrator/Integrator.m @@ -87,7 +87,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 index 2347116..7407d07 100644 --- a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m +++ b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m @@ -74,8 +74,7 @@ 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 + % 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 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/+log/@LoggerDynRel/LoggerDynRel.m b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m index 4cd2a86..3630f23 100644 --- a/+mystica/+log/@LoggerDynRel/LoggerDynRel.m +++ b/+mystica/+log/@LoggerDynRel/LoggerDynRel.m @@ -83,9 +83,6 @@ function store(obj,input) % massMatrix mBodyTwAcc_0 = mBodyWrenchExt + mBodyWrenchJcF_0 % % mBodyWrenchExt = - wrenchGravity - wrenchCoriolis + wrenchFriction + wrenchInput - - mBodyWrench_0 = input.stateDynMBody.csdFn.mBodyWrench_0('mBodyPosVel_0',input.stateDynMBody.mBodyPosVel_0,'motorsCurrent',input.motorsCurrent); - 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)); diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index 693fdac..95f526f 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -182,7 +182,7 @@ function getDynamicQuantities(obj,model,stgsIntegrator) E = (Jc*dV+dJc*V)+(kpFeedbackJcV*Jc*V)+(kiFeedbackJcV*intJcV); opti.minimize(E'*E); opti.subject_to(M*dV==W+Jc'*f); - opti.subject_to(E==0); +% opti.subject_to(E==0); optFun = opti.to_function('mBodyVelAcc',{mBodyPosVel,motorsCurrent},{x}); % X = optFun(mBodyPosVel,motorsCurrent); From 2a544678777f06dfd1c2b90daf828f3a174fe9ee Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Thu, 7 Jul 2022 11:30:45 +0200 Subject: [PATCH 09/19] implement smoothed coulomb friction --- .../+state/@StateDynMBody/StateDynMBody.m | 5 +++-- .../@StateDynMBody/getDynamicQuantities.m | 10 +++++++--- +mystica/+stgs/getDefaultSettingsSimDynRel.m | 10 +++++++++- +mystica/+utils/smoothSign.m | 19 +++++++++++++++++++ +mystica/runSimDynRel.m | 3 ++- 5 files changed, 40 insertions(+), 7 deletions(-) create mode 100644 +mystica/+utils/smoothSign.m diff --git a/+mystica/+state/@StateDynMBody/StateDynMBody.m b/+mystica/+state/@StateDynMBody/StateDynMBody.m index 734ec92..d3daad4 100644 --- a/+mystica/+state/@StateDynMBody/StateDynMBody.m +++ b/+mystica/+state/@StateDynMBody/StateDynMBody.m @@ -18,6 +18,7 @@ input.mBodyTwist_0 input.stgsStateDynMBody input.stgsIntegrator + input.stgsModel end obj@mystica.state.StateKinMBody('model',input.model,... @@ -28,7 +29,7 @@ obj.csdSy.motorsCurrent = casadi.SX.sym('I',input.model.constants.motorsAngVel,1); obj.csdSy.mBodyPosVel_0 = casadi.SX.sym('chi',input.model.constants.mBodyPosVel,1); - obj.getDynamicQuantities(input.model,input.stgsIntegrator); + obj.getDynamicQuantities(input.model,input.stgsIntegrator,input.stgsModel); % generate MEX file containing (dyn) casadi funtions obj.generateMEX_dyn(); % Evaluate initial state @@ -57,7 +58,7 @@ function generateMEX(obj) end methods (Access=protected) - getDynamicQuantities(obj,model,stgsIntegrator) + getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) end methods (Access=private) function generateMEX_dyn(obj) diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index 95f526f..d82b5eb 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -1,4 +1,4 @@ -function getDynamicQuantities(obj,model,stgsIntegrator) +function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) %% Compute Casadi Symbolic Variables @@ -35,7 +35,9 @@ function getDynamicQuantities(obj,model,stgsIntegrator) Kv = model.joints{j}.coefficients.viscousFriction(indexTemp,indexTemp); Jm = model.joints{j}.inertiaTensMotorRot_j_g(indexTemp,indexTemp); % friction actuated axis of rotation - frictionActAngVel{j} = - (Kv * gamma.^2 * eff * actAxesAngVelPJ + Kc * gamma * eff * sign(actAxesAngVelPJ)); + viscousFriction = - Kv * gamma.^2 * eff * actAxesAngVelPJ; + coulombFriction = - Kc * gamma * eff * mystica.utils.smoothSign(actAxesAngVelPJ,'method',stgsModel.friction.coulomb.smoothSign.method,'settlingTime',stgsModel.friction.coulomb.smoothSign.settlingTime,'toleranceBands',stgsModel.friction.coulomb.smoothSign.toleranceBands); + frictionActAngVel{j} = viscousFriction*stgsModel.friction.viscous.apply + coulombFriction*stgsModel.friction.coulomb.apply; Ki{j} = eff * gamma * Kt; Kdw{j} = Jm * gamma.^2 * eff; end @@ -45,7 +47,9 @@ function getDynamicQuantities(obj,model,stgsIntegrator) Kc = model.joints{j}.coefficients.coulombFriction(indexTemp,indexTemp); Kv = model.joints{j}.coefficients.viscousFriction(indexTemp,indexTemp); % friction passive axis of rotation (actuated joints + passive joints) - frictionPasAngVel{j} = - (Kv * pasAxesAngVelPJ + Kc * sign(pasAxesAngVelPJ)); + viscousFriction = - Kv * pasAxesAngVelPJ; + coulombFriction = - Kc * mystica.utils.smoothSign(pasAxesAngVelPJ,'method',stgsModel.friction.coulomb.smoothSign.method,'settlingTime',stgsModel.friction.coulomb.smoothSign.settlingTime,'toleranceBands',stgsModel.friction.coulomb.smoothSign.toleranceBands); + frictionPasAngVel{j} = viscousFriction*stgsModel.friction.viscous.apply + coulombFriction*stgsModel.friction.coulomb.apply; end end diff --git a/+mystica/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m index c3d39e4..aede0f0 100644 --- a/+mystica/+stgs/getDefaultSettingsSimDynRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimDynRel.m @@ -24,7 +24,15 @@ stgs.saving.workspace.name = ['dynRel_',model.name,'_',strTime,'.mat']; stgs.saving.workspace.clearCasadi = 0; - %% StateKin Settings + %% 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'; 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/runSimDynRel.m b/+mystica/runSimDynRel.m index aa76f71..3a77332 100644 --- a/+mystica/runSimDynRel.m +++ b/+mystica/runSimDynRel.m @@ -20,7 +20,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,... From b7f3895f9e6ea6ef22551431994e6d8b5eeb0edc Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Fri, 15 Jul 2022 15:16:40 +0200 Subject: [PATCH 10/19] `opt`: change scaling and implement qpoases --- .../@StateDynMBody/getDynamicQuantities.m | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index d82b5eb..026ec01 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -145,19 +145,22 @@ function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) kiFeedbackJcV = stgsIntegrator.dxdtParam.feedbackJacobianConstraintsV(2); switch stgsIntegrator.dxdtOpts.optOpts.name + case 'qpoases' + opti = casadi.Opti('conic'); + p_opts = struct('expand',true,'error_on_fail',false,'printLevel','none'); + s_opts = struct(); case 'osqp' opti = casadi.Opti('conic'); p_opts = struct('expand',true,'error_on_fail',false); - s_opts = struct(); - opti.solver(stgsIntegrator.dxdtOpts.optOpts.name,p_opts,s_opts); + s_opts = struct('adaptive_rho',1); case 'ipopt' opti = casadi.Opti(); p_opts = struct('expand',true,'error_on_fail',false,'print_time',false); s_opts = struct('print_level',0); - opti.solver(stgsIntegrator.dxdtOpts.optOpts.name,p_opts,s_opts); otherwise error('solver not valid') end + opti.solver(stgsIntegrator.dxdtOpts.optOpts.name,p_opts,s_opts); % scaling opti variable k_dV = ones(model.constants.mBodyTwist,1); @@ -168,9 +171,12 @@ function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) k_f = ones(model.constants.nConstraints,1); k_f(model.selector.indexes_constrainedAngVel_from_JcV) = (model.linksAttributes{1}.mass*model.linksAttributes{1}.linkDimension^2); k_f(model.selector.indexes_constrainedLinVel_from_JcV) = (model.linksAttributes{1}.mass*model.linksAttributes{1}.linkDimension); + k_f = diag(k_f); + k_dV = diag(k_dV); + % Variable definitions - dV = opti.variable(model.constants.mBodyTwist ,1).*k_dV; % mBodyTwAcc_0 -> dV - f = opti.variable(model.constants.nConstraints,1).*k_f; % jointsWrenchConstr_pj -> f + dV = k_dV*opti.variable(model.constants.mBodyTwist ,1); % mBodyTwAcc_0 -> dV + f = k_f*opti.variable(model.constants.nConstraints,1); % jointsWrenchConstr_pj -> f x = [dV;f]; mBodyPosVel = opti.parameter(model.constants.mBodyPosVel ,1); motorsCurrent = opti.parameter(model.constants.motorsAngVel,1); @@ -184,9 +190,10 @@ function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) 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(E'*E); - opti.subject_to(M*dV==W+Jc'*f); -% opti.subject_to(E==0); + opti.subject_to(D==0); + opti.subject_to(E==0); optFun = opti.to_function('mBodyVelAcc',{mBodyPosVel,motorsCurrent},{x}); % X = optFun(mBodyPosVel,motorsCurrent); From 17697d5128f720d6fb460d22dcbf7e7757b6ccc7 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti <38210073+FabioBergonti@users.noreply.github.com> Date: Fri, 15 Jul 2022 17:14:14 +0200 Subject: [PATCH 11/19] Implement fixed step integrators (#13) - ode1 - ode2 - ode4 --- +mystica/+intgr/@Integrator/Integrator.m | 2 +- .../@IntegratorDynRel/IntegratorDynRel.m | 3 +- +mystica/+intgr/ode1.m | 26 +++++++++++++++++ +mystica/+intgr/ode2.m | 26 +++++++++++++++++ +mystica/+intgr/ode4.m | 28 +++++++++++++++++++ +mystica/+stgs/getDefaultSettingsSimDynRel.m | 7 +++-- +mystica/+stgs/getDefaultSettingsSimKinAbs.m | 7 +++-- +mystica/+stgs/getDefaultSettingsSimKinRel.m | 7 +++-- 8 files changed, 95 insertions(+), 11 deletions(-) create mode 100644 +mystica/+intgr/ode1.m create mode 100644 +mystica/+intgr/ode2.m create mode 100644 +mystica/+intgr/ode4.m diff --git a/+mystica/+intgr/@Integrator/Integrator.m b/+mystica/+intgr/@Integrator/Integrator.m index da358e4..edd007f 100644 --- a/+mystica/+intgr/@Integrator/Integrator.m +++ b/+mystica/+intgr/@Integrator/Integrator.m @@ -57,7 +57,7 @@ obj.xf = full(r.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 diff --git a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m index 7407d07..a769af9 100644 --- a/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m +++ b/+mystica/+intgr/@IntegratorDynRel/IntegratorDynRel.m @@ -48,7 +48,8 @@ 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 + 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,... 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/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m index aede0f0..a7b8aff 100644 --- a/+mystica/+stgs/getDefaultSettingsSimDynRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimDynRel.m @@ -43,9 +43,10 @@ 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 = 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) diff --git a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m index 0957ec1..c58a450 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m @@ -35,9 +35,10 @@ 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 = 0; stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1; diff --git a/+mystica/+stgs/getDefaultSettingsSimKinRel.m b/+mystica/+stgs/getDefaultSettingsSimKinRel.m index ec6b29c..c651507 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinRel.m @@ -35,9 +35,10 @@ 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 = 0; stgs.integrator.dxdtParam.baumgarteIntegralCoefficient = 5e1; From 5308f6c8f9b8441eba532f56a343037d12a87a40 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Fri, 15 Jul 2022 18:04:56 +0200 Subject: [PATCH 12/19] Add class to measure computational time --- +mystica/+utils/MeasurePerformance.m | 18 ++++++++++++++++++ +mystica/runSimDynRel.m | 8 ++++++-- +mystica/runSimKinAbs.m | 8 ++++++-- +mystica/runSimKinRel.m | 8 ++++++-- 4 files changed, 36 insertions(+), 6 deletions(-) create mode 100644 +mystica/+utils/MeasurePerformance.m 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/runSimDynRel.m b/+mystica/runSimDynRel.m index 3a77332..ba33f59 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); @@ -55,9 +57,11 @@ stateDyn.setMBodyPosVel('mBodyPosVel_0',mBodyPosVel_0,'model',model); 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 98ea73c..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); @@ -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 9561d6c..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); @@ -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 From 4f5f723fb09d80ae272ad304660e6e5b6df87e31 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti <38210073+FabioBergonti@users.noreply.github.com> Date: Tue, 2 Aug 2022 11:11:54 +0200 Subject: [PATCH 13/19] Add the possibility to use the official casadi binaries (#14) --- install.m | 113 +++++++++++++++++++++++++++++++++--------------------- 1 file changed, 69 insertions(+), 44 deletions(-) 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 From 154cd67f80a78246486795c44b78acb55642d721 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti <38210073+FabioBergonti@users.noreply.github.com> Date: Mon, 8 Aug 2022 14:44:45 +0200 Subject: [PATCH 14/19] Add angular contribution in the computation of the integral of Jc (#16) --- +mystica/+rbm/getQuatGivenRotm.m | 18 +++++ +mystica/+rbm/getRotmGivenQuat.m | 6 +- +mystica/+rbm/invQuat.m | 6 ++ +mystica/+rbm/logQuat.m | 19 +++++ +mystica/+rbm/multiplyQuat.m | 10 +++ .../@StateKinMBody/getJacobianConstraints.m | 71 ++++++++++++------- +mystica/+utils/skewVee.m | 19 +++-- 7 files changed, 108 insertions(+), 41 deletions(-) create mode 100644 +mystica/+rbm/invQuat.m create mode 100644 +mystica/+rbm/logQuat.m create mode 100644 +mystica/+rbm/multiplyQuat.m 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 Date: Mon, 8 Aug 2022 14:52:27 +0200 Subject: [PATCH 15/19] Implement pseudoinverse inside NullSpace class (#15) --- +mystica/+utils/@NullSpace/NullSpace.m | 67 ++++++++++++++++++++------ 1 file changed, 53 insertions(+), 14 deletions(-) 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 From e0dc918dd325fc5f1c63ddf3519bdbe93bbe78a0 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti <38210073+FabioBergonti@users.noreply.github.com> Date: Tue, 30 Aug 2022 10:06:04 +0200 Subject: [PATCH 16/19] Implement `opt` using Gauss Method (#17) --- .../@StateDynMBody/getDynamicQuantities.m | 19 ++++++++----------- .../get_mBodyVelAcc0_from_motorsCurrent.m | 15 +++++++++++---- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m index 026ec01..cac4331 100644 --- a/+mystica/+state/@StateDynMBody/getDynamicQuantities.m +++ b/+mystica/+state/@StateDynMBody/getDynamicQuantities.m @@ -147,7 +147,7 @@ function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) switch stgsIntegrator.dxdtOpts.optOpts.name case 'qpoases' opti = casadi.Opti('conic'); - p_opts = struct('expand',true,'error_on_fail',false,'printLevel','none'); + p_opts = struct('expand',true,'error_on_fail',false,'printLevel','none','enableFullLITests',true,'enableCholeskyRefactorisation',1,'numRefinementSteps',3); s_opts = struct(); case 'osqp' opti = casadi.Opti('conic'); @@ -176,8 +176,7 @@ function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) % Variable definitions dV = k_dV*opti.variable(model.constants.mBodyTwist ,1); % mBodyTwAcc_0 -> dV - f = k_f*opti.variable(model.constants.nConstraints,1); % jointsWrenchConstr_pj -> f - x = [dV;f]; + x = dV; mBodyPosVel = opti.parameter(model.constants.mBodyPosVel ,1); motorsCurrent = opti.parameter(model.constants.motorsAngVel,1); % @@ -190,25 +189,23 @@ function getDynamicQuantities(obj,model,stgsIntegrator,stgsModel) 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(E'*E); - opti.subject_to(D==0); + %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(1:model.constants.mBodyTwist,1); - jointsWrenchConstr_PJ = X(model.constants.mBodyTwist+1:end,1); + mBodyTwAcc_0 = X; mBodyVelAcc_0 = [mBodyVelQuat ; mBodyTwAcc_0]; % obj.csdFn.mBodyVelAcc_0 = casadi.Function('mBodyVelAcc_0',... {mBodyPosVel,motorsCurrent},... - {mBodyVelAcc_0,jointsWrenchConstr_PJ},... + {mBodyVelAcc_0},... {'mBodyPosVel','motorsCurrent'},... - {'mBodyVelAcc_0','jointsWrenchConstr_PJ'}); + {'mBodyVelAcc_0'}); - % alternative for computing [dV;f] + % alternative for computing [dV] obj.opti = opti; obj.optiVar.mBodyPosVel = mBodyPosVel; obj.optiVar.motorsCurrent = motorsCurrent; diff --git a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m index 9604a18..3ff94ce 100644 --- a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m +++ b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m @@ -70,7 +70,8 @@ end if 0 - [mBodyVelAcc_0,jointsWrenchConstr_PJ] = obj.csdFn.mBodyVelAcc_0(obj.mBodyPosVel_0,input.motorsCurrent); + 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); @@ -81,12 +82,18 @@ 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); - jointsWrenchConstr_PJ = X(input.model.constants.mBodyTwist+1:end,1); mBodyVelAcc_0 = [mBodyVelQuat_0 ; mBodyTwAcc_0]; end - mBodyVelAcc_0 = full(mBodyVelAcc_0); - jointsWrenchConstr_PJ = full(jointsWrenchConstr_PJ); + 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 From c18efc545f611c9896d5d14025f257bcbc4f3531 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Mon, 10 Jul 2023 17:02:02 +0200 Subject: [PATCH 17/19] logger dynamic simulator - add noise dynamic simulator - compute dxdt - create logger dynamic simulator --- +mystica/+intgr/@Integrator/Integrator.m | 33 +++++++- .../@IntegratorDynRel/IntegratorDynRel.m | 3 +- +mystica/+log/@LoggerDynRel/LoggerDynRel.m | 17 ++++ +mystica/+noise/@NoiseDynRel/NoiseDynRel.m | 77 +++++++++++++++++++ +mystica/+stgs/getDefaultSettingsSimDynRel.m | 14 +++- +mystica/+stgs/getDefaultSettingsSimKinAbs.m | 2 +- +mystica/+stgs/getDefaultSettingsSimKinRel.m | 2 +- +mystica/+viz/visualizeDynRel.m | 3 +- +mystica/runSimDynRel.m | 17 +++- README.md | 3 +- 10 files changed, 156 insertions(+), 15 deletions(-) create mode 100644 +mystica/+noise/@NoiseDynRel/NoiseDynRel.m diff --git a/+mystica/+intgr/@Integrator/Integrator.m b/+mystica/+intgr/@Integrator/Integrator.m index edd007f..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,6 +61,8 @@ 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'}; @@ -62,8 +70,8 @@ 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 index a769af9..1fcf291 100644 --- 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 index 3630f23..2a4a1fb 100644 --- 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 @@ 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); @@ -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 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/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m index a7b8aff..3051d93 100644 --- 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,6 +87,13 @@ 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; @@ -93,7 +101,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 = 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 index c58a450..a37f94d 100644 --- 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 index c651507..1d94c6c 100644 --- 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 index 2a22333..d76a480 100644 --- 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 index ba33f59..2b7a42e 100644 --- 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 index 8d3411f..efc9495 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ 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) @@ -31,6 +31,7 @@ The _maximal_ representation consists of a set of _non-minimum_ variables 𝐪 c ## :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: From 846e5a52c8de3752809987e6be64a1ff62e44404 Mon Sep 17 00:00:00 2001 From: Fabio Bergonti Date: Wed, 12 Jul 2023 16:22:44 +0200 Subject: [PATCH 18/19] cleanup README.md --- README.md | 52 +++++++++++++++++++++++++++------------------------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index efc9495..5670c16 100644 --- a/README.md +++ b/README.md @@ -7,20 +7,20 @@ **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) @@ -30,67 +30,69 @@ The _maximal_ representation consists of a set of _non-minimum_ variables 𝐪 c ## :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 +- [`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 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: From f639146fbfbbc32b9f6779067413316e055ffec6 Mon Sep 17 00:00:00 2001 From: Fabio Date: Thu, 20 Jul 2023 10:56:34 +0200 Subject: [PATCH 19/19] cleanup settings parameters --- +mystica/+stgs/getDefaultSettingsSimDynRel.m | 22 +------------------- +mystica/+stgs/getDefaultSettingsSimKinAbs.m | 6 +----- +mystica/+stgs/getDefaultSettingsSimKinRel.m | 17 ++------------- 3 files changed, 4 insertions(+), 41 deletions(-) diff --git a/+mystica/+stgs/getDefaultSettingsSimDynRel.m b/+mystica/+stgs/getDefaultSettingsSimDynRel.m index 3051d93..db0f078 100644 --- a/+mystica/+stgs/getDefaultSettingsSimDynRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimDynRel.m @@ -63,37 +63,17 @@ stgs.integrator.statusTracker.timeTrackerFile.baseName = ['dynRel_',model.name]; %[char] stgs.integrator.statusTracker.limitMaximumTime = stgs.integrator.limitMaximumTime; - %% Controller [TODO] remove stgs.controller + %% 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; - - 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; diff --git a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m index a37f94d..2f478ea 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinAbs.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinAbs.m @@ -50,21 +50,17 @@ stgs.integrator.statusTracker.timeTrackerFile.baseName = ['kinAbs_',model.name]; %[char] stgs.integrator.statusTracker.limitMaximumTime = stgs.integrator.limitMaximumTime; - %% Controller [TODO] remove stgs.controller + %% 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 1d94c6c..c829c7a 100644 --- a/+mystica/+stgs/getDefaultSettingsSimKinRel.m +++ b/+mystica/+stgs/getDefaultSettingsSimKinRel.m @@ -51,30 +51,17 @@ stgs.integrator.statusTracker.timeTrackerFile.baseName = ['kinRel_',model.name]; %[char] stgs.integrator.statusTracker.limitMaximumTime = stgs.integrator.limitMaximumTime; - %% Controller [TODO] remove stgs.controller + %% 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; @@ -82,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