Commit 64a38328 authored by Alexandre Rocca's avatar Alexandre Rocca

Added C++ and python code for simulating example of IFAC paper

parent e857e669
This diff is collapsed.
/* Siconos is a program dedicated to modeling, simulation and control
* of non smooth dynamical systems.
*
* Copyright 2018 INRIA.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "SiconosKernel.hpp"
#include "lcp_cst.h"
using namespace std;
enum ProblemType { SLIDING_CROSSING, SLIDING_REPULSIVE, NON_NULL_A };
class Problem
{
public:
SP::SiconosMatrix A;
SP::SimpleMatrix R;
SP::SiconosVector b;
SP::SimpleMatrix C;
SP::SimpleMatrix D;
SP::SiconosVector e;
SP::SimpleMatrix M;
SP::SiconosVector init;
double t0;
double T;
ProblemType type;
unsigned int dimX = 3; // forced for example
unsigned int dimLambda = 3; // forced for example
Problem(SP::SiconosMatrix lA, SP::SimpleMatrix lR, SP::SiconosVector lb,
SP::SimpleMatrix lC, SP::SimpleMatrix lD, SP::SiconosVector le,
SP::SimpleMatrix lM, SP::SiconosVector linit,
double lt0, double lT, ProblemType ltype ){
A = lA;
R = lR;
b = lb;
C = lC;
D = lD;
e = le;
M = lM;
init = linit;
t0 = lt0;
T = lT;
type = ltype;
}
~Problem(){
}
};
class ConvergenceTest
{
public:
ConvergenceTest(Problem* lproblem, vector<double> ltime_steps){
problem = lproblem;
time_steps = ltime_steps;
}
/*Simulate a nonsmooth DAE for a given time step
The size of the problem is fixed
*/
SP::SimpleMatrix simulate(Problem* p, double h, int *out){
unsigned int dimX = p->dimX; // Dimension of the system state variables
unsigned int dimLambda = p->dimLambda; // Dimension of the system lambda variables
double t0 = p->t0; // initial computation time
double T = p->T; // final computation tim
// -------------------------
// --- Dynamical systems ---
// -------------------------
SP::FirstOrderLinearTIDS dyn(new FirstOrderLinearTIDS(p->init,p->A));
dyn->setbPtr(p->b);
dyn->setMPtr(p->M);
// -------------------------
// --- LCP Relation ---
// -------------------------
// Relation LCP lhs
SP::FirstOrderLinearTIR relation(new FirstOrderLinearTIR(p->C, p->R) );
relation->setDPtr(p->D);
relation->setePtr(p->e);
// NonSmooth law: LCP
SP::NonSmoothLaw nslaw(new ComplementarityConditionNSL(dimLambda));
// interaction
SP::Interaction inter(new Interaction(nslaw, relation));
// -----------------------------
// --- Siconos Model Entity ---
// ----------------------------
SP::NonSmoothDynamicalSystem switch_dae(new NonSmoothDynamicalSystem(t0, T));
// add the dynamical system in the non smooth dynamical system
switch_dae->insertDynamicalSystem(dyn);
// link the interaction and the dynamical system
switch_dae->link(inter, dyn);
// -----------------------------
// --- Simulation Definition ---
// -----------------------------
// -- (1) OneStepIntegrators --
double theta = 1.0;
double gamma = 1.0;
SP::EulerMoreauOSI osi(new EulerMoreauOSI(theta,gamma));
// -- (2) Time discretisation --
SP::TimeDiscretisation td(new TimeDiscretisation(t0, h));
// -- (3) one step non smooth problem
SP::LCP osnspb(new LCP(SICONOS_LCP_ENUM));
osnspb->numericsSolverOptions()->iparam[SICONOS_LCP_IPARAM_ENUM_MULTIPLE_SOLUTIONS] = 1;
osnspb->numericsSolverOptions()->iparam[SICONOS_LCP_IPARAM_SKIP_TRIVIAL] = SICONOS_LCP_SKIP_TRIVIAL_NO;
osnspb->numericsSolverOptions()->iparam[SICONOS_LCP_IPARAM_ENUM_SEED] = 0; // SEED
// osnspb->setNumericsVerboseMode(true);
// -- (4) Simulation setup with (1) (2) (3)
SP::TimeStepping s(new TimeStepping(switch_dae, td, osi, osnspb));
// =========================== End of model definition ===========================
// // ================================= Computation =================================
int N = ceil((T - t0) / h)+1; // Number of time steps
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 7;
SP::SimpleMatrix dataPlot( new SimpleMatrix(N + 1, outputSize));
SP::SiconosVector x = dyn->x();
SP::SiconosVector lambda = inter->lambda(0);
(*dataPlot)(0, 0) = switch_dae->t0();
(*dataPlot)(0, 1) = (*x)(0); // x1
(*dataPlot)(0, 2) = (*x)(1); // x2
(*dataPlot)(0, 3) = (*x)(2); // z
(*dataPlot)(0, 4) = (*lambda)(0);
(*dataPlot)(0, 5) = (*lambda)(1); //
(*dataPlot)(0, 6) = (*lambda)(2); //
// --- Time loop ---
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
SiconosVector xk = SiconosVector(*(p->init));
SiconosVector xsol = SiconosVector(*(p->init));
SiconosVector lsol = SiconosVector({0, 0, 0});
SiconosVector diff = SiconosVector(*(p->init));
double norm = INFINITY;
int nb_modes = 8;
double norm_diff;
while (s->hasNextEvent())
{
for(int i=0;i<nb_modes;i++)
{
osnspb->numericsSolverOptions()->iparam[SICONOS_LCP_IPARAM_ENUM_SEED] = i; // SEED
s->computeOneStep();
diff = *x-xk;
norm_diff = diff.norm2();
if(norm_diff<norm)
{
xsol = *x;
lsol = *lambda;
norm = norm_diff;
}
// osnspb->display();
}
(*x) = xsol;
(*lambda) = lsol;
// --- Get values to be plotted ---
(*dataPlot)(k, 0) = s->nextTime();
(*dataPlot)(k, 1) = (*x)(0);
(*dataPlot)(k, 2) = (*x)(1);
(*dataPlot)(k, 3) = (*x)(2);
(*dataPlot)(k, 4) = (*lambda)(0);
(*dataPlot)(k, 5) = (*lambda)(1);
(*dataPlot)(k, 6) = (*lambda)(2);
xk = xsol;
norm =INFINITY;
s->nextStep();
++show_progress;
k++;
}
// cout << "End of computation - Number of iterations done: " << k - 1 << endl;
cout << "Computation Time " << time.elapsed() << endl;
*out = k;
return dataPlot;
}
/* run the simulation and compare the results with the corresponding analytic solution for a given set of time steps*/
vector<double> run(){
vector<double> errors(time_steps.size());
for(unsigned int i=0;i<time_steps.size();i++)
{
double h = time_steps[i];
SP::SimpleMatrix current_results;
int k;
current_results = simulate(problem,h,&k);
// int N = ceil((problem->T - problem->t0) / h)+1; // not satisfying
double x1f = (*current_results)(k-1,1);
double x2f = (*current_results)(k-1,2);
// temporary as SiconosVector({x1f, x2f}) is not accepted by IDE
vector<double> tmp({x1f, x2f});
SiconosVector xf = SiconosVector(tmp);
// diff.norm2()
switch(problem->type) {
case SLIDING_CROSSING :
tmp = vector<double>({6.5*2./3.,6.5*2./3. + 1});
break;
case SLIDING_REPULSIVE :
tmp = vector<double>({8/3.,1.+8/3.});
break;
case NON_NULL_A :
tmp = vector<double>({1.8867421308653363, 2.8867421308653363});
break;
default:
cout << "Invalid Problem Type" << endl;
break;
}
SiconosVector diff = SiconosVector(tmp);
// cout << xf << endl;
// cout << diff << endl;
// cout << k-1 << endl;
diff = diff - xf;
double current_error = diff.norm2();
errors[i] = current_error;
cout << current_error << endl;
}
return errors;
}
~ConvergenceTest(){
delete(problem);
}
private:
vector<double> time_steps;
Problem* problem;
};
/* Siconos is a program dedicated to modeling, simulation and control
* of non smooth dynamical systems.
*
* Copyright 2018 INRIA.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/*
Example of Linear switching DAE with a constraint defined by a multivalued operator
Here we considere the parametrization of the sliding_repulsive case
*/
#include "example_class.hpp"
int main(int argc, char* argv[])
{
try
{
unsigned int dimX = 3; // Dimension of the system state variables
unsigned int dimLambda = 3; // Dimension of the system lambda variables
double t0 = 0.0; // initial computation time
double T = 10.0; // final computation time
double x1_0 = 0.0; // initial condition in state variable x1
double x2_0 = 0.0; // initial condition in state variable x2
double z_0 = 0; // initial condition in algebraic variable z
SP::SiconosVector init(new SiconosVector({x1_0, x2_0, z_0}));
SP::SiconosMatrix A( new SimpleMatrix(dimX,dimX) );
double B0 = -1.0;
double B1 = 0.5;
A->setRow(0,SiconosVector({0.0, 0.0, B0}));
A->setRow(1,SiconosVector({0.0, 0.0, B1}));
A->setRow(2,SiconosVector({1.0, -1.0, 0.0}));
SP::SimpleMatrix M(new SimpleMatrix(dimX,dimX));
(*M)(0,0) = 1.0;
(*M)(1,1) = 1.0;
SP::SiconosVector b(new SiconosVector({1.0, 0.0, 1.0}));
SP::SimpleMatrix C( new SimpleMatrix(dimLambda,dimX) );
(*C)(0,0) = 2.0;
(*C)(1,0) = 1.0;
SP::SimpleMatrix D( new SimpleMatrix(dimLambda,dimLambda) );
(*D)(0,0) = 1.0;
(*D)(1,2) = 1.0;
(*D)(2,1) = -1.0;
SP::SimpleMatrix R( new SimpleMatrix(dimX,dimLambda) );
(*R)(2,0) = 1.0;
(*R)(2,1) = -1.0;
SP::SiconosVector e(new SiconosVector({0.0, 0.0, 2.0}));
ProblemType type = SLIDING_REPULSIVE;
Problem* problem = new Problem( A, R, b, C, D, e, M, init, t0, T, type);
//generated by np.logspace(-4.4,0.0,100,endpoint=True, base=10.0) in python 3
// vector<double> time_steps({ 3.98107171e-05, 4.41005945e-05, 4.88527357e-05, 5.41169527e-05,
// 5.99484250e-05, 6.64082785e-05, 7.35642254e-05, 8.14912747e-05,
// 9.02725178e-05, 1.00000000e-04, 1.10775685e-04, 1.22712524e-04,
// 1.35935639e-04, 1.50583635e-04, 1.66810054e-04, 1.84784980e-04,
// 2.04696827e-04, 2.26754313e-04, 2.51188643e-04, 2.78255940e-04,
// 3.08239924e-04, 3.41454887e-04, 3.78248991e-04, 4.19007911e-04,
// 4.64158883e-04, 5.14175183e-04, 5.69581081e-04, 6.30957344e-04,
// 6.98947321e-04, 7.74263683e-04, 8.57695899e-04, 9.50118507e-04,
// 1.05250029e-03, 1.16591440e-03, 1.29154967e-03, 1.43072299e-03,
// 1.58489319e-03, 1.75567629e-03, 1.94486244e-03, 2.15443469e-03,
// 2.38658979e-03, 2.64376119e-03, 2.92864456e-03, 3.24422608e-03,
// 3.59381366e-03, 3.98107171e-03, 4.41005945e-03, 4.88527357e-03,
// 5.41169527e-03, 5.99484250e-03, 6.64082785e-03, 7.35642254e-03,
// 8.14912747e-03, 9.02725178e-03, 1.00000000e-02, 1.10775685e-02,
// 1.22712524e-02, 1.35935639e-02, 1.50583635e-02, 1.66810054e-02,
// 1.84784980e-02, 2.04696827e-02, 2.26754313e-02, 2.51188643e-02,
// 2.78255940e-02, 3.08239924e-02, 3.41454887e-02, 3.78248991e-02,
// 4.19007911e-02, 4.64158883e-02, 5.14175183e-02, 5.69581081e-02,
// 6.30957344e-02, 6.98947321e-02, 7.74263683e-02, 8.57695899e-02,
// 9.50118507e-02, 1.05250029e-01, 1.16591440e-01, 1.29154967e-01,
// 1.43072299e-01, 1.58489319e-01, 1.75567629e-01, 1.94486244e-01,
// 2.15443469e-01, 2.38658979e-01, 2.64376119e-01, 2.92864456e-01,
// 3.24422608e-01, 3.59381366e-01, 3.98107171e-01, 4.41005945e-01,
// 4.88527357e-01, 5.41169527e-01, 5.99484250e-01, 6.64082785e-01,
// 7.35642254e-01, 8.14912747e-01, 9.02725178e-01, 1.00000000e+00});
vector<double> time_steps({ 0.009, 0.09, 0.9, 1.0});
ConvergenceTest test(problem, time_steps);
vector<double> errors = test.run();
SiconosVector res(errors);
cout << res << endl;
SimpleMatrix results = SimpleMatrix(time_steps.size(),1);
results.setCol(0,res);
// --- Output files ---
cout << "====> Output file writing ..." << endl;
ioMatrix::write("convergence_error.dat", "ascii", results, "noDim");
}
catch (SiconosException& e)
{
cerr << e.report() << endl;
return 1;
}
catch (...)
{
cerr << "Exception caught in absolute-value-and-sign-lcp.cpp" << endl;
return 1;
}
}
\ No newline at end of file
/* Siconos is a program dedicated to modeling, simulation and control
* of non smooth dynamical systems.
*
* Copyright 2018 INRIA.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/*
Example of Linear switching DAE with a constraint defined by a multivalued operator
Here we considere the parametrization of the sliding_repulsive case
*/
#include "example_class.hpp"
int main(int argc, char* argv[])
{
try
{
unsigned int dimX = 3; // Dimension of the system state variables
unsigned int dimLambda = 3; // Dimension of the system lambda variables
double t0 = 0.0; // initial computation time
double T = 10.0; // final computation time
double x1_0 = 0.0; // initial condition in state variable x1
double x2_0 = 0.0; // initial condition in state variable x2
double z_0 = 0; // initial condition in algebraic variable z
SP::SiconosVector init(new SiconosVector({x1_0, x2_0, z_0}));
SP::SiconosMatrix A( new SimpleMatrix(dimX,dimX) );
double B0 = -1.0;
double B1 = 0.5;
A->setRow(0,SiconosVector({0.0, 0.0, B0}));
A->setRow(1,SiconosVector({0.0, 0.0, B1}));
A->setRow(2,SiconosVector({1.0, -1.0, 0.0}));
SP::SimpleMatrix M(new SimpleMatrix(dimX,dimX));
(*M)(0,0) = 1.0;
(*M)(1,1) = 1.0;
SP::SiconosVector b(new SiconosVector({1.0, 0.0, 1.0}));
SP::SimpleMatrix C( new SimpleMatrix(dimLambda,dimX) );
(*C)(0,0) = 2.0;
(*C)(1,0) = 1.0;
SP::SimpleMatrix D( new SimpleMatrix(dimLambda,dimLambda) );
(*D)(0,0) = 1.0;
(*D)(1,2) = 1.0;
(*D)(2,1) = -1.0;
SP::SimpleMatrix R( new SimpleMatrix(dimX,dimLambda) );
(*R)(2,0) = 1.0;
(*R)(2,1) = -1.0;
SP::SiconosVector e(new SiconosVector({0.0, 0.0, 2.0}));
ProblemType type = SLIDING_REPULSIVE;
Problem* problem = new Problem( A, R, b, C, D, e, M, init, t0, T, type);
vector<double> time_steps({ 0.009, 0.09, 0.9});
ConvergenceTest test(problem, time_steps);
int k;
SP::SimpleMatrix results = test.simulate(problem,0.2,&k);
cout << (*results) << endl;
unsigned int outputSize = 7;
results->resize(k, outputSize);
ioMatrix::write("simulations_results.dat", "ascii", (*results), "noDim");
}
catch (SiconosException& e)
{
cerr << e.report() << endl;
return 1;
}
catch (...)
{
cerr << "Exception caught in absolute-value-and-sign-lcp.cpp" << endl;
return 1;
}
}
\ No newline at end of file
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment