本文整理汇总了C++中sp::SiconosVector::getValue方法的典型用法代码示例。如果您正苦于以下问题:C++ SiconosVector::getValue方法的具体用法?C++ SiconosVector::getValue怎么用?C++ SiconosVector::getValue使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sp::SiconosVector
的用法示例。
在下文中一共展示了SiconosVector::getValue方法的12个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: computeJacobianFGyrqDot
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: normalize
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);
}
示例3: printf
/*
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: computeJacobianFGyrq
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: checkInitPos
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: fromInertialToSpatialFrame
/* 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: MBTB_updateDSFromSiconos
/*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: computeFGyr
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: computeDotJachq
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");
}
示例10: computeRotationMatrix
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: main
//.........这里部分代码省略.........
// -- (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: _MBTB_BodyBuildComputeInitPosition
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);
}