1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19
20 #include "FirstOrderLinearDS.hpp"
21 #include "TimeStepping.hpp"
22 #include "SiconosAlgebraProd.hpp"
23
24 #include "LinearSMCimproved.hpp"
25 #include "SiconosVector.hpp"
26 #include "ControlSensor.hpp"
27 #include "ZeroOrderHoldOSI.hpp"
28 #include "TimeDiscretisation.hpp"
29 #include "ActuatorFactory.hpp"
30
31 #include <boost/circular_buffer.hpp>
32
LinearSMCimproved(SP::ControlSensor sensor)33 LinearSMCimproved::LinearSMCimproved(SP::ControlSensor sensor):
34 LinearSMC(sensor, LINEAR_SMC_IMPROVED), _predictionPerturbation(false), _inDisceteTimeSlidingPhase(false), _ubPerturbation(0.0)
35 {
36 }
37
LinearSMCimproved(SP::ControlSensor sensor,SP::SimpleMatrix B,SP::SimpleMatrix D)38 LinearSMCimproved::LinearSMCimproved(SP::ControlSensor sensor, SP::SimpleMatrix B, SP::SimpleMatrix D):
39 LinearSMC(sensor, B, D, LINEAR_SMC_IMPROVED), _predictionPerturbation(false), _inDisceteTimeSlidingPhase(false),_ubPerturbation(0.0)
40 {
41 }
42
~LinearSMCimproved()43 LinearSMCimproved::~LinearSMCimproved()
44 {
45 }
46
initialize(const NonSmoothDynamicalSystem & nsds,const Simulation & s)47 void LinearSMCimproved::initialize(const NonSmoothDynamicalSystem& nsds, const Simulation & s)
48 {
49 LinearSMC::initialize(nsds,s);
50 _up.reset(new SiconosVector(_us->size()));
51 _measuredPert.reset(new boost::circular_buffer<SP::SiconosVector>(0));
52 _predictedPert.reset(new boost::circular_buffer<SP::SiconosVector>(0));
53 }
54
predictionPerturbation(const SiconosVector & xTk,SimpleMatrix & CBstar)55 void LinearSMCimproved::predictionPerturbation(const SiconosVector& xTk, SimpleMatrix& CBstar)
56 {
57 if(_us->normInf() < _alpha)
58 {
59 if(_inDisceteTimeSlidingPhase)
60 {
61 SiconosVector& up = *_up;
62 if(_measuredPert->full())
63 {
64 if(_measuredPert->size() > 1)
65 {
66 _measuredPert->rotate(_measuredPert->end()-1);
67 _predictedPert->rotate(_predictedPert->end()-1);
68 }
69 }
70 else
71 {
72 // inject new vector in the case where the measurement vector is not full.
73 SP::SiconosVector sp1(new SiconosVector(_us->size(), 0));
74 SP::SiconosVector sp2(new SiconosVector(_us->size(), 0));
75 _measuredPert->push_front(sp1);
76 _predictedPert->push_front(sp2);
77 }
78
79 // inject new measured value and also perturbation prediction
80 SiconosVector& predictedPertC = *(*_predictedPert)[0];
81 SiconosVector& measuredPertC = *(*_measuredPert)[0];
82
83 // Cp_k = s_k + Cp_k-tilde
84 prod(*_Csurface, xTk, measuredPertC);
85 measuredPertC += *(*_predictedPert)[std::min((unsigned int)1, (unsigned int)_predictedPert->size()-1)];
86
87 // compute prediction
88 switch(_measuredPert->size()-1)
89 {
90 case 0:
91 predictedPertC = measuredPertC;
92 break;
93 case 1:
94 predictedPertC = 2*measuredPertC - *(*_measuredPert)[1];
95 break;
96 case 2:
97 predictedPertC = 3*measuredPertC - 3*(*(*_measuredPert)[1]) + *(*_measuredPert)[2];
98 break;
99 default:
100 THROW_EXCEPTION("LinearSMCimproved::predictionPerturbation: unknown order " + std::to_string(_measuredPert->size()));
101 }
102
103 // Compute the control to counteract the perturbation
104 up = predictedPertC;
105 up *= -1;
106 CBstar.Solve(up);
107
108 // project onto feasible set
109 double norm = up.norm2();
110 if(norm > _ubPerturbation)
111 {
112 up *= _ubPerturbation/norm;
113 predictedPertC *= _ubPerturbation/norm;
114 }
115 }
116 else
117 _inDisceteTimeSlidingPhase = true;
118 }
119 else if(_inDisceteTimeSlidingPhase)
120 {
121 _inDisceteTimeSlidingPhase = false;
122 _up->zero();
123 }
124 }
125
actuate()126 void LinearSMCimproved::actuate()
127 {
128 unsigned int sDim = _u->size();
129 SP::SimpleMatrix tmpM1(new SimpleMatrix(*_Csurface));
130 SP::SimpleMatrix CBstar(new SimpleMatrix(sDim, sDim, 0));
131 SP::SiconosVector xTk(new SiconosVector(_sensor->y()));
132
133 ZeroOrderHoldOSI& zoh = *std::static_pointer_cast<ZeroOrderHoldOSI>(_integratorSMC);
134
135 // equivalent part
136 zoh.updateMatrices(_DS_SMC);
137 prod(*_Csurface, zoh.Ad(_DS_SMC), *tmpM1);
138 *tmpM1 *= -1.0;
139 *tmpM1 += *_Csurface;
140 prod(*_Csurface, zoh.Bd(_DS_SMC), *CBstar);
141 // compute C(I-e^{Ah})x_k
142 prod(*tmpM1, *xTk, *_ueq);
143 // compute the solution u^eq of the system CB^{*}u^eq = C(I-e^{Ah})x_k
144 CBstar->Solve(*_ueq);
145
146 *(_DS_SMC->x()) = *xTk;
147 prod(*_B, *_ueq, *(std::static_pointer_cast<FirstOrderLinearDS>(_DS_SMC)->b()));
148 _simulationSMC->computeOneStep();
149 _simulationSMC->nextStep();
150
151 *_us = *_lambda;
152
153 // inject those in the system
154 *_u = *_us;
155 *_u += *_ueq;
156
157 // prediction of the perturbation
158 if(_predictionPerturbation)
159 {
160 predictionPerturbation(*xTk, *CBstar);
161 *_u += *_up;
162 }
163
164 _indx++;
165
166 }
167
setPredictionOrder(unsigned int order)168 void LinearSMCimproved::setPredictionOrder(unsigned int order)
169 {
170 _measuredPert->set_capacity(order+1);
171 _predictedPert->set_capacity(order+1);
172 }
173
174 AUTO_REGISTER_ACTUATOR(LINEAR_SMC_IMPROVED, LinearSMCimproved)
175