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;
}
void LagrangianDSTest::testcomputeDS()
{
std::cout << "-->Test: computeDS." <<std::endl;
DynamicalSystem * ds(new LagrangianDS(tmpxml2));
SP::LagrangianDS copy = std11::static_pointer_cast<LagrangianDS>(ds);
double time = 1.5;
ds->initialize("EventDriven", time);
ds->computeRhs(time);
std::cout << "-->Test: computeDS." <<std::endl;
ds->computeJacobianRhsx(time);
std::cout << "-->Test: computeDS." <<std::endl;
SimpleMatrix M(3, 3);
M(0, 0) = 1;
M(1, 1) = 2;
M(2, 2) = 3;
SP::SiconosMatrix jx = ds->jacobianRhsx();
SP::SiconosVector vf = ds->rhs();
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSI : ", *(vf->vector(0)) == *velocity0, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSJ : ", prod(M, *(vf->vector(1))) == (copy->getFExt() - copy->getFInt() - copy->getFGyr()) , true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 0))) == (copy->getJacobianFL(0)) , true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testComputeDSL : ", prod(M, *(jx->block(1, 1))) == (copy->getJacobianFL(1)) , true);
std::cout << "--> computeDS test ended with success." <<std::endl;
}
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;
}
SphereLDS::SphereLDS(double r, double m,
SP::SiconosVector qinit,
SP::SiconosVector vinit)
: LagrangianDS(qinit, vinit), radius(r), massValue(m)
{
normalize(q(), 3);
normalize(q(), 4);
normalize(q(), 5);
_ndof = 6;
assert(qinit->size() == _ndof);
assert(vinit->size() == _ndof);
_mass.reset(new SimpleMatrix(_ndof, _ndof));
_mass->zero();
I = massValue * radius * radius * 2. / 5.;
(*_mass)(0, 0) = (*_mass)(1, 1) = (*_mass)(2, 2) = massValue; ;
(*_mass)(3, 3) = (*_mass)(4, 4) = (*_mass)(5, 5) = I;
computeMass();
_jacobianFGyrq.reset(new SimpleMatrix(_ndof, _ndof));
_jacobianFGyrqDot.reset(new SimpleMatrix(_ndof, _ndof));
_fGyr.reset(new SiconosVector(_ndof));
_fGyr->zero();
computeFGyr();
}
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);
}
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
}
/*
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 DynamicalSystem::setX0Ptr(SP::SiconosVector newPtr)
{
// check dimensions ...
if (newPtr->size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setX0Ptr - inconsistent sizes between x0 input and n - Maybe you forget to set n?");
_x0 = newPtr;
_normRef = _x0->norm2() + 1;
}
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));
}
void DynamicalSystem::setRhsPtr(SP::SiconosVector newPtr)
{
// Warning: this only sets the pointer (*x)[1]
// check dimensions ...
if (newPtr->size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setRhsPtr - inconsistent sizes between x input and n - Maybe you forget to set n?");
_x[1] = newPtr;
}
void private_prod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::SiconosVector y, bool init)
{
assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");
// Computes y = subA *x (or += if init = false), subA being a sub-matrix of trans(A), between el. of A of index (col) startCol and startCol + sizeY
if (init) // y = subA * x , else y += subA * x
y->zero();
private_addprod(x, A, startCol, 0 , y);
}
void private_prod(double a, SPC::SiconosMatrix A, unsigned int startRow, SPC::SiconosVector x, SP::SiconosVector y, bool init)
{
assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");
// Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY
if (init) // y = subA * x , else y += subA * x
y->zero();
private_addprod(a, A, startRow, 0, x, y);
}
/* 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);
}
BulletDS::BulletDS(SP::BulletWeightedShape weightedShape,
SP::SiconosVector position,
SP::SiconosVector velocity,
SP::SiconosVector relative_position,
SP::SiconosVector relative_orientation,
int group) :
NewtonEulerDS(position, velocity, weightedShape->mass(),
weightedShape->inertia()),
_weightedShape(weightedShape),
_collisionObjects(new CollisionObjects())
{
SiconosVector& q = *_q;
/* with 32bits input ... 1e-7 */
if (fabs(sqrt(pow(q(3), 2) + pow(q(4), 2) +
pow(q(5), 2) + pow(q(6), 2)) - 1.) >= 1e-7)
{
RuntimeException::selfThrow(
"BulletDS: quaternion in position parameter is not a unit quaternion "
);
}
/* initialisation is done with the weighted shape as the only one
* collision object */
if (! relative_position)
{
relative_position.reset(new SiconosVector(3));
relative_position->zero();
}
if (! relative_orientation)
{
relative_orientation.reset(new SiconosVector(4));
relative_orientation->zero();
(*relative_orientation)(1) = 1;
}
addCollisionShape(weightedShape->collisionShape(), relative_position,
relative_orientation, group);
}
/*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 private_addprod(double a, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SPC::SiconosVector x, SP::SiconosVector y)
{
assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");
if (A->isBlock())
SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for block matrix.");
// we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
// Then computation of y = subA*x + y.
unsigned int numA = A->getNum();
unsigned int numY = y->getNum();
unsigned int numX = x->getNum();
unsigned int sizeX = x->size();
unsigned int sizeY = y->size();
if (numX != numY)
SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x and y of different types.");
if (numY == 1 && numX == 1)
{
assert(y->dense() != x->dense());
if (numA == 1)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else if (numA == 2)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else if (numA == 3)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else if (numA == 4)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else //if(numA==5)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
}
else // x and y sparse
{
if (numA == 4)
*y->sparse() += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->sparse());
else
SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x, y sparse and A not sparse.");
}
}
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 SimpleMatrix::normInfByColumn(SP::SiconosVector vIn) const
{
if (_num == 1)
{
if (vIn->size() != size(1))
RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: the given vector does not have the right length");
DenseVect tmpV = DenseVect(size(0));
for (unsigned int i = 0; i < size(1); i++)
{
ublas::noalias(tmpV) = ublas::column(*mat.Dense, i);
(*vIn)(i) = norm_inf(tmpV);
}
}
else
RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: not implemented for data other than DenseMat");
}
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 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(););
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;
}
// ================= 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);
}
}
// ================= 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);
}
}
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::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));
}
double SchatzmanPaoliOSI::computeResidu()
{
// This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
// It then computes the norm of each of them and finally return the maximum
// value for those norms.
//
// The state values used are those saved in the DS, ie the last computed ones.
// $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
// $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $
double t = simulationLink->nextTime(); // End of the time step
double told = simulationLink->startingTime(); // Beginning of the time step
double h = t - told; // time step length
// Operators computed at told have index i, and (i+1) at t.
// Iteration through the set of Dynamical Systems.
//
DSIterator it;
SP::DynamicalSystem ds; // Current Dynamical System.
Type::Siconos dsType ; // Type of the current DS.
double maxResidu = 0;
double normResidu = maxResidu;
for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
ds = *it; // the considered dynamical system
dsType = Type::value(*ds); // Its type
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);
// 1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
// // residu = M(q*)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) - pi+1
// // -- Convert the DS into a Lagrangian one.
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// SP::SiconosVector q =d->q();
// d->computeMass();
// SP::SiconosMatrix M = d->mass();
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// //residuFree->zero();
// // std::cout << "(*v-*vold)->norm2()" << (*v-*vold).norm2() << std::endl;
// prod(*M, (*v-*vold), *residuFree); // residuFree = M(v - vold)
// if (d->forces()) // if fL exists
// {
// // computes forces(ti,vi,qi)
// d->computeForces(told,qold,vold);
// double coef = -h*(1-_theta);
// // residuFree += coef * fL_i
// scal(coef, *d->forces(), *residuFree, false);
// // computes forces(ti+1, v_k,i+1, q_k,i+1) = forces(t,v,q)
// //d->computeForces(t);
// // or forces(ti+1, v_k,i+1, q(v_k,i+1))
// //or
// SP::SiconosVector qbasedonv(new SiconosVector(*qold));
// *qbasedonv += h*( (1-_theta)* *vold + _theta * *v );
// d->computeForces(t,qbasedonv,v);
// coef = -h*_theta;
// // residuFree += coef * fL_k,i+1
// scal(coef, *d->forces(), *residuFree, false);
// }
// if (d->boundaryConditions())
// {
// d->boundaryConditions()->computePrescribedVelocity(t);
// unsigned int columnindex=0;
// SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// double DeltaPrescribedVelocity =
// d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
// - vold->getValue(columnindex);
// WBoundaryConditions->getCol(columnindex,*columntmp);
// *residuFree -= *columntmp * (DeltaPrescribedVelocity);
// residuFree->setValue(*itindex, columntmp->getValue(*itindex) *(DeltaPrescribedVelocity));
// columnindex ++;
// }
// }
// *(d->workspace(DynamicalSystem::free))=*residuFree; // copy residuFree in Workfree
// // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residufree :" << std::endl;
// // residuFree->display();
// if (d->p(1))
// *(d->workspace(DynamicalSystem::free)) -= *d->p(1); // Compute Residu in Workfree Notation !!
// // std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianDS residu :" << std::endl;
// // d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// 2 - Lagrangian Linear Systems
else if (dsType == Type::LagrangianLinearTIDS)
{
// ResiduFree = M(-q_{k}+q_{k-1}) + h^2 (K q_k)+ h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k)) (1)
// This formulae is only valid for the first computation of the residual for q = q_k
// otherwise the complete formulae must be applied, that is
// ResiduFree M(q-2q_{k}+q_{k-1}) + h^2 (K(\theta q+ (1-\theta) q_k)))+ h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k)) (2)
// for q != q_k, the formulae (1) is wrong.
// in the sequel, only the equation (1) is implemented
// -- Convert the DS into a Lagrangian one.
SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
// Get state i (previous time step) from Memories -> var. indexed with "Old"
SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k
SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl;
// q_k_1->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl;
// q_k->display();
// std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl;
// v_k->display();
// --- ResiduFree computation Equation (1) ---
residuFree->zero();
double coeff;
// -- No need to update W --
//SP::SiconosVector v = d->velocity(); // v = v_k,i+1
SP::SiconosMatrix M = d->mass();
prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1})
SP::SiconosMatrix K = d->K();
if (K)
{
prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi
}
SP::SiconosMatrix C = d->C();
if (C)
prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k) , *residuFree, false);
// residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))
SP::SiconosVector Fext = d->fExt();
if (Fext)
{
// computes Fext(ti)
d->computeFExt(told);
coeff = -h * h * (1 - _theta);
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti)
// computes Fext(ti+1)
d->computeFExt(t);
coeff = -h * h * _theta;
scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1)
}
// if (d->boundaryConditions())
// {
// d->boundaryConditions()->computePrescribedVelocity(t);
// unsigned int columnindex=0;
// SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
// SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));
// for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ;
// itindex != d->boundaryConditions()->velocityIndices()->end();
// ++itindex)
// {
// double DeltaPrescribedVelocity =
// d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
// -vold->getValue(columnindex);
// WBoundaryConditions->getCol(columnindex,*columntmp);
// *residuFree += *columntmp * (DeltaPrescribedVelocity);
// residuFree->setValue(*itindex, - columntmp->getValue(*itindex) *(DeltaPrescribedVelocity));
// columnindex ++;
// }
// }
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :" << std::endl;
// residuFree->display();
(* d->workspace(DynamicalSystem::free)) = *residuFree; // copy residuFree in Workfree
if (d->p(0))
*(d->workspace(DynamicalSystem::free)) -= *d->p(0); // Compute Residu in Workfree Notation !!
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS p(0) :" << std::endl;
// if (d->p(0))
// d->p(0)->display();
// else
// std::cout << " p(0) :" << std::endl;
// std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residu :" << std::endl;
// d->workspace(DynamicalSystem::free)->display();
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
normResidu = 0.0; // we assume that v = vfree + W^(-1) p
// normResidu = realresiduFree->norm2();
}
else if (dsType == Type::NewtonEulerDS)
{
// // residu = M(q*)(v_k,i+1 - v_i) - h*_theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-_theta)*forces(ti,vi,qi) - pi+1
// // -- Convert the DS into a Lagrangian one.
// SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
// // Get state i (previous time step) from Memories -> var. indexed with "Old"
// SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
// SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// SP::SiconosVector q =d->q();
// SP::SiconosMatrix massMatrix = d->massMatrix();
// SP::SiconosVector v = d->velocity(); // v = v_k,i+1
// prod(*massMatrix, (*v-*vold), *residuFree); // residuFree = M(v - vold)
// if (d->forces()) // if fL exists
// {
// // computes forces(ti,vi,qi)
// SP::SiconosVector fLold=d->fLMemory()->getSiconosVector(0);
// double _thetaFL=0.5;
// double coef = -h*(1-_thetaFL);
// // residuFree += coef * fL_i
// scal(coef, *fLold, *residuFree, false);
// d->computeForces(t);
// // printf("cpmputeFreeState d->FL():\n");
// // d->forces()->display();
// coef = -h*_thetaFL;
// scal(coef, *d->forces(), *residuFree, false);
// }
// *(d->workspace(DynamicalSystem::free))=*residuFree;
// //cout<<"SchatzmanPaoliOSI::computeResidu :\n";
// // residuFree->display();
// if ( d->p(1) )
// *(d->workspace(DynamicalSystem::free)) -= *d->p(1);
// normResidu = d->workspace(DynamicalSystem::free)->norm2();
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
if (normResidu > maxResidu) maxResidu = normResidu;
}
return maxResidu;
}
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)));
}
}
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;
}
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);
}
