void Simulation::computeLevelsForInputAndOutput(SP::Interaction inter, bool init) { DEBUG_PRINT("Simulation::computeLevelsForInputAndOutput(SP::Interaction inter, bool init)\n"); /** \warning. We test only for the first Dynamical of the interaction. * we assume that the osi(s) are consistent for one interaction */ SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet(0); SP::DynamicalSystem ds = indexSet0->properties(indexSet0->descriptor(inter)).source; // Note FP : we should probably connect osi and graph before, in simulation->initialize? DSOSIConstIterator it = _osiMap.find(ds); SP::OneStepIntegrator osi = it->second; if (!osi) RuntimeException::selfThrow("Simulation::computeLevelsForInputAndOutput osi does not exists"); indexSet0->properties(indexSet0->descriptor(inter)).osi = osi; std11::shared_ptr<SetupLevels> setupLevels; setupLevels.reset(new SetupLevels(shared_from_this(), inter, ds)); osi->accept(*(setupLevels.get())); if (!init) // We are not computing the levels at the initialization { SP::Topology topo = model()->nonSmoothDynamicalSystem()->topology(); unsigned int indxSize = topo->indexSetsSize(); assert (_numberOfIndexSets >0); if ((indxSize == LEVELMAX) || (indxSize < _numberOfIndexSets )) { topo->indexSetsResize(_numberOfIndexSets); // Init if the size has changed for (unsigned int i = indxSize; i < topo->indexSetsSize(); i++) // ++i ??? topo->resetIndexSetPtr(i); } } }
void EventDriven::updateIndexSetsWithDoubleCondition() { assert(_nsds); assert(_nsds->topology()); // for all Interactions in indexSet[i-1], compute y[i-1] and // update the indexSet[i] SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); InteractionsGraph::VIterator ui, uiend, vnext; std11::tie(ui, uiend) = indexSet2->vertices(); for (vnext = ui; ui != uiend; ui = vnext) { ++vnext; SP::Interaction inter = indexSet2->bundle(*ui); double gamma = inter->getYRef(2); double F = inter->getLambdaRef(2); if (fabs(F) < _TOL_ED) indexSet2->remove_vertex(inter); else if ((gamma < -_TOL_ED) || (F < -_TOL_ED)) RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), output[2] and lambda[2] for Interactionof indexSet[2] must be higher or equal to zero."); else if (((fabs(gamma) > _TOL_ED) && (fabs(F) > _TOL_ED))) RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), something is wrong for the LCP resolution."); } }
void Simulation::computeLevelsForInputAndOutput() { DEBUG_PRINT("Simulation::computeLevelsForInputAndOutput()\n"); SP::Topology topo = model()->nonSmoothDynamicalSystem()->topology(); InteractionsGraph::VIterator ui, uiend; SP::InteractionsGraph indexSet0 = topo->indexSet0(); for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui) { computeLevelsForInputAndOutput(indexSet0->bundle(*ui), true); } unsigned int indxSize = topo->indexSetsSize(); if ((indxSize == LEVELMAX) || (indxSize < _numberOfIndexSets )) { topo->indexSetsResize(_numberOfIndexSets ); // Init if the size has changed for (unsigned int i = indxSize; i < topo->indexSetsSize(); i++) // ++i ??? topo->resetIndexSetPtr(i); } DEBUG_PRINTF("_numberOfIndexSets =%d\n", _numberOfIndexSets); DEBUG_PRINTF("_levelMinForInput =%d\n", _levelMinForInput); DEBUG_PRINTF("_levelMaxForInput =%d\n", _levelMaxForInput); DEBUG_PRINTF("_levelMinForOutput =%d\n", _levelMinForInput); DEBUG_PRINTF("_levelMaxForOutput =%d\n", _levelMaxForInput); }
void FrictionContact::initialize(SP::Simulation sim) { // - Checks memory allocation for main variables (M,q,w,z) // - Formalizes the problem if the topology is time-invariant // This function performs all steps that are time-invariant // General initialize for OneStepNSProblem LinearOSNS::initialize(sim); // Connect to the right function according to dim. of the problem // get topology SP::Topology topology = simulation()->model()->nonSmoothDynamicalSystem()->topology(); // Note that interactionBlocks is up to date since updateInteractionBlocks // has been called during OneStepNSProblem::initialize() // Fill vector of friction coefficients int sizeMu = simulation()->model()->nonSmoothDynamicalSystem() ->topology()->indexSet(0)->size(); _mu->reserve(sizeMu); // If the topology is TimeInvariant ie if M structure does not // change during simulation: if (topology->indexSet0()->size()>0) { // Get index set from Simulation SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel()); InteractionsGraph::VIterator ui, uiend; for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui) { _mu->push_back(std11::static_pointer_cast<NewtonImpactFrictionNSL> (indexSet->bundle(*ui)->nonSmoothLaw())->mu()); } } }
void TimeStepping::initOSNS() { // === creates links between work vector in OSI and work vector in // Interactions SP::OneStepIntegrator osi; SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet0 = topo->indexSet(0); InteractionsGraph::VIterator ui, uiend; if (!_allNSProblems->empty()) // ie if some Interactions have been // declared and a Non smooth problem // built. { //if (_allNSProblems->size()>1) // RuntimeException::selfThrow("TimeStepping::initialize, at the time, a time stepping simulation can not have more than one non smooth problem."); // At the time, we consider that for all systems, levelMin is // equal to the minimum value of the relative degree - 1 except // for degree 0 case where we keep 0. // === update all index sets === updateIndexSets(); // initialization of OneStepNonSmoothProblem for (OSNSIterator itOsns = _allNSProblems->begin(); itOsns != _allNSProblems->end(); ++itOsns) { if (*itOsns) (*itOsns)->initialize(shared_from_this()); else RuntimeException::selfThrow("TimeStepping::initOSNS failed. A OneStepNSProblem has not been set. "); } } }
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); }
/** get the number of Dynamical Systems present in the NSDS \return an unsigned int */ inline unsigned int getNumberOfDS() const { return _topology->dSG(0)->size(); }
/** get Dynamical system number I * \param nb the identifier of the DynamicalSystem to get * \return a pointer on DynamicalSystem */ inline SP::DynamicalSystem dynamicalSystem(int nb) const { return _topology->getDynamicalSystem(nb); }
void TimeStepping::updateIndexSet(unsigned int i) { // To update IndexSet i: add or remove Interactions from // this set, depending on y values. // boost::default_color_type is used to organize update in InteractionsGraph: // - white_color : undiscovered vertex (Interaction) // - gray_color : discovered vertex (Interaction) but searching descendants // - black_color : discovered vertex (Interaction) together with the descendants assert(_nsds); assert(_nsds->topology()); SP::Topology topo = _nsds->topology(); assert(i < topo->indexSetsSize() && "TimeStepping::updateIndexSet(i), indexSets[i] does not exist."); // IndexSets[0] must not be updated in simulation, since it belongs to Topology. assert(i > 0 && "TimeStepping::updateIndexSet(i=0), indexSets[0] cannot be updated."); // For all Interactions in indexSet[i-1], compute y[i-1] and // update the indexSet[i]. SP::InteractionsGraph indexSet0 = topo->indexSet(0); SP::InteractionsGraph indexSet1 = topo->indexSet(1); assert(indexSet0); assert(indexSet1); DynamicalSystemsGraph& DSG0= *nonSmoothDynamicalSystem()->dynamicalSystems(); topo->setHasChanged(false); DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update indexSets start : indexSet0 size : %ld\n", indexSet0->size()); DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update IndexSets start : indexSet1 size : %ld\n", indexSet1->size()); // Check indexSet1 InteractionsGraph::VIterator ui1, ui1end, v1next; std11::tie(ui1, ui1end) = indexSet1->vertices(); //Remove interactions from the indexSet1 for (v1next = ui1; ui1 != ui1end; ui1 = v1next) { ++v1next; SP::Interaction inter1 = indexSet1->bundle(*ui1); if (indexSet0->is_vertex(inter1)) { InteractionsGraph::VDescriptor inter1_descr0 = indexSet0->descriptor(inter1); assert((indexSet0->color(inter1_descr0) == boost::white_color)); indexSet0->color(inter1_descr0) = boost::gray_color; if (Type::value(*(inter1->nonSmoothLaw())) != Type::EqualityConditionNSL) { // We assume that the integrator of the ds1 drive the update of the index set //SP::OneStepIntegrator Osi = indexSet1->properties(*ui1).osi; SP::DynamicalSystem ds1 = indexSet1->properties(*ui1).source; OneStepIntegrator& osi = *DSG0.properties(DSG0.descriptor(ds1)).osi; //if(predictorDeactivate(inter1,i)) if (osi.removeInteractionFromIndexSet(inter1, i)) { // Interaction is not active // ui1 becomes invalid indexSet0->color(inter1_descr0) = boost::black_color; indexSet1->eraseProperties(*ui1); InteractionsGraph::OEIterator oei, oeiend; for (std11::tie(oei, oeiend) = indexSet1->out_edges(*ui1); oei != oeiend; ++oei) { InteractionsGraph::EDescriptor ed1, ed2; std11::tie(ed1, ed2) = indexSet1->edges(indexSet1->source(*oei), indexSet1->target(*oei)); if (ed2 != ed1) { indexSet1->eraseProperties(ed1); indexSet1->eraseProperties(ed2); } else { indexSet1->eraseProperties(ed1); } } indexSet1->remove_vertex(inter1); /* \warning V.A. 25/05/2012 : Multiplier lambda are only set to zero if they are removed from the IndexSet*/ inter1->lambda(1)->zero(); topo->setHasChanged(true); } } } else { // Interaction is not in indexSet0 anymore. // ui1 becomes invalid indexSet1->eraseProperties(*ui1); InteractionsGraph::OEIterator oei, oeiend; for (std11::tie(oei, oeiend) = indexSet1->out_edges(*ui1); oei != oeiend; ++oei) { InteractionsGraph::EDescriptor ed1, ed2; std11::tie(ed1, ed2) = indexSet1->edges(indexSet1->source(*oei), indexSet1->target(*oei)); if (ed2 != ed1) { indexSet1->eraseProperties(ed1); indexSet1->eraseProperties(ed2); } else { indexSet1->eraseProperties(ed1); } } indexSet1->remove_vertex(inter1); topo->setHasChanged(true); } } // indexSet0\indexSet1 scan InteractionsGraph::VIterator ui0, ui0end; //Add interaction in indexSet1 for (std11::tie(ui0, ui0end) = indexSet0->vertices(); ui0 != ui0end; ++ui0) { if (indexSet0->color(*ui0) == boost::black_color) { // reset indexSet0->color(*ui0) = boost::white_color ; } else { if (indexSet0->color(*ui0) == boost::gray_color) { // reset indexSet0->color(*ui0) = boost::white_color; assert(indexSet1->is_vertex(indexSet0->bundle(*ui0))); /*assert( { !predictorDeactivate(indexSet0->bundle(*ui0),i) || Type::value(*(indexSet0->bundle(*ui0)->nonSmoothLaw())) == Type::EqualityConditionNSL ; });*/ } else { assert(indexSet0->color(*ui0) == boost::white_color); SP::Interaction inter0 = indexSet0->bundle(*ui0); assert(!indexSet1->is_vertex(inter0)); bool activate = true; if (Type::value(*(inter0->nonSmoothLaw())) != Type::EqualityConditionNSL && Type::value(*(inter0->nonSmoothLaw())) != Type::RelayNSL) { //SP::OneStepIntegrator Osi = indexSet0->properties(*ui0).osi; // We assume that the integrator of the ds1 drive the update of the index set SP::DynamicalSystem ds1 = indexSet1->properties(*ui0).source; OneStepIntegrator& osi = *DSG0.properties(DSG0.descriptor(ds1)).osi; activate = osi.addInteractionInIndexSet(inter0, i); } if (activate) { assert(!indexSet1->is_vertex(inter0)); // vertex and edges insertion in indexSet1 indexSet1->copy_vertex(inter0, *indexSet0); topo->setHasChanged(true); assert(indexSet1->is_vertex(inter0)); } } } } assert(indexSet1->size() <= indexSet0->size()); DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update indexSets end : indexSet0 size : %ld\n", indexSet0->size()); DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update IndexSets end : indexSet1 size : %ld\n", indexSet1->size()); }
void EventDriven::updateIndexSet(unsigned int i) { assert(_nsds); assert(_nsds->topology()); SP::Topology topo = _nsds->topology(); assert(i < topo->indexSetsSize() && "EventDriven::updateIndexSet(i), indexSets[i] does not exist."); // IndexSets[0] must not be updated in simulation, since it belongs to Topology. assert(i > 0 && "EventDriven::updateIndexSet(i=0), indexSets[0] cannot be updated."); // For all Interactions in indexSet[i-1], compute y[i-1] and // update the indexSet[i]. SP::InteractionsGraph indexSet1 = topo->indexSet(1); SP::InteractionsGraph indexSet2 = topo->indexSet(2); assert(_indexSet0); assert(indexSet1); assert(indexSet2); // DEBUG_PRINTF("update indexSets start : indexSet0 size : %ld\n", indexSet0->size()); // DEBUG_PRINTF("update IndexSets start : indexSet1 size : %ld\n", indexSet1->size()); // DEBUG_PRINTF("update IndexSets start : indexSet2 size : %ld\n", indexSet2->size()); InteractionsGraph::VIterator uibegin, uipend, uip; std11::tie(uibegin, uipend) = _indexSet0->vertices(); // loop over all vertices of the indexSet[i-1] for (uip = uibegin; uip != uipend; ++uip) { SP::Interaction inter = _indexSet0->bundle(*uip); if (i == 1) // IndexSet[1] { // if indexSet[1]=>getYRef(0): output y // if indexSet[2]=>getYRef(1): output ydot double y = inter->getYRef(0); // output to define the IndexSets at this Interaction if (y < -_TOL_ED) // y[0] < 0 { inter->display(); cout << "y = " << y << " < -_TOL_ED = " << -_TOL_ED <<endl; RuntimeException::selfThrow("EventDriven::updateIndexSet, output of level 0 must be positive!!! "); } // 1 - If the Interaction is not yet in the set if (!indexSet1->is_vertex(inter)) // Interaction is not yet in the indexSet[i] { if (fabs(y) <= _TOL_ED) { // vertex and edges insertions indexSet1->copy_vertex(inter, *_indexSet0); } } else // if the Interaction was already in the set { if (fabs(y) > _TOL_ED) { indexSet1->remove_vertex(inter); // remove the Interaction from IndexSet[1] inter->lambda(1)->zero(); // reset the lambda[1] to zero } } } else if (i == 2) // IndexSet[2] { if (indexSet1->is_vertex(inter)) // Interaction is in the indexSet[1] { double y = inter->getYRef(1); // output of level 1 at this Interaction if (!indexSet2->is_vertex(inter)) // Interaction is not yet in the indexSet[2] { if (fabs(y) <= _TOL_ED) { // vertex and edges insertions indexSet2->copy_vertex(inter, *_indexSet0); } } else // if the Interaction was already in the set { if (fabs(y) > _TOL_ED) { indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[1] inter->lambda(2)->zero(); // reset the lambda[i] to zero } } } else // Interaction is not in the indexSet[1] { if (indexSet2->is_vertex(inter)) // Interaction is in the indexSet[2] { indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[2] inter->lambda(2)->zero(); // reset the lambda[i] to zero } } } else { RuntimeException::selfThrow("EventDriven::updateIndexSet, IndexSet[i > 2] doesn't exist"); } } // DEBUG_PRINTF("update indexSets end : indexSet0 size : %ld\n", indexSet0->size()); // DEBUG_PRINTF("update IndexSets end : indexSet1 size : %ld\n", indexSet1->size()); // DEBUG_PRINTF("update IndexSets end : indexSet2 size : %ld\n", indexSet2->size()); }
/** add a dynamical system * \param ds a pointer to the system to add */ inline void insertDynamicalSystem(SP::DynamicalSystem ds) { _topology->insertDynamicalSystem(ds); _mIsLinear = ((ds)->isLinear() && _mIsLinear); };
/** remove an interaction to the system * \param inter a pointer to the interaction to remove */ inline void removeInteraction(SP::Interaction inter) { _topology->removeInteraction(inter); };
/** get the number of Interactions present in the NSDS. * \return an unsigned int */ inline unsigned int getNumberOfInteractions() const { return _topology->indexSet0()->size(); };
/** get all the dynamical systems declared in the NonSmoothDynamicalSystem. * \return a SP::DynamicalSystemsGraph */ inline const SP::DynamicalSystemsGraph dynamicalSystems() const { return _topology->dSG(0); }
void EventDriven::initOSNS() { assert(_nsds); assert(_nsds->topology()); // for all Interactions in indexSet[i-1], compute y[i-1] and // update the indexSet[i] // Note that interactions set may be empty. InteractionsGraph::VIterator ui, uiend; SP::Topology topo = _nsds->topology(); // === update all index sets === updateIndexSets(); initOSIRhs(); if (!_allNSProblems->empty()) // ie if at least a non smooth problem has been built. { OSI::TYPES osiType = (*_allOSI->begin())->getType(); if (osiType == OSI::LSODAROSI || osiType == OSI::HEM5OSI) //EventDriven associated with LsodarOSI OSI { } else if (osiType == OSI::NEWMARKALPHAOSI) // EventDriven associated with NewMarkAlpha { if (_allNSProblems->size() != 3) RuntimeException::selfThrow (" EventDriven::initialize, \n an EventDriven simulation associated with NewMarkAlphaOSI must have three non smooth problems.\n Here, there are " + _allNSProblems->size()); // Initialize OSNSP at position level (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_POS]->setInputOutputLevel(2); (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_POS]->setIndexSetLevel(2); (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_POS]->initialize(shared_from_this()); } else { RuntimeException::selfThrow(" EventDriven::initialize, OSI not yet implemented."); } if (!((*_allNSProblems)[SICONOS_OSNSP_ED_IMPACT])) /* ie if the impact problem does not * exist */ RuntimeException::selfThrow ("EventDriven::initialize, an EventDriven simulation must have an 'impact' non smooth problem."); if (!((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC])) /* ie if the acceleration-level problem * does not exist */ RuntimeException::selfThrow ("EventDriven::initialize, an EventDriven simulation must have an 'acceleration' non smooth problem."); // Initialize OSNSP for impact problem and at the acceleration level // WARNING: only for Lagrangian systems - To be reviewed for other ones. (*_allNSProblems)[SICONOS_OSNSP_ED_IMPACT]->setInputOutputLevel(1); (*_allNSProblems)[SICONOS_OSNSP_ED_IMPACT]->setIndexSetLevel(1); (*_allNSProblems)[SICONOS_OSNSP_ED_IMPACT]->initialize(shared_from_this()); (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->setInputOutputLevel(2); (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->setIndexSetLevel(2); (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->initialize(shared_from_this()); // // Detect NonSmoothEvent at the beginning of the simulation if( topo->indexSetsSize() > 1) { SP::InteractionsGraph indexSet1 = _nsds->topology()->indexSet(1); if (indexSet1->size() != 0) // There is one non-smooth event to be added { _eventsManager->scheduleNonSmoothEvent(*this, _eventsManager->startingTime(), false); }; } } }
void EventDriven::computeg(SP::OneStepIntegrator osi, integer * sizeOfX, doublereal* time, doublereal* x, integer * ng, doublereal * gOut) { assert(_nsds); assert(_nsds->topology()); InteractionsGraph::VIterator ui, uiend; SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); unsigned int nsLawSize, k = 0 ; SP::SiconosVector y, ydot, yddot, lambda; SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(osi); // Solve LCP at acceleration level to calculate the lambda[2] at Interaction of indexSet[2] lsodar->fillXWork(sizeOfX, x); // double t = *time; if (!_allNSProblems->empty()) { if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions())) { (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t); } }; /* double * xdottmp = (double *)malloc(*sizeOfX*sizeof(double)); computef(osi, sizeOfX,time,x,xdottmp); free(xdottmp); */ // Update the output from level 0 to level 1 _nsds->updateOutput(t,0); _nsds->updateOutput(t,1); _nsds->updateOutput(t,2); // for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui) { SP::Interaction inter = _indexSet0->bundle(*ui); nsLawSize = inter->nonSmoothLaw()->size(); y = inter->y(0); // output y at this Interaction ydot = inter->y(1); // output of level 1 at this Interaction yddot = inter->y(2); lambda = inter->lambda(2); // input of level 2 at this Interaction if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2] { for (unsigned int i = 0; i < nsLawSize; ++i) { if ((*y)(i) > _TOL_ED) { gOut[k] = (*y)(i); } else { if ((*ydot)(i) > -_TOL_ED) { gOut[k] = 100 * _TOL_ED; } else { gOut[k] = (*y)(i); } } k++; } } else // If Interaction is in the indexSet[2] { for (unsigned int i = 0; i < nsLawSize; ++i) { if ((*lambda)(i) > _TOL_ED) { gOut[k] = (*lambda)(i); // g = lambda[2] } else { if ((*yddot)(i) > _TOL_ED) { gOut[k] = (*lambda)(i); } else { gOut[k] = 100 * _TOL_ED; } } k++; } } } }
/** set the one step integrator for this Dynamical System * \param ds a pointer to the system * \param OSI the integrator to use for this DS */ inline void setOSI(SP::DynamicalSystem ds, SP::OneStepIntegrator OSI) { _topology->setOSI(ds, OSI); _mIsLinear = ((ds)->isLinear() && _mIsLinear); };
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ double EventDriven::detectEvents(bool updateIstate) { double _minResiduOutput = 0.0; // maximum of g_i with i running over all activated or deactivated contacts // Loop over all interactions to detect whether some constraints are activated or deactivated bool _IsContactClosed = false; bool _IsContactOpened = false; bool _IsFirstTime = true; InteractionsGraph::VIterator ui, uiend; SP::SiconosVector y, ydot, lambda; SP::Topology topo = _nsds->topology(); SP::InteractionsGraph indexSet2 = topo->indexSet(2); // #ifdef DEBUG_MESSAGES cout << "======== In EventDriven::detectEvents =========" <<endl; #endif for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui) { SP::Interaction inter = _indexSet0->bundle(*ui); double nsLawSize = inter->nonSmoothLaw()->size(); if (nsLawSize != 1) { RuntimeException::selfThrow("In EventDriven::detectEvents, the interaction size > 1 has not been implemented yet!!!"); } y = inter->y(0); // output y at this Interaction ydot = inter->y(1); // output of level 1 at this Interaction lambda = inter->lambda(2); // input of level 2 at this Interaction if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2] { if ((*y)(0) < _TOL_ED) // gap at the current interaction <= 0 { _IsContactClosed = true; } if (_IsFirstTime) { _minResiduOutput = (*y)(0); _IsFirstTime = false; } else { if (_minResiduOutput > (*y)(0)) { _minResiduOutput = (*y)(0); } } } else // If interaction is in the indexSet[2] { if ((*lambda)(0) < _TOL_ED) // normal force at the current interaction <= 0 { _IsContactOpened = true; } if (_IsFirstTime) { _minResiduOutput = (*lambda)(0); _IsFirstTime = false; } else { if (_minResiduOutput > (*lambda)(0)) { _minResiduOutput = (*lambda)(0); } } } // #ifdef DEBUG_MESSAGES cout.precision(15); cout << "Contact number: " << inter->number() <<endl; cout << "Contact gap: " << (*y)(0) <<endl; cout << "Contact force: " << (*lambda)(0) <<endl; cout << "Is contact is closed: " << _IsContactClosed <<endl; cout << "Is contact is opened: " << _IsContactOpened <<endl; #endif // } // if (updateIstate) { if ((!_IsContactClosed) && (!_IsContactOpened)) { _istate = 2; //no event is detected } else if ((_IsContactClosed) && (!_IsContactOpened)) { _istate = 3; // Only some contacts are closed } else if ((!_IsContactClosed) && (_IsContactOpened)) { _istate = 4; // Only some contacts are opened } else { _istate = 5; // Some contacts are closed AND some contacts are opened } } // return _minResiduOutput; }
/** set the name for this Dynamical System * \param ds a pointer to the system * \param name the name of the DynamicalSystem */ inline void setName(SP::DynamicalSystem ds, const std::string& name) { _topology->setName(ds, name); };
/** specify id the given Interaction is for controlling the DS * \param inter the Interaction * \param isControlInteraction true if the Interaction is used for * control purposes **/ void setControlProperty(SP::Interaction inter, const bool isControlInteraction) { _topology->setControlProperty(inter, isControlInteraction); }
int main(int argc, char* argv[]) { try { // ================= Creation of the model ======================= // parameters according to Table 1 unsigned int nDof = 3; // degrees of freedom for robot arm double t0 = 0; // initial computation time double T = 0.2; // final computation time double h = 1e-5; // time step : do not decrease, because of strong penetrations // initial conditions SP::SiconosVector q0(new SiconosVector(nDof)); SP::SiconosVector v0(new SiconosVector(nDof)); q0->zero(); (*q0)(0) = y_0; (*q0)(1) = phi_0; v0->zero(); (*v0)(0) = v_0; (*v0)(1) = omega_0; // ------------------------- // --- Dynamical systems --- // ------------------------- cout << "====> Model loading ..." << endl << endl; SP::LagrangianDS slider(new LagrangianDS(q0, v0, "OscillatorPlugin:mass")); slider->setComputeNNLFunction("OscillatorPlugin", "NNL"); slider->setComputeJacobianNNLqFunction("OscillatorPlugin", "jacobianNNLq"); slider->setComputeJacobianNNLqDotFunction("OscillatorPlugin", "jacobianNNLqDot"); slider->setComputeFIntFunction("OscillatorPlugin", "FInt"); slider->setComputeJacobianFIntqFunction("OscillatorPlugin", "jacobianFIntq"); slider->setComputeJacobianFIntqDotFunction("OscillatorPlugin", "jacobianFIntqDot"); // ------------------- // --- Interactions--- // ------------------- // -- corner 1 -- #ifdef WITH_FRICTION SP::NonSmoothLaw nslaw1(new NewtonImpactFrictionNSL(eN1, eT1, mu1, 2)); SP::Relation relation1(new LagrangianScleronomousR("SliderCrankPlugin:g1", "SliderCrankPlugin:W1")); SP::Interaction inter1(new Interaction(2, nslaw1, relation1, 1)); // -- corner 2 -- SP::NonSmoothLaw nslaw2(new NewtonImpactFrictionNSL(eN2, eT2, mu2, 2)); SP::Relation relation2(new LagrangianScleronomousR("SliderCrankPlugin:g2", "SliderCrankPlugin:W2")); SP::Interaction inter2(new Interaction(2, nslaw2, relation2, 2)); // -- corner 3 -- SP::NonSmoothLaw nslaw3(new NewtonImpactFrictionNSL(eN3, eT3, mu3, 2)); SP::Relation relation3(new LagrangianScleronomousR("SliderCrankPlugin:g3", "SliderCrankPlugin:W3")); SP::Interaction inter3(new Interaction(2, nslaw3, relation3, 3)); // -- corner 4 -- SP::NonSmoothLaw nslaw4(new NewtonImpactFrictionNSL(eN4, eT4, mu4, 2)); SP::Relation relation4(new LagrangianScleronomousR("SliderCrankPlugin:g4", "SliderCrankPlugin:W4")); SP::Interaction inter4(new Interaction(2, nslaw4, relation4, 4)); #else // -- corner 1 -- SP::NonSmoothLaw nslaw1(new NewtonImpactNSL(eN1)); SP::Relation relation1(new LagrangianScleronomousR("SliderCrankPlugin:g1", "SliderCrankPlugin:W1")); SP::Interaction inter1(new Interaction(1, nslaw1, relation1, 1)); // -- corner 2 -- SP::NonSmoothLaw nslaw2(new NewtonImpactNSL(eN2)); SP::Relation relation2(new LagrangianScleronomousR("SliderCrankPlugin:g2", "SliderCrankPlugin:W2")); SP::Interaction inter2(new Interaction(1, nslaw2, relation2, 2)); // -- corner 3 -- SP::NonSmoothLaw nslaw3(new NewtonImpactNSL(eN3)); SP::Relation relation3(new LagrangianScleronomousR("SliderCrankPlugin:g3", "SliderCrankPlugin:W3")); SP::Interaction inter3(new Interaction(1, nslaw3, relation3, 3)); // -- corner 4 -- SP::NonSmoothLaw nslaw4(new NewtonImpactNSL(eN4)); SP::Relation relation4(new LagrangianScleronomousR("SliderCrankPlugin:g4", "SliderCrankPlugin:W4")); SP::Interaction inter4(new Interaction(1, nslaw4, relation4, 4)); #endif // ------------- // --- Model --- // ------------- SP::Model sliderWithClearance(new Model(t0, T)); sliderWithClearance->nonSmoothDynamicalSystem()->insertDynamicalSystem(slider); sliderWithClearance->nonSmoothDynamicalSystem()->link(inter1, slider); sliderWithClearance->nonSmoothDynamicalSystem()->link(inter2, slider); sliderWithClearance->nonSmoothDynamicalSystem()->link(inter3, slider); sliderWithClearance->nonSmoothDynamicalSystem()->link(inter4, slider); // ---------------- // --- Simulation --- // ---------------- SP::MoreauJeanOSI OSI(new MoreauJeanOSI(slider, 0.5, 0.0)); SP::TimeDiscretisation t(new TimeDiscretisation(t0, h)); #ifdef WITH_FRICTION SP::OneStepNSProblem impact(new FrictionContact(2, SICONOS_FRICTION_2D_ENUM)); #else SP::OneStepNSProblem impact(new LCP(SICONOS_LCP_LEMKE)); #endif impact->numericsSolverOptions()->dparam[0] = 1e-08; impact->numericsSolverOptions()->iparam[0] = 100; impact->numericsSolverOptions()->iparam[2] = 1; // random SP::TimeStepping s(new TimeStepping(t)); s->insertIntegrator(OSI); s->insertNonSmoothProblem(impact, SICONOS_OSNSP_TS_VELOCITY); s->setNewtonTolerance(1e-10); s->setNewtonMaxIteration(200); SP::Topology topo = sliderWithClearance->nonSmoothDynamicalSystem()->topology(); // =========================== End of model definition =========================== // ================================= Computation ================================= // --- Simulation initialization --- cout << "====> Initialisation ..." << endl << endl; sliderWithClearance->initialize(s); int N = ceil((T - t0) / h) + 1; // Number of time steps // --- Get the values to be plotted --- // -> saved in a matrix dataPlot unsigned int outputSize = 27; SimpleMatrix dataPlot(N + 1, outputSize); SP::SiconosVector q = slider->q(); SP::SiconosVector v = slider->velocity(); dataPlot(0, 0) = sliderWithClearance->t0(); dataPlot(0, 1) = (*q)(0) / (2.*M_PI); // crank revolution dataPlot(0, 2) = (*q)(1); dataPlot(0, 3) = (*q)(2); dataPlot(0, 4) = (*v)(0); dataPlot(0, 5) = (*v)(1); dataPlot(0, 6) = (*v)(2); dataPlot(0, 7) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 1 (normalized) dataPlot(0, 8) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 2 (normalized) dataPlot(0, 9) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (-c); // y corner 3 (normalized) dataPlot(0, 10) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (-c); // y corner 4 (normalized) dataPlot(0, 11) = (l1 * cos((*q)(0)) + l2 * cos((*q)(1)) - l2) / l1; // x slider (normalized) dataPlot(0, 12) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1))) / c; // y slider (normalized dataPlot(0, 13) = (*inter1->y(0))(0) ; // g1 dataPlot(0, 14) = (*inter2->y(0))(0) ; // g2 dataPlot(0, 15) = (*inter3->y(0))(0) ; // g3 dataPlot(0, 16) = (*inter4->y(0))(0) ; // g4 dataPlot(0, 17) = (*inter1->y(1))(0) ; // dot g1 dataPlot(0, 18) = (*inter2->y(1))(0) ; // dot g2 dataPlot(0, 19) = (*inter3->y(1))(0) ; // dot g3 dataPlot(0, 20) = (*inter4->y(1))(0) ; // dot g4 dataPlot(0, 21) = (*inter1->lambda(1))(0) ; // lambda1 dataPlot(0, 22) = (*inter2->lambda(1))(0) ; // lambda1 dataPlot(0, 23) = (*inter3->lambda(1))(0) ; // lambda3 dataPlot(0, 24) = (*inter4->lambda(1))(0) ; // lambda4 dataPlot(0, 25) = 0; dataPlot(0, 26) = 0; // --- Time loop --- cout << "====> Start computation ... " << endl << endl; // ==== Simulation loop - Writing without explicit event handling ===== int k = 1; boost::progress_display show_progress(N); boost::timer time; time.restart(); SP::InteractionsGraph indexSet1 = topo->indexSet(1); while ((s->hasNextEvent()) && (k<= 3000)) // while ((s->hasNextEvent())) { std::cout <<"=====================================================" <<std::endl; std::cout <<"=====================================================" <<std::endl; std::cout <<"=====================================================" <<std::endl; std::cout <<"Iteration k = " << k <<std::endl; std::cout <<"s->nextTime() = " <<s->nextTime() <<std::endl; std::cout <<"=====================================================" <<std::endl; //std::cout << "=============== Step k ="<< k<< std::endl; s->advanceToEvent(); impact->setNumericsVerboseMode(0); // --- Get values to be plotted --- dataPlot(k, 0) = s->nextTime(); dataPlot(k, 1) = (*q)(0) / (2.*M_PI); // crank revolution dataPlot(k, 2) = (*q)(1); dataPlot(k, 3) = (*q)(2); dataPlot(k, 4) = (*v)(0); dataPlot(k, 5) = (*v)(1); dataPlot(k, 6) = (*v)(2); dataPlot(k, 7) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 1 (normalized) dataPlot(k, 8) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) + b * cos((*q)(2)) - b) / c; // y corner 2 (normalized) dataPlot(k, 9) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) - a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (c); // y corner 3 (normalized) dataPlot(k, 10) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1)) + a * sin((*q)(2)) - b * cos((*q)(2)) + b) / (c); // y corner 4 (normalized) dataPlot(k, 11) = (l1 * cos((*q)(0)) + l2 * cos((*q)(1)) - l2) / l1; // x slider (normalized) dataPlot(k, 12) = (l1 * sin((*q)(0)) + l2 * sin((*q)(1))) / c; // y slider (normalized) dataPlot(k, 13) = (*inter1->y(0))(0) ; // g1 dataPlot(k, 14) = (*inter2->y(0))(0) ; // g2 dataPlot(k, 15) = (*inter3->y(0))(0) ; // g3 dataPlot(k, 16) = (*inter4->y(0))(0) ; // g4 dataPlot(k, 17) = (*inter1->y(1))(0) ; // dot g1 dataPlot(k, 18) = (*inter2->y(1))(0) ; // dot g2 dataPlot(k, 19) = (*inter3->y(1))(0) ; // dot g3 dataPlot(k, 20) = (*inter4->y(1))(0) ; // dot g4 dataPlot(k, 21) = (*inter1->lambda(1))(0) ; // lambda1 dataPlot(k, 22) = (*inter2->lambda(1))(0) ; // lambda1 dataPlot(k, 23) = (*inter3->lambda(1))(0) ; // lambda3 dataPlot(k, 24) = (*inter4->lambda(1))(0) ; // lambda4 dataPlot(k, 25) = s->getNewtonNbSteps(); dataPlot(k, 26) = indexSet1->size(); if (indexSet1->size() > 5) { impact->display(); } // if (s->nextTime() > 0.035 and (*inter1->lambda(1))(0) >0.0) #ifdef DISPLAY_INTER std::cout << "=============== Step k =" << k << std::endl; std::cout << "Time " << s->nextTime() << std::endl; impact->display(); std::cout << " (*inter1->lambda(1))(0) " << (*inter1->lambda(1))(0) << std:: endl; std::cout << " (*inter2->lambda(1))(0) " << (*inter2->lambda(1))(0) << std:: endl; std::cout << " (*inter3->lambda(1))(0) " << (*inter3->lambda(1))(0) << std:: endl; std::cout << " (*inter4->lambda(1))(0) " << (*inter4->lambda(1))(0) << std:: endl; #endif s->processEvents(); ++show_progress; k++; } cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl; cout << "Computation Time " << time.elapsed() << endl; // --- Output files --- cout << "====> Output file writing ..." << endl; dataPlot.resize(k, outputSize); ioMatrix::write("result.dat", "ascii", dataPlot, "noDim"); } catch (SiconosException e) { cout << e.report() << endl; } catch (...) { cout << "Exception caught in SliderCrankD1MinusLinearOSI.cpp" << endl; } }