예제 #1
0
void SphereLDS::computeJacobianFGyrqDot(SP::SiconosVector q, SP::SiconosVector v)
{
  double theta    = q->getValue(3);

  double thetadot = v->getValue(3);
  double phidot   = v->getValue(4);
  double psidot   = v->getValue(5);

  double sintheta   = sin(theta);

  _jacobianFGyrqDot->zero();


  (*_jacobianFGyrqDot)(3, 3) = 0;
  (*_jacobianFGyrqDot)(3, 4) = I * psidot * sintheta;
  (*_jacobianFGyrqDot)(3, 5) = I * phidot * sintheta;

  (*_jacobianFGyrqDot)(4, 3) = -I * psidot * sintheta;
  (*_jacobianFGyrqDot)(4, 4) = 0;
  (*_jacobianFGyrqDot)(4, 5) = -I * thetadot * sintheta;

  (*_jacobianFGyrqDot)(5, 3) =  -I * phidot * sintheta;
  (*_jacobianFGyrqDot)(5, 4) =  -I * thetadot * sintheta;
  (*_jacobianFGyrqDot)(5, 5) = 0;

}
예제 #2
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);

}
/*
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
}
예제 #4
0
void SphereLDS::computeJacobianFGyrq(SP::SiconosVector q, SP::SiconosVector v)
{
  double theta    = q->getValue(3);

  double thetadot = v->getValue(3);
  double phidot   = v->getValue(4);
  double psidot   = v->getValue(5);

  double costheta = cos(theta);

  _jacobianFGyrq->zero();

  (*_jacobianFGyrq)(3, 3) = -I * psidot * phidot * costheta;
  (*_jacobianFGyrq)(4, 3) = I * psidot * thetadot * costheta;
  (*_jacobianFGyrq)(5, 3) = I * psidot * thetadot * costheta;


}
예제 #5
0
void KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )
{

  //x1->display();
  double X1 = x1->getValue(0);
  double Y1 = x1->getValue(1);
  double Z1 = x1->getValue(2);
  double q10 = x1->getValue(3);
  double q11 = x1->getValue(4);
  double q12 = x1->getValue(5);
  double q13 = x1->getValue(6);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(x2)
  {
    //printf("checkInitPos x2:\n");
    //x2->display();
    X2 = x2->getValue(0);
    Y2 = x2->getValue(1);
    Z2 = x2->getValue(2);
    q20 = x2->getValue(3);
    q21 = x2->getValue(4);
    q22 = x2->getValue(5);
    q23 = x2->getValue(6);
  }

  if (Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hx is large :" << Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
  if (Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hy is large :" << Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
  if (Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) > DBL_EPSILON )
  {
    std::cout << "KneeJointR::checkInitPos( SP::SiconosVector x1 ,  SP::SiconosVector x2 )" << std::endl;
    std::cout << " Hz is large :" << Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23) << std::endl;
  }
     
  
  // printf("checkInitPos Hx : %e\n", Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  // printf("checkInitPos Hy : %e\n", Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  // printf("checkInitPos Hz : %e\n", Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));


}
예제 #6
0
/* Given a position of a point in the Inertial Frame and the configuration vector q of a solid
 * returns a position in the spatial frame.
 */
