SP::SiconosVector Simulation::y(unsigned int level, unsigned int coor)
{
// return output(level) (ie with y[level]) for all Interactions.
// assert(level>=0);
DEBUG_BEGIN("Simulation::output(unsigned int level, unsigned int coor)\n");
DEBUG_PRINTF("with level = %i and coor = %i \n", level,coor);
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
SP::SiconosVector y (new SiconosVector (_nsds->topology()->indexSet0()->size() ));
int i=0;
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
inter = indexSet0->bundle(*ui);
assert(inter->lowerLevelForOutput() <= level);
assert(inter->upperLevelForOutput() >= level);
y->setValue(i,inter->y(level)->getValue(coor));
i++;
}
DEBUG_END("Simulation::output(unsigned int level, unsigned int coor)\n");
return y;
}
void MLCPProjectOnConstraints::computeqBlock(InteractionsGraph::VDescriptor& vertex_inter, unsigned int pos)
{
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
for (unsigned int i = 0; i < sizeY; i++)
_q->setValue(pos + i, inter->y(0)->getValue(0 + i));
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::computeqBlock, _q from y(0)\n");
_q->display();
#endif
}
void MLCPProjectOnConstraints::postComputeNewtonEulerR(SP::Interaction inter, unsigned int pos)
{
SP::NewtonEulerR ner = (std11::static_pointer_cast<NewtonEulerR>(inter->relation()));
SP::SiconosVector lambda = inter->lambda(0);
SP::SiconosVector y = inter->y(0);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// Copy _w/_z values, starting from index pos into y/lambda.
//setBlock(*_w, y, sizeY, pos, 0);
setBlock(*_z, lambda, sizeY, pos, 0);
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
double EventDriven::detectEvents(bool updateIstate)
{
double _minResiduOutput = 0.0; // maximum of g_i with i running over all activated or deactivated contacts
// Loop over all interactions to detect whether some constraints are activated or deactivated
bool _IsContactClosed = false;
bool _IsContactOpened = false;
bool _IsFirstTime = true;
InteractionsGraph::VIterator ui, uiend;
SP::SiconosVector y, ydot, lambda;
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
//
#ifdef DEBUG_MESSAGES
cout << "======== In EventDriven::detectEvents =========" <<endl;
#endif
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = _indexSet0->bundle(*ui);
double nsLawSize = inter->nonSmoothLaw()->size();
if (nsLawSize != 1)
{
RuntimeException::selfThrow("In EventDriven::detectEvents, the interaction size > 1 has not been implemented yet!!!");
}
y = inter->y(0); // output y at this Interaction
ydot = inter->y(1); // output of level 1 at this Interaction
lambda = inter->lambda(2); // input of level 2 at this Interaction
if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
{
if ((*y)(0) < _TOL_ED) // gap at the current interaction <= 0
{
_IsContactClosed = true;
}
if (_IsFirstTime)
{
_minResiduOutput = (*y)(0);
_IsFirstTime = false;
}
else
{
if (_minResiduOutput > (*y)(0))
{
_minResiduOutput = (*y)(0);
}
}
}
else // If interaction is in the indexSet[2]
{
if ((*lambda)(0) < _TOL_ED) // normal force at the current interaction <= 0
{
_IsContactOpened = true;
}
if (_IsFirstTime)
{
_minResiduOutput = (*lambda)(0);
_IsFirstTime = false;
}
else
{
if (_minResiduOutput > (*lambda)(0))
{
_minResiduOutput = (*lambda)(0);
}
}
}
//
#ifdef DEBUG_MESSAGES
cout.precision(15);
cout << "Contact number: " << inter->number() <<endl;
cout << "Contact gap: " << (*y)(0) <<endl;
cout << "Contact force: " << (*lambda)(0) <<endl;
cout << "Is contact is closed: " << _IsContactClosed <<endl;
cout << "Is contact is opened: " << _IsContactOpened <<endl;
#endif
//
}
//
if (updateIstate)
{
if ((!_IsContactClosed) && (!_IsContactOpened))
{
_istate = 2; //no event is detected
}
else if ((_IsContactClosed) && (!_IsContactOpened))
{
_istate = 3; // Only some contacts are closed
}
else if ((!_IsContactClosed) && (_IsContactOpened))
{
_istate = 4; // Only some contacts are opened
}
else
{
_istate = 5; // Some contacts are closed AND some contacts are opened
}
}
//
return _minResiduOutput;
}
void EventDriven::computeg(SP::OneStepIntegrator osi,
integer * sizeOfX, doublereal* time,
doublereal* x, integer * ng,
doublereal * gOut)
{
assert(_nsds);
assert(_nsds->topology());
InteractionsGraph::VIterator ui, uiend;
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
unsigned int nsLawSize, k = 0 ;
SP::SiconosVector y, ydot, yddot, lambda;
SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(osi);
// Solve LCP at acceleration level to calculate the lambda[2] at Interaction of indexSet[2]
lsodar->fillXWork(sizeOfX, x);
//
double t = *time;
if (!_allNSProblems->empty())
{
if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions()))
{
(*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t);
}
};
/*
double * xdottmp = (double *)malloc(*sizeOfX*sizeof(double));
computef(osi, sizeOfX,time,x,xdottmp);
free(xdottmp);
*/
// Update the output from level 0 to level 1
_nsds->updateOutput(t,0);
_nsds->updateOutput(t,1);
_nsds->updateOutput(t,2);
//
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = _indexSet0->bundle(*ui);
nsLawSize = inter->nonSmoothLaw()->size();
y = inter->y(0); // output y at this Interaction
ydot = inter->y(1); // output of level 1 at this Interaction
yddot = inter->y(2);
lambda = inter->lambda(2); // input of level 2 at this Interaction
if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
{
for (unsigned int i = 0; i < nsLawSize; ++i)
{
if ((*y)(i) > _TOL_ED)
{
gOut[k] = (*y)(i);
}
else
{
if ((*ydot)(i) > -_TOL_ED)
{
gOut[k] = 100 * _TOL_ED;
}
else
{
gOut[k] = (*y)(i);
}
}
k++;
}
}
else // If Interaction is in the indexSet[2]
{
for (unsigned int i = 0; i < nsLawSize; ++i)
{
if ((*lambda)(i) > _TOL_ED)
{
gOut[k] = (*lambda)(i); // g = lambda[2]
}
else
{
if ((*yddot)(i) > _TOL_ED)
{
gOut[k] = (*lambda)(i);
}
else
{
gOut[k] = 100 * _TOL_ED;
}
}
k++;
}
}
}
}
int main(int argc, char* argv[])
{
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDof = 3; // degrees of freedom for the ball
double t0 = 0; // initial computation time
double T = 2.0; // final computation time
double h = 0.0005; // time step
double position_init = 1.0; // initial position for lowest bead.
double velocity_init = 0.0; // initial velocity for lowest bead.
double theta = 0.5; // theta for MoreauJeanOSI integrator
double R = 0.1; // Ball radius
double m = 1; // Ball mass
double g = 9.81; // Gravity
// -------------------------
// --- Dynamical systems ---
// -------------------------
cout << "====> Model loading ..." << endl << endl;
// Number of Beads
unsigned int nBeads = 10;
double initialGap = 0.25;
double alert = 0.02;
SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof));
(*Mass)(0, 0) = m;
(*Mass)(1, 1) = m;
(*Mass)(2, 2) = 3. / 5 * m * R * R;
// -- Initial positions and velocities --
std::vector<SP::SiconosVector> q0(nBeads);
std::vector<SP::SiconosVector> v0(nBeads);
for (unsigned int i = 0; i < nBeads; i++)
{
(q0[i]).reset(new SiconosVector(nDof));
(v0[i]).reset(new SiconosVector(nDof));
(q0[i])->setValue(0, position_init + i * initialGap);
(v0[i])->setValue(0, velocity_init);
}
// -- The dynamical system --
SP::SiconosVector weight(new SiconosVector(nDof));
(*weight)(0) = -m * g;
std::vector<SP::LagrangianLinearTIDS> beads(nBeads);
for (unsigned int i = 0; i < nBeads; i++)
{
beads[i].reset(new LagrangianLinearTIDS(q0[i], v0[i], Mass));
// -- Set external forces (weight) --
beads[i]->setFExtPtr(weight);
}
// --------------------
// --- Interactions ---
// --------------------
// -- nslaw --
double e = 0.9;
// Interaction ball-floor
//
SP::SimpleMatrix H(new SimpleMatrix(1, nDof));
(*H)(0, 0) = 1.0;
SP::SiconosVector b(new SiconosVector(1));
(*b)(0) = -R;
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
SP::Relation relation(new LagrangianLinearTIR(H, b));
SP::Interaction inter;
// beads/beads interactions
SP::SimpleMatrix HOfBeads(new SimpleMatrix(1, 2 * nDof));
(*HOfBeads)(0, 0) = -1.0;
(*HOfBeads)(0, 3) = 1.0;
SP::SiconosVector bOfBeads(new SiconosVector(1));
(*bOfBeads)(0) = -2 * R;
// This doesn't work !!!
//SP::Relation relationOfBeads(new LagrangianLinearTIR(HOfBeads));
//std::vector<SP::Interaction > interOfBeads(nBeads-1);
// for (unsigned int i =0; i< nBeads-1; i++)
// {
// interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads));
// }
// This works !!
std::vector<SP::Relation > relationOfBeads(nBeads - 1);
std::vector<SP::Interaction > interOfBeads(nBeads - 1);
// for (unsigned int i =0; i< nBeads-1; i++)
// {
// relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads,bOfBeads));
// interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i]));
// }
// --------------------------------------
// --- Model and simulation ---
// --------------------------------------
SP::Model columnOfBeads(new Model(t0, T));
// -- (1) OneStepIntegrators --
SP::MoreauJeanOSI OSI(new MoreauJeanOSI(theta));
// add the dynamical system in the non smooth dynamical system
for (unsigned int i = 0; i < nBeads; i++)
{
columnOfBeads->nonSmoothDynamicalSystem()->insertDynamicalSystem(beads[i]);
}
// // link the interaction and the dynamical system
// for (unsigned int i =0; i< nBeads-1; i++)
// {
// columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i]);
// columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i+1]);
// }
// -- (2) Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
// -- (3) one step non smooth problem
SP::OneStepNSProblem osnspb(new LCP());
// -- (4) Simulation setup with (1) (2) (3)
SP::TimeStepping s(new TimeStepping(t, OSI, osnspb));
columnOfBeads->setSimulation(s);
// =========================== End of model definition ===========================
// ================================= Computation =================================
// --- Simulation initialization ---
cout << "====> Initialisation ..." << endl << endl;
columnOfBeads->initialize();
int N = ceil((T - t0) / h); // Number of time steps
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 1 + nBeads * 4;
SimpleMatrix dataPlot(N + 1, outputSize);
dataPlot(0, 0) = columnOfBeads->t0();
for (unsigned int i = 0; i < nBeads; i++)
{
dataPlot(0, 1 + i * 2) = (beads[i]->q())->getValue(0);
dataPlot(0, 2 + i * 2) = (beads[i]->velocity())->getValue(0);
// dataPlot(0,3+i*4) = (beads[i]->p(1))->getValue(0);
}
// for (unsigned int i =1; i< nBeads; i++)
// {
// dataPlot(0,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0);
// }
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
int ncontact = 0 ;
bool isOSNSinitialized = false;
while (s->hasNextEvent())
{
// Rough contact detection
for (unsigned int i = 0; i < nBeads - 1; i++)
{
// Between first bead and plane
if (abs(((beads[i])->q())->getValue(0) - R) < alert)
{
if (!inter)
{
ncontact++;
// std::cout << "Number of contact = " << ncontact << std::endl;
inter.reset(new Interaction(1, nslaw, relation));
columnOfBeads->nonSmoothDynamicalSystem()->link(inter, beads[0]);
s->initializeInteraction(s->nextTime(), inter);
if (!isOSNSinitialized)
{
s->initOSNS();
isOSNSinitialized = true;
}
assert(inter->y(0)->getValue(0) >= 0);
}
}
// Between two beads
if (abs(((beads[i + 1])->q())->getValue(0) - ((beads[i])->q())->getValue(0) - 2 * R) < alert)
{
//std::cout << "Alert distance for declaring contact = ";
//std::cout << abs(((beads[i])->q())->getValue(0)-((beads[i+1])->q())->getValue(0)) <<std::endl;
if (!interOfBeads[i].get())
{
ncontact++;
// std::cout << "Number of contact = " << ncontact << std::endl;
relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads, bOfBeads));
interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i]));
columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i], beads[i], beads[i+1]);
s->initializeInteraction(s->nextTime(), interOfBeads[i]);
if (!isOSNSinitialized)
{
s->initOSNS();
isOSNSinitialized = true;
}
assert(interOfBeads[i]->y(0)->getValue(0) >= 0);
}
}
}
s->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = s->nextTime();
for (unsigned int i = 0; i < nBeads; i++)
{
dataPlot(k, 1 + i * 2) = (beads[i]->q())->getValue(0);
dataPlot(k, 2 + i * 2) = (beads[i]->velocity())->getValue(0);
}
// for (unsigned int i =1; i< nBeads; i++)
// {
// dataPlot(k,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0);
// }
// for (unsigned int i =1; i< nBeads; i++)
// {
// std::cout << (interOfBeads[i-1]->y(0))->getValue(0) << std::endl ;
// }
s->nextStep();
++show_progress;
k++;
}
cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
cout << "Computation Time " << time.elapsed() << endl;
// --- Output files ---
cout << "====> Output file writing ..." << 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);
cout << "====> Comparison with reference file ..." << endl;
std::cout << "Error w.r.t. reference file : " << (dataPlot - dataPlotRef).normInf() << std::endl;
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The result is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught in ColumnOfBeadsTS.cpp" << endl;
}
}
void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos)
{
SP::LagrangianR lr = std11::static_pointer_cast<LagrangianR>(inter->relation());
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n");
inter->y(0)->display();
printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n");
lr->jachq()->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n");
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter);
InteractionsGraph::OEIterator oei, oeiend;
for(std11::tie(oei, oeiend) = indexSet->out_edges(ui);
oei != oeiend; ++oei)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei));
lds->q()->display();
}
#endif
//unsigned int sizeY = inter->nonSmoothLaw()->size();
// y and lambda vectors
SP::SiconosVector lambda = inter->lambda(0);
SP::SiconosVector y = inter->y(0);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// Copy _w/_z values, starting from index pos into y/lambda.
//setBlock(*_w, y, sizeY, pos, 0);
setBlock(*_z, lambda, sizeY, pos, 0);
#ifdef MLCPPROJ_DEBUG
printf("MLCPP lambda of Interaction is pos =%i :\n", pos);
// aBuff->display();
lambda->display();
unsigned int nslawsize = inter->nonSmoothLaw()->size();
SP::SiconosVector aBuff(new SiconosVector(nslawsize));
setBlock(*_z, aBuff, sizeY, pos, 0);
SP::SiconosMatrix J = lr->jachq();
SP::SimpleMatrix aux(new SimpleMatrix(*J));
aux->trans();
// SP::SiconosVector tmp(new SiconosVector(*(lr->q())));
// prod(*aux, *aBuff, *(tmp), false);
// //prod(*aux,*lambda,*(lr->q()),false);
// std:: std::cout << " tmp = tmp + J^T * lambda" << std::endl;
// tmp->display();
#endif
// // WARNING : Must not be done here. and should be called with the correct time.
// // compute p(0)
// inter->computeInput(0.0 ,0);
// // \warning aBuff should normally be in lambda[0]
// // The update of the position in DS should be made
// // in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// SP::SiconosMatrix J=lr->jachq();
// SP::SimpleMatrix aux(new SimpleMatrix(*J));
// aux->trans();
// SP::SiconosVector tmp (new SiconosVector(*(lr->q())));
// std:: std::cout << " tmp ="<<std::endl;
// tmp->display();
// std:: std::cout << " lr->q() ="<<std::endl;
// lr->q()->display();
// //prod(*aux,*lambda,*(lr->q()),false);
// prod(*aux,*aBuff,*(tmp),false);
// std:: std::cout << " tmp = tmp + J * lambda"<<std::endl;
// tmp->display();
// // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// DSIterator itDS = inter->dynamicalSystemsBegin();
// while(itDS!=inter->dynamicalSystemsEnd())
// {
// Type::Siconos dsType = Type::value(**itDS);
// if((dsType !=Type::LagrangianDS) and
// (dsType !=Type::LagrangianLinearTIDS) )
// {
// RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type.");
// }
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS);
// SP::SiconosVector q = d->q();
// *q += *d->p(0);
// std::cout << " q=" << std::endl;
// q->display();
// itDS++;
// }
// if ((*lr->q() - *tmp).normInf() > 1e-12)
// {
// RuntimeException::selfThrow("youyou");
// }
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangianR _z\n");
_z->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR updated\n");
VectorOfBlockVectors& DSlink = *(indexSet->properties(ui)).DSlink;
// (*DSlink[LagrangianR::q0]).display();
// (lr->q())->display();
#endif
//RuntimeException::selfThrow("MLCPProjectOnConstraints::postComputeLagrangianR() - not yet implemented");
}