void EventDriven::initOSIs() { for (OSIIterator itosi = _allOSI->begin(); itosi != _allOSI->end(); ++itosi) { // Initialize the acceleration like for NewMarkAlphaScheme if ((*itosi)->getType() == OSI::NEWMARKALPHAOSI) { SP::NewMarkAlphaOSI osi_NewMark = std11::static_pointer_cast<NewMarkAlphaOSI>(*itosi); DynamicalSystemsGraph::VIterator dsi, dsend; SP::DynamicalSystemsGraph osiDSGraph = (*itosi)->dynamicalSystemsGraph(); for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi) { if (!(*itosi)->checkOSI(dsi)) continue; SP::DynamicalSystem ds = osiDSGraph->bundle(*dsi); if ((Type::value(*ds) == Type::LagrangianDS) || (Type::value(*ds) == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS>(ds); *(d->workspace(DynamicalSystem::acce_like)) = *(d->acceleration()); // set a0 = ddotq0 // Allocate the memory to stock coefficients of the polynomial for the dense output d->allocateWorkMatrix(LagrangianDS::coeffs_denseoutput, ds->dimension(), (osi_NewMark->getOrderDenseOutput() + 1)); } } } } }
void EulerMoreauOSI::setW(const SiconosMatrix& newValue, SP::DynamicalSystem ds) { // Check if ds is in the OSI if (!OSIDynamicalSystems->isIn(ds)) RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds does not belong to this Integrator ..."); // Check dimensions consistency unsigned int line = newValue.size(0); unsigned int col = newValue.size(1); if (line != col) // Check that newValue is square RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - newVal is not square! "); if (!ds) RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds == NULL."); unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. unsigned int dsN = ds->number(); if (line != sizeW) // check consistency between newValue and dynamical system size RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - unconsistent dimension between newVal and dynamical system to be integrated "); // Memory allocation for W, if required if (!WMap[dsN]) // allocate a new W if required { WMap[dsN].reset(new SimpleMatrix(newValue)); } else // or fill-in an existing one if dimensions are consistent. { if (line == WMap[dsN]->size(0) && col == WMap[dsN]->size(1)) *(WMap[dsN]) = newValue; else RuntimeException::selfThrow("EulerMoreauOSI - setW: inconsistent dimensions with problem size for given input matrix W"); } }
const SimpleMatrix SchatzmanPaoliOSI::getWBoundaryConditions(SP::DynamicalSystem ds) { assert(ds && "SchatzmanPaoliOSI::getWBoundaryConditions(ds): ds == NULL."); // return *(WBoundaryConditionsMap[0]); assert(_WBoundaryConditionsMap[ds->number()] && "SchatzmanPaoliOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL."); return *(_WBoundaryConditionsMap[ds->number()]); // Copy !! }
void Hem5OSI::computeJacobianRhs(double t) { DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); ds->computeJacobianRhsx(t); } }
void SchatzmanPaoliOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) { DEBUG_BEGIN("SchatzmanPaoliOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)\n"); // Get work buffers from the graph VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds); // Check dynamical system type Type::Siconos dsType = Type::value(*ds); assert(dsType == Type::LagrangianLinearTIDS); if(dsType == Type::LagrangianLinearTIDS) { SP::LagrangianLinearTIDS lltids = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); // buffers allocation (inside the graph) ds_work_vectors.resize(SchatzmanPaoliOSI::WORK_LENGTH); ds_work_vectors[SchatzmanPaoliOSI::RESIDU_FREE].reset(new SiconosVector(lltids->dimension())); ds_work_vectors[SchatzmanPaoliOSI::FREE].reset(new SiconosVector(lltids->dimension())); ds_work_vectors[SchatzmanPaoliOSI::LOCAL_BUFFER].reset(new SiconosVector(lltids->dimension())); SP::SiconosVector q0 = lltids->q0(); SP::SiconosVector q = lltids->q(); SP::SiconosVector v0 = lltids->velocity0(); SP::SiconosVector velocity = lltids->velocity(); // We first swap the initial value contained in q and v after initialization. lltids->swapInMemory(); // we compute the new state values double h = _simulation->timeStep(); *q = *q0 + h* * v0; //*velocity=*velocity; we do nothing for the velocity lltids->swapInMemory(); } // W initialization initializeIterationMatrixW(t, ds); for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++) { ds->initializeNonSmoothInput(k); } // if ((*itDS)->getType() == Type::LagrangianDS || (*itDS)->getType() == Type::FirstOrderNonLinearDS) DEBUG_EXPR(ds->display()); DEBUG_END("SchatzmanPaoliOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)\n"); }
void Topology::insertDynamicalSystem(SP::DynamicalSystem ds) { DynamicalSystemsGraph::VDescriptor dsgv = _DSG[0]->add_vertex(ds); _DSG[0]->properties(dsgv).workVectors.reset(new VectorOfVectors()); _DSG[0]->properties(dsgv).workMatrices.reset(new VectorOfMatrices()); ds->initWorkSpace(*_DSG[0]->properties(dsgv).workVectors, *_DSG[0]->properties(dsgv).workMatrices); }
void MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) { DEBUG_BEGIN("MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n"); MoreauJeanOSI::initializeWorkVectorsForDS(t, ds); const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds); VectorOfVectors& workVectors = *_dynamicalSystemsGraph->properties(dsv).workVectors; Type::Siconos dsType = Type::value(*ds); if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->dimension())); } else if(dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS>(ds); workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->getqDim())); } else { RuntimeException::selfThrow("MoreauJeanDirectProjectionOSI::initialize() - DS not of the right type"); } for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++) { DEBUG_PRINTF("ds->initializeNonSmoothInput(%i)\n", k); ds->initializeNonSmoothInput(k); DEBUG_EXPR_WE( SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); if (d->p(k)) std::cout << "d->p(" << k <<" ) exists" << std::endl; ); }
void EulerMoreauOSI::setWPtr(SP::SimpleMatrix newPtr, SP::DynamicalSystem ds) { unsigned int line = newPtr->size(0); unsigned int col = newPtr->size(1); if (line != col) // Check that newPtr is square RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal) - newVal is not square! "); if (!ds) RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal,ds) - ds == NULL."); unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. if (line != sizeW) // check consistency between newValue and dynamical system size RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal) - unconsistent dimension between newVal and dynamical system to be integrated "); WMap[ds->number()] = newPtr; // link with new pointer }
SP::SiconosMatrix EulerMoreauOSI::WBoundaryConditions(SP::DynamicalSystem ds) { assert(ds && "EulerMoreauOSI::WBoundaryConditions(ds): ds == NULL."); // return WBoundaryConditionsMap[0]; // if(WBoundaryConditionsMap[ds]==NULL) // RuntimeException::selfThrow("EulerMoreauOSI::WBoundaryConditions(ds): W[ds] == NULL."); return _WBoundaryConditionsMap[ds->number()]; }
SP::SimpleMatrix EulerMoreauOSI::W(SP::DynamicalSystem ds) { assert(ds && "EulerMoreauOSI::W(ds): ds == NULL."); // return WMap[0]; // if(WMap[ds]==NULL) // RuntimeException::selfThrow("EulerMoreauOSI::W(ds): W[ds] == NULL."); return WMap[ds->number()]; }
SP::DynamicalSystem Topology::getDynamicalSystem(unsigned int requiredNumber) { DynamicalSystemsGraph::VIterator vi, vdend; SP::DynamicalSystem ds; unsigned int currentNumber; for (std11::tie(vi, vdend) = _DSG[0]->vertices(); vi != vdend; ++vi) { ds = _DSG[0]->bundle(*vi); currentNumber = ds->number(); if (currentNumber == requiredNumber) return ds; } RuntimeException::selfThrow("Topology::getDynamicalSystem(n) ds not found."); return ds; }
const SimpleMatrix SchatzmanPaoliOSI::getW(SP::DynamicalSystem ds) { assert(ds && "SchatzmanPaoliOSI::getW(ds): ds == NULL."); // return *(WMap[0]); unsigned int dsN = ds->number(); assert(WMap[dsN] && "SchatzmanPaoliOSI::getW(ds): W[ds] == NULL."); return *(WMap[dsN]); // Copy !! }
void SchatzmanPaoliOSI::display() { OneStepIntegrator::display(); std::cout << "====== SchatzmanPaoliOSI OSI display ======" <<std::endl; DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); std::cout << "--------------------------------" <<std::endl; std::cout << "--> W of dynamical system number " << ds->number() << ": " <<std::endl; if (_dynamicalSystemsGraph->properties(*dsi).W) _dynamicalSystemsGraph->properties(*dsi).W->display(); else std::cout << "-> NULL" <<std::endl; std::cout << "--> and corresponding theta is: " << _theta <<std::endl; } std::cout << "================================" <<std::endl; }
const SimpleMatrix EulerMoreauOSI::getWBoundaryConditions(SP::DynamicalSystem ds) { assert(ds && "EulerMoreauOSI::getWBoundaryConditions(ds): ds == NULL."); // return *(WBoundaryConditionsMap[0]); unsigned int dsN = ds->number(); assert(_WBoundaryConditionsMap[dsN] && "EulerMoreauOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL."); return *(_WBoundaryConditionsMap[dsN]); // Copy !! }
const SimpleMatrix EulerMoreauOSI::getW(SP::DynamicalSystem ds) { int dsN = ds->number(); assert(ds && "EulerMoreauOSI::getW(ds): ds == NULL."); // return *(WMap[0]); assert(WMap[dsN] && "EulerMoreauOSI::getW(ds): W[ds] == NULL."); return *(WMap[dsN]); // Copy !! }
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor> Topology::link(SP::Interaction inter, SP::DynamicalSystem ds, SP::DynamicalSystem ds2) { DEBUG_PRINTF("Topology::link : inter %p, ds1 %p, ds2 %p\n", &*inter, &*ds, &*ds2); if (indexSet0()->is_vertex(inter)) { removeInteractionFromIndexSet(inter); } unsigned int sumOfDSSizes = 0, sumOfZSizes = 0; sumOfDSSizes += ds->getDim(); if(ds->z()) sumOfZSizes += ds->z()->size(); if(ds2) { sumOfDSSizes += ds2->getDim(); if(ds->z()) sumOfZSizes += ds2->z()->size(); inter->setHas2Bodies(true); } DEBUG_PRINTF("sumOfDSSizes = %i\t, sumOfZSizes = %i\n ", sumOfDSSizes, sumOfZSizes); inter->setDSSizes(sumOfDSSizes, sumOfZSizes); return addInteractionInIndexSet0(inter, ds, ds2); }
void EventDriven::initOSIRhs() { // === initialization for OneStepIntegrators === OSI::TYPES osiType = (*_allOSI->begin())->getType(); for (OSIIterator itosi = _allOSI->begin(); itosi != _allOSI->end(); ++itosi) { //Check whether OSIs used are of the same type if ((*itosi)->getType() != osiType) RuntimeException::selfThrow("OSIs used must be of the same type"); // perform the initialization DynamicalSystemsGraph::VIterator dsi, dsend; SP::DynamicalSystemsGraph osiDSGraph = (*itosi)->dynamicalSystemsGraph(); for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi) { if (!(*itosi)->checkOSI(dsi)) continue; SP::DynamicalSystem ds = osiDSGraph->bundle(*dsi); // Initialize right-hand side ds->initRhs(startingTime()); } } }
void D1MinusLinearOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds) { // Get work buffers from the graph VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds); // Check dynamical system type Type::Siconos dsType = Type::value(*ds); assert(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS || dsType == Type::NewtonEulerDS); if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds); lds->init_generalized_coordinates(2); // acceleration is required for the ds lds->init_inverse_mass(); // invMass required to update post-impact velocity ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH); ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(lds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(lds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(lds->dimension())); // Update dynamical system components (for memory swap). lds->computeForces(t, lds->q(), lds->velocity()); lds->swapInMemory(); } else if(dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds); neds->init_inverse_mass(); // invMass required to update post-impact velocity ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH); ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(neds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(neds->dimension())); ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(neds->dimension())); //Compute a first value of the forces to store it in _forcesMemory neds->computeForces(t, neds->q(), neds->twist()); neds->swapInMemory(); } else RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType); for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++) { ds->initializeNonSmoothInput(k); } }
void SchatzmanPaoliOSI::initialize(Model& m) { OneStepIntegrator::initialize(m); // Get initial time double t0 = _simulation->startingTime(); // Compute W(t0) for all ds DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); Type::Siconos dsType = Type::value(*ds); if (dsType == Type::LagrangianLinearTIDS) { // Computation of the first step for starting SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); SP::SiconosVector q0 = d->q0(); SP::SiconosVector q = d->q(); SP::SiconosVector v0 = d->velocity0(); SP::SiconosVector velocity = d->velocity(); // std::cout << " q0 = " << std::endl; // q0->display(); // std::cout << " v0 = " << std::endl; // v0->display(); // We first swap the initial value contained in q and v after initialization. d->qMemory()->swap(*q); d->velocityMemory()->swap(*velocity); // we compute the new state values double h = _simulation->timeStep(); *q = *q0 + h* * v0; //*velocity=*velocity; we do nothing for the velocity // This value will swapped when OneStepIntegrator::saveInMemory will be called // by the rest of Simulation::initialize (_eventsManager->preUpdate();) // SP::SiconosVector qprev = d->qMemory()->getSiconosVector(0); // SP::SiconosVector qprev2 = d->qMemory()->getSiconosVector(1); // SP::SiconosVector vprev = d->velocityMemory()->getSiconosVector(0); // std::cout << " qprev = " << std::endl; // qprev->display(); // std::cout << " qprev2 = " << std::endl; // qprev2->display(); // std::cout << " vprev = " << std::endl; // vprev->display(); } // Memory allocation for workX. workX[ds*] corresponds to xfree (or vfree in lagrangian case). // workX[*itDS].reset(new SiconosVector((*itDS)->dimension())); // W initialization initW(t0, ds, *dsi); // if ((*itDS)->getType() == Type::LagrangianDS || (*itDS)->getType() == Type::FirstOrderNonLinearDS) ds->allocateWorkVector(DynamicalSystem::local_buffer,_dynamicalSystemsGraph->properties(*dsi).W->size(0)); } }
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 SchatzmanPaoliOSI::updateState(const unsigned int level) { double h = simulationLink->timeStep(); double RelativeTol = simulationLink->relativeConvergenceTol(); bool useRCC = simulationLink->useRelativeConvergenceCriteron(); if (useRCC) simulationLink->setRelativeConvergenceCriterionHeld(true); DSIterator it; SP::SiconosMatrix W; for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { SP::DynamicalSystem ds = *it; W = WMap[ds->number()]; // Get the DS type Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian Systems if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { // get dynamical system SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SiconosVector *vfree = d->velocityFree(); SP::SiconosVector q = d->q(); bool baux = dsType == Type::LagrangianDS && useRCC && simulationLink->relativeConvergenceCriterionHeld(); if (level != LEVELMAX) { // To compute q, we solve W(q - qfree) = p if (d->p(level)) { *q = *d->p(level); // q = p W->PLUForwardBackwardInPlace(*q); } // if (d->boundaryConditions()) // for (vector<unsigned int>::iterator // itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // v->setValue(*itindex, 0.0); *q += * ds->workspace(DynamicalSystem::free); } else *q = * ds->workspace(DynamicalSystem::free); // Computation of the velocity SP::SiconosVector v = d->velocity(); SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1} // std::cout << "SchatzmanPaoliOSI::updateState - q_k_1 =" <<std::endl; // q_k_1->display(); // std::cout << "SchatzmanPaoliOSI::updateState - q =" <<std::endl; // q->display(); *v = 1.0 / (2.0 * h) * (*q - *q_k_1); // std::cout << "SchatzmanPaoliOSI::updateState - v =" <<std::endl; // v->display(); // int bc=0; // SP::SiconosVector columntmp(new SiconosVector(ds->getDim())); // if (d->boundaryConditions()) // { // for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // { // _WBoundaryConditionsMap[ds]->getCol(bc,*columntmp); // /*\warning we assume that W is symmetric in the Lagrangian case*/ // double value = - inner_prod(*columntmp, *v); // value += (d->p(level))->getValue(*itindex); // /* \warning the computation of reactionToBoundaryConditions take into // account the contact impulse but not the external and internal forces. // A complete computation of the residue should be better */ // d->reactionToBoundaryConditions()->setValue(bc,value) ; // bc++; // } if (baux) { ds->subWorkVector(q, DynamicalSystem::local_buffer); double aux = ((ds->workspace(DynamicalSystem::local_buffer))->norm2()) / (ds->normRef()); if (aux > RelativeTol) simulationLink->setRelativeConvergenceCriterionHeld(false); } } //2 - Newton Euler Systems else if (dsType == Type::NewtonEulerDS) { // // get dynamical system // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // SP::SiconosVector v = d->velocity(); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate prev v"<<endl; // v->display(); // #endif // /*d->p has been fill by the Relation->computeInput, it contains // B \lambda _{k+1}*/ // *v = *d->p(level); // v = p // d->luW()->PLUForwardBackwardInPlace(*v); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate hWB lambda"<<endl; // v->display(); // #endif // *v += * ds->workspace(DynamicalSystem::free); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate work free"<<endl; // ds->workspace(DynamicalSystem::free)->display(); // std::cout<<"SchatzmanPaoliOSI::updatestate new v"<<endl; // v->display(); // #endif // //compute q // //first step consists in computing \dot q. // //second step consists in updating q. // // // SP::SiconosMatrix T = d->T(); // SP::SiconosVector dotq = d->dotq(); // prod(*T,*v,*dotq,true); // // std::cout<<"SchatzmanPaoliOSI::updateState v"<<endl; // // v->display(); // // std::cout<<"SchatzmanPaoliOSI::updateState dotq"<<endl; // // dotq->display(); // SP::SiconosVector q = d->q(); // // -> get previous time step state // SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0); // SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // // *q = *qold + h*(theta * *v +(1.0 - theta)* *vold) // double coeff = h*_theta; // scal(coeff, *dotq, *q) ; // q = h*theta*v // coeff = h*(1-_theta); // scal(coeff,*dotqold,*q,false); // q += h(1-theta)*vold // *q += *qold; // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"new q before normalizing"<<endl; // q->display(); // #endif // //q[3:6] must be normalized // d->normalizeq(); // dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h); // dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h); // dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h); // dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h); // d->updateT(); RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } }
void SchatzmanPaoliOSI::initW(double t, SP::DynamicalSystem ds) { // This function: // - allocate memory for a matrix W // - insert this matrix into WMap with ds as a key if (!ds) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds == NULL"); if (!OSIDynamicalSystems->isIn(ds)) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds does not belong to the OSI."); unsigned int dsN = ds->number(); if (WMap.find(dsN) != WMap.end()) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - W(ds) is already in the map and has been initialized."); //unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. // Memory allocation for W // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW)); // SP::SiconosMatrix W = WMap[ds]; double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian non linear systems if (dsType == Type::LagrangianDS) { // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q // SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity // WMap[ds].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass(); // SP::SiconosMatrix W = WMap[ds]; // if (C) // scal(-h*_theta, *C,*W,false); // W -= h*_theta*C // if (K) // scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K; // // WBoundaryConditions initialization // if (d->boundaryConditions()) // initWBoundaryConditions(d); RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType); } // 4 - Lagrangian linear systems else if (dsType == Type::LagrangianLinearTIDS) { SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); SP::SiconosMatrix K = d->K(); SP::SiconosMatrix C = d->C(); WMap[dsN].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass(); SP::SiconosMatrix W = WMap[dsN]; if (C) scal(1 / 2.0 * h * _theta, *C, *W, false); // W += 1/2.0*h*_theta *C if (K) scal(h * h * _theta * _theta, *K, *W, false); // W = h*h*_theta*_theta*K // WBoundaryConditions initialization if (d->boundaryConditions()) initWBoundaryConditions(d); } // === === else if (dsType == Type::NewtonEulerDS) { //WMap[ds].reset(new SimpleMatrix(3,3)); RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType); // Remark: W is not LU-factorized nor inversed here. // Function PLUForwardBackward will do that if required. }
void SchatzmanPaoliOSI::computeW(double t, SP::DynamicalSystem ds) { // Compute W matrix of the Dynamical System ds, at time t and for the current ds state. // When this function is called, WMap[ds] is supposed to exist and not to be null // Memory allocation has been done during initW. assert(ds && "SchatzmanPaoliOSI::computeW(t,ds) - ds == NULL"); unsigned int dsN = ds->number(); assert((WMap.find(dsN) != WMap.end()) && "SchatzmanPaoliOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?"); //double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(*ds); SP::SiconosMatrix W = WMap[dsN]; // 1 - Lagrangian non linear systems if (dsType == Type::LagrangianDS) { // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q // SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity // d->computeMass(); // *W = *d->mass(); // if (C) // { // d->computeJacobianqDotForces(t); // scal(-h*_theta, *C,*W,false); // W -= h*_theta*C // } // if (K) // { // d->computeJacobianqForces(t); // scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K; // } RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType); } // 4 - Lagrangian linear systems else if (dsType == Type::LagrangianLinearTIDS) { // Nothing: W does not depend on time. } // === === else if (dsType == Type::NewtonEulerDS) { // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // d->computeJacobianvFL(t); // double thetaFL=_theta; // *(d->luW())=*(d->jacobianvFL()); // scal(h*thetaFL,*(d->jacobianvFL()),*(d->luW()),true); // *(d->luW())+=*(d->massMatrix()); // //cout<<"SchatzmanPaoliOSI::computeW luW before LUFact\n"; // //d->luW()->display(); // d->luW()->PLUFactorizationInPlace(); RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType); // Remark: W is not LU-factorized here. // Function PLUForwardBackward will do that if required. }
double SchatzmanPaoliOSI::computeResidu() { // This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system. // It then computes the norm of each of them and finally return the maximum // value for those norms. // // The state values used are those saved in the DS, ie the last computed ones. // $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$ // $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $ double t = simulationLink->nextTime(); // End of the time step double told = simulationLink->startingTime(); // Beginning of the time step double h = t - told; // time step length // Operators computed at told have index i, and (i+1) at t. // Iteration through the set of Dynamical Systems. // DSIterator it; SP::DynamicalSystem ds; // Current Dynamical System. Type::Siconos dsType ; // Type of the current DS. double maxResidu = 0; double normResidu = maxResidu; for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { ds = *it; // the considered dynamical system dsType = Type::value(*ds); // Its type SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // 1 - Lagrangian Non Linear Systems if (dsType == Type::LagrangianDS) { // // residu = M(q*)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) - pi+1 // // -- Convert the DS into a Lagrangian one. // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector qold =d->qMemory()->getSiconosVector(0); // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // SP::SiconosVector q =d->q(); // d->computeMass(); // SP::SiconosMatrix M = d->mass(); // SP::SiconosVector v = d->velocity(); // v = v_k,i+1 // //residuFree->zero(); // // std::cout << "(*v-*vold)->norm2()" << (*v-*vold).norm2() << std::endl; // prod(*M, (*v-*vold), *residuFree); // residuFree = M(v - vold) // if (d->forces()) // if fL exists // { // // computes forces(ti,vi,qi) // d->computeForces(told,qold,vold); // double coef = -h*(1-_theta); // // residuFree += coef * fL_i // scal(coef, *d->forces(), *residuFree, false); // // computes forces(ti+1, v_k,i+1, q_k,i+1) = forces(t,v,q) // //d->computeForces(t); // // or forces(ti+1, v_k,i+1, q(v_k,i+1)) // //or // SP::SiconosVector qbasedonv(new SiconosVector(*qold)); // *qbasedonv += h*( (1-_theta)* *vold + _theta * *v ); // d->computeForces(t,qbasedonv,v); // coef = -h*_theta; // // residuFree += coef * fL_k,i+1 // scal(coef, *d->forces(), *residuFree, false); // } // if (d->boundaryConditions()) // { // d->boundaryConditions()->computePrescribedVelocity(t); // unsigned int columnindex=0; // SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds]; // SP::SiconosVector columntmp(new SiconosVector(ds->getDim())); // for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // { // double DeltaPrescribedVelocity = // d->boundaryConditions()->prescribedVelocity()->getValue(columnindex) // - vold->getValue(columnindex); // WBoundaryConditions->getCol(columnindex,*columntmp); // *residuFree -= *columntmp * (DeltaPrescribedVelocity); // residuFree->setValue(*itindex, columntmp->getValue(*itindex) *(DeltaPrescribedVelocity)); // columnindex ++; // } // } // *(d->workspace(DynamicalSystem::free))=*residuFree; // copy residuFree in Workfree // // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residufree :" << std::endl; // // residuFree->display(); // if (d->p(1)) // *(d->workspace(DynamicalSystem::free)) -= *d->p(1); // Compute Residu in Workfree Notation !! // // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residu :" << std::endl; // // d->workspace(DynamicalSystem::free)->display(); // normResidu = d->workspace(DynamicalSystem::free)->norm2(); RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } // 2 - Lagrangian Linear Systems else if (dsType == Type::LagrangianLinearTIDS) { // ResiduFree = M(-q_{k}+q_{k-1}) + h^2 (K q_k)+ h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k)) (1) // This formulae is only valid for the first computation of the residual for q = q_k // otherwise the complete formulae must be applied, that is // ResiduFree M(q-2q_{k}+q_{k-1}) + h^2 (K(\theta q+ (1-\theta) q_k)))+ h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) (2) // for q != q_k, the formulae (1) is wrong. // in the sequel, only the equation (1) is implemented // -- Convert the DS into a Lagrangian one. SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); // Get state i (previous time step) from Memories -> var. indexed with "Old" SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1} SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k // std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl; // q_k_1->display(); // std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl; // q_k->display(); // std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl; // v_k->display(); // --- ResiduFree computation Equation (1) --- residuFree->zero(); double coeff; // -- No need to update W -- //SP::SiconosVector v = d->velocity(); // v = v_k,i+1 SP::SiconosMatrix M = d->mass(); prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1}) SP::SiconosMatrix K = d->K(); if (K) { prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi } SP::SiconosMatrix C = d->C(); if (C) prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k) , *residuFree, false); // residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) SP::SiconosVector Fext = d->fExt(); if (Fext) { // computes Fext(ti) d->computeFExt(told); coeff = -h * h * (1 - _theta); scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti) // computes Fext(ti+1) d->computeFExt(t); coeff = -h * h * _theta; scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1) } // if (d->boundaryConditions()) // { // d->boundaryConditions()->computePrescribedVelocity(t); // unsigned int columnindex=0; // SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds]; // SP::SiconosVector columntmp(new SiconosVector(ds->getDim())); // for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // { // double DeltaPrescribedVelocity = // d->boundaryConditions()->prescribedVelocity()->getValue(columnindex) // -vold->getValue(columnindex); // WBoundaryConditions->getCol(columnindex,*columntmp); // *residuFree += *columntmp * (DeltaPrescribedVelocity); // residuFree->setValue(*itindex, - columntmp->getValue(*itindex) *(DeltaPrescribedVelocity)); // columnindex ++; // } // } // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :" << std::endl; // residuFree->display(); (* d->workspace(DynamicalSystem::free)) = *residuFree; // copy residuFree in Workfree if (d->p(0)) *(d->workspace(DynamicalSystem::free)) -= *d->p(0); // Compute Residu in Workfree Notation !! // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS p(0) :" << std::endl; // if (d->p(0)) // d->p(0)->display(); // else // std::cout << " p(0) :" << std::endl; // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residu :" << std::endl; // d->workspace(DynamicalSystem::free)->display(); // normResidu = d->workspace(DynamicalSystem::free)->norm2(); normResidu = 0.0; // we assume that v = vfree + W^(-1) p // normResidu = realresiduFree->norm2(); } else if (dsType == Type::NewtonEulerDS) { // // residu = M(q*)(v_k,i+1 - v_i) - h*_theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-_theta)*forces(ti,vi,qi) - pi+1 // // -- Convert the DS into a Lagrangian one. // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector qold =d->qMemory()->getSiconosVector(0); // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // SP::SiconosVector q =d->q(); // SP::SiconosMatrix massMatrix = d->massMatrix(); // SP::SiconosVector v = d->velocity(); // v = v_k,i+1 // prod(*massMatrix, (*v-*vold), *residuFree); // residuFree = M(v - vold) // if (d->forces()) // if fL exists // { // // computes forces(ti,vi,qi) // SP::SiconosVector fLold=d->fLMemory()->getSiconosVector(0); // double _thetaFL=0.5; // double coef = -h*(1-_thetaFL); // // residuFree += coef * fL_i // scal(coef, *fLold, *residuFree, false); // d->computeForces(t); // // printf("cpmputeFreeState d->FL():\n"); // // d->forces()->display(); // coef = -h*_thetaFL; // scal(coef, *d->forces(), *residuFree, false); // } // *(d->workspace(DynamicalSystem::free))=*residuFree; // //cout<<"SchatzmanPaoliOSI::computeResidu :\n"; // // residuFree->display(); // if ( d->p(1) ) // *(d->workspace(DynamicalSystem::free)) -= *d->p(1); // normResidu = d->workspace(DynamicalSystem::free)->norm2(); RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); if (normResidu > maxResidu) maxResidu = normResidu; } return maxResidu; }
void EulerMoreauOSI::computeFreeState() { // This function computes "free" states of the DS belonging to this Integrator. // "Free" means without taking non-smooth effects into account. DEBUG_PRINT("EulerMoreauOSI::computeFreeState() starts\n"); double t = simulationLink->nextTime(); // End of the time step double told = simulationLink->startingTime(); // Beginning of the time step double h = t - told; // time step length // Operators computed at told have index i, and (i+1) at t. // Note: integration of r with a theta method has been removed // SiconosVector *rold = static_cast<SiconosVector*>(d.rMemory()->getSiconosVector(0)); // Iteration through the set of Dynamical Systems. // DSIterator it; // Iterator through the set of DS. SP::DynamicalSystem ds; // Current Dynamical System. Type::Siconos dsType ; // Type of the current DS. // XXX to be removed -- xhub Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology(); DynamicalSystemsGraph& DSG0 = *topo.dSG(0); for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { ds = *it; // the considered dynamical system // XXX TMP hack -- xhub // we have to iterate over the edges of the DSG0 -> the following won't be necessary anymore // Maurice will do that with subgraph :) DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(ds); VectorOfVectors& workVectors = *DSG0.properties(dsgVD).workVectors; dsType = Type::value(*ds); // Its type SiconosMatrix& W = *WMap[ds->number()]; // Its W EulerMoreauOSI matrix of iteration. // 1 - First Order Non Linear Systems if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS) { // xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold) // with ResiduFree = = M(x - x_k) - h*theta*f(t_{k+1}, x) - h*(1-theta)*f(t_k, x_k) // to be updated at current time: W, f // fold is f at t_k // not time dependant: M FirstOrderNonLinearDS& d = *std11::static_pointer_cast<FirstOrderNonLinearDS>(ds); // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector xold = d->xMemory()->getSiconosVector(0); // xi SiconosVector& x = *d.x(); // x = x_k or x = x_{k+1}^{\alpha} // xfree gets ResiduFree at first SiconosVector& xfree = *workVectors[FirstOrderDS::xfree]; xfree = *workVectors[FirstOrderDS::residuFree]; DEBUG_PRINT("EulerMoreauOSI::computeFreeState xfree <- residuFree\n"); DEBUG_EXPR(xfree.display()); if (_useGamma) { SiconosVector& rold = *d.rMemory()->getSiconosVector(0); double coeff = -h * (1 - _gamma); scal(coeff, rold, xfree, false); // xfree += -h(1-gamma)*rold } // At this point xfree = (ResiduFree - h(1-gamma)*rold) // -> Solve WX = xfree and set xfree = X W.PLUForwardBackwardInPlace(xfree); // at this point, xfree = W^{-1} (ResiduFree - h(1-gamma)*rold) // -> compute real xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold) xfree *= -1.0; xfree += x; DEBUG_EXPR(xfree.display()); // now the crazy intermediate variables // xPartialNS was updated before this fonction call // It constains either 0 (first Newton iterate) // or g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x SiconosVector& xPartialNS = *workVectors[FirstOrderDS::xPartialNS]; DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS from Interaction\n"); DEBUG_EXPR(xPartialNS.display()); // -> Solve WX = g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x // and set xPartialNS = X W.PLUForwardBackwardInPlace(xPartialNS); scal(h, xPartialNS, xPartialNS); // compute real xPartialNS = xfree + ... xPartialNS += xfree; DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS real value\n"); DEBUG_EXPR(xPartialNS.display()); // deltaxForRelation = (\widetilde{K}_{k+1}^{\alpha})^{-1} xPartialNS - x SiconosVector& deltaxForRelation = *workVectors[FirstOrderDS::deltaxForRelation]; deltaxForRelation = xPartialNS; deltaxForRelation -= x; DEBUG_EXPR(deltaxForRelation.display()); // have a look at the end of the DevNotes for this part if (_useGammaForRelation) { if (!(dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)) RuntimeException::selfThrow("EulerMoreauOSI::computeFreeState - _useGammaForRelation == true is only implemented for FirstOrderLinearDS or FirstOrderLinearTIDS"); deltaxForRelation = xfree; scal(_gamma, deltaxForRelation, deltaxForRelation); SiconosVector& xold = *d.xMemory()->getSiconosVector(0); scal(1.0 - _gamma, xold, deltaxForRelation, false); } // some output DEBUG_EXPR(xfree.display();); DEBUG_EXPR(xPartialNS.display(););
void EulerMoreauOSI::initW(double t, SP::DynamicalSystem ds) { // This function: // - allocate memory for a matrix W // - insert this matrix into WMap with ds as a key if (!ds) RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds == NULL"); if (!OSIDynamicalSystems->isIn(ds)) RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds does not belong to the OSI."); unsigned int dsN = ds->number(); if (WMap.find(dsN) != WMap.end()) RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - W(ds) is already in the map and has been initialized."); unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. // Memory allocation for W // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW)); // SP::SiconosMatrix W = WMap[ds]; double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(*ds); // 1 - First order non linear systems if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS) { // // Memory allocation for W // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW)); // SP::SiconosMatrix W = WMap[ds]; // W = M - h*_theta* [jacobian_x f(t,x,z)] SP::FirstOrderNonLinearDS d = std11::static_pointer_cast<FirstOrderNonLinearDS> (ds); // Copy M or I if M is Null into W // SP::SiconosMatrix W = WMap[ds]; if (d->M()) // *W = *d->M(); WMap[dsN].reset(new SimpleMatrix(*d->M())); else { //W->eye(); // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW,Siconos::IDENTITY)); WMap[dsN].reset(new SimpleMatrix(sizeW, sizeW)); // Warning if the Jacobian is a sparse matrix WMap[dsN]->eye(); } SP::SiconosMatrix W = WMap[dsN]; // d->computeJacobianfx(t); // Computation of JacxF is not required here // since it must have been done in OSI->initialize, before a call to this function. // Add -h*_theta*jacobian_XF to W scal(-h * _theta, *d->jacobianfx(), *W, false); } else RuntimeException::selfThrow("EulerMoreauOSI::initW - not yet implemented for Dynamical system type :" + dsType); // Remark: W is not LU-factorized nor inversed here. // Function PLUForwardBackward will do that if required. }
void SchatzmanPaoliOSI::computeFreeState() { // This function computes "free" states of the DS belonging to this Integrator. // "Free" means without taking non-smooth effects into account. //double t = simulationLink->nextTime(); // End of the time step //double told = simulationLink->startingTime(); // Beginning of the time step //double h = t-told; // time step length // Operators computed at told have index i, and (i+1) at t. // Note: integration of r with a theta method has been removed // SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0)); // Iteration through the set of Dynamical Systems. // DSIterator it; // Iterator through the set of DS. SP::DynamicalSystem ds; // Current Dynamical System. SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS. Type::Siconos dsType ; // Type of the current DS. for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { ds = *it; // the considered dynamical system dsType = Type::value(*ds); // Its type W = WMap[ds->number()]; // Its W SchatzmanPaoliOSI matrix of iteration. //1 - Lagrangian Non Linear Systems if (dsType == Type::LagrangianDS) { // // IN to be updated at current time: W, M, q, v, fL // // IN at told: qi,vi, fLi // // Note: indices i/i+1 corresponds to value at the beginning/end of the time step. // // Index k stands for Newton iteration and thus corresponds to the last computed // // value, ie the one saved in the DynamicalSystem. // // "i" values are saved in memory vectors. // // vFree = v_k,i+1 - W^{-1} ResiduFree // // with // // ResiduFree = M(q_k,i+1)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) // // -- Convert the DS into a Lagrangian one. // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector qold =d->qMemory()->getSiconosVector(0); // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // // --- ResiduFree computation --- // // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)] // // // // vFree pointer is used to compute and save ResiduFree in this first step. // SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d]; // (*vfree)=*(d->workspace(DynamicalSystem::freeresidu)); // // -- Update W -- // // Note: during computeW, mass and jacobians of fL will be computed/ // computeW(t,d); // SP::SiconosVector v = d->velocity(); // v = v_k,i+1 // // -- vfree = v - W^{-1} ResiduFree -- // // At this point vfree = residuFree // // -> Solve WX = vfree and set vfree = X // W->PLUForwardBackwardInPlace(*vfree); // // -> compute real vfree // *vfree *= -1.0; // *vfree += *v; RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType); } // 2 - Lagrangian Linear Systems else if (dsType == Type::LagrangianLinearTIDS) { // IN to be updated at current time: Fext // IN at told: qi,vi, fext // IN constants: K,C // Note: indices i/i+1 corresponds to value at the beginning/end of the time step. // "i" values are saved in memory vectors. // vFree = v_i + W^{-1} ResiduFree // with // ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i // -- Convert the DS into a Lagrangian one. SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); // Get state i (previous time step) from Memories -> var. indexed with "Old" SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // q_k // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k // --- ResiduFree computation --- // vFree pointer is used to compute and save ResiduFree in this first step. // Velocity free and residu. vFree = RESfree (pointer equality !!). SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d]; (*qfree) = *(d->workspace(DynamicalSystem::freeresidu)); W->PLUForwardBackwardInPlace(*qfree); *qfree *= -1.0; *qfree += *qold; } // 3 - Newton Euler Systems else if (dsType == Type::NewtonEulerDS) { // // IN to be updated at current time: W, M, q, v, fL // // IN at told: qi,vi, fLi // // Note: indices i/i+1 corresponds to value at the beginning/end of the time step. // // Index k stands for Newton iteration and thus corresponds to the last computed // // value, ie the one saved in the DynamicalSystem. // // "i" values are saved in memory vectors. // // vFree = v_k,i+1 - W^{-1} ResiduFree // // with // // ResiduFree = M(q_k,i+1)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) // // -- Convert the DS into a Lagrangian one. // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // computeW(t,d); // // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector qold =d->qMemory()->getSiconosVector(0); // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // // --- ResiduFree computation --- // // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)] // // // // vFree pointer is used to compute and save ResiduFree in this first step. // SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d]; // (*vfree)=*(d->workspace(DynamicalSystem::freeresidu)); // //*(d->vPredictor())=*(d->workspace(DynamicalSystem::freeresidu)); // // -- Update W -- // // Note: during computeW, mass and jacobians of fL will be computed/ // SP::SiconosVector v = d->velocity(); // v = v_k,i+1 // // -- vfree = v - W^{-1} ResiduFree -- // // At this point vfree = residuFree // // -> Solve WX = vfree and set vfree = X // // std::cout<<"SchatzmanPaoliOSI::computeFreeState residu free"<<endl; // // vfree->display(); // d->luW()->PLUForwardBackwardInPlace(*vfree); // // std::cout<<"SchatzmanPaoliOSI::computeFreeState -WRfree"<<endl; // // vfree->display(); // // scal(h,*vfree,*vfree); // // -> compute real vfree // *vfree *= -1.0; // *vfree += *v; RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType); } }
double D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel() { DEBUG_BEGIN("\n D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()\n"); double t = _simulation->nextTime(); // end of the time step double told = _simulation->startingTime(); // beginning of the time step double h = _simulation->timeStep(); // time step length SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); // all OSNSP SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); /************************************************************************************************************** * Step 1- solve a LCP at acceleration level for lambda^+_{k} for the last set indices * if index2 is empty we should skip this step **************************************************************************************************************/ DEBUG_PRINT("\nEVALUATE LEFT HAND SIDE\n"); DEBUG_EXPR(std::cout<< "allOSNS->empty() " << std::boolalpha << allOSNS->empty() << std::endl << std::endl); DEBUG_EXPR(std::cout<< "allOSNS->size() " << allOSNS->size() << std::endl << std::endl); // -- LEFT SIDE -- DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); Type::Siconos dsType = Type::value(*ds); SP::SiconosVector accFree; SP::SiconosVector work_tdg; SP::SiconosMatrix Mold; DEBUG_EXPR((*it)->display()); if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); accFree = d->workspace(DynamicalSystem::free); /* POINTER CONSTRUCTOR : will contain * the acceleration without contact force */ accFree->zero(); // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit Mold = d->mass(); DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); DEBUG_EXPR(Mold->display()); if (! d->workspace(DynamicalSystem::free_tdg)) { d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ; } work_tdg = d->workspace(DynamicalSystem::free_tdg); work_tdg->zero(); DEBUG_EXPR(work_tdg->display()); if (d->forces()) { d->computeForces(told, qold, vold); DEBUG_EXPR(d->forces()->display()); *accFree += *(d->forces()); } Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree } else if(dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force accFree->zero(); // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit //Mold = d->mass(); assert(!d->mass()->isPLUInversed()); Mold.reset(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); DEBUG_EXPR(Mold->display()); if (! d->workspace(DynamicalSystem::free_tdg)) { d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ; } work_tdg = d->workspace(DynamicalSystem::free_tdg); work_tdg->zero(); DEBUG_EXPR(work_tdg->display()); if (d->forces()) { d->computeForces(told, qold, vold); DEBUG_EXPR(d->forces()->display()); *accFree += *(d->forces()); } Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree } else { RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } DEBUG_PRINT("accFree contains right limit acceleration at t^+_k with contact force :\n"); DEBUG_EXPR(accFree->display()); DEBUG_PRINT("work_tdg contains right limit acceleration at t^+_k without contact force :\n"); DEBUG_EXPR(work_tdg->display()); } if (!allOSNS->empty()) { if (indexSet2->size() >0) { InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui) { inter = indexSet2->bundle(*ui); inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui)); inter->relation()->computeJacg(told, *inter, indexSet2->properties(*ui)); } if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged()) { for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns) { (*itOsns)->setHasBeenUpdated(false); } } assert((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]); if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) // it should be equivalent to indexSet2 { DEBUG_PRINT("We compute lambda^+_{k} \n"); (*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(told); DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display()); } // Note Franck : at the time this results in a call to swapInMem of all Interactions of the NSDS // So let the simu do this. //(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->saveInMemory(); // we push y and lambda in Memories _simulation->nonSmoothDynamicalSystem()->pushInteractionsInMemory(); _simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2); for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); Type::Siconos dsType = Type::value(*ds); if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force SP::SiconosMatrix Mold = d->mass(); Mold->PLUForwardBackwardInPlace(*dummy); *accFree += *(dummy); DEBUG_EXPR(d->p(2)->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force SP::SiconosMatrix Mold(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization DEBUG_EXPR(Mold->display()); Mold->PLUForwardBackwardInPlace(*dummy); *accFree += *(dummy); DEBUG_EXPR(d->p(2)->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } } } /************************************************************************************************************** * Step 2 - compute v_{k,1} **************************************************************************************************************/ DEBUG_PRINT("\n PREDICT RIGHT HAND SIDE\n"); for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); // type of the current DS Type::Siconos dsType = Type::value(*ds); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // contains acceleration without contact force // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // initialize *it->residuFree and predicted right velocity (left limit) SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity residuFree->zero(); v->zero(); DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); *residuFree -= 0.5 * h**accFree; *v += h**accFree; *v += *vold; DEBUG_EXPR(residuFree->display()); DEBUG_EXPR(v->display()); SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1} *q = *qold; scal(0.5 * h, *vold + *v, *q, false); DEBUG_EXPR(q->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // get left state from memory SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // initialize *it->residuFree and predicted right velocity (left limit) SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity residuFree->zero(); v->zero(); DEBUG_EXPR(accFree->display()); DEBUG_EXPR(qold->display()); DEBUG_EXPR(vold->display()); *residuFree -= 0.5 * h**accFree; *v += h**accFree; *v += *vold; DEBUG_EXPR(residuFree->display()); DEBUG_EXPR(v->display()); //first step consists in computing \dot q. //second step consists in updating q. // SP::SiconosMatrix T = d->T(); SP::SiconosVector dotq = d->dotq(); prod(*T, *v, *dotq, true); SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0); SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1} *q = *qold; scal(0.5 * h, *dotqold + *dotq, *q, false); DEBUG_PRINT("new q before normalizing\n"); DEBUG_EXPR(q->display()); //q[3:6] must be normalized d->normalizeq(); d->computeT(); DEBUG_PRINT("new q after normalizing\n"); DEBUG_EXPR(q->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); /** At this step, we obtain * \f[ * \begin{cases} * v_{k,0} = \mbox{\tt vold} \\ * q_{k,0} = qold \\ * F_{k,+} = F(told,qold,vold) \\ * Work_{freefree} = M^{-1}_k (F^+_{k}) \mbox{stored in work_tdg} \\ * Work_{free} = M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in accFree} \\ * R_{free} = -h/2 * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in ResiduFree} \\ * v_{k,1} = v_{k,0} + h * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in v} \\ * q_{k,1} = q_{k,0} + \frac{h}{2} (v_{k,0} + v_{k,1}) \mbox{stored in q} \\ * \end{cases} * \f] **/ } DEBUG_PRINT("\n DECIDE STRATEGY\n"); /** Decide of the strategy impact or smooth multiplier. * Compute _isThereImpactInTheTimeStep */ _isThereImpactInTheTimeStep = false; if (!allOSNS->empty()) { for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++) { _simulation->nonSmoothDynamicalSystem()->updateOutput(_simulation->nextTime(),level); } _simulation->updateIndexSets(); SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology(); SP::InteractionsGraph indexSet3 = topo->indexSet(3); if (indexSet3->size() > 0) { _isThereImpactInTheTimeStep = true; DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true;\n"); } else { _isThereImpactInTheTimeStep = false; DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n"); } } /* If _isThereImpactInTheTimeStep = true; * we recompute residuFree by removing the contribution of the nonimpulsive contact forces. * We add the contribution of the external forces at the end * of the time--step * If _isThereImpactInTheTimeStep = false; * we recompute residuFree by adding the contribution of the external forces at the end * and the contribution of the nonimpulsive contact forces that are computed by solving the osnsp. */ if (_isThereImpactInTheTimeStep) { DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true\n"); for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); // type of the current DS Type::Siconos dsType = Type::value(*ds); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu); SP::SiconosVector v = d->velocity(); SP::SiconosVector q = d->q(); SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix //residuFree->zero(); //v->zero(); SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg); assert(work_tdg); *residuFree = - 0.5 * h**work_tdg; d->computeMass(); DEBUG_EXPR(M->display()); if (d->forces()) { d->computeForces(t, q, v); *work_tdg = *(d->forces()); DEBUG_EXPR(d->forces()->display()); } M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force *residuFree -= 0.5 * h**work_tdg; DEBUG_EXPR(residuFree->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu); SP::SiconosVector v = d->velocity(); SP::SiconosVector q = d->q(); SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization; DEBUG_EXPR(M->display()); //residuFree->zero(); v->zero(); SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg); assert(work_tdg); *residuFree = 0.5 * h**work_tdg; work_tdg->zero(); if (d->forces()) { d->computeForces(t, q, v); *work_tdg += *(d->forces()); } M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force *residuFree -= 0.5 * h**work_tdg; DEBUG_EXPR(residuFree->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } } else { DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n"); // -- RIGHT SIDE -- // calculate acceleration without contact force for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); // type of the current DS Type::Siconos dsType = Type::value(*ds); /* \warning the following conditional statement should be removed with a MechanicalDS class */ if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS)) { SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force accFree->zero(); // get right state from memory SP::SiconosVector q = d->q(); // contains position q_{k+1} SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix DEBUG_EXPR(accFree->display()); DEBUG_EXPR(q->display()); DEBUG_EXPR(v->display()); // Lagrangian Nonlinear Systems if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { d->computeMass(); DEBUG_EXPR(M->display()); if (d->forces()) { d->computeForces(t, q, v); *accFree += *(d->forces()); } } else RuntimeException::selfThrow ("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n"); DEBUG_EXPR(accFree->display()); } else if (dsType == Type::NewtonEulerDS) { SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force accFree->zero(); // get right state from memory SP::SiconosVector q = d->q(); // contains position q_{k+1} SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization; DEBUG_EXPR(accFree->display()); DEBUG_EXPR(q->display()); DEBUG_EXPR(v->display()); if (d->forces()) { d->computeForces(t, q, v); *accFree += *(d->forces()); } M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n"); DEBUG_EXPR(accFree->display()); } else RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType); } // solve a LCP at acceleration level only for contacts which have been active at the beginning of the time-step if (!allOSNS->empty()) { // for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++) // { // _simulation->updateOutput(level); // } // _simulation->updateIndexSets(); DEBUG_PRINT("We compute lambda^-_{k+1} \n"); InteractionsGraph::VIterator ui, uiend; SP::Interaction inter; for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui) { inter = indexSet2->bundle(*ui); inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui)); inter->relation()->computeJacg(t, *inter, indexSet2->properties(*ui)); } if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged()) { for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns) { (*itOsns)->setHasBeenUpdated(false); } } if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) { (*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(t); DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display();); _simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2); }
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; } }