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