SP::SiconosVector Simulation::lambda(unsigned int level, unsigned int coor)
{
// return input(level) (ie with lambda[level]) for all Interactions.
// assert(level>=0);
DEBUG_BEGIN("Simulation::input(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 lambda (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);
lambda->setValue(i,inter->lambda(level)->getValue(coor));
i++;
}
DEBUG_END("Simulation::input(unsigned int level, unsigned int coor)\n");
return lambda;
}
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::updateIndexSet(unsigned int i)
{
assert(_nsds);
assert(_nsds->topology());
SP::Topology topo = _nsds->topology();
assert(i < topo->indexSetsSize() &&
"EventDriven::updateIndexSet(i), indexSets[i] does not exist.");
// IndexSets[0] must not be updated in simulation, since it belongs to Topology.
assert(i > 0 &&
"EventDriven::updateIndexSet(i=0), indexSets[0] cannot be updated.");
// For all Interactions in indexSet[i-1], compute y[i-1] and
// update the indexSet[i].
SP::InteractionsGraph indexSet1 = topo->indexSet(1);
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
assert(_indexSet0);
assert(indexSet1);
assert(indexSet2);
// DEBUG_PRINTF("update indexSets start : indexSet0 size : %ld\n", indexSet0->size());
// DEBUG_PRINTF("update IndexSets start : indexSet1 size : %ld\n", indexSet1->size());
// DEBUG_PRINTF("update IndexSets start : indexSet2 size : %ld\n", indexSet2->size());
InteractionsGraph::VIterator uibegin, uipend, uip;
std11::tie(uibegin, uipend) = _indexSet0->vertices();
// loop over all vertices of the indexSet[i-1]
for (uip = uibegin; uip != uipend; ++uip)
{
SP::Interaction inter = _indexSet0->bundle(*uip);
if (i == 1) // IndexSet[1]
{
// if indexSet[1]=>getYRef(0): output y
// if indexSet[2]=>getYRef(1): output ydot
double y = inter->getYRef(0); // output to define the IndexSets at this Interaction
if (y < -_TOL_ED) // y[0] < 0
{
inter->display();
cout << "y = " << y << " < -_TOL_ED = " << -_TOL_ED <<endl;
RuntimeException::selfThrow("EventDriven::updateIndexSet, output of level 0 must be positive!!! ");
}
// 1 - If the Interaction is not yet in the set
if (!indexSet1->is_vertex(inter)) // Interaction is not yet in the indexSet[i]
{
if (fabs(y) <= _TOL_ED)
{
// vertex and edges insertions
indexSet1->copy_vertex(inter, *_indexSet0);
}
}
else // if the Interaction was already in the set
{
if (fabs(y) > _TOL_ED)
{
indexSet1->remove_vertex(inter); // remove the Interaction from IndexSet[1]
inter->lambda(1)->zero(); // reset the lambda[1] to zero
}
}
}
else if (i == 2) // IndexSet[2]
{
if (indexSet1->is_vertex(inter)) // Interaction is in the indexSet[1]
{
double y = inter->getYRef(1); // output of level 1 at this Interaction
if (!indexSet2->is_vertex(inter)) // Interaction is not yet in the indexSet[2]
{
if (fabs(y) <= _TOL_ED)
{
// vertex and edges insertions
indexSet2->copy_vertex(inter, *_indexSet0);
}
}
else // if the Interaction was already in the set
{
if (fabs(y) > _TOL_ED)
{
indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[1]
inter->lambda(2)->zero(); // reset the lambda[i] to zero
}
}
}
else // Interaction is not in the indexSet[1]
{
if (indexSet2->is_vertex(inter)) // Interaction is in the indexSet[2]
{
indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[2]
inter->lambda(2)->zero(); // reset the lambda[i] to zero
}
}
}
else
{
RuntimeException::selfThrow("EventDriven::updateIndexSet, IndexSet[i > 2] doesn't exist");
}
}
// DEBUG_PRINTF("update indexSets end : indexSet0 size : %ld\n", indexSet0->size());
// DEBUG_PRINTF("update IndexSets end : indexSet1 size : %ld\n", indexSet1->size());
// DEBUG_PRINTF("update IndexSets end : indexSet2 size : %ld\n", indexSet2->size());
}
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++;
}
}
}
}
void KernelTest::t6()
{
SP::Model bouncingBall = Siconos::load("BouncingBall1.xml");
try
{
double T = bouncingBall->finalT();
double t0 = bouncingBall->t0();
double h = bouncingBall->simulation()->timeStep();
int N = (int)((T - t0) / h); // Number of time steps
SP::DynamicalSystemsGraph dsg =
bouncingBall->nonSmoothDynamicalSystem()->topology()->dSG(0);
SP::LagrangianDS ball = std11::static_pointer_cast<LagrangianDS>
(dsg->bundle(*(dsg->begin())));
SP::TimeStepping s = std11::static_pointer_cast<TimeStepping>(bouncingBall->simulation());
SP::Interaction inter;
InteractionsGraph::VIterator ui, uiend;
SP::InteractionsGraph indexSet0 = bouncingBall->nonSmoothDynamicalSystem()->topology()->indexSet(0);
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
inter = indexSet0->bundle(*ui);
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 5;
SimpleMatrix dataPlot(N + 1, outputSize);
SP::SiconosVector q = ball->q();
SP::SiconosVector v = ball->velocity();
SP::SiconosVector p = ball->p(1);
SP::SiconosVector lambda = inter->lambda(1);
dataPlot(0, 0) = bouncingBall->t0();
dataPlot(0, 1) = (*q)(0);
dataPlot(0, 2) = (*v)(0);
dataPlot(0, 3) = (*p)(0);
dataPlot(0, 4) = (*lambda)(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();
while (s->hasNextEvent())
{
s->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = s->nextTime();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*v)(0);
dataPlot(k, 3) = (*p)(0);
dataPlot(k, 4) = (*lambda)(0);
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);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout <<
"Warning. The results is rather different from the reference file :"
<<
(dataPlot - dataPlotRef).normInf()
<<
std::endl;
CPPUNIT_ASSERT(false);
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
CPPUNIT_ASSERT(false);
}
catch (...)
{
cout << "Exception caught in BouncingBallTS.cpp" << endl;
CPPUNIT_ASSERT(false);
}
}
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");
}