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"); }