/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
double Nx = _Nc->getValue(0);
double Ny = _Nc->getValue(1);
double Nz = _Nc->getValue(2);
double Px = _Pc1->getValue(0);
double Py = _Pc1->getValue(1);
double Pz = _Pc1->getValue(2);
double G1x = q1->getValue(0);
double G1y = q1->getValue(1);
double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
printf("contact normal:\n");
_Nc->display();
printf("point de contact :\n");
_Pc1->display();
printf("center of masse :\n");
q1->display();
#endif
_RotationAbsToContactFrame->setValue(0, 0, Nx);
_RotationAbsToContactFrame->setValue(0, 1, Ny);
_RotationAbsToContactFrame->setValue(0, 2, Nz);
_NPG1->zero();
(*_NPG1)(0, 0) = 0;
(*_NPG1)(0, 1) = -(G1z - Pz);
(*_NPG1)(0, 2) = (G1y - Py);
(*_NPG1)(1, 0) = (G1z - Pz);
(*_NPG1)(1, 1) = 0;
(*_NPG1)(1, 2) = -(G1x - Px);
(*_NPG1)(2, 0) = -(G1y - Py);
(*_NPG1)(2, 1) = (G1x - Px);
(*_NPG1)(2, 2) = 0;
computeRotationMatrix(q1,_rotationMatrixAbsToBody);
prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);
prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));
#ifdef NEFC3D_DEBUG
printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
_jachqT->display();
#endif
}
void adjointInput::beta(double t, SiconosVector& xvalue, SP::SiconosVector beta)
{
beta->setValue(0, -1.0 / 2.0 * xvalue(1) + 1.0 / 2.0) ;
beta->setValue(1, 1.0 / 2.0 * xvalue(0)) ;
#ifdef SICONOS_DEBUG
std::cout << "beta\n" << std::endl;;
beta->display();
#endif
}
void rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )
{
DEBUG_BEGIN("::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )\n");
DEBUG_EXPR(v->display(););
void D1MinusLinearOSI::computeFreeState()
{
DEBUG_PRINT("\n D1MinusLinearOSI::computeFreeState(), start\n");
for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
// type of the current DS
Type::Siconos dsType = Type::value(**it);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
// Lagrangian Systems
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it);
// get left state from memory
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
DEBUG_EXPR(vold->display());
// get right information
//SP::SiconosMatrix M = d->mass();
SP::SiconosVector vfree = d->velocity(); // POINTER CONSTRUCTOR : contains free velocity
(*vfree) = *(d->workspace(DynamicalSystem::freeresidu));
DEBUG_EXPR(d->workspace(DynamicalSystem::freeresidu)->display());
// d->computeMass();
// M->resetLU();
// M->PLUForwardBackwardInPlace(*vfree);
// DEBUG_EXPR(M->display());
*vfree *= -1.;
*vfree += *vold;
DEBUG_EXPR(vfree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
// NewtonEuler Systems
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it);
// get left state from memory
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
DEBUG_EXPR(vold->display());
// get right information
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
SP::SiconosVector vfree = d->velocity(); // POINTER CONSTRUCTOR : contains free velocity
(*vfree) = *(d->workspace(DynamicalSystem::freeresidu));
DEBUG_EXPR(d->workspace(DynamicalSystem::freeresidu)->display());
*vfree *= -1.;
*vfree += *vold;
DEBUG_EXPR(vfree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("D1MinusLinearOSI::computeFreeState(), end\n");
}
void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{
DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
DEBUG_PRINTF("with time = %f\n",time);
DEBUG_PRINTF("with inter = %p\n",&inter);
_jachq->setValue(0, 0, _Nc->getValue(0));
_jachq->setValue(0, 1, _Nc->getValue(1));
_jachq->setValue(0, 2, _Nc->getValue(2));
if (inter.has2Bodies())
{
_jachq->setValue(0, 7, -_Nc->getValue(0));
_jachq->setValue(0, 8, -_Nc->getValue(1));
_jachq->setValue(0, 9, -_Nc->getValue(2));
}
for (unsigned int iDS =0 ; iDS < q0->getNumberOfBlocks() ; iDS++)
{
SP::SiconosVector q = (q0->getAllVect())[iDS];
double sign = 1.0;
DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS);
DEBUG_EXPR_WE(q->display(););
::boost::math::quaternion<double> quatGP;
if (iDS == 0)
{
::boost::math::quaternion<double> quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1),
_Pc1->getValue(2) - q->getValue(2));
quatGP = quatAux;
}
else
{
sign = -1.0;
//cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n";
::boost::math::quaternion<double> quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1),
_Pc2->getValue(2) - q->getValue(2));
quatGP = quatAux;
}
DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4());
DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
::boost::math::quaternion<double> quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
::boost::math::quaternion<double> quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6));
::boost::math::quaternion<double> quat0(1, 0, 0, 0);
::boost::math::quaternion<double> quatBuff;
::boost::math::quaternion<double> _2qiquatGP;
_2qiquatGP = quatGP;
_2qiquatGP *= 2 * (q->getValue(3));
quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP;
DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4());
_jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
//cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n";
//_jachq->setValue(0,7*iDS+3,0);
for (unsigned int i = 1; i < 4; i++)
{
::boost::math::quaternion<double> quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0);
_2qiquatGP = quatGP;
_2qiquatGP *= 2 * (q->getValue(3 + i));
quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP;
_jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
}
}
double D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()
{
DEBUG_BEGIN("\n D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()\n");
double t = _simulation->nextTime(); // end of the time step
double told = _simulation->startingTime(); // beginning of the time step
double h = _simulation->timeStep(); // time step length
SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); // all OSNSP
SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
/**************************************************************************************************************
* Step 1- solve a LCP at acceleration level for lambda^+_{k} for the last set indices
* if index2 is empty we should skip this step
**************************************************************************************************************/
DEBUG_PRINT("\nEVALUATE LEFT HAND SIDE\n");
DEBUG_EXPR(std::cout<< "allOSNS->empty() " << std::boolalpha << allOSNS->empty() << std::endl << std::endl);
DEBUG_EXPR(std::cout<< "allOSNS->size() " << allOSNS->size() << std::endl << std::endl);
// -- LEFT SIDE --
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
Type::Siconos dsType = Type::value(*ds);
SP::SiconosVector accFree;
SP::SiconosVector work_tdg;
SP::SiconosMatrix Mold;
DEBUG_EXPR((*it)->display());
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
accFree = d->workspace(DynamicalSystem::free); /* POINTER CONSTRUCTOR : will contain
* the acceleration without contact force */
accFree->zero();
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
Mold = d->mass();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
DEBUG_EXPR(Mold->display());
if (! d->workspace(DynamicalSystem::free_tdg))
{
d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
}
work_tdg = d->workspace(DynamicalSystem::free_tdg);
work_tdg->zero();
DEBUG_EXPR(work_tdg->display());
if (d->forces())
{
d->computeForces(told, qold, vold);
DEBUG_EXPR(d->forces()->display());
*accFree += *(d->forces());
}
Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
//Mold = d->mass();
assert(!d->mass()->isPLUInversed());
Mold.reset(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
DEBUG_EXPR(Mold->display());
if (! d->workspace(DynamicalSystem::free_tdg))
{
d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
}
work_tdg = d->workspace(DynamicalSystem::free_tdg);
work_tdg->zero();
DEBUG_EXPR(work_tdg->display());
if (d->forces())
{
d->computeForces(told, qold, vold);
DEBUG_EXPR(d->forces()->display());
*accFree += *(d->forces());
}
Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
}
else
{
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("accFree contains right limit acceleration at t^+_k with contact force :\n");
DEBUG_EXPR(accFree->display());
DEBUG_PRINT("work_tdg contains right limit acceleration at t^+_k without contact force :\n");
DEBUG_EXPR(work_tdg->display());
}
if (!allOSNS->empty())
{
if (indexSet2->size() >0)
{
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui)
{
inter = indexSet2->bundle(*ui);
inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui));
inter->relation()->computeJacg(told, *inter, indexSet2->properties(*ui));
}
if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged())
{
for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns)
{
(*itOsns)->setHasBeenUpdated(false);
}
}
assert((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]);
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) // it should be equivalent to indexSet2
{
DEBUG_PRINT("We compute lambda^+_{k} \n");
(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(told);
DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display());
}
// Note Franck : at the time this results in a call to swapInMem of all Interactions of the NSDS
// So let the simu do this.
//(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->saveInMemory(); // we push y and lambda in Memories
_simulation->nonSmoothDynamicalSystem()->pushInteractionsInMemory();
_simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2);
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
Type::Siconos dsType = Type::value(*ds);
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force
SP::SiconosMatrix Mold = d->mass();
Mold->PLUForwardBackwardInPlace(*dummy);
*accFree += *(dummy);
DEBUG_EXPR(d->p(2)->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force
SP::SiconosMatrix Mold(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
DEBUG_EXPR(Mold->display());
Mold->PLUForwardBackwardInPlace(*dummy);
*accFree += *(dummy);
DEBUG_EXPR(d->p(2)->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
}
}
/**************************************************************************************************************
* Step 2 - compute v_{k,1}
**************************************************************************************************************/
DEBUG_PRINT("\n PREDICT RIGHT HAND SIDE\n");
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // contains acceleration without contact force
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// initialize *it->residuFree and predicted right velocity (left limit)
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect
SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity
residuFree->zero();
v->zero();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
*residuFree -= 0.5 * h**accFree;
*v += h**accFree;
*v += *vold;
DEBUG_EXPR(residuFree->display());
DEBUG_EXPR(v->display());
SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1}
*q = *qold;
scal(0.5 * h, *vold + *v, *q, false);
DEBUG_EXPR(q->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free);
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// initialize *it->residuFree and predicted right velocity (left limit)
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect
SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity
residuFree->zero();
v->zero();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
*residuFree -= 0.5 * h**accFree;
*v += h**accFree;
*v += *vold;
DEBUG_EXPR(residuFree->display());
DEBUG_EXPR(v->display());
//first step consists in computing \dot q.
//second step consists in updating q.
//
SP::SiconosMatrix T = d->T();
SP::SiconosVector dotq = d->dotq();
prod(*T, *v, *dotq, true);
SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0);
SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1}
*q = *qold;
scal(0.5 * h, *dotqold + *dotq, *q, false);
DEBUG_PRINT("new q before normalizing\n");
DEBUG_EXPR(q->display());
//q[3:6] must be normalized
d->normalizeq();
d->computeT();
DEBUG_PRINT("new q after normalizing\n");
DEBUG_EXPR(q->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
/** At this step, we obtain
* \f[
* \begin{cases}
* v_{k,0} = \mbox{\tt vold} \\
* q_{k,0} = qold \\
* F_{k,+} = F(told,qold,vold) \\
* Work_{freefree} = M^{-1}_k (F^+_{k}) \mbox{stored in work_tdg} \\
* Work_{free} = M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in accFree} \\
* R_{free} = -h/2 * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in ResiduFree} \\
* v_{k,1} = v_{k,0} + h * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in v} \\
* q_{k,1} = q_{k,0} + \frac{h}{2} (v_{k,0} + v_{k,1}) \mbox{stored in q} \\
* \end{cases}
* \f]
**/
}
DEBUG_PRINT("\n DECIDE STRATEGY\n");
/** Decide of the strategy impact or smooth multiplier.
* Compute _isThereImpactInTheTimeStep
*/
_isThereImpactInTheTimeStep = false;
if (!allOSNS->empty())
{
for (unsigned int level = _simulation->levelMinForOutput();
level < _simulation->levelMaxForOutput(); level++)
{
_simulation->nonSmoothDynamicalSystem()->updateOutput(_simulation->nextTime(),level);
}
_simulation->updateIndexSets();
SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology();
SP::InteractionsGraph indexSet3 = topo->indexSet(3);
if (indexSet3->size() > 0)
{
_isThereImpactInTheTimeStep = true;
DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true;\n");
}
else
{
_isThereImpactInTheTimeStep = false;
DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n");
}
}
/* If _isThereImpactInTheTimeStep = true;
* we recompute residuFree by removing the contribution of the nonimpulsive contact forces.
* We add the contribution of the external forces at the end
* of the time--step
* If _isThereImpactInTheTimeStep = false;
* we recompute residuFree by adding the contribution of the external forces at the end
* and the contribution of the nonimpulsive contact forces that are computed by solving the osnsp.
*/
if (_isThereImpactInTheTimeStep)
{
DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true\n");
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu);
SP::SiconosVector v = d->velocity();
SP::SiconosVector q = d->q();
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix
//residuFree->zero();
//v->zero();
SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg);
assert(work_tdg);
*residuFree = - 0.5 * h**work_tdg;
d->computeMass();
DEBUG_EXPR(M->display());
if (d->forces())
{
d->computeForces(t, q, v);
*work_tdg = *(d->forces());
DEBUG_EXPR(d->forces()->display());
}
M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force
*residuFree -= 0.5 * h**work_tdg;
DEBUG_EXPR(residuFree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu);
SP::SiconosVector v = d->velocity();
SP::SiconosVector q = d->q();
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
DEBUG_EXPR(M->display());
//residuFree->zero();
v->zero();
SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg);
assert(work_tdg);
*residuFree = 0.5 * h**work_tdg;
work_tdg->zero();
if (d->forces())
{
d->computeForces(t, q, v);
*work_tdg += *(d->forces());
}
M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force
*residuFree -= 0.5 * h**work_tdg;
DEBUG_EXPR(residuFree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
}
else
{
DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n");
// -- RIGHT SIDE --
// calculate acceleration without contact force
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get right state from memory
SP::SiconosVector q = d->q(); // contains position q_{k+1}
SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity
SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(q->display());
DEBUG_EXPR(v->display());
// Lagrangian Nonlinear Systems
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
d->computeMass();
DEBUG_EXPR(M->display());
if (d->forces())
{
d->computeForces(t, q, v);
*accFree += *(d->forces());
}
}
else
RuntimeException::selfThrow
("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force
DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n");
DEBUG_EXPR(accFree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get right state from memory
SP::SiconosVector q = d->q(); // contains position q_{k+1}
SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(q->display());
DEBUG_EXPR(v->display());
if (d->forces())
{
d->computeForces(t, q, v);
*accFree += *(d->forces());
}
M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force
DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n");
DEBUG_EXPR(accFree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// solve a LCP at acceleration level only for contacts which have been active at the beginning of the time-step
if (!allOSNS->empty())
{
// for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++)
// {
// _simulation->updateOutput(level);
// }
// _simulation->updateIndexSets();
DEBUG_PRINT("We compute lambda^-_{k+1} \n");
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui)
{
inter = indexSet2->bundle(*ui);
inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui));
inter->relation()->computeJacg(t, *inter, indexSet2->properties(*ui));
}
if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged())
{
for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns)
{
(*itOsns)->setHasBeenUpdated(false);
}
}
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions()))
{
(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(t);
DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display(););
_simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2);
}
void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos)
{
SP::LagrangianR lr = std11::static_pointer_cast<LagrangianR>(inter->relation());
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n");
inter->y(0)->display();
printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n");
lr->jachq()->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n");
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter);
InteractionsGraph::OEIterator oei, oeiend;
for(std11::tie(oei, oeiend) = indexSet->out_edges(ui);
oei != oeiend; ++oei)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei));
lds->q()->display();
}
#endif
//unsigned int sizeY = inter->nonSmoothLaw()->size();
// y and lambda vectors
SP::SiconosVector lambda = inter->lambda(0);
SP::SiconosVector y = inter->y(0);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// Copy _w/_z values, starting from index pos into y/lambda.
//setBlock(*_w, y, sizeY, pos, 0);
setBlock(*_z, lambda, sizeY, pos, 0);
#ifdef MLCPPROJ_DEBUG
printf("MLCPP lambda of Interaction is pos =%i :\n", pos);
// aBuff->display();
lambda->display();
unsigned int nslawsize = inter->nonSmoothLaw()->size();
SP::SiconosVector aBuff(new SiconosVector(nslawsize));
setBlock(*_z, aBuff, sizeY, pos, 0);
SP::SiconosMatrix J = lr->jachq();
SP::SimpleMatrix aux(new SimpleMatrix(*J));
aux->trans();
// SP::SiconosVector tmp(new SiconosVector(*(lr->q())));
// prod(*aux, *aBuff, *(tmp), false);
// //prod(*aux,*lambda,*(lr->q()),false);
// std:: std::cout << " tmp = tmp + J^T * lambda" << std::endl;
// tmp->display();
#endif
// // WARNING : Must not be done here. and should be called with the correct time.
// // compute p(0)
// inter->computeInput(0.0 ,0);
// // \warning aBuff should normally be in lambda[0]
// // The update of the position in DS should be made
// // in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// SP::SiconosMatrix J=lr->jachq();
// SP::SimpleMatrix aux(new SimpleMatrix(*J));
// aux->trans();
// SP::SiconosVector tmp (new SiconosVector(*(lr->q())));
// std:: std::cout << " tmp ="<<std::endl;
// tmp->display();
// std:: std::cout << " lr->q() ="<<std::endl;
// lr->q()->display();
// //prod(*aux,*lambda,*(lr->q()),false);
// prod(*aux,*aBuff,*(tmp),false);
// std:: std::cout << " tmp = tmp + J * lambda"<<std::endl;
// tmp->display();
// // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// DSIterator itDS = inter->dynamicalSystemsBegin();
// while(itDS!=inter->dynamicalSystemsEnd())
// {
// Type::Siconos dsType = Type::value(**itDS);
// if((dsType !=Type::LagrangianDS) and
// (dsType !=Type::LagrangianLinearTIDS) )
// {
// RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type.");
// }
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS);
// SP::SiconosVector q = d->q();
// *q += *d->p(0);
// std::cout << " q=" << std::endl;
// q->display();
// itDS++;
// }
// if ((*lr->q() - *tmp).normInf() > 1e-12)
// {
// RuntimeException::selfThrow("youyou");
// }
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangianR _z\n");
_z->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR updated\n");
VectorOfBlockVectors& DSlink = *(indexSet->properties(ui)).DSlink;
// (*DSlink[LagrangianR::q0]).display();
// (lr->q())->display();
#endif
//RuntimeException::selfThrow("MLCPProjectOnConstraints::postComputeLagrangianR() - not yet implemented");
}
// main program
int main(int argc, char* argv[])
{
// Exception handling
try
{
// == User-defined parameters ==
//unsigned int ndof = 4; // number of degrees of freedom of your system
double t0 = 0.0;
double T = 0.02; // Total simulation time
double h = 1.0e-6; // Time step
//double Vinit= 1.0;
double Cp = 60e-6;
double leq = 0.385e-3;
double Rl = 5;
double m = 3;
double L1 = 1.5e-3;
double L2 = 1.22e-3;
double L3 = 0.9e-3;
//leq=1/((1/L1)+(1/L2)+(1/L3));
double rl1 = 94e-3;
double rl2 = 116e-3;
double rl3 = 100e-3;
double E1 = 70;
double E2 = 70;
double E3 = 70;
double k1 = 1;
double k2 = 6e-5;
// ================= Creation of the model =======================
// Steps:
// - create some Dynamical Systems
// - create some Interactions between those Dynamical Systems
// Interaction = some relations (constraints) and a NonSmoothLaw
// - create a NonSmoothDynamicalSystem with the DynamicalSystems and the Interactions
// - add this NonSmoothDynamicalSystem into a Model
// - add a Simulation to the model
// Simulation = TimeDiscretisation + OneStepIntegrator and OneStepNSProblem
// -------------------------
// --- Dynamical systems ---
// -------------------------
// First System:
// dx/dt = Ax + u(t) + r
// x(0) = x0
// Note: r = Blambda, B defines in relation below.
SP::SiconosMatrix A(new SimpleMatrix(ndof, ndof));
(*A)(0, 0) = 0;
(*A)(0, 1) = 1;
(*A)(0, 2) = 0;
(*A)(0, 3) = 0;
(*A)(1, 0) = (-1 / (Cp * leq)) - (rl1 / (Cp * L1 * Rl));
(*A)(1, 1) = (-rl1 / L1) - (1 / (Cp * Rl));
(*A)(1, 2) = (rl1 / (Cp * L1)) - (rl2 / (Cp * L2));
(*A)(1, 3) = (rl1 / (Cp * L1)) - (rl3 / (Cp * L3));
(*A)(2, 0) = -1 / L2;
(*A)(2, 1) = 0;
(*A)(2, 2) = -rl2 / L2;
(*A)(2, 3) = 0;
(*A)(3, 0) = -1 / L3;
(*A)(3, 1) = 0;
(*A)(3, 2) = 0;
(*A)(3, 3) = -rl3 / L3;
A->display();
SP::SiconosVector x0(new SiconosVector(ndof));
(*x0)(0) = 50;
(*x0)(1) = 7;
(*x0)(2) = 4;
(*x0)(3) = 4;
SP::FirstOrderLinearDS process(new FirstOrderLinearDS(x0, A));
SP::SiconosVector zProc(new SiconosVector(1, 0));
process->setzPtr(zProc);
// --------------------
// --- Interactions ---
// --------------------
unsigned int ninter = 3; // dimension of your Interaction = size of y and lambda vectors
// First relation, related to the process
// y = Cx + Dlambda +eDLS
// r = Blambda
SP::SimpleMatrix B(new SimpleMatrix(ndof, ninter));
(*B)(0, 0) = 0;
(*B)(0, 1) = 0;
(*B)(0, 2) = 0;
(*B)(1, 0) = E1 / (Cp * L1);
(*B)(1, 1) = E2 / (Cp * L2);
(*B)(1, 2) = E3 / (Cp * L3);
(*B)(2, 0) = 0;
(*B)(2, 1) = E2 / L2;
(*B)(2, 2) = 0;
(*B)(3, 0) = 0;
(*B)(3, 1) = 0;
(*B)(3, 2) = E3 / L3;
B->display();
*B = 1 * (*B);
SP::SimpleMatrix C(new SimpleMatrix(ninter, ndof));
(*C)(0, 0) = (Cp * k1 / k2) + ((m - 1) / Rl);
(*C)(0, 1) = m * Cp;
(*C)(0, 2) = -m;
(*C)(0, 3) = -m;
(*C)(1, 0) = (Cp * k1 / k2) - (1 / Rl);
(*C)(1, 1) = 0;
(*C)(1, 2) = m;
(*C)(1, 3) = 0;
(*C)(2, 0) = (Cp * k1 / k2) - (1 / Rl);
(*C)(2, 1) = 0;
(*C)(2, 2) = 0;
(*C)(2, 3) = m;
C->display();
//((*C)*(*B))->display();
SP::FirstOrderLinearR myProcessRelation(new FirstOrderLinearR(C, B));
SP::SimpleMatrix D(new SimpleMatrix(ninter, ninter));
(*D)(0, 0) = 0.0;
(*D)(0, 1) = 0.0;
(*D)(0, 2) = 0.0;
(*D)(1, 0) = 0.0;
(*D)(1, 1) = 0.0;
(*D)(1, 2) = 0.0;
(*D)(2, 0) = 0.0;
(*D)(2, 1) = 0.0;
(*D)(2, 2) = 0.0;
myProcessRelation->setComputeEFunction("plugins", "eLDS");
myProcessRelation->setDPtr(D);
//myProcessRelation->setComputeEFunction("ObserverLCSPlugin","computeE");
// Second relation, related to the observer
// haty = C hatX + D hatLambda + E
// hatR = B hatLambda
// NonSmoothLaw
unsigned int nslawSize = 3;
SP::NonSmoothLaw myNslaw(new RelayNSL(nslawSize));
myNslaw->display();
// The Interaction which involves the first DS (the process)
SP::Interaction myProcessInteraction(new Interaction( ninter, myNslaw, myProcessRelation));
// -------------
// --- Model ---
// -------------
SP::Model simpleExampleRelay(new Model(t0, T));
simpleExampleRelay->nonSmoothDynamicalSystem()->insertDynamicalSystem(process);
simpleExampleRelay->nonSmoothDynamicalSystem()->link(myProcessInteraction, process);
// ------------------
// --- Simulation ---
// ------------------
// TimeDiscretisation
SP::TimeDiscretisation td(new TimeDiscretisation(t0, h));
// == Creation of the Simulation ==
SP::TimeStepping s(new TimeStepping(td));
// -- OneStepIntegrators --
double theta = 0.5;
SP::EulerMoreauOSI myIntegrator(new EulerMoreauOSI(theta));
s->insertIntegrator(myIntegrator);
// -- OneStepNsProblem --
// -- OneStepNsProblem --
SP::Relay osnspb(new Relay(SICONOS_RELAY_ENUM));
//osnspb->setNumericsSolverName("Lemke");
//osnspb->numericsSolverOptions()->dparam[0]=1e-08;
s->insertNonSmoothProblem(osnspb);
//SP::LCP osnspb1(new LCP());
//SP::Relay osnspb(new Relay("Lemke"));
//s->insertNonSmoothProblem(osnspb);
simpleExampleRelay->setSimulation(s);
// =========================== End of model definition ===========================
// ================================= Computation =================================
// --- Simulation initialization ---
cout << "====> Simulation initialisation ..." << endl << endl;
simpleExampleRelay->initialize();
// (s->oneStepNSProblems)[0]->initialize();
// --- Get the values to be plotted ---
unsigned int outputSize = 17; // number of required data
unsigned int N = ceil((T - t0) / h); // Number of time steps
SimpleMatrix dataPlot(N, outputSize);
SP::SiconosVector xProc = process->x();
SP::SiconosVector lambdaProc = myProcessInteraction->lambda(0);
SP::SiconosVector yProc = myProcessInteraction->y(0);
// -> saved in a matrix dataPlot
dataPlot(0, 0) = simpleExampleRelay->t0(); // Initial time of the model
dataPlot(0, 1) = (*xProc)(0);
dataPlot(0, 2) = (*xProc)(1);
dataPlot(0, 3) = (*xProc)(2);
dataPlot(0, 4) = (*xProc)(3);
dataPlot(0, 5) = (*lambdaProc)(0);
dataPlot(0, 6) = (*lambdaProc)(1);
dataPlot(0, 7) = (*lambdaProc)(2);
dataPlot(0, 8) = (*yProc)(0);
dataPlot(0, 9) = (*yProc)(1);
dataPlot(0, 10) = (*yProc)(2);
dataPlot(0, 11) = (*zProc)(0); //v_ref
dataPlot(0, 12) = (*zProc)(0) / Rl; //i_ref
dataPlot(0, 13) = 0; //voltage_error
dataPlot(0, 14) = 0; //current_error
dataPlot(0, 15) = 0; //il1
dataPlot(0, 16) = (*xProc)(1);
// ==== Simulation loop =====
cout << "====> Start computation ... " << endl << endl;
// *z = *(myProcessInteraction->y(0)->getVectorPtr(0));
unsigned int k = 0; // Current step
// Simulation loop
boost::timer time;
time.restart();
unsigned int i = 0;
int j = 0;
SP::SiconosVector err(new SiconosVector(2));
SP::SiconosVector il1(new SiconosVector(1));
SP::SiconosVector temps(new SiconosVector(N + 1));
while (k < N - 1)
{
k++;
// osnspb->setNumericsVerboseMode(1);
// *z = *(myProcessInteraction->y(0)->getVectorPtr(0));
s->computeOneStep();
(*err)(0) = abs((*zProc)(0) - (*xProc)(0)); //voltage error
(*err)(1) = abs(((*zProc)(0) / Rl) - ((*xProc)(0) / Rl)); //current error
(*il1)(0) = ((*xProc)(0) / Rl) - ((*xProc)(2) + (*xProc)(3)) + Cp * (*xProc)(1); //current through L_1
dataPlot(k, 0) = s->nextTime();
dataPlot(k, 1) = (*xProc)(0); //output voltage v0
dataPlot(k, 2) = (*xProc)(0) / Rl; //output current i0
dataPlot(k, 3) = (*xProc)(2); //il2
dataPlot(k, 4) = (*xProc)(3); //il3
dataPlot(k, 5) = (*lambdaProc)(0); //u1
dataPlot(k, 6) = (*lambdaProc)(1); //u2
dataPlot(k, 7) = (*lambdaProc)(2); //u3
dataPlot(k, 8) = (*yProc)(0); //y1
dataPlot(k, 9) = (*yProc)(1); //y2
dataPlot(k, 10) = (*yProc)(2); //y3
dataPlot(k, 11) = (*zProc)(0); //v_ref
dataPlot(k, 12) = (*zProc)(0) / Rl; //i_ref
dataPlot(k, 13) = (*err)(0); //voltage_error
dataPlot(k, 14) = (*err)(1); //current_error
dataPlot(k, 15) = (*il1)(0); //il1
dataPlot(k, 16) = (*xProc)(1); //v'0
s->nextStep();
//////////////////////////////////////////////////////////////////////////////////
if (((*yProc)(0) > 1e-8) || ((*yProc)(0) < -1e-8))
{
(*temps)(k) = (*yProc)(0);
}
///////////////////////////////////////////////////////////////////////////////////
}
while (i < N - 1)
{
if ((*temps)(i) == 0)
j = j + 1;
i++;
}
cout << "The sliding mode appears at the step number : \n" << N - 1 - j << endl;
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;
ioMatrix::write("ParallelInverter.dat", "ascii", dataPlot, "noDim");
std::cout << "Comparison with a reference file" << std::endl;
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("ParallelInverter.ref", "ascii", dataPlotRef);
SP::SiconosVector errSim = compareMatrices(dataPlot, dataPlotRef);
std::cout << "errSim display :" << std::endl;
errSim->display();
double error = ((dataPlot - dataPlotRef).normInf())/(dataPlotRef.normInf());
std::cout << "error =" << error << std::endl;
if (error > 1e-12) // some data are > 1e4
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught in SimpleExampleRelay.cpp" << endl;
}
}
