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);
}
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);
}
// ================= 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);
}
}
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);
}
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;
}
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;
}
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 = _simulation->nextTime(); // End of the time step
double told = _simulation->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.
//
SP::DynamicalSystem ds; // Current Dynamical System.
Type::Siconos dsType ; // Type of the current DS.
double maxResidu = 0;
double normResidu = maxResidu;
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
dsType = Type::value(*ds); // Its type
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);
// 1 - Lagrangian Non Linear Systems
if (dsType == Type::LagrangianDS)
{
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)
}
// 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)
{
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 _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);
}
