void ZOHTest::testMatrixIntegration1()
{
std::cout << "===========================================" <<std::endl;
std::cout << " ===== ZOH tests start ... ===== " <<std::endl;
std::cout << "===========================================" <<std::endl;
std::cout << "------- Integrate an oscillator -------" <<std::endl;
_A->zero();
(*_A)(0, 1) = 1;
(*_A)(1, 0) = -1;
_x0->zero();
(*_x0)(0) = 1;
init();
SimpleMatrix dataPlot((unsigned)ceil((_T - _t0) / _h) + 10, 3);
SiconosVector& xProc = *_DS->x();
unsigned int k = 0;
dataPlot(0, 0) = _t0;
dataPlot(0, 1) = (*_x0)(0);
dataPlot(0, 2) = (*_x0)(1);
while (_sim->hasNextEvent())
{
_sim->computeOneStep();
k++;
dataPlot(k, 0) = _sim->nextTime();
dataPlot(k, 1) = xProc(0);
dataPlot(k, 2) = xProc(1);
_sim->nextStep();
_sim->eventsManager()->display();
}
dataPlot.display();
std::cout <<std::endl <<std::endl;
dataPlot.resize(k, 3);
ioMatrix::write("testMatrixIntegration1.dat", "ascii", dataPlot, "noDim");
// Reference Matrix
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
//magic line to compute the following:
// python -c "import numpy as np; t = np.linspace(0, 9.9, 100); np.savetxt('testMatrixIntegration1.ref', np.transpose([t, np.cos(t), -np.sin(t)]))" && sed -i "1i100 3" testMatrixIntegration1.ref
ioMatrix::read("testMatrixIntegration1.ref", "ascii", dataPlotRef);
std::cout << "------- Integration Ok, error = " << (dataPlot - dataPlotRef).normInf() << " -------" <<std::endl;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp1 : ", (dataPlot - dataPlotRef).normInf() < _tol, true);
}
void OSNSPTest::testAVI()
{
std::cout << "===========================================" <<std::endl;
std::cout << " ===== OSNSP tests start ... ===== " <<std::endl;
std::cout << "===========================================" <<std::endl;
std::cout << "------- implicit Twisting relation -------" <<std::endl;
_h = 1e-1;
_T = 20.0;
double G = 10.0;
double beta = .3;
_A->zero();
(*_A)(0, 1) = 1.0;
_x0->zero();
(*_x0)(0) = 10.0;
(*_x0)(1) = 10.0;
SP::SimpleMatrix B(new SimpleMatrix(_n, _n, 0));
SP::SimpleMatrix C(new SimpleMatrix(_n, _n));
(*B)(1, 0) = G;
(*B)(1, 1) = G*beta;
C->eye();
SP::FirstOrderLinearTIR rel(new FirstOrderLinearTIR(C, B));
SP::SimpleMatrix H(new SimpleMatrix(4, 2));
(*H)(0, 0) = 1.0;
(*H)(1, 0) = -_h/2.0;
(*H)(2, 0) = -1.0;
(*H)(3, 0) = _h/2.0;
(*H)(1, 1) = 1.0;
(*H)(3, 1) = -1.0;
SP::SiconosVector K(new SiconosVector(4));
(*K)(0) = -1.0;
(*K)(1) = -1.0;
(*K)(2) = -1.0;
(*K)(3) = -1.0;
SP::NonSmoothLaw nslaw(new NormalConeNSL(_n, H, K));
_DS.reset(new FirstOrderLinearTIDS(_x0, _A, _b));
_TD.reset(new TimeDiscretisation(_t0, _h));
_model.reset(new Model(_t0, _T));
SP::Interaction inter(new Interaction(_n, nslaw, rel));
_osi.reset(new EulerMoreauOSI(_theta));
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(_DS);
_model->nonSmoothDynamicalSystem()->setOSI(_DS, _osi);
_model->nonSmoothDynamicalSystem()->link(inter, _DS);
_sim.reset(new TimeStepping(_TD));
_sim->insertIntegrator(_osi);
SP::AVI osnspb(new AVI());
_sim->insertNonSmoothProblem(osnspb);
_model->initialize(_sim);
SimpleMatrix dataPlot((unsigned)ceil((_T - _t0) / _h) + 10, 5);
SiconosVector& xProc = *_DS->x();
SiconosVector& lambda = *inter->lambda(0);
unsigned int k = 0;
dataPlot(0, 0) = _t0;
dataPlot(0, 1) = (*_x0)(0);
dataPlot(0, 2) = (*_x0)(1);
dataPlot(0, 3) = -1.0;
dataPlot(0, 4) = -1.0;
while (_sim->hasNextEvent())
{
_sim->computeOneStep();
k++;
dataPlot(k, 0) = _sim->nextTime();
dataPlot(k, 1) = xProc(0);
dataPlot(k, 2) = xProc(1);
dataPlot(k, 3) = lambda(0);
dataPlot(k, 4) = lambda(1);
_sim->nextStep();
}
std::cout <<std::endl <<std::endl;
dataPlot.resize(k, dataPlot.size(1));
ioMatrix::write("testAVI.dat", "ascii", dataPlot, "noDim");
// Reference Matrix
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("testAVI.ref", "ascii", dataPlotRef);
std::cout << "------- Integration Ok, error = " << (dataPlot - dataPlotRef).normInf() << " -------" <<std::endl;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testAVI : ", (dataPlot - dataPlotRef).normInf() < _tol, true);
}
void ZOHTest::testMatrixIntegration4()
{
std::cout << "===========================================" <<std::endl;
std::cout << " ===== ZOH tests start ... ===== " <<std::endl;
std::cout << "===========================================" <<std::endl;
std::cout << "------- Integrate Orlov's controller -------" <<std::endl;
_h = .001;
_A->zero();
(*_A)(0, 1) = 1;
_x0->zero();
(*_x0)(0) = 1;
(*_x0)(1) = 1;
SP::SimpleMatrix B(new SimpleMatrix(_n, _n, 0));
SP::SimpleMatrix C(new SimpleMatrix(_n, _n));
(*B)(1, 0) = 2;
(*B)(1, 1) = 1;
C->eye();
SP::FirstOrderLinearTIR rel(new FirstOrderLinearTIR(C, B));
SP::SimpleMatrix D(new SimpleMatrix(_n, _n, 0));
rel->setDPtr(D);
SP::NonSmoothLaw nslaw(new RelayNSL(_n));
_DS.reset(new FirstOrderLinearDS(_x0, _A, _b));
_TD.reset(new TimeDiscretisation(_t0, _h));
_model.reset(new Model(_t0, _T));
SP::Interaction inter(new Interaction(_n, nslaw, rel));
_ZOH.reset(new ZeroOrderHoldOSI());
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(_DS);
_model->nonSmoothDynamicalSystem()->topology()->setOSI(_DS, _ZOH);
_model->nonSmoothDynamicalSystem()->link(inter, _DS);
_model->nonSmoothDynamicalSystem()->setControlProperty(inter, true);
_sim.reset(new TimeStepping(_TD, 1));
_sim->insertIntegrator(_ZOH);
SP::Relay osnspb(new Relay());
_sim->insertNonSmoothProblem(osnspb);
_model->setSimulation(_sim);
_model->initialize();
SimpleMatrix dataPlot((unsigned)ceil((_T - _t0) / _h) + 10, 7);
SiconosVector& xProc = *_DS->x();
SiconosVector& lambda = *inter->lambda(0);
SiconosVector sampledControl(_n);
unsigned int k = 0;
dataPlot(0, 0) = _t0;
dataPlot(0, 1) = (*_x0)(0);
dataPlot(0, 2) = (*_x0)(1);
dataPlot(0, 3) = 0;
dataPlot(0, 4) = 0;
dataPlot(0, 5) = 0;
dataPlot(0, 6) = 0;
while (_sim->hasNextEvent())
{
_sim->computeOneStep();
prod(*B, lambda, sampledControl, true);
k++;
dataPlot(k, 0) = _sim->nextTime();
dataPlot(k, 1) = xProc(0);
dataPlot(k, 2) = xProc(1);
dataPlot(k, 3) = sampledControl(0);
dataPlot(k, 4) = sampledControl(1);
dataPlot(k, 5) = lambda(0);
dataPlot(k, 6) = lambda(1);
_sim->nextStep();
}
dataPlot.resize(k, 7);
dataPlot.display();
std::cout <<std::endl <<std::endl;
ioMatrix::write("testMatrixIntegration4.dat", "ascii", dataPlot, "noDim");
// Reference Matrix
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("testMatrixIntegration4.ref", "ascii", dataPlotRef);
std::cout << "------- Integration Ok, error = " << (dataPlot - dataPlotRef).normInf() << " -------" <<std::endl;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp4 : ", (dataPlot - dataPlotRef).normInf() < _tol, true);
}
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;
}
