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