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; }