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;
}
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
}
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;
}
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));
}
/* 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);
}
/*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();
}
}
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;
}
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));
}
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;
}
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);
}