void Simulation::computeLevelsForInputAndOutput(SP::Interaction inter, bool init)
{
DEBUG_PRINT("Simulation::computeLevelsForInputAndOutput(SP::Interaction inter, bool init)\n");
/** \warning. We test only for the first Dynamical of the interaction.
* we assume that the osi(s) are consistent for one interaction
*/
SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet(0);
SP::DynamicalSystem ds = indexSet0->properties(indexSet0->descriptor(inter)).source;
// Note FP : we should probably connect osi and graph before, in simulation->initialize?
DSOSIConstIterator it = _osiMap.find(ds);
SP::OneStepIntegrator osi = it->second;
if (!osi)
RuntimeException::selfThrow("Simulation::computeLevelsForInputAndOutput osi does not exists");
indexSet0->properties(indexSet0->descriptor(inter)).osi = osi;
std11::shared_ptr<SetupLevels> setupLevels;
setupLevels.reset(new SetupLevels(shared_from_this(), inter, ds));
osi->accept(*(setupLevels.get()));
if (!init) // We are not computing the levels at the initialization
{
SP::Topology topo = model()->nonSmoothDynamicalSystem()->topology();
unsigned int indxSize = topo->indexSetsSize();
assert (_numberOfIndexSets >0);
if ((indxSize == LEVELMAX) || (indxSize < _numberOfIndexSets ))
{
topo->indexSetsResize(_numberOfIndexSets);
// Init if the size has changed
for (unsigned int i = indxSize; i < topo->indexSetsSize(); i++) // ++i ???
topo->resetIndexSetPtr(i);
}
}
}
void EventDriven::insertIntegrator(SP::OneStepIntegrator osi)
{
Simulation::insertIntegrator(osi);
// Determine the number of OneStepNSproblems depending on the OneStepIntegrator type
OSI::TYPES osiType = osi->getType();
if (osiType == OSI::NEWMARKALPHAOSI) // EventDrivent asscociated with NewMarkAlpha
{
_numberOfOneStepNSproblems = 3;
if (_allNSProblems->size() != 3)
{
(*_allNSProblems).resize(_numberOfOneStepNSproblems);
}
}
}
// ================= Creation of the model =======================
void Disks::init()
{
SP::TimeDiscretisation timedisc_;
SP::TimeStepping simulation_;
SP::FrictionContact osnspb_;
// User-defined main parameters
double t0 = 0; // initial computation time
double T = std::numeric_limits<double>::infinity();
double h = 0.01; // time step
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
std::string solverName = "NSGS";
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
double R;
double m;
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
_plans.reset(new SimpleMatrix("plans.dat", true));
if (_plans->size(0) == 0)
{
/* default plans */
double A1 = P1A;
double B1 = P1B;
double C1 = P1C;
double A2 = P2A;
double B2 = P2B;
double C2 = P2C;
_plans.reset(new SimpleMatrix(6, 6));
_plans->zero();
(*_plans)(0, 0) = 0;
(*_plans)(0, 1) = 1;
(*_plans)(0, 2) = -GROUND;
(*_plans)(1, 0) = 1;
(*_plans)(1, 1) = 0;
(*_plans)(1, 2) = WALL;
(*_plans)(2, 0) = 1;
(*_plans)(2, 1) = 0;
(*_plans)(2, 2) = -WALL;
(*_plans)(3, 0) = 0;
(*_plans)(3, 1) = 1;
(*_plans)(3, 2) = -TOP;
(*_plans)(4, 0) = A1;
(*_plans)(4, 1) = B1;
(*_plans)(4, 2) = C1;
(*_plans)(5, 0) = A2;
(*_plans)(5, 1) = B2;
(*_plans)(5, 2) = C2;
}
/* set center positions */
for (unsigned int i = 0 ; i < _plans->size(0); ++i)
{
SP::DiskPlanR tmpr;
tmpr.reset(new DiskPlanR(1, (*_plans)(i, 0), (*_plans)(i, 1), (*_plans)(i, 2),
(*_plans)(i, 3), (*_plans)(i, 4), (*_plans)(i, 5)));
(*_plans)(i, 3) = tmpr->getXCenter();
(*_plans)(i, 4) = tmpr->getYCenter();
}
/* _moving_plans.reset(new FMatrix(1,6));
(*_moving_plans)(0,0) = &A;
(*_moving_plans)(0,1) = &B;
(*_moving_plans)(0,2) = &C;
(*_moving_plans)(0,3) = &DA;
(*_moving_plans)(0,4) = &DB;
(*_moving_plans)(0,5) = &DC;*/
SP::SiconosMatrix Disks;
Disks.reset(new SimpleMatrix("disks.dat", true));
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
_model.reset(new Model(t0, T));
for (unsigned int i = 0; i < Disks->size(0); i++)
{
R = Disks->getValue(i, 2);
m = Disks->getValue(i, 3);
SP::SiconosVector qTmp;
SP::SiconosVector vTmp;
qTmp.reset(new SiconosVector(NDOF));
vTmp.reset(new SiconosVector(NDOF));
vTmp->zero();
(*qTmp)(0) = (*Disks)(i, 0);
(*qTmp)(1) = (*Disks)(i, 1);
SP::LagrangianDS body;
if (R > 0)
body.reset(new Disk(R, m, qTmp, vTmp));
else
body.reset(new Circle(-R, m, qTmp, vTmp));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(NDOF));
FExt->zero();
FExt->setValue(1, -m * g);
body->setFExtPtr(FExt);
// add the dynamical system to the one step integrator
osi->insertDynamicalSystem(body);
// add the dynamical system in the non smooth dynamical system
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
}
_model->nonSmoothDynamicalSystem()->setSymmetric(true);
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
timedisc_.reset(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
osnspb_.reset(new FrictionContact(2));
osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
// iterations
osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
// iterations
osnspb_->numericsSolverOptions()->dparam[0] = 1e-3; // Tolerance
osnspb_->setMaxSize(6 * ((3 * Ll * Ll + 3 * Ll) / 2 - Ll));
osnspb_->setMStorageType(1); // Sparse storage
osnspb_->setNumericsVerboseMode(0);
osnspb_->setKeepLambdaAndYState(true); // inject previous solution
// -- Simulation --
simulation_.reset(new TimeStepping(timedisc_));
std11::static_pointer_cast<TimeStepping>(simulation_)->setNewtonMaxIteration(3);
simulation_->insertIntegrator(osi);
simulation_->insertNonSmoothProblem(osnspb_);
simulation_->setCheckSolverFunction(localCheckSolverOuput);
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.3, 2));
_playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));
_playground->insert(nslaw, 0, 0);
_model->initialize(simulation_);
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in Disks::init()" << std::endl;
exit(1);
}
}
// In those two functions, we should store in the graph the information
// that the ds is integrated by the OSI, instead of storing this info
// in the OSI -- xhub
void Topology::setOSI(SP::DynamicalSystem ds, SP::OneStepIntegrator OSI)
{
// to be move on the graph
OSI->insertDynamicalSystem(ds);
}
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");
// Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
// necessary) one or two DS are concerned by inter1 . How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vd);
// Get osi property from interaction
// We assume that all ds in vertex_inter have the same osi.
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
//SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
OSI::TYPES osiType = Osi->getType();
// At most 2 DS are linked by an Interaction
SP::DynamicalSystem DS1;
SP::DynamicalSystem DS2;
unsigned int pos1, pos2;
// --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
if (indexSet->properties(vd).source != indexSet->properties(vd).target)
{
DEBUG_PRINT("a two DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = indexSet->properties(vd).target;
}
else
{
DEBUG_PRINT("a single DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = DS1;
// \warning this looks like some debug code, but it gets executed even with NDEBUG.
// may be compiler does something smarter, but still it should be rewritten. --xhub
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
oei != oeiend; ++oei)
{
// note : at most 4 edges
DS2 = indexSet->bundle(*oei);
if (DS2 != DS1)
{
assert(false);
break;
}
}
}
assert(DS1);
assert(DS2);
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
// --- Check block size ---
assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());
// --- Compute diagonal block ---
// Block to be set in OSNS Matrix, corresponding to
// the current interaction
SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType;
double h = simulation()->currentTimeStep();
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType = inter->relation()->getType();
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
// loop over the DS connected to the interaction.
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
unsigned int sizeDS = ds->dimension();
// get _interactionBlocks corresponding to the current DS
// These _interactionBlocks depends on the relation type.
leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
DEBUG_EXPR(leftInteractionBlock->display(););
// Computing depends on relation type -> move this in Interaction method?
if (relationType == FirstOrder)
{
rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize));
inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter);
if (osiType == OSI::EULERMOREAUOSI)
{
if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation())
{
*rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma();
}
}
// for ZOH, we have a different formula ...
if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl)
{
*rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
// centralInteractionBlock contains a lu-factorized matrix and we solve
// centralInteractionBlock * X = rightInteractionBlock with PLU
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0
// integration of r with theta method removed
// *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B
//gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock);
*leftInteractionBlock *= h;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
//left = C, right = inv(W).B
}
}
else if (relationType == Lagrangian ||
relationType == NewtonEuler)
{
SP::BoundaryCondition bc;
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
if (bc)
{
for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
itindex != bc->velocityIndices()->end();
++itindex)
{
// (nslawSize,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(nslawSize));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
DEBUG_EXPR(leftInteractionBlock->display(););
// ================= Creation of the model =======================
void Spheres::init()
{
SP::TimeDiscretisation timedisc_;
SP::Simulation simulation_;
SP::FrictionContact osnspb_;
// User-defined main parameters
double t0 = 0; // initial computation time
double T = std::numeric_limits<double>::infinity();
double h = 0.01; // time step
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
std::string solverName = "NSGS";
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
double R;
double m;
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
_plans.reset(new SimpleMatrix("plans.dat", true));
SP::SiconosMatrix Spheres;
Spheres.reset(new SimpleMatrix("spheres.dat", true));
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
_model.reset(new Model(t0, T));
for (unsigned int i = 0; i < Spheres->size(0); i++)
{
R = Spheres->getValue(i, 3);
m = Spheres->getValue(i, 4);
SP::SiconosVector qTmp;
SP::SiconosVector vTmp;
qTmp.reset(new SiconosVector(NDOF));
vTmp.reset(new SiconosVector(NDOF));
vTmp->zero();
(*qTmp)(0) = (*Spheres)(i, 0);
(*qTmp)(1) = (*Spheres)(i, 1);
(*qTmp)(2) = (*Spheres)(i, 2);
(*qTmp)(3) = M_PI / 2;
(*qTmp)(4) = M_PI / 4;
(*qTmp)(5) = M_PI / 2;
(*vTmp)(0) = 0;
(*vTmp)(1) = 0;
(*vTmp)(2) = 0;
(*vTmp)(3) = 0;
(*vTmp)(4) = 0;
(*vTmp)(5) = 0;
SP::LagrangianDS body;
body.reset(new SphereLDS(R, m, std11::shared_ptr<SiconosVector>(qTmp), std11::shared_ptr<SiconosVector>(vTmp)));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(NDOF));
FExt->zero();
FExt->setValue(2, -m * g);
body->setFExtPtr(FExt);
// add the dynamical system to the one step integrator
osi->insertDynamicalSystem(body);
// add the dynamical system in the non smooth dynamical system
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
}
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
timedisc_.reset(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
osnspb_.reset(new FrictionContact(3));
osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
// iterations
osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
// iterations
osnspb_->numericsSolverOptions()->iparam[4] = 2; // projection
osnspb_->numericsSolverOptions()->dparam[0] = 1e-6; // Tolerance
osnspb_->numericsSolverOptions()->dparam[2] = 1e-8; // Local tolerance
osnspb_->setMaxSize(16384); // max number of interactions
osnspb_->setMStorageType(1); // Sparse storage
osnspb_->setNumericsVerboseMode(0); // 0 silent, 1 verbose
osnspb_->setKeepLambdaAndYState(true); // inject previous solution
simulation_.reset(new TimeStepping(timedisc_));
simulation_->insertIntegrator(osi);
simulation_->insertNonSmoothProblem(osnspb_);
// simulation_->setCheckSolverFunction(localCheckSolverOuput);
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.8, 3));
_playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));
_playground->insert(nslaw, 0, 0);
_model->initialize(simulation_);
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in Spheres::init()" << std::endl;
exit(1);
}
}
int main(int argc, char *argv[])
{
//printf("argc %i\n", argc);
int cmp=0;
int dimX = 2;
char filename[50] = "simu.";
//***** Set the initial condition
SP::SiconosVector xti(new SiconosVector(dimX));
if (argc==1)
{
xti->setValue(0,1);
xti->setValue(1,6);
strncpy(&filename[5],"1.6.log",7);
}
else if (argc==3)
{
//printf("argv[0] %s\n", argv[0]);
printf("xti(0) is set to %f\n", atof(argv[1]));
printf("xti(1) is set to %f\n", atof(argv[2]));
xti->setValue(0,atof(argv[1]));
xti->setValue(1,atof(argv[2]));
int sizeofargv1 = strlen(argv[1]);
// printf("sizeofargv1 %i\n",sizeofargv1);
strncpy(&filename[5],argv[1],sizeofargv1);
int sizeofargv2 = strlen(argv[2]);
//printf("sizeofargv2 %i\n",sizeofargv2);
strncpy(&filename[5+sizeofargv1],".",1);
strncpy(&filename[5+sizeofargv1+1],argv[2],sizeofargv2);
strncpy(&filename[5+sizeofargv1+sizeofargv2+1],".log",4);
printf("Output is written in filename %s\n", filename);
}
else
{
cout << "wrong number of arguments = " << argc << endl;
}
int NBStep = (int) floor(sTf/sStep);
// NBStep =1;
//*****BUILD THE DYNAMICAL SYSTEM
SP::MyDS aDS ;
aDS.reset(new MyDS(xti));
DynamicalSystemsSet Inter_DS ;
Inter_DS.insert(aDS);
//******BUILD THE RELATION
SP::NonlinearRelationReduced2 aR;
aR.reset(new NonlinearRelationReduced2());
//*****BUILD THE NSLAW
SP::NonSmoothLaw aNSL;
RelayNSL * rNSL = new RelayNSL(sNSLawSize);
aNSL.reset(rNSL); //nr de lambda din pb LCP
rNSL->setLb(-0.0);
rNSL->setUb(1.0);
// unsigned int sNSLawSize = 4;
//****BUILD THE INTERACTION
SP::Interaction aI(new Interaction(sNSLawSize,aNSL,aR));
//****BUILD THE SYSTEM
SP::Model aM(new Model(0,sTf));
aM->nonSmoothDynamicalSystem()->insertDynamicalSystem(aDS);
aM->nonSmoothDynamicalSystem()->link(aI,aDS);
// -- (1) OneStepIntegrators --
SP::OneStepIntegrator aEulerMoreauOSI ;
aEulerMoreauOSI.reset(new EulerMoreauOSI(0.5));
// -- (2) Time discretisation --
SP::TimeDiscretisation aTD(new TimeDiscretisation(0,sStep));
// -- (3) Non smooth problem
SP::Relay osnspb(new Relay(SICONOS_RELAY_LEMKE));
osnspb->numericsSolverOptions()->dparam[0]=1e-08;
osnspb->numericsSolverOptions()->iparam[0]=0; // Multiple solutions 0 or 1
// osnspb->numericsSolverOptions()->iparam[3]=48;
osnspb->setNumericsVerboseMode(0);
// -- (4) Simulation setup with (1) (2) (3)
SP::TimeStepping aS(new TimeStepping(aTD,aEulerMoreauOSI,osnspb));
aS->setComputeResiduY(true);
aS->setUseRelativeConvergenceCriteron(false);
// Initialization
printf("-> Initialisation \n");
aM->setSimulation(aS);
aM->initialize();
printf("-> End of initialization \n");
// BUILD THE STEP INTEGRATOR
SP::SiconosVector x = aDS->x();
SP::SiconosVector vectorfield = aDS->rhs();
SP::SiconosVector y = aI->y(0);
SP::SiconosVector lambda = aI->lambda(0);
unsigned int outputSize = 9;
SimpleMatrix dataPlot(NBStep+1,outputSize);
cout << "=== Start of simulation: "<<NBStep<<" steps ===" << endl;
printf("=== Start of simulation: %d steps === \n", NBStep);
dataPlot(0, 0) = aM->t0();
dataPlot(0,1) = x->getValue(0);
dataPlot(0,2) = x->getValue(1);
dataPlot(0, 3) = lambda->getValue(0);
dataPlot(0, 7) = vectorfield->getValue(0);
dataPlot(0, 8) = vectorfield->getValue(1);
boost::progress_display show_progress(NBStep);
boost::timer time;
time.restart();
for(int k = 0 ; k < NBStep ; k++)
{
#ifdef SICONOS_DEBUG
std::cout<<"-> Running step:"<<k<<std::endl;
#endif
cmp++;
aS->newtonSolve(1e-4, 200);
dataPlot(cmp, 0) = aS->nextTime();
dataPlot(cmp, 1) = x->getValue(0);
dataPlot(cmp, 2) = x->getValue(1);
dataPlot(cmp, 3) = lambda->getValue(0);
aDS->computeRhs(aS->nextTime(),true);
if (cmp==1) // tricks just for display to avoid the computation of the initial Rhs
{
dataPlot(cmp-1, 7) = vectorfield->getValue(0);
dataPlot(cmp-1, 8) = vectorfield->getValue(1);
}
dataPlot(cmp, 7) = vectorfield->getValue(0);
dataPlot(cmp, 8) = vectorfield->getValue(1);
++show_progress;
aS->nextStep();
// (*fout)<<cmp<<" "<<x->getValue(0)<<" "<<x->getValue(1)<<" "<<lambda->getValue(0)<<" "<<lambda->getValue(1)<<" "<<lambda->getValue(2)<<" "<<lambda->getValue(3)<<endl;
}
dataPlot.resize(cmp,outputSize);
ioMatrix::write(filename, "ascii", dataPlot, "noDim");
cout << "=== End of simulation. === " << endl;
return 0;
}
int main()
{
int cmp = 0;
/************************************************************/
/************************************************************/
int dimX = 4;
SP::SiconosVector x0(new SiconosVector(dimX));
//Point de départ hors arc singulier
x0->setValue(0, 3.4999939172);
x0->setValue(1, -2.2788416237);
x0->setValue(2, 1.1935988302);
x0->setValue(3, -0.6365413023);
double sT = 10;
double sStep = 2e-3;
unsigned int NBStep = floor(sT / sStep);
//NBStep =2;
// NBStep = 3;
//*****BUILD THE DYNAMIC SYSTEM
SP::MyDS aDS ;
aDS.reset(new MyDS(x0));
//******BUILD THE RELATION
SP::adjointInput aR;
aR.reset(new adjointInput());
int sN = 2;
//*****BUILD THE NSLAW
SP::NonSmoothLaw aNSL;
aNSL.reset(new ComplementarityConditionNSL(sN));
//****BUILD THE INTERACTION
SP::Interaction aI(new Interaction(sNSLawSize, aNSL, aR));
//****BUILD THE SYSTEM
SP::Model aM(new Model(0, sT));
aM->nonSmoothDynamicalSystem()->insertDynamicalSystem(aDS);
aM->nonSmoothDynamicalSystem()->link(aI,aDS);
SP::TimeDiscretisation aTD(new TimeDiscretisation(0, sStep));
SP::TimeStepping aS(new TimeStepping(aTD));
aS->setComputeResiduY(true);
aS->setComputeResiduR(true);
aS->setUseRelativeConvergenceCriteron(false);
//*****BUILD THE STEP INTEGRATOR
SP::OneStepIntegrator aEulerMoreauOSI ;
aEulerMoreauOSI.reset(new EulerMoreauOSI(0.5));
aS->insertIntegrator(aEulerMoreauOSI);
//**** BUILD THE STEP NS PROBLEM
SP::LCP aLCP ;
aLCP.reset(new LCP(SICONOS_LCP_ENUM));
// aLCP.reset(new LCP(SICONOS_LCP_NEWTONFB));
aS->insertNonSmoothProblem(aLCP);
aM->setSimulation(aS);
aM->initialize();
setNumericsVerbose(0);
SP::SiconosVector x = aDS->x();
SP::SiconosVector y = aI->y(0);
SP::SiconosVector lambda = aI->lambda(0);
unsigned int outputSize = 9; // number of required data
SimpleMatrix dataPlot(NBStep+10, outputSize);
SP::SiconosVector z = aDS->x();
SP::SiconosVector lambdaOut = aI->lambda(0);
SP::SiconosVector yOut = aI->y(0);
dataPlot(0, 0) = aM->t0(); // Initial time of the model
dataPlot(0, 1) = (*z)(0);
dataPlot(0, 2) = (*z)(1);
dataPlot(0, 3) = (*z)(2);
dataPlot(0, 4) = (*z)(3);
dataPlot(0, 5) = (*lambdaOut)(0);
dataPlot(0, 6) = (*lambdaOut)(1);
dataPlot(0, 7) = (*yOut)(0);
dataPlot(0, 8) = (*yOut)(1);
// do simulation while events remains in the "future events" list of events manager.
cout << " ==== Start of simulation : " << NBStep << " steps====" << endl;
boost::progress_display show_progress(NBStep);
boost::timer time;
time.restart();
unsigned int k = 0;
while (aS->hasNextEvent())
{
k++;
// if (cmp==150)
// setNumericsVerbose(à);
// else if (cmp==151)
setNumericsVerbose(1);
++show_progress;
cmp++;
// solve ...
// aS->computeOneStep();
aS->newtonSolve(1.1e-11, 50);
x = aDS->x();
lambda = aI->lambda(0);
dataPlot(k, 0) = aS->nextTime(); // Initial time of the model
dataPlot(k, 1) = (*x)(0);
dataPlot(k, 2) = (*x)(1);
dataPlot(k, 3) = (*x)(2);
dataPlot(k, 4) = (*x)(3);
dataPlot(k, 5) = (*lambda)(0);
dataPlot(k, 6) = (*lambda)(1);
dataPlot(k, 7) = (*yOut)(0);
dataPlot(k, 8) = (*yOut)(1);
aS->nextStep();
}
cout << "===== End of simulation. ==== " << endl;
dataPlot.resize(k+1, 9);
// --- Output files ---
cout << "====> Output file writing ..." << endl;
ioMatrix::write("OptimalControl.dat", "ascii", dataPlot, "noDim");
std::cout << "Comparison with a reference file: " ;
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("OptimalControl.ref", "ascii", dataPlotRef);
std::cout << "error="<< (dataPlot-dataPlotRef).normInf() <<std::endl;
if ((dataPlot - dataPlotRef).normInf() > 5e-11)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
return 0;
}
void Simulation::initialize(SP::Model m, bool withOSI)
{
// === Connection with the model ===
assert(m && "Simulation::initialize(model) - model = NULL.");
_T = m->finalT();
_nsds = m->nonSmoothDynamicalSystem();
// === Events manager initialization ===
_eventsManager->initialize(_T);
_tinit = _eventsManager->startingTime();
//===
if (withOSI)
{
if (numberOfOSI() == 0)
RuntimeException::selfThrow("Simulation::initialize No OSI !");
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph DSG = _nsds->topology()->dSG(0);
for (std11::tie(dsi, dsend) = DSG->vertices(); dsi != dsend; ++dsi)
{
SP::OneStepIntegrator osi = DSG->properties(*dsi).osi;
SP::DynamicalSystem ds = DSG->bundle(*dsi);
if (!osi)
{
// By default, if the user has not set the OSI, we assign the first OSI to all DS
_nsds->topology()->setOSI(ds,*_allOSI->begin());
//std::cout << "By default, if the user has not set the OSI, we assign the first OSI to all DS"<<std::endl;
}
osi = DSG->properties(*dsi).osi;
ds->initialize(m->t0(), osi->getSizeMem());
}
// === OneStepIntegrators initialization ===
for (OSIIterator itosi = _allOSI->begin();
itosi != _allOSI->end(); ++itosi)
{
// for (DSIterator itds = (*itosi)->dynamicalSystems()->begin();
// itds != (*itosi)->dynamicalSystems()->end();
// ++itds)
// {
// (*itds)->initialize(startingTime(),
// (*itosi)->getSizeMem());
// addInOSIMap(*itds, *itosi);
// }
(*itosi)->setSimulationPtr(shared_from_this());
(*itosi)->initialize(*m);
}
}
// This is the default
_levelMinForInput = LEVELMAX;
_levelMaxForInput = 0;
_levelMinForOutput = LEVELMAX;
_levelMaxForOutput = 0;
computeLevelsForInputAndOutput();
// Loop over all DS in the graph, to reset NS part of each DS.
// Note FP : this was formerly done in inter->initialize call with local levels values
// but I think it's ok (better?) to do it with the simulation levels values.
DynamicalSystemsGraph::VIterator dsi, dsend;
SP::DynamicalSystemsGraph DSG = _nsds->topology()->dSG(0);
for (std11::tie(dsi, dsend) = DSG->vertices(); dsi != dsend; ++dsi)
{
//assert(_levelMinForInput <= _levelMaxForInput);
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
DSG->bundle(*dsi)->initializeNonSmoothInput(k);
}
}
InteractionsGraph::VIterator ui, uiend;
SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
Interaction& inter = *indexSet0->bundle(*ui);
inter.initialize(_tinit, indexSet0->properties(*ui));
}
// Initialize OneStepNSProblem(s). Depends on the type of simulation.
// Warning FP : must be done in any case, even if the interactions set
// is empty.
initOSNS();
// Process events at time _tinit. Useful to save values in memories
// for example. Warning: can not be called during
// eventsManager->initialize, because it needs the initialization of
// OSI, OSNS ...
_eventsManager->preUpdate(*this);
_tend = _eventsManager->nextTime();
// End of initialize:
// - all OSI and OSNS (ie DS and Interactions) states are computed
// - for time _tinit and saved into memories.
// - Sensors or related objects are updated for t=_tinit.
// - current time of the model is equal to t1, time of the first
// - event after _tinit.
// - currentEvent of the simu. corresponds to _tinit and nextEvent
// - to _tend.
// If _printStat is true, open output file.
if (_printStat)
{
statOut.open("simulationStat.dat", std::ios::out | std::ios::trunc);
if (!statOut.is_open())
SiconosVectorException::selfThrow("writing error : Fail to open file simulationStat.dat ");
statOut << "============================================" <<std::endl;
statOut << " Siconos Simulation of type " << Type::name(*this) << "." <<std::endl;
statOut <<std::endl;
statOut << "The tolerance parameter is equal to: " << _tolerance <<std::endl;
statOut <<std::endl <<std::endl;
}
}
int main()
{
// User-defined main parameters
double t0 = 0; // initial computation time
double T = 20.0; // end of computation time
double h = 0.005; // time step
double position_init = 10.0; // initial position
double velocity_init = 0.0; // initial velocity
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
SP::Model model(new Model(t0, T));
std::vector<SP::BulletWeightedShape> shapes;
// note: no rebound with a simple Bullet Box, why ?
// the distance after the broadphase contact detection is negative
// and then stay negative.
// SP::btCollisionShape box(new btBoxShape(btVector3(1,1,1)));
// SP::BulletWeightedShape box1(new BulletWeightedShape(box,1.0));
// This is ok if we build one with btConveHullShape
SP::btCollisionShape box(new btConvexHullShape());
{
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, 1.0));
}
SP::BulletWeightedShape box1(new BulletWeightedShape(box, 1.0));
shapes.push_back(box1);
SP::SiconosVector q0(new SiconosVector(7));
SP::SiconosVector v0(new SiconosVector(6));
v0->zero();
q0->zero();
(*q0)(2) = position_init;
(*q0)(3) = 1.0;
(*v0)(2) = velocity_init;
// -- The dynamical system --
// -- the default contactor is the shape given in the constructor
// -- the contactor id is 0
SP::BulletDS body(new BulletDS(box1, q0, v0));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(3)); //
FExt->zero();
FExt->setValue(2, - g * box1->mass());
body->setFExtPtr(FExt);
// -- Add the dynamical system in the non smooth dynamical system
model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
SP::btCollisionObject ground(new btCollisionObject());
ground->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
SP::btCollisionShape groundShape(new btBoxShape(btVector3(30, 30, .5)));
btMatrix3x3 basis;
basis.setIdentity();
ground->getWorldTransform().setBasis(basis);
ground->setCollisionShape(&*groundShape);
ground->getWorldTransform().getOrigin().setZ(-.50);
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
SP::TimeDiscretisation timedisc(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
SP::FrictionContact osnspb(new FrictionContact(3));
// -- Some configuration
osnspb->numericsSolverOptions()->iparam[0] = 1000; // Max number of
// iterations
osnspb->numericsSolverOptions()->dparam[0] = 1e-5; // Tolerance
osnspb->setMaxSize(16384); // max number of
// interactions
osnspb->setMStorageType(1); // Sparse storage
osnspb->setNumericsVerboseMode(0); // 0 silent, 1
// verbose
osnspb->setKeepLambdaAndYState(true); // inject
// previous
// solution
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
int N = ceil((T - t0) / h); // Number of time steps
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0.8, 0., 0.0, 3));
// -- The space filter performs broadphase collision detection
SP::BulletSpaceFilter space_filter(new BulletSpaceFilter(model));
// -- insert a non smooth law for contactors id 0
space_filter->insert(nslaw, 0, 0);
// -- add multipoint iterations, this is needed to gather at least
// -- 3 contact points and avoid objects penetration, see Bullet
// -- documentation
space_filter->collisionConfiguration()->setConvexConvexMultipointIterations();
space_filter->collisionConfiguration()->setPlaneConvexMultipointIterations();
// -- The ground is a static object
// -- we give it a group contactor id : 0
space_filter->addStaticObject(ground, 0);
// -- MoreauJeanOSI Time Stepping with Bullet Dynamical Systems
SP::BulletTimeStepping simulation(new BulletTimeStepping(timedisc));
simulation->insertIntegrator(osi);
simulation->insertNonSmoothProblem(osnspb);
model->setSimulation(simulation);
model->initialize();
std::cout << "====> End of initialisation ..." << std::endl << std::endl;
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 4;
SimpleMatrix dataPlot(N + 1, outputSize);
dataPlot.zero();
SP::SiconosVector q = body->q();
SP::SiconosVector v = body->velocity();
dataPlot(0, 0) = model->t0();
dataPlot(0, 1) = (*q)(2);
dataPlot(0, 2) = (*v)(2);
// --- Time loop ---
std::cout << "====> Start computation ... " << std::endl << std::endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
while (simulation->hasNextEvent())
{
space_filter->buildInteractions(model->currentTime());
simulation->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = simulation->nextTime();
dataPlot(k, 1) = (*q)(2);
dataPlot(k, 2) = (*v)(2);
// If broadphase collision detection shows some contacts then we may
// display contact forces.
if (space_filter->collisionWorld()->getDispatcher()->getNumManifolds() > 0)
{
// we *must* have an indexSet0, filled by Bullet broadphase
// collision detection and an indexSet1, filled by
// TimeStepping::updateIndexSet with the help of Bullet
// getDistance() function
if (model->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() == 2)
{
SP::InteractionsGraph index1 = simulation->indexSet(1);
// This is the narrow phase contact detection : if
// TimeStepping::updateIndexSet has filled indexSet1 then we
// have some contact forces to display
if (index1->size() > 0)
{
// Four contact points for a cube with a side facing the
// ground. Note : changing Bullet margin for collision
// detection may lead this assertion to be false.
if (index1->size() == 4)
{
InteractionsGraph::VIterator iur = index1->begin();
// different version of bullet may not gives the same
// contact points! So we only keep the summation.
dataPlot(k, 3) =
index1->bundle(*iur)-> lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2();
}
}
}
}
simulation->nextStep();
++show_progress;
k++;
}
std::cout << std::endl << "End of computation - Number of iterations done: " << k - 1 << std::endl;
std::cout << "Computation Time " << time.elapsed() << std::endl;
// --- Output files ---
std::cout << "====> Output file writing ..." << std::endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("result.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The result is rather different from the reference file : "
<< (dataPlot - dataPlotRef).normInf() << std::endl;
return 1;
}
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in BulletBouncingBox" << std::endl;
exit(1);
}
return 0;
}