void visit(const NewtonImpactNSL& nslaw) { double e; e = nslaw.e(); Index subCoord(4); subCoord[0] = 0; subCoord[1] = _inter->nonSmoothLaw()->size(); subCoord[2] = 0; subCoord[3] = subCoord[1]; subscal(e, *_inter->yOld(_osnsp->inputOutputLevel()), *(_inter->yForNSsolver()), subCoord, false); // q = q + e * q }
void visit(const NewtonImpactNSL& nslaw) { double e; e = nslaw.e(); Index subCoord(4); subCoord[0] = 0; subCoord[1] = _inter->nonSmoothLaw()->size(); subCoord[2] = 0; subCoord[3] = subCoord[1]; // Only the normal part is multiplied by e SP::SiconosVector y_k_1 ; y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1); // std::cout << "y_k_1 " << std::endl; // y_k_1->display(); subscal(e, *y_k_1, *(_inter->yForNSsolver()), subCoord, false); }
void visit(const NewtonImpactNSL& nslaw) { double e; e = nslaw.e(); Index subCoord(4); subCoord[0] = 0; subCoord[1] = _inter->nonSmoothLaw()->size(); subCoord[2] = 0; subCoord[3] = subCoord[1]; // Only the normal part is multiplied by e const SiconosVector& y_k_1( _inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1)); DEBUG_PRINTF("_osnsp->inputOutputLevel() = %i \n ",_osnsp->inputOutputLevel() ); DEBUG_EXPR(y_k_1.display());; SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS]; subscal(e, y_k_1, osnsp_rhs, subCoord, false); }
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { DEBUG_BEGIN("SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)\n"); /** \warning: ensures that it can also work with two different osi for two different ds ? */ SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); VectorOfBlockVectors& inter_work_block = *indexSet->properties(vertex_inter).workBlockVectors; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; SP::SiconosMatrix D; SP::SiconosMatrix F; SP::BlockVector deltax; SiconosVector& osnsp_rhs = *(*indexSet->properties(vertex_inter).workVectors)[SchatzmanPaoliOSI::OSNSP_RHS]; SP::SiconosVector e; SP::BlockVector Xfree = inter_work_block[SchatzmanPaoliOSI::xfree];; assert(Xfree); SP::Interaction mainInteraction = inter; assert(mainInteraction); assert(mainInteraction->relation()); DEBUG_EXPR(inter->display(););
// Fill the SparseMat void BlockCSRMatrix::fill(SP::InteractionsGraph indexSet) { // ======> Aim: find inter1 and inter2 both in indexSets[level] and which // have common DynamicalSystems. Then get the corresponding matrix // from map blocks. assert(indexSet); // Number of blocks in a row = number of active constraints. _nr = indexSet->size(); // (re)allocate memory for ublas matrix _blockCSR->resize(_nr, _nr, false); _diagsize0->resize(_nr); _diagsize1->resize(_nr); // === Loop through "active" Interactions (ie present in // indexSets[level]) === int sizeV = 0; InteractionsGraph::VIterator vi, viend; for (std11::tie(vi, viend) = indexSet->vertices(); vi != viend; ++vi) { SP::Interaction inter = indexSet->bundle(*vi); assert(inter->nonSmoothLaw()->size() > 0); sizeV += inter->nonSmoothLaw()->size(); (*_diagsize0)[indexSet->index(*vi)] = sizeV; (*_diagsize1)[indexSet->index(*vi)] = sizeV; assert((*_diagsize0)[indexSet->index(*vi)] > 0); assert((*_diagsize1)[indexSet->index(*vi)] > 0); (*_blockCSR)(indexSet->index(*vi), indexSet->index(*vi)) = indexSet->properties(*vi).block->getArray(); } InteractionsGraph::EIterator ei, eiend; for (std11::tie(ei, eiend) = indexSet->edges(); ei != eiend; ++ei) { InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei); InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei); SP::Interaction inter1 = indexSet->bundle(vd1); SP::Interaction inter2 = indexSet->bundle(vd2); assert(indexSet->index(vd1) < _nr); assert(indexSet->index(vd2) < _nr); assert(indexSet->is_vertex(inter2)); assert(vd2 == indexSet->descriptor(inter2)); assert(indexSet->index(vd2) == indexSet->index(indexSet->descriptor(inter2))); unsigned int pos = indexSet->index(vd1); unsigned int col = indexSet->index(vd2); assert(pos != col); (*_blockCSR)(std::min(pos, col), std::max(pos, col)) = indexSet->properties(*ei).upper_block->getArray(); (*_blockCSR)(std::max(pos, col), std::min(pos, col)) = indexSet->properties(*ei).lower_block->getArray(); } DEBUG_EXPR(display(););
SetupLevels(SP::Simulation s, SP::Interaction inter, SP::DynamicalSystem ds) : _parent(s), _interaction(inter), _ds(ds) { _nonSmoothLaw = inter->nonSmoothLaw(); };
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 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++; } } } }
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(););
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor> Topology::addInteractionInIndexSet0(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2) { // !! Private function !! // // Add inter and ds into IG/DSG // Compute number of constraints unsigned int nsLawSize = inter->nonSmoothLaw()->size(); unsigned int m = inter->getSizeOfY() / nsLawSize; if (m > 1) RuntimeException::selfThrow("Topology::addInteractionInIndexSet0 - m > 1. Obsolete !"); _numberOfConstraints += nsLawSize; SP::DynamicalSystem ds2_ = ds2; // _DSG is the hyper forest : (vertices : dynamical systems, edges : // Interactions) // // _IG is the hyper graph : (vertices : Interactions, edges : // dynamical systems) assert(_DSG[0]->edges_number() == _IG[0]->size()); // _IG = L(_DSG), L is the line graph transformation // vector of the Interaction DynamicalSystemsGraph::VDescriptor dsgv1, dsgv2; dsgv1 = _DSG[0]->add_vertex(ds1); SP::VectorOfVectors workVds1 = _DSG[0]->properties(dsgv1).workVectors; SP::VectorOfVectors workVds2; if (!workVds1) { workVds1.reset(new VectorOfVectors()); _DSG[0]->properties(dsgv1).workMatrices.reset(new VectorOfMatrices()); ds1->initWorkSpace(*workVds1, *_DSG[0]->properties(dsgv1).workMatrices); } if(ds2) { dsgv2 = _DSG[0]->add_vertex(ds2); workVds2 = _DSG[0]->properties(dsgv2).workVectors; if (!workVds2) { workVds2.reset(new VectorOfVectors()); _DSG[0]->properties(dsgv2).workMatrices.reset(new VectorOfMatrices()); ds2->initWorkSpace(*workVds2, *_DSG[0]->properties(dsgv2).workMatrices); } } else { dsgv2 = dsgv1; ds2_ = ds1; workVds2 = workVds1; } // this may be a multi edges graph assert(!_DSG[0]->is_edge(dsgv1, dsgv2, inter)); assert(!_IG[0]->is_vertex(inter)); InteractionsGraph::VDescriptor ig_new_ve; DynamicalSystemsGraph::EDescriptor new_ed; std11::tie(new_ed, ig_new_ve) = _DSG[0]->add_edge(dsgv1, dsgv2, inter, *_IG[0]); InteractionProperties& interProp = _IG[0]->properties(ig_new_ve); interProp.DSlink.reset(new VectorOfBlockVectors); interProp.workVectors.reset(new VectorOfVectors); interProp.workMatrices.reset(new VectorOfSMatrices); unsigned int nslawSize = inter->nonSmoothLaw()->size(); interProp.block.reset(new SimpleMatrix(nslawSize, nslawSize)); inter->setDSLinkAndWorkspace(interProp, *ds1, *workVds1, *ds2_, *workVds2); // add self branches in vertex properties // note : boost graph SEGFAULT on self branch removal // see https://svn.boost.org/trac/boost/ticket/4622 _IG[0]->properties(ig_new_ve).source = ds1; _IG[0]->properties(ig_new_ve).source_pos = 0; if(!ds2) { _IG[0]->properties(ig_new_ve).target = ds1; _IG[0]->properties(ig_new_ve).target_pos = 0; } else { _IG[0]->properties(ig_new_ve).target = ds2; _IG[0]->properties(ig_new_ve).target_pos = ds1->getDim(); } assert(_IG[0]->bundle(ig_new_ve) == inter); assert(_IG[0]->is_vertex(inter)); assert(_DSG[0]->is_edge(dsgv1, dsgv2, inter)); assert(_DSG[0]->edges_number() == _IG[0]->size()); return std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>(new_ed, ig_new_ve); }
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems(); SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; SP::Interaction mainInteraction = inter; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; // SP::SiconosMatrix D; // SP::SiconosMatrix F; SiconosVector& yForNSsolver = *inter->yForNSsolver(); SP::BlockVector Xfree; // All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used. /* V.A. 10/10/2010 * Following the type of OSNS we need to retrieve the velocity or the acceleration * This tricks is not very nice but for the moment the OSNS do not known if * it is in accelaration of not */ //SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { if (relationType == Lagrangian) { Xfree = DSlink[LagrangianR::xfree]; } // else if (relationType == NewtonEuler) // { // Xfree = inter->data(NewtonEulerR::free); // } assert(Xfree); // std::cout << "Computeqblock Xfree (Gamma)========" << std::endl; // Xfree->display(); } else if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp) { Xfree = DSlink[LagrangianR::q1]; // std::cout << "Computeqblock Xfree (Velocity)========" << std::endl; // Xfree->display(); } else RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong "); if (relationType == Lagrangian) { C = mainInteraction->relation()->C(); if (C) { assert(Xfree); coord[3] = C->size(1); coord[5] = C->size(1); subprod(*C, *Xfree, yForNSsolver, coord, true); } SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY)); ID->eye(); Index xcoord(8); xcoord[0] = 0; xcoord[1] = sizeY; xcoord[2] = 0; xcoord[3] = sizeY; xcoord[4] = 0; xcoord[5] = sizeY; xcoord[6] = 0; xcoord[7] = sizeY; // For the relation of type LagrangianRheonomousR if (relationSubType == RheonomousR) { if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR"); } else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp) { SiconosVector q = *DSlink[LagrangianR::q0]; SiconosVector z = *DSlink[LagrangianR::z]; std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z); *DSlink[LagrangianR::z] = z; subprod(*ID, *(std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->hDot()), yForNSsolver, xcoord, false); // y += hDot } else RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not implemented for SICONOS_OSNSP "); } // For the relation of type LagrangianScleronomousR if (relationSubType == ScleronomousR) { if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp) { std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->computedotjacqhXqdot(simulation()->getTkp1(), *inter, DSlink); subprod(*ID, *(std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->dotjacqhXqdot()), yForNSsolver, xcoord, false); // y += NonLinearPart } } } else RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for Relation of type " + relationType); if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp) { if (inter->relation()->getType() == Lagrangian || inter->relation()->getType() == NewtonEuler) { SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter)); inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput); } } }
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp) { /** \warning: ensures that it can also work with two different osi for two different ds ? */ SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel()); SP::Interaction inter = indexSet->bundle(vertex_inter); SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems(); VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink; // Get relation and non smooth law types RELATION::TYPES relationType = inter->relation()->getType(); RELATION::SUBTYPES relationSubType = inter->relation()->getSubType(); unsigned int sizeY = inter->nonSmoothLaw()->size(); unsigned int relativePosition = 0; Index coord(8); coord[0] = relativePosition; coord[1] = relativePosition + sizeY; coord[2] = 0; coord[4] = 0; coord[6] = 0; coord[7] = sizeY; SP::SiconosMatrix C; SP::SiconosMatrix D; SP::SiconosMatrix F; SP::BlockVector deltax; SiconosVector& yForNSsolver = *inter->yForNSsolver(); SP::SiconosVector e; SP::BlockVector Xfree; if (relationType == NewtonEuler) { Xfree = DSlink[NewtonEulerR::xfree]; } else if (relationType == Lagrangian) { Xfree = DSlink[LagrangianR::xfree]; } assert(Xfree); assert(Xfree); SP::Interaction mainInteraction = inter; assert(mainInteraction); assert(mainInteraction->relation()); if (relationSubType == LinearTIR) { if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() != osnsp) RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeOutput not yet implemented for SICONOS_OSNSP "); C = mainInteraction->relation()->C(); if (C) { assert(Xfree); coord[3] = C->size(1); coord[5] = C->size(1); // creates a POINTER link between workX[ds] (xfree) and the // corresponding interactionBlock in each Interactionfor each ds of the // current Interaction. if (_useGammaForRelation) { assert(deltax); subprod(*C, *deltax, yForNSsolver, coord, true); } else { subprod(*C, *Xfree, yForNSsolver, coord, true); // subprod(*C,*(*(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,true); // if (mainInteraction->dynamicalSystems()->size() == 2) // { // subprod(*C,*(*++(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,false); // } } } SP::LagrangianLinearTIR ltir = std11::static_pointer_cast<LagrangianLinearTIR> (mainInteraction->relation()); e = ltir->e(); if (e) { yForNSsolver += *e; } } else RuntimeException::selfThrow("SchatzmanPaoliOSI::ComputeFreeOutput not yet implemented for relation of Type : " + relationType); if (inter->relation()->getSubType() == LinearTIR) { SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter)); inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput); } }
void OneStepNSProblem::updateInteractionBlocks() { DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks() starts\n"); // The present functions checks various conditions and possibly // compute interactionBlocks matrices. // // Let interi and interj be two Interactions. // // Things to be checked are: // 1 - is the topology time invariant? // 2 - does interactionBlocks[interi][interj] already exists (ie has been // computed in a previous time step)? // 3 - do we need to compute this interactionBlock? A interactionBlock is // to be computed if interi and interj are in IndexSet1 AND if interi and // interj have common DynamicalSystems. // // The possible cases are: // // - If 1 and 2 are true then it does nothing. 3 is not checked. // - If 1 == true, 2 == false, 3 == false, it does nothing. // - If 1 == true, 2 == false, 3 == true, it computes the // interactionBlock. // - If 1==false, 2 is not checked, and the interactionBlock is // computed if 3==true. // // Get index set from Simulation SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); bool isLinear = simulation()->model()->nonSmoothDynamicalSystem()->isLinear(); // we put diagonal informations on vertices // self loops with bgl are a *nightmare* at the moment // (patch 65198 on standard boost install) if (indexSet->properties().symmetric) { DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Symmetric case"); InteractionsGraph::VIterator vi, viend; for (std11::tie(vi, viend) = indexSet->vertices(); vi != viend; ++vi) { SP::Interaction inter = indexSet->bundle(*vi); unsigned int nslawSize = inter->nonSmoothLaw()->size(); if (! indexSet->properties(*vi).block) { indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize)); } if (!isLinear || !_hasBeenUpdated) { computeDiagonalInteractionBlock(*vi); } } /* interactionBlock must be zeroed at init */ std::vector<bool> initialized; initialized.resize(indexSet->edges_number()); std::fill(initialized.begin(), initialized.end(), false); InteractionsGraph::EIterator ei, eiend; for (std11::tie(ei, eiend) = indexSet->edges(); ei != eiend; ++ei) { SP::Interaction inter1 = indexSet->bundle(indexSet->source(*ei)); SP::Interaction inter2 = indexSet->bundle(indexSet->target(*ei)); /* on adjoint graph there is at most 2 edges between source and target */ InteractionsGraph::EDescriptor ed1, ed2; std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*ei), indexSet->target(*ei)); assert(*ei == ed1 || *ei == ed2); /* the first edge has the lower index */ assert(indexSet->index(ed1) <= indexSet->index(ed2)); // Memory allocation if needed unsigned int nslawSize1 = inter1->nonSmoothLaw()->size(); unsigned int nslawSize2 = inter2->nonSmoothLaw()->size(); unsigned int isrc = indexSet->index(indexSet->source(*ei)); unsigned int itar = indexSet->index(indexSet->target(*ei)); SP::SiconosMatrix currentInteractionBlock; if (itar > isrc) // upper block { if (! indexSet->properties(ed1).upper_block) { indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2)); if (ed2 != ed1) indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block; } currentInteractionBlock = indexSet->properties(ed1).upper_block; } else // lower block { if (! indexSet->properties(ed1).lower_block) { indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2)); if (ed2 != ed1) indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block; } currentInteractionBlock = indexSet->properties(ed1).lower_block; } if (!initialized[indexSet->index(ed1)]) { initialized[indexSet->index(ed1)] = true; currentInteractionBlock->zero(); } if (!isLinear || !_hasBeenUpdated) { { computeInteractionBlock(*ei); } // allocation for transposed block // should be avoided if (itar > isrc) // upper block has been computed { if (!indexSet->properties(ed1).lower_block) { indexSet->properties(ed1).lower_block. reset(new SimpleMatrix(indexSet->properties(ed1).upper_block->size(1), indexSet->properties(ed1).upper_block->size(0))); } indexSet->properties(ed1).lower_block->trans(*indexSet->properties(ed1).upper_block); indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block; } else { assert(itar < isrc); // lower block has been computed if (!indexSet->properties(ed1).upper_block) { indexSet->properties(ed1).upper_block. reset(new SimpleMatrix(indexSet->properties(ed1).lower_block->size(1), indexSet->properties(ed1).lower_block->size(0))); } indexSet->properties(ed1).upper_block->trans(*indexSet->properties(ed1).lower_block); indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block; } } } } else // not symmetric => follow out_edges for each vertices { DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Non symmetric case\n"); InteractionsGraph::VIterator vi, viend; for (std11::tie(vi, viend) = indexSet->vertices(); vi != viend; ++vi) { DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of diaganal block\n"); SP::Interaction inter = indexSet->bundle(*vi); unsigned int nslawSize = inter->nonSmoothLaw()->size(); if (! indexSet->properties(*vi).block) { indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize)); } if (!isLinear || !_hasBeenUpdated) { computeDiagonalInteractionBlock(*vi); } /* on a undirected graph, out_edges gives all incident edges */ InteractionsGraph::OEIterator oei, oeiend; /* interactionBlock must be zeroed at init */ std::map<SP::SiconosMatrix, bool> initialized; for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi); oei != oeiend; ++oei) { /* on adjoint graph there is at most 2 edges between source and target */ InteractionsGraph::EDescriptor ed1, ed2; std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei)); if (indexSet->properties(ed1).upper_block) { initialized[indexSet->properties(ed1).upper_block] = false; } // if(indexSet->properties(ed2).upper_block) // { // initialized[indexSet->properties(ed2).upper_block] = false; // } if (indexSet->properties(ed1).lower_block) { initialized[indexSet->properties(ed1).lower_block] = false; } // if(indexSet->properties(ed2).lower_block) // { // initialized[indexSet->properties(ed2).lower_block] = false; // } } for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi); oei != oeiend; ++oei) { DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of extra-diaganal block\n"); /* on adjoint graph there is at most 2 edges between source and target */ InteractionsGraph::EDescriptor ed1, ed2; std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei)); assert(*oei == ed1 || *oei == ed2); /* the first edge as the lower index */ assert(indexSet->index(ed1) <= indexSet->index(ed2)); SP::Interaction inter1 = indexSet->bundle(indexSet->source(*oei)); SP::Interaction inter2 = indexSet->bundle(indexSet->target(*oei)); // Memory allocation if needed unsigned int nslawSize1 = inter1->nonSmoothLaw()->size(); unsigned int nslawSize2 = inter2->nonSmoothLaw()->size(); unsigned int isrc = indexSet->index(indexSet->source(*oei)); unsigned int itar = indexSet->index(indexSet->target(*oei)); SP::SiconosMatrix currentInteractionBlock; if (itar > isrc) // upper block { if (! indexSet->properties(ed1).upper_block) { indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2)); initialized[indexSet->properties(ed1).upper_block] = false; if (ed2 != ed1) indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block; } currentInteractionBlock = indexSet->properties(ed1).upper_block; } else // lower block { if (! indexSet->properties(ed1).lower_block) { indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2)); initialized[indexSet->properties(ed1).lower_block] = false; if (ed2 != ed1) indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block; } currentInteractionBlock = indexSet->properties(ed1).lower_block; } if (!initialized[currentInteractionBlock]) { initialized[currentInteractionBlock] = true; currentInteractionBlock->zero(); } if (!isLinear || !_hasBeenUpdated) { if (isrc != itar) computeInteractionBlock(*oei); } } } } DEBUG_EXPR(displayBlocks(indexSet););
void MLCPProjectOnConstraints::computeOptions(SP::Interaction inter1, SP::Interaction inter2) { // printf("MLCPProjectOnConstraints::computeOptions\n"); // Get dimension of the NonSmoothLaw (ie dim of the interactionBlock) RELATION::TYPES relationType1; relationType1 = inter1->relation()->getType(); // Retrieve size of Y (projected variable) unsigned int sizeY1; sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints> (_M)->computeSizeForProjection(inter1); // Compute the number of equalities unsigned int equalitySize1 = sizeY1; //default behavior if (Type::value(*(inter1->nonSmoothLaw())) == Type::NewtonImpactFrictionNSL || Type::value(*(inter1->nonSmoothLaw())) == Type::NewtonImpactNSL) { if (_doProjOnEquality) { equalitySize1 = sizeY1; } else { equalitySize1 = 0; } } else if (Type::value(*(inter1->nonSmoothLaw())) == Type::MixedComplementarityConditionNSL) { equalitySize1 = std11::static_pointer_cast<MixedComplementarityConditionNSL>(inter1->nonSmoothLaw())->getEqualitySize(); } // Compute the number of inequalities unsigned int inequalitySize1 = sizeY1 - equalitySize1; if (inter1 == inter2) { //inter1->getExtraInteractionBlock(currentInteractionBlock); _m += inequalitySize1; _n += equalitySize1; // _m=0; //_n=6; if (_curBlock > MLCP_NB_BLOCKS - 2) printf("MLCP.cpp : number of block to small, memory crach below!!!\n"); /*add an equality block.*/ // #ifdef MLCPPROJ_DEBUG // printf("MLCPProjectOnConstraints::computeOptions()\n"); // #endif if (equalitySize1 > 0) { _numerics_problem.blocksRows[_curBlock + 1] = _numerics_problem.blocksRows[_curBlock] + equalitySize1; _numerics_problem.blocksIsComp[_curBlock] = 0; // #ifdef MLCPPROJ_DEBUG // std::cout << "_curBlock : " << _curBlock <<std::endl; // std::cout << "_numerics_problem.blocksRows["<<_curBlock+1 <<" ] : " << _numerics_problem.blocksRows[_curBlock+1] <<std::endl; // std::cout << "_numerics_problem.blocksIsComp["<<_curBlock <<" ] : " << _numerics_problem.blocksIsComp[_curBlock] <<std::endl; // #endif _curBlock++; } /*add a complementarity block.*/ if (inequalitySize1 > 0) { _numerics_problem.blocksRows[_curBlock + 1] = _numerics_problem.blocksRows[_curBlock] + inequalitySize1; _numerics_problem.blocksIsComp[_curBlock] = 1; // #ifdef MLCPPROJ_DEBUG // std::cout << "_curBlock : " << _curBlock <<std::endl; // std::cout << "_numerics_problem.blocksRows["<<_curBlock+1<< "] : " << _numerics_problem.blocksRows[_curBlock+1] <<std::endl; // std::cout << "_numerics_problem.blocksIsComp["<<_curBlock<< "] : " << _numerics_problem.blocksIsComp[_curBlock] <<std::endl; // #endif _curBlock++; } } // #ifdef MLCPPROJ_DEBUG // std::cout << "_m : " << _m <<std::endl; // std::cout << "_n : " << _n <<std::endl; // #endif }
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"); }