void fromInertialToSpatialFrame(double *positionInInertialFrame, double *positionInSpatialFrame, SP::SiconosVector  q  )
{
double q0 = q->getValue(3);
double q1 = q->getValue(4);
double q2 = q->getValue(5);
double q3 = q->getValue(6);

::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
::boost::math::quaternion<double>    quatpos(0, positionInInertialFrame[0], positionInInertialFrame[1], positionInInertialFrame[2]);
::boost::math::quaternion<double>    quatBuff;

//perform the rotation
quatBuff = quatQ * quatpos * quatcQ;

positionInSpatialFrame[0] = quatBuff.R_component_2()+q->getValue(0);
positionInSpatialFrame[1] = quatBuff.R_component_3()+q->getValue(1);
positionInSpatialFrame[2] = quatBuff.R_component_4()+q->getValue(2);

}
예제 #7
0
/*get the quaternion from siconos and 1787update the CADs model*/
void MBTB_updateDSFromSiconos()
{
  //ACE_times[ACE_TIMER_UPDATE_POS].start();
  for(unsigned int numDS=0; numDS<sNbOfBodies; numDS++)
  {
    SP::SiconosVector q = sDS[numDS]->q();
    //printf("step %d siconos %s ->q:\n",mTimerCmp,sPieceName[numDS]);
    //q->display();
    double x=q->getValue(0);
    double y=q->getValue(1);
    double z=q->getValue(2);
    double q1=q->getValue(3);
    double q2=q->getValue(4);
    double q3=q->getValue(5);
    double q4=q->getValue(6);
    ACE_times[ACE_TIMER_UPDATE_POS].start();
    CADMBTB_moveObjectFromQ(numDS,x,y,z,q1,q2,q3,q4);
    ACE_times[ACE_TIMER_UPDATE_POS].stop();
    int res = sTimerCmp%FREQ_UPDATE_GRAPHIC;
    ACE_times[ACE_TIMER_GRAPHIC].start();
    if(!res)
    {
      /*THIS CODE REBUILD THE GRAPHICAL MODEL
      getContext()->Erase(spAISToposDS[numDS]);
      spAISToposDS[numDS] = new AIS_Shape( sTopoDSPiece[numDS] );
      getContext()->Display( spAISToposDS[numDS], false );*/

      //spAISToposDS[numDS]->SetTransformation(&(sGeomTrsf[numDS]),true,false);//new Geom_Transformation(sTrsfPiece[numDS]),true);

      CADMBTB_moveGraphicalModelFromModel(numDS,numDS);

      //spAISToposDS[numDS]->SetTransformation(&(sGeomTrsf[numDS])
      //				     ,false,true);
      //      getContext()->Display( spAISToposDS[numDS], false );
    }
    ACE_times[ACE_TIMER_GRAPHIC].stop();
  }

}
예제 #8
0
void SphereLDS::computeFGyr(SP::SiconosVector q, SP::SiconosVector v)
{

  assert(q->size() == 6);
  assert(v->size() == 6);

  //  normalize(q,3);
  //normalize(q,4);
  //normalize(q,5);

  double theta    = q->getValue(3);

  double thetadot = v->getValue(3);
  double phidot   = v->getValue(4);
  double psidot   = v->getValue(5);

  double sintheta   = sin(theta);

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

  (*_fGyr)(3) = I * psidot * phidot * sintheta;
  (*_fGyr)(4) = -I * psidot * thetadot * sintheta;
  (*_fGyr)(5) = -I * phidot * thetadot * sintheta;
}
예제 #9
0
void KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 )
{
  DEBUG_BEGIN("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2) \n");
  _dotjachq->zero();
  
  double Xdot1 = qdot1->getValue(0);
  double Ydot1 = qdot1->getValue(1);
  double Zdot1 = qdot1->getValue(2);
  double qdot10 = qdot1->getValue(3);
  double qdot11 = qdot1->getValue(4);
  double qdot12 = qdot1->getValue(5);
  double qdot13 = qdot1->getValue(6);

  double Xdot2 = 0;
  double Ydot2 = 0;
  double Zdot2 = 0;
  double qdot20 = 1;
  double qdot21 = 0;
  double qdot22 = 0;
  double qdot23 = 0;

  if(qdot2)
  {
    Xdot2 = qdot2->getValue(0);
    Ydot2 = qdot2->getValue(1);
    Zdot2 = qdot2->getValue(2);
    qdot20 = qdot2->getValue(3);
    qdot21 = qdot2->getValue(4);
    qdot22 = qdot2->getValue(5);
    qdot23 = qdot2->getValue(6);
    DotJd1d2(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13, Xdot2, Ydot2, Zdot2, qdot20, qdot21, qdot22, qdot23);
  }
  else
    DotJd1(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13);

  DEBUG_END("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 ) \n");
}
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  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);

  _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));

  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  _NPG2->zero();
  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;



  computeRotationMatrix(q2,_rotationMatrixAbsToBody);
  prod(*_NPG2, *_rotationMatrixAbsToBody, *_AUX1, true);

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj + 6, -_RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj + 6, -_AUX2->getValue(0, jj - 3));
}
예제 #11
0
int main(int argc, char *argv[])
{
  //printf("argc %i\n", argc);
  int cmp=0;
  int dimX = 2;

  char  filename[50] = "sign.";

  //***** Set the initial condition
  // default, x0 = (1, 6)
  // else read from command line
  SP::SiconosVector xti(new SiconosVector(dimX));
  if (argc==1)
  {
    xti->setValue(0,1);
    xti->setValue(1,6);
    strncpy(&filename[5],"1.6.log",7);
  }
  else if (argc==3)
  {
    //printf("argv[0] %s\n", argv[0]);
    printf("xti(0) is set to %f\n", atof(argv[1]));
    printf("xti(1) is set to %f\n", atof(argv[2]));

    xti->setValue(0,atof(argv[1]));
    xti->setValue(1,atof(argv[2]));
    int sizeofargv1 = strlen(argv[1]);
    // printf("sizeofargv1 %i\n",sizeofargv1);
    strncpy(&filename[5],argv[1],sizeofargv1);
    int sizeofargv2 = strlen(argv[2]);
    //printf("sizeofargv2 %i\n",sizeofargv2);
    strncpy(&filename[5+sizeofargv1],".",1);

    strncpy(&filename[5+sizeofargv1+1],argv[2],sizeofargv2);
    strncpy(&filename[5+sizeofargv1+sizeofargv2+1],".log",4);

    printf("Output is written in filename %s\n",  filename);
  }
  else
  {
    cout << "wrong  number of arguments = " << argc << endl;
  }

  int NBStep = (int) floor(sTf/sStep);
  // NBStep =1;
  //*****BUILD THE DYNAMICAL SYSTEM
  
  SP::MyDS aDS(new MyDS(xti));

//******BUILD THE RELATION
//  SP::NonlinearRelationWithSign aR;
//  aR.reset(new NonlinearRelationWithSign());
  SP::NonlinearRelationWithSignInversed aR(new NonlinearRelationWithSignInversed());

//*****BUILD THE NSLAW
  double ub = -1.;
  double lb = 1.;
  SP::NonSmoothLaw aNSL(new RelayNSL(sNSLawSize, lb, ub));
  
  //****BUILD THE INTERACTION
  SP::Interaction aI(new Interaction(sNSLawSize,aNSL,aR));

//****BUILD THE model
  SP::Model  aM(new Model(0,sTf));
  aM->nonSmoothDynamicalSystem()->insertDynamicalSystem(aDS);
  aM->nonSmoothDynamicalSystem()->link(aI,aDS);

  // -- (1) OneStepIntegrators --
  SP::OneStepIntegrator  aEulerMoreauOSI(new EulerMoreauOSI(0.5));

  // -- (2) Time discretisation --
  SP::TimeDiscretisation  aTD(new TimeDiscretisation(0,sStep));

  // -- (3) Non smooth problem
  SP::Relay osnspb(new Relay(SICONOS_RELAY_LEMKE));
  osnspb->numericsSolverOptions()->dparam[0]=1e-08;
  osnspb->numericsSolverOptions()->iparam[0]=0;  // Multiple solutions 0 or 1
  //osnspb->numericsSolverOptions()->iparam[3]=48;

  osnspb->setNumericsVerboseMode(0);

  // -- (4) Simulation setup with (1) (2) (3)
  SP::TimeStepping aS(new TimeStepping(aTD,aEulerMoreauOSI,osnspb));
  aS->setComputeResiduY(true);
  aS->setUseRelativeConvergenceCriteron(false);

// Initialization
  printf("-> Initialisation \n");
  aM->setSimulation(aS);
  aM->initialize();
  printf("-> End of initialization \n");

// BUILD THE STEP INTEGRATOR

  SP::SiconosVector  x = aDS->x();
  SP::SiconosVector  vectorfield = aDS->rhs();
  SP::SiconosVector  y = aI->y(0);
  SP::SiconosVector  lambda = aI->lambda(0);

  unsigned int outputSize = 9;
  SimpleMatrix dataPlot(NBStep+1,outputSize);

  cout << "=== Start of simulation: "<<NBStep<<" steps ===" << endl;

  printf("=== Start of simulation: %d steps ===  \n", NBStep);

  dataPlot(0, 0) = aM->t0();
  dataPlot(0,1) = x->getValue(0);
  dataPlot(0,2) = x->getValue(1);
  dataPlot(0, 3) = lambda->getValue(0);
  dataPlot(0, 4) = lambda->getValue(1);
  dataPlot(0, 5) = lambda->getValue(2);
  dataPlot(0, 6) = lambda->getValue(3);
  dataPlot(0, 7) = vectorfield->getValue(0);
  dataPlot(0, 8) = vectorfield->getValue(1);


  boost::progress_display show_progress(NBStep);

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

  
  std::cout << NBStep << std::endl;
  NBStep = 10;
  for(int k = 0 ; k < NBStep ; k++)
  {
#ifdef SICONOS_DEBUG
    std::cout<<"-> Running step:"<<k<<std::endl;
#endif
    cmp++;

    aS->newtonSolve(1e-4, 200);

    dataPlot(cmp, 0) = aS->nextTime();
    dataPlot(cmp, 1) = x->getValue(0);
    dataPlot(cmp, 2) = x->getValue(1);
    dataPlot(cmp, 3) = lambda->getValue(0);
    dataPlot(cmp, 4) = lambda->getValue(1);
    dataPlot(cmp, 5) = lambda->getValue(2);
    dataPlot(cmp, 6) = lambda->getValue(3);

    aDS->computeRhs(aS->nextTime(),true);

    if (cmp==1) // tricks just for display to avoid the computation of the initial Rhs
    {
      dataPlot(cmp-1, 7) = vectorfield->getValue(0);
      dataPlot(cmp-1, 8) = vectorfield->getValue(1);
    }

    dataPlot(cmp, 7) = vectorfield->getValue(0);
    dataPlot(cmp, 8) = vectorfield->getValue(1);
    ++show_progress;

    aS->nextStep();

    // (*fout)<<cmp<<" "<<x->getValue(0)<<" "<<x->getValue(1)<<" "<<lambda->getValue(0)<<" "<<lambda->getValue(1)<<" "<<lambda->getValue(2)<<" "<<lambda->getValue(3)<<endl;
  }


  dataPlot.resize(cmp,outputSize);
  ioMatrix::write(filename, "ascii", dataPlot, "noDim");
  cout << "=== End of simulation. === " << endl;
  return 0;

}
예제 #12
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);
}