/*
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
}
Example #2
0
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

}
Example #3
0
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(););
Example #4
0
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");
}
Example #8
0
// 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;
  }
}