void MLCPProjectOnConstraints::postCompute()
{
_hasBeenUpdated = true;
// This function is used to set y/lambda values using output from
// lcp_driver (w,z). Only Interactions (ie Interactions) of
// indexSet(leveMin) are concerned.
// === Get index set from Topology ===
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
// y and lambda vectors
SP::SiconosVector lambda;
SP::SiconosVector y;
// === Loop through "active" Interactions (ie present in
// indexSets[1]) ===
/** We chose to do a small step _alpha in view of stabilized the algorithm.*/
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postCompute damping value = %f\n", _alpha);
#endif
(*_z) *= _alpha;
unsigned int pos = 0;
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postCompute _z\n");
_z->display();
display();
#endif
InteractionsGraph::VIterator ui, uiend;
for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = indexSet->bundle(*ui);
// Get the relative position of inter-interactionBlock in the vector w
// or z
pos = _M->getPositionOfInteractionBlock(*inter);
RELATION::TYPES relationType = inter->relation()->getType();
if (relationType == NewtonEuler)
{
postComputeNewtonEulerR(inter, pos);
}
else if (relationType == Lagrangian)
{
postComputeLagrangianR(inter, pos);
}
else
{
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock - relation type is not from Lagrangian type neither NewtonEuler.");
}
}
}
void Simulation::updateInput(unsigned int level)
{
// To compute input(level) (ie with lambda[level]) for all Interactions.
// assert(level>=0);
// double time = nextTime();
double time = model()->currentTime();
// Set dynamical systems non-smooth part to zero.
reset(level);
// We compute input using lambda(level).
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
{
inter = indexSet0->bundle(*ui);
assert(inter->lowerLevelForInput() <= level);
assert(inter->upperLevelForInput() >= level);
inter->computeInput(time, indexSet0->properties(*ui), level);
}
}
void Simulation::initializeInteraction(double time, SP::Interaction inter)
{
// determine which (lower and upper) levels are required for this Interaction
// in this Simulation.
computeLevelsForInputAndOutput(inter);
// Get the interaction properties from the topology for initialization.
SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
InteractionsGraph::VDescriptor ui = indexSet0->descriptor(inter);
// This calls computeOutput() and initializes qMemory and q_k.
inter->initialize(time, indexSet0->properties(ui));
}
void EulerMoreauOSI::computeW(double t, DynamicalSystem& ds, DynamicalSystemsGraph::VDescriptor& dsgVD)
{
// Compute W matrix of the Dynamical System ds, at time t and for the current ds state.
// When this function is called, WMap[ds] is supposed to exist and not to be null
// Memory allocation has been done during initW.
unsigned int dsN = ds.number();
assert((WMap.find(dsN) != WMap.end()) &&
"EulerMoreauOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?");
double h = simulationLink->timeStep();
Type::Siconos dsType = Type::value(ds);
SiconosMatrix& W = *WMap[dsN];
// 1 - First order non linear systems
if (dsType == Type::FirstOrderNonLinearDS)
{
// W = M - h*_theta* [jacobian_x f(t,x,z)]
FirstOrderNonLinearDS& d = static_cast<FirstOrderNonLinearDS&> (ds);
// Copy M or I if M is Null into W
if (d.M())
W = *d.M();
else
W.eye();
d.computeJacobianfx(t);
// Add -h*_theta*jacobianfx to W
scal(-h * _theta, *d.jacobianfx(), W, false);
}
// 2 - First order linear systems
else if (dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
{
FirstOrderLinearDS& d = static_cast<FirstOrderLinearDS&> (ds);
if (dsType == Type::FirstOrderLinearDS)
d.computeA(t);
if (d.M())
W = *d.M();
else
W.eye();
scal(-h * _theta, *d.A(), W, false);
}
else RuntimeException::selfThrow("EulerMoreauOSI::computeW - not yet implemented for Dynamical system type :" + dsType);
// if (_useGamma)
{
Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology();
DynamicalSystemsGraph& DSG0 = *topo.dSG(0);
InteractionsGraph& indexSet = *topo.indexSet(0);
DynamicalSystemsGraph::OEIterator oei, oeiend;
InteractionsGraph::VDescriptor ivd;
SP::SiconosMatrix K;
SP::Interaction inter;
for (std11::tie(oei, oeiend) = DSG0.out_edges(dsgVD); oei != oeiend; ++oei)
{
inter = DSG0.bundle(*oei);
ivd = indexSet.descriptor(inter);
FirstOrderR& rel = static_cast<FirstOrderR&>(*inter->relation());
K = rel.K();
if (!K) K = (*indexSet.properties(ivd).workMatrices)[FirstOrderR::mat_K];
if (K)
{
scal(-h * _gamma, *K, W, false);
}
}
}
// Remark: W is not LU-factorized here.
// Function PLUForwardBackward will do that if required.
}
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());
}
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems();
SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink;
// Get relation and non smooth law types
RELATION::TYPES relationType = inter->relation()->getType();
RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
unsigned int sizeY = inter->nonSmoothLaw()->size();
unsigned int relativePosition = 0;
SP::Interaction mainInteraction = inter;
Index coord(8);
coord[0] = relativePosition;
coord[1] = relativePosition + sizeY;
coord[2] = 0;
coord[4] = 0;
coord[6] = 0;
coord[7] = sizeY;
SP::SiconosMatrix C;
// SP::SiconosMatrix D;
// SP::SiconosMatrix F;
SiconosVector& yForNSsolver = *inter->yForNSsolver();
SP::BlockVector Xfree;
// All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used.
/* V.A. 10/10/2010
* Following the type of OSNS we need to retrieve the velocity or the acceleration
* This tricks is not very nice but for the moment the OSNS do not known if
* it is in accelaration of not
*/
//SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems();
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
if (relationType == Lagrangian)
{
Xfree = DSlink[LagrangianR::xfree];
}
// else if (relationType == NewtonEuler)
// {
// Xfree = inter->data(NewtonEulerR::free);
// }
assert(Xfree);
// std::cout << "Computeqblock Xfree (Gamma)========" << std::endl;
// Xfree->display();
}
else if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
{
Xfree = DSlink[LagrangianR::q1];
// std::cout << "Computeqblock Xfree (Velocity)========" << std::endl;
// Xfree->display();
}
else
RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong ");
if (relationType == Lagrangian)
{
C = mainInteraction->relation()->C();
if (C)
{
assert(Xfree);
coord[3] = C->size(1);
coord[5] = C->size(1);
subprod(*C, *Xfree, yForNSsolver, coord, true);
}
SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY));
ID->eye();
Index xcoord(8);
xcoord[0] = 0;
xcoord[1] = sizeY;
xcoord[2] = 0;
xcoord[3] = sizeY;
xcoord[4] = 0;
xcoord[5] = sizeY;
xcoord[6] = 0;
xcoord[7] = sizeY;
// For the relation of type LagrangianRheonomousR
if (relationSubType == RheonomousR)
{
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR");
}
else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp)
{
SiconosVector q = *DSlink[LagrangianR::q0];
SiconosVector z = *DSlink[LagrangianR::z];
std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z);
*DSlink[LagrangianR::z] = z;
//.........这里部分代码省略.........
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");
// Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
// necessary) one or two DS are concerned by inter1 . How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vd);
// Get osi property from interaction
// We assume that all ds in vertex_inter have the same osi.
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
//SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
OSI::TYPES osiType = Osi->getType();
// At most 2 DS are linked by an Interaction
SP::DynamicalSystem DS1;
SP::DynamicalSystem DS2;
unsigned int pos1, pos2;
// --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
if (indexSet->properties(vd).source != indexSet->properties(vd).target)
{
DEBUG_PRINT("a two DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = indexSet->properties(vd).target;
}
else
{
DEBUG_PRINT("a single DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = DS1;
// \warning this looks like some debug code, but it gets executed even with NDEBUG.
// may be compiler does something smarter, but still it should be rewritten. --xhub
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
oei != oeiend; ++oei)
{
// note : at most 4 edges
DS2 = indexSet->bundle(*oei);
if (DS2 != DS1)
{
assert(false);
break;
}
}
}
assert(DS1);
assert(DS2);
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
// --- Check block size ---
assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());
// --- Compute diagonal block ---
// Block to be set in OSNS Matrix, corresponding to
// the current interaction
SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType;
double h = simulation()->currentTimeStep();
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType = inter->relation()->getType();
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
// loop over the DS connected to the interaction.
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
unsigned int sizeDS = ds->dimension();
// get _interactionBlocks corresponding to the current DS
// These _interactionBlocks depends on the relation type.
leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
DEBUG_EXPR(leftInteractionBlock->display(););
// Computing depends on relation type -> move this in Interaction method?
if (relationType == FirstOrder)
{
//.........这里部分代码省略.........
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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;
//.........这里部分代码省略.........
客服电话
APP下载
官方微信
请发表评论