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 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); } }
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; }
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 = _simulation->nextTime(); // End of the time step double told = _simulation->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. // SP::DynamicalSystem ds; // Current Dynamical System. Type::Siconos dsType ; // Type of the current DS. double maxResidu = 0; double normResidu = maxResidu; DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); dsType = Type::value(*ds); // Its type SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // 1 - Lagrangian Non Linear Systems if (dsType == Type::LagrangianDS) { 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) } // 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) { 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; }