Esempio n. 1
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

}
Esempio n. 2
0
SP::SiconosVector Simulation::lambda(unsigned int level, unsigned int coor)
{
  // return input(level) (ie with lambda[level]) for all Interactions.
  // assert(level>=0);

  DEBUG_BEGIN("Simulation::input(unsigned int level, unsigned int coor)\n");
  DEBUG_PRINTF("with level = %i and coor = %i \n", level,coor);

  InteractionsGraph::VIterator ui, uiend;
  SP::Interaction inter;
  SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();

  SP::SiconosVector lambda (new SiconosVector (_nsds->topology()->indexSet0()->size() ));
  int i=0;
  for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
  {
    inter = indexSet0->bundle(*ui);
    assert(inter->lowerLevelForOutput() <= level);
    assert(inter->upperLevelForOutput() >= level);
    lambda->setValue(i,inter->lambda(level)->getValue(coor));
    i++;
  }
  DEBUG_END("Simulation::input(unsigned int level, unsigned int coor)\n");
  return lambda;
}
Esempio n. 3
0
void normalize(SP::SiconosVector q, unsigned int i)
{

  q->setValue(i, fmod(q->getValue(i), _2PI));

  assert(fabs(q->getValue(i)) - std::numeric_limits<double>::epsilon() >= 0.);
  assert(fabs(q->getValue(i)) < _2PI);

}
Esempio n. 4
0
// ================= Creation of the model =======================
void Disks::init()
{

    SP::TimeDiscretisation timedisc_;
    SP::TimeStepping simulation_;
    SP::FrictionContact osnspb_;

    // User-defined main parameters

    double t0 = 0;                   // initial computation time

    double T =  std::numeric_limits<double>::infinity();

    double h = 0.01;                // time step
    double g = 9.81;

    double theta = 0.5;              // theta for MoreauJeanOSI integrator

    std::string solverName = "NSGS";

    // -----------------------------------------
    // --- Dynamical systems && interactions ---
    // -----------------------------------------

    double R;
    double m;

    try
    {

        // ------------
        // --- Init ---
        // ------------

        std::cout << "====> Model loading ..." << std::endl << std::endl;

        _plans.reset(new SimpleMatrix("plans.dat", true));
        if (_plans->size(0) == 0)
        {
            /* default plans */
            double A1 = P1A;
            double B1 = P1B;
            double C1 = P1C;

            double A2 = P2A;
            double B2 = P2B;
            double C2 = P2C;

            _plans.reset(new SimpleMatrix(6, 6));
            _plans->zero();
            (*_plans)(0, 0) = 0;
            (*_plans)(0, 1) = 1;
            (*_plans)(0, 2) = -GROUND;

            (*_plans)(1, 0) = 1;
            (*_plans)(1, 1) = 0;
            (*_plans)(1, 2) = WALL;

            (*_plans)(2, 0) = 1;
            (*_plans)(2, 1) = 0;
            (*_plans)(2, 2) = -WALL;

            (*_plans)(3, 0) = 0;
            (*_plans)(3, 1) = 1;
            (*_plans)(3, 2) = -TOP;

            (*_plans)(4, 0) = A1;
            (*_plans)(4, 1) = B1;
            (*_plans)(4, 2) = C1;

            (*_plans)(5, 0) = A2;
            (*_plans)(5, 1) = B2;
            (*_plans)(5, 2) = C2;

        }

        /* set center positions */
        for (unsigned int i = 0 ; i < _plans->size(0); ++i)
        {
            SP::DiskPlanR tmpr;
            tmpr.reset(new DiskPlanR(1, (*_plans)(i, 0), (*_plans)(i, 1), (*_plans)(i, 2),
                                     (*_plans)(i, 3), (*_plans)(i, 4), (*_plans)(i, 5)));
            (*_plans)(i, 3) = tmpr->getXCenter();
            (*_plans)(i, 4) = tmpr->getYCenter();
        }

        /*    _moving_plans.reset(new FMatrix(1,6));
            (*_moving_plans)(0,0) = &A;
            (*_moving_plans)(0,1) = &B;
            (*_moving_plans)(0,2) = &C;
            (*_moving_plans)(0,3) = &DA;
            (*_moving_plans)(0,4) = &DB;
            (*_moving_plans)(0,5) = &DC;*/



        SP::SiconosMatrix Disks;
        Disks.reset(new SimpleMatrix("disks.dat", true));

        // -- OneStepIntegrators --
        SP::OneStepIntegrator osi;
        osi.reset(new MoreauJeanOSI(theta));

        // -- Model --
        _model.reset(new Model(t0, T));

        for (unsigned int i = 0; i < Disks->size(0); i++)
        {
            R = Disks->getValue(i, 2);
            m = Disks->getValue(i, 3);

            SP::SiconosVector qTmp;
            SP::SiconosVector vTmp;

            qTmp.reset(new SiconosVector(NDOF));
            vTmp.reset(new SiconosVector(NDOF));
            vTmp->zero();
            (*qTmp)(0) = (*Disks)(i, 0);
            (*qTmp)(1) = (*Disks)(i, 1);

            SP::LagrangianDS body;
            if (R > 0)
                body.reset(new Disk(R, m, qTmp, vTmp));
            else
                body.reset(new Circle(-R, m, qTmp, vTmp));

            // -- Set external forces (weight) --
            SP::SiconosVector FExt;
            FExt.reset(new SiconosVector(NDOF));
            FExt->zero();
            FExt->setValue(1, -m * g);
            body->setFExtPtr(FExt);

            // add the dynamical system to the one step integrator
            osi->insertDynamicalSystem(body);

            // add the dynamical system in the non smooth dynamical system
            _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);

        }


        _model->nonSmoothDynamicalSystem()->setSymmetric(true);


        // ------------------
        // --- Simulation ---
        // ------------------

        // -- Time discretisation --
        timedisc_.reset(new TimeDiscretisation(t0, h));

        // -- OneStepNsProblem --
        osnspb_.reset(new FrictionContact(2));

        osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
        // iterations
        osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
        // iterations
        osnspb_->numericsSolverOptions()->dparam[0] = 1e-3; // Tolerance


        osnspb_->setMaxSize(6 * ((3 * Ll * Ll + 3 * Ll) / 2 - Ll));
        osnspb_->setMStorageType(1);            // Sparse storage
        osnspb_->setNumericsVerboseMode(0);

        osnspb_->setKeepLambdaAndYState(true);  // inject previous solution

        // -- Simulation --
        simulation_.reset(new TimeStepping(timedisc_));

        std11::static_pointer_cast<TimeStepping>(simulation_)->setNewtonMaxIteration(3);

        simulation_->insertIntegrator(osi);
        simulation_->insertNonSmoothProblem(osnspb_);

        simulation_->setCheckSolverFunction(localCheckSolverOuput);

        // --- Simulation initialization ---

        std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;

        SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.3, 2));

        _playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));

        _playground->insert(nslaw, 0, 0);

        _model->initialize(simulation_);

    }

    catch (SiconosException e)
    {
        std::cout << e.report() << std::endl;
        exit(1);
    }
    catch (...)
    {
        std::cout << "Exception caught in Disks::init()" << std::endl;
        exit(1);
    }
}
Esempio n. 5
0
// ================= Creation of the model =======================
void Spheres::init()
{

  SP::TimeDiscretisation timedisc_;
  SP::Simulation simulation_;
  SP::FrictionContact osnspb_;


  // User-defined main parameters

  double t0 = 0;                   // initial computation time

  double T = std::numeric_limits<double>::infinity();

  double h = 0.01;                // time step
  double g = 9.81;

  double theta = 0.5;              // theta for MoreauJeanOSI integrator

  std::string solverName = "NSGS";

  // -----------------------------------------
  // --- Dynamical systems && interactions ---
  // -----------------------------------------


  double R;
  double m;

  try
  {

    // ------------
    // --- Init ---
    // ------------

    std::cout << "====> Model loading ..." << std::endl << std::endl;

    _plans.reset(new SimpleMatrix("plans.dat", true));

    SP::SiconosMatrix Spheres;
    Spheres.reset(new SimpleMatrix("spheres.dat", true));

    // -- OneStepIntegrators --
    SP::OneStepIntegrator osi;
    osi.reset(new MoreauJeanOSI(theta));

    // -- Model --
    _model.reset(new Model(t0, T));

    for (unsigned int i = 0; i < Spheres->size(0); i++)
    {
      R = Spheres->getValue(i, 3);
      m = Spheres->getValue(i, 4);

      SP::SiconosVector qTmp;
      SP::SiconosVector vTmp;

      qTmp.reset(new SiconosVector(NDOF));
      vTmp.reset(new SiconosVector(NDOF));
      vTmp->zero();
      (*qTmp)(0) = (*Spheres)(i, 0);
      (*qTmp)(1) = (*Spheres)(i, 1);
      (*qTmp)(2) = (*Spheres)(i, 2);

      (*qTmp)(3) = M_PI / 2;
      (*qTmp)(4) = M_PI / 4;
      (*qTmp)(5) = M_PI / 2;

      (*vTmp)(0) = 0;
      (*vTmp)(1) = 0;
      (*vTmp)(2) = 0;


      (*vTmp)(3) = 0;
      (*vTmp)(4) = 0;
      (*vTmp)(5) = 0;


      SP::LagrangianDS body;
      body.reset(new SphereLDS(R, m, std11::shared_ptr<SiconosVector>(qTmp), std11::shared_ptr<SiconosVector>(vTmp)));

      // -- Set external forces (weight) --
      SP::SiconosVector FExt;
      FExt.reset(new SiconosVector(NDOF));
      FExt->zero();
      FExt->setValue(2, -m * g);
      body->setFExtPtr(FExt);

      // add the dynamical system to the one step integrator
      osi->insertDynamicalSystem(body);

      // add the dynamical system in the non smooth dynamical system
      _model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);

    }

    // ------------------
    // --- Simulation ---
    // ------------------

    // -- Time discretisation --
    timedisc_.reset(new TimeDiscretisation(t0, h));

    // -- OneStepNsProblem --
    osnspb_.reset(new FrictionContact(3));

    osnspb_->numericsSolverOptions()->iparam[0] = 100; // Max number of
    // iterations
    osnspb_->numericsSolverOptions()->iparam[1] = 20; // compute error
    // iterations

    osnspb_->numericsSolverOptions()->iparam[4] = 2; // projection

    osnspb_->numericsSolverOptions()->dparam[0] = 1e-6; // Tolerance
    osnspb_->numericsSolverOptions()->dparam[2] = 1e-8; // Local tolerance


    osnspb_->setMaxSize(16384);       // max number of interactions
    osnspb_->setMStorageType(1);      // Sparse storage
    osnspb_->setNumericsVerboseMode(0); // 0 silent, 1 verbose
    osnspb_->setKeepLambdaAndYState(true); // inject previous solution

    simulation_.reset(new TimeStepping(timedisc_));
    simulation_->insertIntegrator(osi);
    simulation_->insertNonSmoothProblem(osnspb_);
    //     simulation_->setCheckSolverFunction(localCheckSolverOuput);

    // --- Simulation initialization ---

    std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;

    SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0, 0, 0.8, 3));

    _playground.reset(new SpaceFilter(3, 6, _model, _plans, _moving_plans));

    _playground->insert(nslaw, 0, 0);

    _model->initialize(simulation_);

  }

  catch (SiconosException e)
  {
    std::cout << e.report() << std::endl;
    exit(1);
  }
  catch (...)
  {
    std::cout << "Exception caught in Spheres::init()" << std::endl;
    exit(1);
  }
}
Esempio n. 6
0
int main()
{

  // User-defined main parameters
  double t0 = 0;                   // initial computation time
  double T = 20.0;                 // end of computation time
  double h = 0.005;                // time step
  double position_init = 10.0;     // initial position
  double velocity_init = 0.0;      // initial velocity

  double g = 9.81;
  double theta = 0.5;              // theta for MoreauJeanOSI integrator

  // -----------------------------------------
  // --- Dynamical systems && interactions ---
  // -----------------------------------------

  try
  {

    // ------------
    // --- Init ---
    // ------------

    std::cout << "====> Model loading ..." << std::endl << std::endl;


    // -- OneStepIntegrators --
    SP::OneStepIntegrator osi;
    osi.reset(new MoreauJeanOSI(theta));

    // -- Model --
    SP::Model model(new Model(t0, T));

    std::vector<SP::BulletWeightedShape> shapes;

    // note: no rebound with a simple Bullet Box, why ?
    // the distance after the broadphase contact detection is negative
    // and then stay negative.
    // SP::btCollisionShape box(new btBoxShape(btVector3(1,1,1)));
    // SP::BulletWeightedShape box1(new BulletWeightedShape(box,1.0));
    // This is ok if we build one with btConveHullShape
    SP::btCollisionShape box(new btConvexHullShape());
    {
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, 1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, 1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, 1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, 1.0));
    }
    SP::BulletWeightedShape box1(new BulletWeightedShape(box, 1.0));
    shapes.push_back(box1);

    SP::SiconosVector q0(new SiconosVector(7));
    SP::SiconosVector v0(new SiconosVector(6));
    v0->zero();
    q0->zero();

    (*q0)(2) = position_init;
    (*q0)(3) = 1.0;
    (*v0)(2) = velocity_init;

    // -- The dynamical system --
    // -- the default contactor is the shape given in the constructor
    // -- the contactor id is 0
    SP::BulletDS body(new BulletDS(box1, q0, v0));

    // -- Set external forces (weight) --
    SP::SiconosVector FExt;
    FExt.reset(new SiconosVector(3)); //
    FExt->zero();
    FExt->setValue(2, - g * box1->mass());
    body->setFExtPtr(FExt);

    // -- Add the dynamical system in the non smooth dynamical system
    model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);

    SP::btCollisionObject ground(new btCollisionObject());
    ground->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
    SP::btCollisionShape groundShape(new btBoxShape(btVector3(30, 30, .5)));
    btMatrix3x3 basis;
    basis.setIdentity();
    ground->getWorldTransform().setBasis(basis);
    ground->setCollisionShape(&*groundShape);
    ground->getWorldTransform().getOrigin().setZ(-.50);

    // ------------------
    // --- Simulation ---
    // ------------------

    // -- Time discretisation --
    SP::TimeDiscretisation timedisc(new TimeDiscretisation(t0, h));

    // -- OneStepNsProblem --
    SP::FrictionContact osnspb(new FrictionContact(3));

    // -- Some configuration

    osnspb->numericsSolverOptions()->iparam[0] = 1000; // Max number of
    // iterations
    osnspb->numericsSolverOptions()->dparam[0] = 1e-5; // Tolerance


    osnspb->setMaxSize(16384);                        // max number of
    // interactions

    osnspb->setMStorageType(1);                      // Sparse storage

    osnspb->setNumericsVerboseMode(0);               // 0 silent, 1
    // verbose

    osnspb->setKeepLambdaAndYState(true);            // inject
    // previous
    // solution

    // --- Simulation initialization ---

    std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;

    int N = ceil((T - t0) / h); // Number of time steps


    SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0.8, 0., 0.0, 3));

    // -- The space filter performs broadphase collision detection
    SP::BulletSpaceFilter space_filter(new BulletSpaceFilter(model));

    // -- insert a non smooth law for contactors id 0
    space_filter->insert(nslaw, 0, 0);

    // -- add multipoint iterations, this is needed to gather at least
    // -- 3 contact points and avoid objects penetration, see Bullet
    // -- documentation
    space_filter->collisionConfiguration()->setConvexConvexMultipointIterations();
    space_filter->collisionConfiguration()->setPlaneConvexMultipointIterations();

    // -- The ground is a static object
    // -- we give it a group contactor id : 0
    space_filter->addStaticObject(ground, 0);

    // -- MoreauJeanOSI Time Stepping with Bullet Dynamical Systems
    SP::BulletTimeStepping simulation(new BulletTimeStepping(timedisc));

    simulation->insertIntegrator(osi);
    simulation->insertNonSmoothProblem(osnspb);
    model->setSimulation(simulation);

    model->initialize();

    std::cout << "====> End of initialisation ..." << std::endl << std::endl;

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 4;
    SimpleMatrix dataPlot(N + 1, outputSize);
    dataPlot.zero();

    SP::SiconosVector q = body->q();
    SP::SiconosVector v = body->velocity();

    dataPlot(0, 0) = model->t0();
    dataPlot(0, 1) = (*q)(2);
    dataPlot(0, 2) = (*v)(2);

    // --- Time loop ---

    std::cout << "====> Start computation ... " << std::endl << std::endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 1;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();

    while (simulation->hasNextEvent())
    {
      space_filter->buildInteractions(model->currentTime());

      simulation->computeOneStep();

      // --- Get values to be plotted ---
      dataPlot(k, 0) =  simulation->nextTime();
      dataPlot(k, 1) = (*q)(2);
      dataPlot(k, 2) = (*v)(2);

      // If broadphase collision detection shows some contacts then we may
      // display contact forces.
      if (space_filter->collisionWorld()->getDispatcher()->getNumManifolds() > 0)
      {

        // we *must* have an indexSet0, filled by Bullet broadphase
        // collision detection and an indexSet1, filled by
        // TimeStepping::updateIndexSet with the help of Bullet
        // getDistance() function
        if (model->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() == 2)
        {
          SP::InteractionsGraph index1 = simulation->indexSet(1);

          // This is the narrow phase contact detection : if
          // TimeStepping::updateIndexSet has filled indexSet1 then we
          // have some contact forces to display
          if (index1->size() > 0)
          {

            // Four contact points for a cube with a side facing the
            // ground. Note : changing Bullet margin for collision
            // detection may lead this assertion to be false.
            if (index1->size() == 4)
            {
              InteractionsGraph::VIterator iur = index1->begin();

              // different version of bullet may not gives the same
              // contact points! So we only keep the summation.
              dataPlot(k, 3) =
                index1->bundle(*iur)-> lambda(1)->norm2() +
                index1->bundle(*++iur)->lambda(1)->norm2() +
                index1->bundle(*++iur)->lambda(1)->norm2() +
                index1->bundle(*++iur)->lambda(1)->norm2();
            }
          }
        }
      }

      simulation->nextStep();
      ++show_progress;
      k++;
    }


    std::cout << std::endl << "End of computation - Number of iterations done: " << k - 1 << std::endl;
    std::cout << "Computation Time " << time.elapsed()  << std::endl;

    // --- Output files ---
    std::cout << "====> Output file writing ..." << std::endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");

    // Comparison with a reference file
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("result.ref", "ascii", dataPlotRef);

    if ((dataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The result is rather different from the reference file : "
                << (dataPlot - dataPlotRef).normInf() << std::endl;
      return 1;
    }



  }

  catch (SiconosException e)
  {
    std::cout << e.report() << std::endl;
    exit(1);
  }
  catch (...)
  {
    std::cout << "Exception caught in BulletBouncingBox" << std::endl;
    exit(1);
  }

  return 0;
}
Esempio n. 7
0
void _MBTB_BodyBuildComputeInitPosition(unsigned int numDS,   double mass,
                                        SP::SiconosVector initPos, SP::SiconosVector modelCenterMass,SP::SimpleMatrix inertialMatrix, SP::SiconosVector& q10,SP::SiconosVector& v10)
{
  assert(sNbOfBodies > numDS &&"MBTB_BodyBuild numDS out of range.");
  /*2)  move the cad model to the initial position*/
  /*It consists in going to the position (x,y,z,q1,q2,q3,q4) starting from (0,0,0,1,0,0,0).
    Endeed, after loading the CAD, the cad model must be moved to the initial position of the simulation.
    This position is not q0 of the siconos::DS because siconos work in the frame of G, and G is not necessary at the origin.*/
  double q1=cos(0.5*initPos->getValue(6));
  double q2=initPos->getValue(3)*sin(0.5*initPos->getValue(6));
  double q3=initPos->getValue(4)*sin(0.5*initPos->getValue(6));
  double q4=initPos->getValue(5)*sin(0.5*initPos->getValue(6));
  double x=initPos->getValue(0);
  double y=initPos->getValue(1);
  double z=initPos->getValue(2);

  CADMBTB_moveObjectFromQ(numDS,
                          x,
                          y,
                          z,
                          q1,
                          q2,
                          q3,
                          q4);
  _MBTB_updateContactFromDS(numDS);
  /*3) compute the q0 of Siconos, that is the coordinate of G at the initial position*/
  //unsigned int qDim=7;
  //unsigned int nDof = 3;
  //unsigned int nDim = 6;
  //SP::SiconosVector q10(new SiconosVector(qDim));
  //SP::SiconosVector v10(new SiconosVector(nDim));
  q10->zero();
  v10->zero();


  /*From the siconos point of view, the dynamic equation are written at the center of gravity.*/
  /*q10 is the coordinate of G in the initial pos:
    --> The initial orientation is still computed.
    --> The translation must be updated because of G.
   */
  ::boost::math::quaternion<double>    quattrf(q1,q2,q3,q4);

  ::boost::math::quaternion<double>    quatOG(0,
      modelCenterMass->getValue(0),
      modelCenterMass->getValue(1),
      modelCenterMass->getValue(2));
  ::boost::math::quaternion<double>    quatRes(0,0,0,0);
  quatRes=quattrf*quatOG/quattrf;

  q10->setValue(0,quatRes.R_component_2()+initPos->getValue(0));
  q10->setValue(1,quatRes.R_component_3()+initPos->getValue(1));
  q10->setValue(2,quatRes.R_component_4()+initPos->getValue(2));
  //In current version, the initial orientation is (1,0,0,0)
  q10->setValue(3,q1);
  q10->setValue(4,q2);
  q10->setValue(5,q3);
  q10->setValue(6,q4);
  //sq10[numDS]->display();
  //gp_Ax3 aux=GetPosition(sTopoDSPiece[numDS]);
  //printf("and sould be : %e, %e, %e\n",aux.Location().X(),aux.Location().Y(),aux.Location().Z());

  //set the translation of the CAD model.
  double q10x=q10->getValue(0);
  double q10y=q10->getValue(1);
  double q10z=q10->getValue(2);
  CADMBTB_setLocation(numDS,q10x,q10y,q10z);

  // sStartPiece[numDS]=Ax3Aux2;
  CADMBTB_moveGraphicalModelFromModel(numDS,numDS);

  // //In current version I = Id3
  // sI[numDS].reset(new SimpleMatrix(3,3));
  // sI[numDS]->zero();
  // //sI[numDS]->setValue(0,0,sMass[numDS]);sI[numDS]->setValue(1,1,sMass[numDS]);sI[numDS]->setValue(2,2,sMass[numDS]);
  // sI[numDS]->setValue(0,0,sMassMatrix[9*numDS+0]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(1,0,sMassMatrix[9*numDS+1]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(2,0,sMassMatrix[9*numDS+2]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(0,1,sMassMatrix[9*numDS+3]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(1,1,sMassMatrix[9*numDS+4]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(2,1,sMassMatrix[9*numDS+5]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(0,2,sMassMatrix[9*numDS+6]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(1,2,sMassMatrix[9*numDS+7]*sMassMatrixScale[numDS]);
  // sI[numDS]->setValue(2,2,sMassMatrix[9*numDS+8]*sMassMatrixScale[numDS]);
  // MBTB_Body * p =new MBTB_Body(q10,v10,mass,inertialMatrix,
  //			       BodyName, CADFile,
  //			       pluginLib, plunginFct);
  // NewtonEulerDS * p1 =new NewtonEulerDS(q10,v10,mass,inertialMatrix);
}