//==========================================================================================================
// Main function
//==========================================================================================================
int main(int argc, char* argv[])
{
//---------------------------- calculate the computation time --------------------------------------------------
boost::timer time;
time.restart();
try
{
//===========================================================================================================
// I: Declare the dynamical systems
//===========================================================================================================
//1. Set the mass matrix
SP::SiconosMatrix Mass(new SimpleMatrix(Nfreedom, Nfreedom));
double InertiaBlock;
InertiaBlock = (MassBlock / 12.0) * ((HeightBlock*HeightBlock) + (LengthBlock*LengthBlock)); // moment of inertia
(*Mass)(0, 0) = MassBlock;
(*Mass)(1, 1) = MassBlock;
(*Mass)(2, 2) = InertiaBlock;
//2. Set the initial position of the block in function of the initial position of the contact point A (left-hand contact)
SP::SiconosVector PosIniBlock(new SiconosVector(Nfreedom));
(*PosIniBlock)(0) = PosXiniPointA + 0.5 * LengthBlock * cos(AngleThetaIni) - 0.5 * HeightBlock * sin(AngleThetaIni);
(*PosIniBlock)(1) = PosYiniPointA + 0.5 * LengthBlock * sin(AngleThetaIni) + 0.5 * HeightBlock * cos(AngleThetaIni);
(*PosIniBlock)(2) = AngleThetaIni;
cout.precision(15);
cout << "x0: " << (*PosIniBlock)(0) << endl;
cout << "y0: " << (*PosIniBlock)(1) << endl;
cout << "theta0: " << (*PosIniBlock)(2) << endl;
cout.precision(15);
cout << "PI: " << PI << endl;
// (*PosIniBlock)(0) = 0.5;
// (*PosIniBlock)(1) = 0.5;
// (*PosIniBlock)(2) = 0.0;
//3. Set the initial velocity of the block in function of the initial relative velocity of the contact point A
SP::SiconosVector VelIniBlock(new SiconosVector(Nfreedom));
(*VelIniBlock)(0) = VelXiniPointA - (0.5 * LengthBlock * sin(AngleThetaIni) + 0.5 * HeightBlock * cos(AngleThetaIni)) * RotVelBlockIni;
(*VelIniBlock)(1) = VelYiniPointA + (0.5 * LengthBlock * cos(AngleThetaIni) - 0.5 * HeightBlock * sin(AngleThetaIni)) * RotVelBlockIni;
(*VelIniBlock)(2) = RotVelBlockIni;
// (*VelIniBlock)(0) = 0.0;
// (*VelIniBlock)(1) = 0.0;
// (*VelIniBlock)(2) = 0.0;
//4. Instantiate the object of "LagrangianTIDS"
SP::LagrangianLinearTIDS RockingBlock(new LagrangianLinearTIDS(PosIniBlock, VelIniBlock, Mass));
//5. Set the external force
SP::SiconosVector ForceExtern(new SiconosVector(Nfreedom));
(*ForceExtern)(1) = -MassBlock * GGearth;
RockingBlock->setFExtPtr(ForceExtern);
//
//----------------------------- Display variables of the dynamical system---------------------------------------
cout << "Initial position of the rocking block:" << endl;
PosIniBlock->display();
cout << "Initial velocity of the rocking block:" << endl;
VelIniBlock->display();
cout << "Mass matrix of the rocking block:" << endl;
Mass->display();
cout << "External force applied on the rocking block:" << endl;
ForceExtern->display();
//==================================================================================================================
// II: Declare the relation et interaction between dynamical systems
//==================================================================================================================
//
/*
SP::SiconosMatrix H(new SimpleMatrix(1,Nfreedom));
(*H)(0,1) = 1.0;
SP::SiconosVector E(new SiconosVector(1));
(*E)(0) = -0.5*HeightBlock;
*/
// Impact law
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
// Interaction at contact point 1
//SP::Relation relation1(new LagrangianLinearTIR(H, E));
SP::Relation relation1(new LagrangianScleronomousR("RockingBlockPlugin:h1", "RockingBlockPlugin:G1", "RockingBlockPlugin:G1dot"));
SP::Interaction inter1(new Interaction(1, nslaw, relation1));
// Interaction at contact point 2
//SP::Relation relation2(new LagrangianLinearTIR(H, E));
SP::Relation relation2(new LagrangianScleronomousR("RockingBlockPlugin:h2", "RockingBlockPlugin:G2", "RockingBlockPlugin:G2dot"));
SP::Interaction inter2(new Interaction(1, nslaw, relation2));
// Interactions for the whole dynamical system
//================================================================================================================
// III. Create the "model" object
//================================================================================================================
SP::Model RoBlockModel(new Model(TimeInitial, TimeFinal));
RoBlockModel->nonSmoothDynamicalSystem()->insertDynamicalSystem(RockingBlock);
RoBlockModel->nonSmoothDynamicalSystem()->link(inter1, RockingBlock);
RoBlockModel->nonSmoothDynamicalSystem()->link(inter2, RockingBlock);
//================================================================================================================
// IV. Create the simulation
//================================================================================================================
//1. Time discretization
SP::TimeDiscretisation TimeDiscret(new TimeDiscretisation(TimeInitial, StepSize));
//2. Integration solver for one step
SP::OneStepIntegrator OSI(new NewMarkAlphaOSI(_rho, IsHandleVelConstraint));
SP::NewMarkAlphaOSI _NewMarkAlpha = std11::static_pointer_cast<NewMarkAlphaOSI>(OSI);
//3. Nonsmooth problem
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem position(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
position->numericsSolverOptions()->dparam[0] = 1e-12;
impact->numericsSolverOptions()->dparam[0] = 1e-12;
acceleration->numericsSolverOptions()->dparam[0] = 1e-12;
//4. Simulation with (1), (2), (3)
SP::Simulation EDscheme(new EventDriven(TimeDiscret));
EDscheme->insertIntegrator(OSI);
EDscheme->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
EDscheme->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
EDscheme->insertNonSmoothProblem(position, SICONOS_OSNSP_ED_SMOOTH_POS);
RoBlockModel->setSimulation(EDscheme); // initialize the model
// bool check1 = EDscheme->hasOneStepNSProblem(impact);
// bool check2 = EDscheme->hasOneStepNSProblem(acceleration);
// cout << "Impact law included in the simulation: " << check1 << endl;
// cout << "LCP at acceleration level included in the simulation: " << check2 << endl;
//==================================================================================================================
// V. Process the simulation
//==================================================================================================================
// -------------------------------- Simulation initialization ------------------------------------------------------
cout << "====> Simulation initialisation ..." << endl << endl;
RoBlockModel->initialize(); // initialize the model
EDscheme->setPrintStat(true);
SP::EventsManager eventsManager = EDscheme->eventsManager(); // ponters point to the "eventsManager" object
SP::SiconosVector PosBlock = RockingBlock->q(); // pointer points to the position vector of the rocking block
SP::SiconosVector VelBlock = RockingBlock->velocity(); // pointer points to the velocity of the rocking block
SP::SiconosVector AcceBlock = RockingBlock->acceleration(); // pointer points to the velocity of the rocking block
SP::SiconosVector GapCon1 = inter1->y(0);
SP::SiconosVector GapCon2 = inter2->y(0);
SP::SiconosVector VelCon1 = inter1->y(1);
SP::SiconosVector VelCon2 = inter2->y(1);
SP::SiconosVector LambdaCon1 = inter1->lambda(2);
SP::SiconosVector LambdaCon2 = inter2->lambda(2);
SP::InteractionsGraph indexSet0 = RoBlockModel->nonSmoothDynamicalSystem()->topology()->indexSet(0);
SP::InteractionsGraph indexSet1 = RoBlockModel->nonSmoothDynamicalSystem()->topology()->indexSet(1);
SP::InteractionsGraph indexSet2 = RoBlockModel->nonSmoothDynamicalSystem()->topology()->indexSet(2);
cout << "Size of IndexSet0: " << indexSet0->size() << endl;
cout << "Size of IndexSet1: " << indexSet1->size() << endl;
cout << "Size of IndexSet2: " << indexSet2->size() << endl;
InteractionsGraph::VIterator ui, uiend;
//-------------------- Save the output during simulation ---------------------------------------------------------
SimpleMatrix DataPlot(NpointSave, SizeOutput);
//------------- At the initial time -----------------------------------------------------------------------------
DataPlot(0, 0) = RoBlockModel->t0();
DataPlot(0, 1) = (*PosBlock)(0); // Position X
DataPlot(0, 2) = (*PosBlock)(1); // Position Y
DataPlot(0, 3) = (*PosBlock)(2); // Angle theta
DataPlot(0, 4) = (*VelBlock)(0); // Velocity Vx
DataPlot(0, 5) = (*VelBlock)(1); // Velocity Vy
DataPlot(0, 6) = (*VelBlock)(2); // Angular velocity
DataPlot(0, 7) = (*GapCon1)(0); // Gap at first contact
DataPlot(0, 8) = (*GapCon2)(0); // Gap at second contact
DataPlot(0, 9) = (*VelCon1)(0); // Relative velocity at first contact
DataPlot(0, 10) = (*VelCon2)(0); // Relative velocity at second contact
DataPlot(0, 11) = (*LambdaCon1)(0); // Force at first contact
DataPlot(0, 12) = (*LambdaCon2)(0); // Force at second contact
//----------------------------------- Simulation starts ----------------------------------------------------------
cout << "====> Start computation ... " << endl << endl;
bool NSEvent = false;
unsigned int NumberNSEvent = 0;
double alpha_m, alpha_f, beta, gamma;
unsigned int k = 1;
boost::progress_display show_progress(NpointSave);
while (EDscheme->hasNextEvent() && (k < NpointSave))
{
if (IsTreatFirstSteps)
{
if (k == 1) // first step
{
alpha_m = 0.0;
alpha_f = 0.0;
gamma = 1.0/2.0 + 1.0/10.0;
beta = 1.0/4.0 * (gamma + 1.0/2.0) *(gamma + 1.0/2.0) ;
_NewMarkAlpha->setAlpha_m(alpha_m);
_NewMarkAlpha->setAlpha_f(alpha_f);
_NewMarkAlpha->setBeta(beta);
_NewMarkAlpha->setGamma(gamma);
}
else if (k == 2)
{
alpha_m = 0.0;
alpha_f = -1.0 / 3.0; // -1/3 <= alpha_f <= 0
gamma = 1.0/2.0 - alpha_f;
beta = 1.0/4.0 * (1.0 - alpha_f)*(1.0 - alpha_f);
_NewMarkAlpha->setAlpha_m(alpha_m);
_NewMarkAlpha->setAlpha_f(alpha_f);
_NewMarkAlpha->setBeta(beta);
_NewMarkAlpha->setGamma(gamma);
}
else
{
_NewMarkAlpha->setParametersFromRho_infty(_rho);
}
}
EDscheme->advanceToEvent(); // lead the simulation run from one event to the next
//---------- detect the statue of the current event ------------------------------------
if (eventsManager->nextEvent()->getType() == 2) // the current event is non-smooth
{
NSEvent = true;
};
EDscheme->processEvents(); // process the current event
//------------------- get data at the beginning of non-smooth events ---------------------------
if (NSEvent)
{
DataPlot(k, 0) = EDscheme->startingTime(); // instant at non-smooth event
DataPlot(k, 1) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(0); // Position X
DataPlot(k, 2) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(1); // Position Y
DataPlot(k, 3) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(2); // Angle theta
DataPlot(k, 4) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(0); // Velocity Vx
DataPlot(k, 5) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(1); // Velocity Vy
DataPlot(k, 6) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(2); // Angular velocity
//EDscheme->update(1);
k++;
++NumberNSEvent;
++show_progress;
NSEvent = false; // The next event is maybe smooth
};
//-------------------- get data at smooth events or at the end of non-smooth events ---------------
DataPlot(k, 0) = EDscheme->startingTime();
DataPlot(k, 1) = (*PosBlock)(0); //Position X
DataPlot(k, 2) = (*PosBlock)(1); //Position Y
DataPlot(k, 3) = (*PosBlock)(2); // Position theta
DataPlot(k, 4) = (*VelBlock)(0); // Velocity Vx
DataPlot(k, 5) = (*VelBlock)(1); // Velocity Vy
DataPlot(k, 6) = (*VelBlock)(2); // Velocity Vtheta
DataPlot(k, 7) = (*GapCon1)(0); // Gap at first contact
DataPlot(k, 8) = (*GapCon2)(0); // Gap at second contact
DataPlot(k, 9) = (*VelCon1)(0); // Relative velocity at first contact
DataPlot(k, 10) = (*VelCon2)(0); // Relative velocity at second contact
DataPlot(k, 11) = (*LambdaCon1)(0); // Force at first contact
DataPlot(k, 12) = (*LambdaCon2)(0); // Force at second contact
// go to the next time step
k++;
++show_progress;
// // Display information
// cout << "********At the end of integation step***************"<< (k - 1) << endl;
// cout << "Information on Dynamical System" << endl;
// cout << "Position: ";
// PosBlock->display();
// cout << "Velocity: ";
// VelBlock->display();
// cout << "Acceleration: ";
// AcceBlock->display();
// cout << "Information on contacts" << endl;
// for(std11::tie(ui,uiend) = indexSet0->vertices(); ui!=uiend; ++ui)
// {
// SP::Interaction inter = indexSet0->bundle(*ui);
// cout << "Contact number: " << inter->number() << endl;
// cout << "Contact gap: ";
// inter->y(0)->display();
// cout << "Contact relative velocity: ";
// inter->y(1)->display();
// cout << "Contact Force: " << endl;
// inter->lambda(2)->display();
// }
};
//----------------------- At the end of the simulation --------------------------
cout << "End of the simulation" << endl;
cout << "Number of events processed during simulation: " << (k + 1) << endl;
cout << "Number of non-smooth events: " << NumberNSEvent << endl;
cout << "====> Output file writing ..." << endl << endl;
ioMatrix::write("result.dat", "ascii", DataPlot, "noDim");
// Comparison with a reference file
std::cout << "Comparison with a reference file" << std::endl;
SimpleMatrix dataPlotRef(DataPlot);
dataPlotRef.zero();
ioMatrix::read("resultED_NewMarkAlpha.ref", "ascii", dataPlotRef);
double error = (DataPlot - dataPlotRef).normInf()/ dataPlotRef.normInf();
std::cout << "Error = "<< error << std::endl;
std::cout << "Error by column = "<< std::endl;
SP::SiconosVector errCol(new SiconosVector(SizeOutput));
(DataPlot - dataPlotRef).normInfByColumn(errCol);
errCol->display();
if (error > 1e-06)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
std::cout << (DataPlot - dataPlotRef).normInf()/ dataPlotRef.normInf() << std::endl;
return 1;
}
}
//============================== Catch exceptions ===================================================================
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught." << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}
int main(int argc, char* argv[])
{
boost::timer time;
time.restart();
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDofBall = 1; // degrees of freedom of ball 1
double Height = 0.05; // Distance between impactor balls and monodisperse balls
// Balls in the tapered chain
unsigned int NumberBalls = 10; // Number
double R_base_taper = 0.005; // Base radii of the tapered chain
double q_taper = 0.0; // Tapering factor
// Material properties of balls
double mass_density = 7780; // mass density
double Res_BallBall = 1.0; // Restitution coefficient at contacts ball-ball
double Res_BallWall = 1.0; // Restitution coefficient at contacts ball-wall
double PowCompLaw = 1.5; // Power of the compliance law: 1.0 for linear contact and 3/2 for the Hertzian contact
string TypeContactLaw = "BiStiffness"; // Type of compliance contact law
// Parameters for the global simulation
double t0 = 0; // initial computation time
double T = 0.5; // final computation time
double h = 0.001; // time step
unsigned int Npointsave = 500; // Number of data points to be saved
//---------------------------------------
// ---- Configuration of chaines
//--------------------------------------
//************* Balls ******************
double NumberContacts = NumberBalls ; // Number of contacts
//(1) Radius of balls
SP::SiconosVector RadiusBalls(new SiconosVector(NumberBalls));
for (unsigned int k = 0; k < NumberBalls; ++k)
{
(*RadiusBalls)(k) = (pow(double(1.0 - q_taper), int(k + 1))) * R_base_taper;
}
// (2) Mass of balls
SP::SiconosVector MassBalls(new SiconosVector(NumberBalls));
for (unsigned int id = 0; id < NumberBalls; ++id)
{
(*MassBalls)(id) = (4.0 / 3.0) * PI * pow((*RadiusBalls)(id), 3) * mass_density;
}
// (3) Initial position of balls
// For the impactor balls
SP::SiconosVector InitPosBalls(new SiconosVector(NumberBalls));
(*InitPosBalls)(0) = Height + (*RadiusBalls)(0);
for (unsigned int j = 1; j < NumberBalls; ++j)
{
(*InitPosBalls)(j) = (*InitPosBalls)(j - 1) + (*RadiusBalls)(j - 1) + (*RadiusBalls)(j);
}
// (4) Initial velocity of balls
SP::SiconosVector InitVelBalls(new SiconosVector(NumberBalls));
for (unsigned int i = 0; i < NumberBalls; ++i)
{
(*InitVelBalls)(i) = 0.0;
}
//****************** Contacts ******************
// (1) Restitution coefficient at contacts
SP::SiconosVector ResCofContacts(new SiconosVector(NumberContacts));
SP::SiconosVector ElasCofContacts(new SiconosVector(NumberContacts));
for (unsigned int id = 0; id < NumberContacts; ++id)
{
if (id == 0) // contact ball-wall
{
(*ResCofContacts)(id) = Res_BallWall;
}
else // contact ball-ball
{
(*ResCofContacts)(id) = Res_BallBall;
}
}
// // Display and save the configuration of the chain simulated
// cout << "Configuation of ball chains" << endl;
// cout.precision(15);
// cout << "Radius of balls: " << endl;
// RadiusBalls->display();
// cout << "Mass of balls: " << endl;
// MassBalls->display();
// cout << "Initial position of balls: " << endl;
// InitPosBalls->display();
// cout << "Initial velocity of balls: " << endl;
// InitVelBalls->display();
// cout << "Restitution coefficient at contacts:" << endl;
// ResCofContacts->display();
// -------------------------
// --- Dynamical systems ---
// -------------------------
cout << "====> Model loading ..." << endl << endl;
std::vector<SP::DynamicalSystem> VecOfallDS;
SP::SiconosMatrix MassBall;
SP::SiconosVector q0Ball;
SP::SiconosVector v0Ball;
SP::LagrangianLinearTIDS ball;
SP::SiconosVector FextBall;
double _Rball, _massBall, _Pos0Ball, _Vel0Ball ;
// -------------
// --- Model ---
// -------------
SP::Model BallChain(new Model(t0, T));
// ----------------
// --- Simulation ---
// ----------------
// -- (1) OneStepIntegrators --
SP::OneStepIntegrator OSI(new LsodarOSI());
for (unsigned int i = 0; i < NumberBalls; ++i)
{
_Rball = (*RadiusBalls)(i); // radius of the ball
_massBall = (*MassBalls)(i); // mass of the ball
_Pos0Ball = (*InitPosBalls)(i); // initial position of the ball
_Vel0Ball = (*InitVelBalls)(i); // initial velocity of the ball
// Declaration of the DS in Siconos
MassBall = SP::SiconosMatrix(new SimpleMatrix(nDofBall, nDofBall));
(*MassBall)(0, 0) = _massBall;
// -- Initial positions and velocities --
q0Ball = SP::SiconosVector(new SiconosVector(nDofBall));
v0Ball = SP::SiconosVector(new SiconosVector(nDofBall));
(*q0Ball)(0) = _Pos0Ball;
(*v0Ball)(0) = _Vel0Ball;
// -- The dynamical system --
ball = SP::LagrangianLinearTIDS(new LagrangianLinearTIDS(q0Ball, v0Ball, MassBall));
// -- Set external forces (weight1) --
FextBall = SP::SiconosVector(new SiconosVector(nDofBall));
(*FextBall)(0) = -_massBall * g;
ball->setFExtPtr(FextBall);
//
VecOfallDS.push_back(ball);
BallChain->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);
}
// --------------------
// --- Interactions ---
// --------------------
SP::SimpleMatrix H;
SP::SiconosVector E;
SP::NonSmoothLaw nslaw;
SP::Relation relation;
SP::Interaction interaction;
double ResCoef, Stiff, ElasPow;
for (unsigned int j = 0; j < NumberContacts; ++j)
{
ResCoef = (*ResCofContacts)(j) ;
if (j == 0) // for contact wall-ball
{
H.reset(new SimpleMatrix(1, nDofBall));
(*H)(0, 0) = 1.0;
E = SP::SiconosVector(new SiconosVector(1));
(*E)(0) = -(*RadiusBalls)(j);
}
else // For ball-ball contact
{
H.reset(new SimpleMatrix(1, (nDofBall + nDofBall)));
(*H)(0, 0) = -1.0;
(*H)(0, 1) = 1.0;
E = SP::SiconosVector(new SiconosVector(1));
(*E)(0) = -1.0 * ((*RadiusBalls)(j - 1) + (*RadiusBalls)(j));
}
//
nslaw = SP::NonSmoothLaw(new NewtonImpactNSL(ResCoef));
relation = SP::Relation(new LagrangianLinearTIR(H, E));
interaction = SP::Interaction(new Interaction(1, nslaw, relation));
if (j == 0) // for contact wall-ball
BallChain->nonSmoothDynamicalSystem()->link(interaction, VecOfallDS[j]);
else // For ball-ball contact
BallChain->nonSmoothDynamicalSystem()->link(interaction, VecOfallDS[j-1],VecOfallDS[j]);
}
// -- (2) Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
// -- (3) Non smooth problem --
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
// -- (4) Simulation setup with (1) (2) (3)
SP::EventDriven s(new EventDriven(t));
s->insertIntegrator(OSI);
s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
BallChain->setSimulation(s);
// =========================== End of model definition ===========================
//----------------------------------- Initialization-------------------------------
s->setPrintStat(true);
BallChain->initialize();
SP::DynamicalSystemsGraph DSG0 = BallChain->nonSmoothDynamicalSystem()->topology()->dSG(0);
SP::InteractionsGraph IndexSet0 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(0);
SP::InteractionsGraph IndexSet1 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(1);
SP::InteractionsGraph IndexSet2 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(2);
// // Display topology of the system
// cout << "Number of vectices of IndexSet0: " << IndexSet0->size() << endl;
// cout << "Number of vectices of IndexSet1: " << IndexSet1->size() << endl;
// cout << "Number of vectices of IndexSet2: " << IndexSet2->size() << endl;
// cout << "Number of vectices of DSG0: " << DSG0->size() << endl;
//
SP::EventsManager eventsManager = s->eventsManager();
boost::progress_display show_progress(Npointsave);
// ================================= Computation =================================
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 2 * NumberBalls + 1;
SimpleMatrix dataPlot(Npointsave, outputSize);
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
// ==== Simulation loop - Writing without explicit event handling =====
bool nonSmooth = false;
unsigned int NumberOfEvents = 0;
unsigned int NumberOfNSEvents = 0;
unsigned int k = 0;
DynamicalSystemsGraph::VIterator ui, uiend;
//====================================================================
while ((k < Npointsave) & (s->hasNextEvent()))
{
dataPlot(k, 0) = s->startingTime();
// Save state of the balls
unsigned int col_pos = 1;
unsigned int col_vel = NumberBalls + 1;
for (boost::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
{
SP::DynamicalSystem ds = DSG0->bundle(*ui);
SP::LagrangianDS lag_ds = std11::dynamic_pointer_cast<LagrangianDS>(ds);
SP::SiconosVector q = lag_ds->q();
SP::SiconosVector v = lag_ds->velocity();
dataPlot(k, col_pos) = (*q)(0);
dataPlot(k, col_vel) = (*v)(0);
col_pos++;
col_vel++;
}
++k;
s->advanceToEvent(); // run simulation from one event to the next
if (eventsManager->nextEvent()->getType() == 2)
{
nonSmooth = true;
};
//
s->processEvents(); // process events
if (nonSmooth)
{
dataPlot(k, 0) = s->startingTime();
// Save state of the balls
unsigned int col_pos = 1;
unsigned int col_vel = NumberBalls + 1;
for (boost::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
{
SP::DynamicalSystem ds = DSG0->bundle(*ui);
SP::LagrangianDS lag_ds = std11::dynamic_pointer_cast<LagrangianDS>(ds);
SP::SiconosVector q = lag_ds->qMemory()->getSiconosVector(1);
SP::SiconosVector v = lag_ds->velocityMemory()->getSiconosVector(1);
dataPlot(k, col_pos) = (*q)(0);
dataPlot(k, col_vel) = (*v)(0);
col_pos++;
col_vel++;
}
nonSmooth = false;
++NumberOfNSEvents;
++NumberOfEvents;
++show_progress;
++k;
}
// --- Get values to be plotted ---
++NumberOfEvents;
++show_progress;
}
cout << "Computation Time " << time.elapsed() << endl;
// --- Output files ---
cout << "====> Output file writing ..." << endl;
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("resultED_NewtonModel.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught." << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}
int main(int argc, char* argv[])
{
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDof = 3; // degrees of freedom for robot arm
double t0 = 0; // initial computation time
double T = 1.9; // final computation time
double h = 0.005; // time step
double e = 0.9; // restit. coef. for impact on the ground.
double e2 = 0.0; // restit. coef for angular stops impacts.
// -> mind to set the initial conditions below.
// -------------------------
// --- Dynamical systems ---
// -------------------------
// unsigned int i;
// --- DS: robot arm ---
// The dof are angles between ground and arm and between differents parts of the arm. (See Robot.fig for more details)
//
// Initial position (angles in radian)
SiconosVector q0(nDof), v0(nDof);
q0(0) = 0.05;
q0(1) = 0.05;
SP::LagrangianDS arm(new LagrangianDS(q0, v0));
// external plug-in
arm->setComputeMassFunction("RobotPlugin", "mass");
arm->setComputeFGyrFunction("RobotPlugin", "FGyr");
arm->setComputeJacobianFGyrFunction(1, "RobotPlugin", "jacobianVFGyr");
arm->setComputeJacobianFGyrFunction(0, "RobotPlugin", "jacobianFGyrq");
// -------------------
// --- Interactions---
// -------------------
// Two interactions:
// - one with Lagrangian non linear relation to define contact with ground
// - the other to define angles limitations (articular stops), with lagrangian linear relation
// Both with newton impact ns laws.
// -- relations --
// => arm-floor relation
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
string G = "RobotPlugin:G2";
SP::Relation relation(new LagrangianScleronomousR("RobotPlugin:h2", G));
SP::Interaction inter(new Interaction(2, nslaw, relation));
// => angular stops
// SimpleMatrix H(6,3);
// SiconosVector b(6);
// H.zero();
// H(0,0) =-1;
// H(1,0) =1;
// H(2,1) =-1;
// H(3,1) =1;
// H(4,2) =-1;
// H(5,2) =1;
// b(0) = 1.7;
// b(1) = 1.7;
// b(2) = 0.3;
// b(3) = 0.3;
// b(4) = 3.14;
// b(5) = 3.14;
double lim0 = 1.6;
double lim1 = 3.1; // -lim_i <= q[i] <= lim_i
SimpleMatrix H(4, 3);
SiconosVector b(4);
H.zero();
H(0, 0) = -1;
H(1, 0) = 1;
H(2, 1) = -1;
H(3, 1) = 1;
b(0) = lim0;
b(1) = lim0;
b(2) = lim1;
b(3) = lim1;
SP::NonSmoothLaw nslaw2(new NewtonImpactNSL(e2));
SP::Relation relation2(new LagrangianLinearTIR(H, b));
SP::Interaction inter2(new Interaction(4, nslaw2, relation2));
// -------------
// --- Model ---
// -------------
SP::Model Robot(new Model(t0, T));
Robot->nonSmoothDynamicalSystem()->insertDynamicalSystem(arm);
Robot->nonSmoothDynamicalSystem()->link(inter1, arm);
Robot->nonSmoothDynamicalSystem()->link(inter2, arm);
// ----------------
// --- Simulation ---
// ----------------
// -- Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
SP::EventDriven s(new EventDriven(t));
// -- OneStepIntegrators --
SP::LsodarOSI OSI(new LsodarOSI(arm));
s->insertIntegrator(OSI);
// -- OneStepNsProblem --
IntParameters iparam(5);
iparam[0] = 20001; // Max number of iteration
DoubleParameters dparam(5);
dparam[0] = 0.005; // Tolerance
string solverName = "PGS" ;
SP::NonSmoothSolver mySolver(new NonSmoothSolver(solverName, iparam, dparam));
SP::OneStepNSProblem impact(new LCP(mySolver));
SP::OneStepNSProblem acceleration(new LCP(mySolver));
s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_ACCELERATION);
Robot->setSimulation(s);
cout << "=== End of model loading === " << endl;
// =========================== End of model definition =========================== dataPlot(k,7) = (*inter->y(0))(0);
// ================================= Computation =================================
// --- Simulation initialization ---
Robot->initialize();
cout << "End of model initialisation" << endl;
int k = 0;
int N = 10630;
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 11;
SimpleMatrix dataPlot(N + 1, outputSize);
// For the initial time step:
// time
SP::SiconosVector q = arm->q();
SP::SiconosVector vel = arm->velocity();
SP::SiconosVector y = inter->y(0);
SP::SiconosVector yDot = inter->y(1);
// When a non-smooth event occurs, pre-impact values are saved in memory vectors at pos. 1:
SP::SiconosVector qMem = arm->getQMemoryPtr()->getSiconosVector(1);
SP::SiconosVector velMem = arm->getVelocityMemoryPtr()->getSiconosVector(1);
SP::SiconosVector yMem = inter->getYOldPtr(0);
SP::SiconosVector yDotMem = inter->getYOldPtr(1);
dataPlot(k, 0) = Robot->t0();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*vel)(0);
dataPlot(k, 3) = (*q)(1);
dataPlot(k, 4) = (*vel)(1);
dataPlot(k, 5) = (*q)(2);
dataPlot(k, 6) = (*vel)(2);
dataPlot(k, 7) = (*y)(0);
dataPlot(k, 8) = (*y)(1);
dataPlot(k, 9) = (*yDot)(0);
dataPlot(k, 10) = (*yDot)(1);
// --- Time loop ---
cout << "Start computation ... " << endl;
boost::timer boostTimer;
boostTimer.restart();
unsigned int numberOfEvent = 0 ;
SP::EventsManager eventsManager = s->eventsManager();
bool nonSmooth = false;
while (s->hasNextEvent())
{
// get current time step
k++;
s->advanceToEvent();
if (eventsManager->nextEvent()->getType() == 2)
nonSmooth = true;
s->processEvents();
// If the treated event is non smooth, we get the pre-impact state.
if (nonSmooth)
{
dataPlot(k, 0) = s->startingTime();
dataPlot(k, 1) = (*qMem)(0);
dataPlot(k, 2) = (*velMem)(0);
dataPlot(k, 3) = (*qMem)(1);
dataPlot(k, 4) = (*velMem)(1);
dataPlot(k, 5) = (*qMem)(2);
dataPlot(k, 6) = (*velMem)(2);
dataPlot(k, 7) = (*yMem)(0);
dataPlot(k, 8) = (*yMem)(1);
dataPlot(k, 9) = (*yDotMem)(0);
dataPlot(k, 10) = (*yDotMem)(1);
k++;
}
dataPlot(k, 0) = s->startingTime();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*vel)(0);
dataPlot(k, 3) = (*q)(1);
dataPlot(k, 4) = (*vel)(1);
dataPlot(k, 5) = (*q)(2);
dataPlot(k, 6) = (*vel)(2);
dataPlot(k, 7) = (*y)(0);
dataPlot(k, 8) = (*y)(1);
dataPlot(k, 9) = (*yDot)(0);
dataPlot(k, 10) = (*yDot)(1);
numberOfEvent++;
// cout << k << endl;
// if (k==N) break;
}
cout << "===== End of Event Driven simulation. " << numberOfEvent << " events have been processed. ==== " << endl << endl;
cout << "Computation Time: " << boostTimer.elapsed() << endl;
cout << endl << "Output writing ..." << endl;
// --- Output files ---
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught in \'sample/MultiBeadsColumn\'" << endl;
}
}
//==========================================================================================================
// Main function
//==========================================================================================================
int main(int argc, char* argv[])
{
//---------------------------- calculate the computation time --------------------------------------------------
boost::timer time;
time.restart();
try
{
//===========================================================================================================
// I: Declare the dynamical systems
//===========================================================================================================
//1. Set the mass matrix
SP::SiconosMatrix Mass(new SimpleMatrix(Nfreedom, Nfreedom));
double InertiaBlock;
InertiaBlock = (MassBlock / 12.0) * (pow(HeightBlock, 2) + pow(LengthBlock, 2)); // moment of inertia
(*Mass)(0, 0) = MassBlock;
(*Mass)(1, 1) = MassBlock;
(*Mass)(2, 2) = InertiaBlock;
//2. Set the initial position of the block in function of the initial position of the contact point A (left-hand contact)
SP::SiconosVector PosIniBlock(new SiconosVector(Nfreedom));
/*
(*PosIniBlock)(0) = PosXiniPointA + 0.5*LengthBlock*cos(AngleThetaIni) - 0.5*HeightBlock*sin(AngleThetaIni);
(*PosIniBlock)(1) = PosYiniPointA + 0.5*LengthBlock*sin(AngleThetaIni) + 0.5*HeightBlock*cos(AngleThetaIni);
(*PosIniBlock)(2) = AngleThetaIni;
*/
(*PosIniBlock)(0) = 0.5;
(*PosIniBlock)(1) = 0.5;
(*PosIniBlock)(2) = 0.0;
//3. Set the initial velocity of the block in function of the initial relative velocity of the contact point A
SP::SiconosVector VelIniBlock(new SiconosVector(Nfreedom));
/*
(*VelIniBlock)(0) = VelXiniPointA - (0.5*LengthBlock*sin(AngleThetaIni) + 0.5*HeightBlock*cos(AngleThetaIni))*RotVelBlockIni;
(*VelIniBlock)(1) = VelYiniPointA + (0.5*LengthBlock*cos(AngleThetaIni) - 0.5*HeightBlock*sin(AngleThetaIni))*RotVelBlockIni;
(*VelIniBlock)(2) = RotVelBlockIni;
*/
(*VelIniBlock)(0) = 0.0;
(*VelIniBlock)(1) = 0.0;
(*VelIniBlock)(2) = 0.0;
//4. Instantiate the object of "LagrangianTIDS"
SP::LagrangianLinearTIDS RockingBlock(new LagrangianLinearTIDS(PosIniBlock, VelIniBlock, Mass));
//5. Set the external force
SP::SiconosVector ForceExtern(new SiconosVector(Nfreedom));
(*ForceExtern)(1) = -MassBlock * GGearth;
RockingBlock->setFExtPtr(ForceExtern);
//
//----------------------------- Display variables of the dynamical system---------------------------------------
cout << "Initial position of the rocking block:" << endl;
PosIniBlock->display();
cout << "Initial velocity of the rocking block:" << endl;
VelIniBlock->display();
cout << "Mass matrix of the rocking block:" << endl;
Mass->display();
cout << "External force applied on the rocking block:" << endl;
ForceExtern->display();
//==================================================================================================================
// II: Declare the relation et interaction between dynamical systems
//==================================================================================================================
//
// Impact law
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
// Interaction at contact point 1
//SP::Relation relation1(new LagrangianLinearTIR(H, E));
SP::Relation relation1(new LagrangianScleronomousR("RockingBlockPlugin:h1", "RockingBlockPlugin:G1", "RockingBlockPlugin:G1dot"));
SP::Interaction inter1(new Interaction(1, nslaw, relation1));
// Interaction at contact point 2
//SP::Relation relation2(new LagrangianLinearTIR(H, E));
SP::Relation relation2(new LagrangianScleronomousR("RockingBlockPlugin:h2", "RockingBlockPlugin:G2", "RockingBlockPlugin:G2dot"));
SP::Interaction inter2(new Interaction(1, nslaw, relation2));
// Interactions for the whole dynamical system
//================================================================================================================
// III. Create the "model" object
//================================================================================================================
SP::Model RoBlockModel(new Model(TimeInitial, TimeFinal));
RoBlockModel->nonSmoothDynamicalSystem()->insertDynamicalSystem(RockingBlock);
RoBlockModel->nonSmoothDynamicalSystem()->link(inter1, RockingBlock);
RoBlockModel->nonSmoothDynamicalSystem()->link(inter2, RockingBlock);
//================================================================================================================
// IV. Create the simulation
//================================================================================================================
//1. Time discretization
SP::TimeDiscretisation TimeDiscret(new TimeDiscretisation(TimeInitial, StepSize));
//2. Integration solver for one step
SP::OneStepIntegrator OSI(new LsodarOSI());
//3. Nonsmooth problem
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
//4. Simulation with (1), (2), (3)
SP::Simulation EDscheme(new EventDriven(TimeDiscret));
EDscheme->insertIntegrator(OSI);
EDscheme->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
EDscheme->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
RoBlockModel->setSimulation(EDscheme); // initialize the model
// bool check1 = EDscheme->hasOneStepNSProblem(impact);
// bool check2 = EDscheme->hasOneStepNSProblem(acceleration);
// cout << "Impact law included in the simulation: " << check1 << endl;
// cout << "LCP at acceleration level included in the simulation: " << check2 << endl;
//==================================================================================================================
// V. Process the simulation
//==================================================================================================================
// -------------------------------- Simulation initialization ------------------------------------------------------
cout << "====> Simulation initialisation ..." << endl << endl;
RoBlockModel->initialize(); // initialize the model
EDscheme->setPrintStat(true);
SP::EventsManager eventsManager = EDscheme->eventsManager(); // ponters point to the "eventsManager" object
SP::SiconosVector PosBlock = RockingBlock->q(); // pointer points to the position vector of the rocking block
SP::SiconosVector VelBlock = RockingBlock->velocity(); // pointer points to the velocity of the rocking block
//-------------------- Save the output during simulation ---------------------------------------------------------
SimpleMatrix DataPlot(NpointSave, SizeOutput);
//------------- At the initial time -----------------------------------------------------------------------------
DataPlot(0, 0) = RoBlockModel->t0();
DataPlot(0, 1) = (*PosBlock)(0); // Position X
DataPlot(0, 2) = (*PosBlock)(1); // Position Y
DataPlot(0, 3) = (*PosBlock)(2); // Angle theta
DataPlot(0, 4) = (*VelBlock)(0); // Velocity Vx
DataPlot(0, 5) = (*VelBlock)(1); // Velocity Vy
DataPlot(0, 6) = (*VelBlock)(2); // Angular velocity
//----------------------------------- Simulation starts ----------------------------------------------------------
cout << "====> Start computation ... " << endl << endl;
bool NSEvent = false;
unsigned int NumberNSEvent = 0;
unsigned int k = 1;
boost::progress_display show_progress(NpointSave);
while (EDscheme->hasNextEvent() && (k < NpointSave))
{
EDscheme->advanceToEvent(); // lead the simulation run from one event to the next
//---------- detect the statue of the current event ------------------------------------
if (eventsManager->nextEvent()->getType() == 2) // the current event is non-smooth
{
NSEvent = true;
};
EDscheme->processEvents(); // process the current event
//------------------- get data at the beginning of non-smooth events ---------------------------
if (NSEvent)
{
DataPlot(k, 0) = EDscheme->startingTime(); // instant at non-smooth event
DataPlot(k, 1) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(0); // Position X
DataPlot(k, 2) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(1); // Position Y
DataPlot(k, 3) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(2); // Angle theta
DataPlot(k, 4) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(0); // Velocity Vx
DataPlot(k, 5) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(1); // Velocity Vy
DataPlot(k, 6) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(2); // Angular velocity
//EDscheme->update(1);
k++;
++NumberNSEvent;
++show_progress;
NSEvent = false; // The next event is maybe smooth
};
//-------------------- get data at smooth events or at the end of non-smooth events ---------------
DataPlot(k, 0) = EDscheme->startingTime();
DataPlot(k, 1) = (*PosBlock)(0); //Position X
DataPlot(k, 2) = (*PosBlock)(1); //Position Y
DataPlot(k, 3) = (*PosBlock)(2); // Position theta
DataPlot(k, 4) = (*VelBlock)(0); // Velocity Vx
DataPlot(k, 5) = (*VelBlock)(1); // Velocity Vy
DataPlot(k, 6) = (*VelBlock)(2); // Velocity Vtheta
// go to the next time step
k++;
++show_progress;
};
//----------------------- At the end of the simulation --------------------------
cout << "End of the simulation" << endl;
cout << "Number of events processed during simulation: " << (k + 1) << endl;
cout << "Number of non-smooth events: " << NumberNSEvent << endl;
cout << "====> Output file writing ..." << endl << endl;
ioMatrix::write("result.dat", "ascii", DataPlot, "noDim");
}
//============================== Catch exceptions ===================================================================
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught." << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}
int main(int argc, char* argv[])
{
boost::timer time;
time.restart();
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDof = 3; // degrees of freedom for the ball
double t0 = 0; // initial computation time
double T = 10.0; // final computation time
double h = 0.01; // time step
double position_init = 1.0; // initial position for lowest bead.
double velocity_init = 10.0; // initial velocity for lowest bead.
double Heightbox = 1.5;
double R = 0.1; // Ball radius
double m = 1; // Ball mass
double g = 10.0; // Gravity
// -------------------------
// --- Dynamical systems ---
// -------------------------
cout << "====> Model loading ..." << endl << endl;
SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof));
(*Mass)(0, 0) = m;
(*Mass)(1, 1) = m;
(*Mass)(2, 2) = 3. / 5 * m * R * R;
// -- Initial positions and velocities --
SP::SiconosVector q0(new SiconosVector(nDof));
SP::SiconosVector v0(new SiconosVector(nDof));
(*q0)(0) = position_init;
(*v0)(0) = velocity_init;
// -- The dynamical system --
SP::LagrangianLinearTIDS ball(new LagrangianLinearTIDS(q0, v0, Mass));
// -- Set external forces (weight) --
SP::SiconosVector weight(new SiconosVector(nDof));
(*weight)(0) = -m * g;
ball->setFExtPtr(weight);
//
// --------------------
// --- Interactions ---
// --------------------
// -- nslaw --
double e = 0.8; // Warning this example does not work with e=0.0
// Interaction ball-floor-ceiling
//
SP::SimpleMatrix H1(new SimpleMatrix(1, nDof));
(*H1)(0, 0) = 1.0;
SP::SiconosVector E1(new SiconosVector(1));
(*E1)(0) = 0.0;//-1.0*R;
//
SP::SimpleMatrix H2(new SimpleMatrix(1, nDof));
(*H2)(0, 0) = -1.0;
SP::SiconosVector E2(new SiconosVector(1));
(*E2)(0) = Heightbox ;//- R;
// impact law
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
// Interaction at contact 1 (ball-floor)
SP::Relation relation1(new LagrangianLinearTIR(H1, E1));
SP::Interaction inter1(new Interaction(1, nslaw, relation1));
// Interaction at contact 2 (ball-ceiling)
SP::Relation relation2(new LagrangianLinearTIR(H2, E2));
SP::Interaction inter2(new Interaction(1, nslaw, relation2));
// --------------------------------
// --- NonSmoothDynamicalSystem ---
// --------------------------------
// -------------
// --- Model ---
// -------------
SP::Model bouncingBall(new Model(t0, T));
// add the dynamical system in the non smooth dynamical system
bouncingBall->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);
// link the interaction and the dynamical system
bouncingBall->nonSmoothDynamicalSystem()->link(inter1, ball);
bouncingBall->nonSmoothDynamicalSystem()->link(inter2, ball);
// ----------------
// --- Simulation ---
// ----------------
// -- (1) OneStepIntegrators --
SP::OneStepIntegrator OSI(new LsodarOSI());
// -- (2) Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
// -- (3) Non smooth problem --
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
// -- (4) Simulation setup with (1) (2) (3)
SP::EventDriven s(new EventDriven(t));
s->insertIntegrator(OSI);
s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
cout << "SICONOS_OSNSP_ED_IMPACT: " << SICONOS_OSNSP_ED_IMPACT << endl;
cout << "SICONOS_OSNSP_ED_ACCELERATION :" << SICONOS_OSNSP_ED_SMOOTH_ACC << endl;
bouncingBall->setSimulation(s);
// =========================== End of model definition ===========================
// ================================= Computation =================================
// --- Simulation initialization ---
cout << "====> Simulation initialisation ..." << endl << endl;
s->setPrintStat(true);
bouncingBall->initialize();
OSI->display();
int N = 1850; // Number of saved points: depends on the number of events ...
int ll = 0;
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 9;
SimpleMatrix dataPlot(N, outputSize);
SP::SiconosVector q = ball->q(); // ball position
SP::SiconosVector v = ball->velocity(); // ball velocity
SP::SiconosVector gamma = ball->acceleration(); // ball velocity
SP::SiconosVector f = ball->p(2); // resultant force deduced from the LCP at acceleration level
SP::SiconosVector p = ball->p(1); // resultant force deduced from the LCP at velocity level
SP::SiconosVector y1 = inter1->y(0);
SP::SiconosVector y2 = inter2->y(0);
// SiconosVector * y = bouncingBall->nonSmoothDynamicalSystem()->interaction(0)->y(0);
SP::EventsManager eventsManager = s->eventsManager();
OSI->display();
// For the initial time step:
// time
dataPlot(0, 0) = bouncingBall->t0();
dataPlot(0, 1) = (*q)(0);
dataPlot(0, 2) = (*v)(0);
dataPlot(0, 3) = (*p)(0);
dataPlot(0, 4) = 0;
dataPlot(0, 5) = (*y1)(0);
dataPlot(0, 6) = (*y2)(0);
dataPlot(0, 7) = (*gamma)(0);
dataPlot(0, 8) = (*f)(0);
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
bool nonSmooth = false;
unsigned int numberOfEvent = 0 ;
double k = 1;
boost::progress_display show_progress(N);
s->setPrintStat(true);
// s->setTolerance(1e-10);
while (s->hasNextEvent())
{
s->advanceToEvent(); // run simulation from one event to the next
if (eventsManager->nextEvent()->getType() == 2)
nonSmooth = true;
s->processEvents(); // process events
// If the treated event is non smooth, the pre-impact state has been solved in memory vectors during process.
if (nonSmooth) // if the event is nonsmooth
{
dataPlot(k,0) = s->startingTime(); // get the time at nonsmooth event
dataPlot(k,1) = (*ball->qMemory()->getSiconosVector(1))(0);
dataPlot(k,2) = (*ball->velocityMemory()->getSiconosVector(1))(0);
k++;
nonSmooth = false;
++show_progress;
dataPlot(k,4) = 1;
++ll;
// cout << "========================================" << endl;
// cout << "Nonsmooth event" << endl;
}
dataPlot(k, 0) = s->startingTime();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*v)(0);
dataPlot(k, 3) = (*p)(0);
dataPlot(k, 5) = (*y1)(0);
dataPlot(k, 6) = (*y2)(0);
dataPlot(k, 4) = 0;
dataPlot(k, 7) = (*gamma)(0);
dataPlot(k, 8) = (*f)(0);
cout << "========================================" << endl;
cout << " time: " << s->startingTime() << endl;
cout << "ball position: " << (*q)(0) << endl;
cout << "ball velocity: " << (*v)(0) << endl;
cout << "gap at contact 1: " << (*y1)(0) << endl;
cout << "gap at contact 2: " << (*y2)(0) << endl;
//
k++;
++numberOfEvent;
++show_progress;
}
// --- Output files ---
cout << endl;
cout << "===== End of Event Driven simulation. " << numberOfEvent << " events have been processed. ==== " << endl << endl;
cout << "Number of nonsmooth events = " << ll << endl;
cout << "====> Output file writing ..." << endl << endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("BouncingBallTwoContactsED.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("BouncingBallTwoContactsED.cpp", "ascii", dataPlotRef);
std:: cout << " Error ="<< (dataPlot - dataPlotRef).normInf() << std::endl;
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught." << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}
int main(int argc, char* argv[])
{
//---------------------------- calculate the computation time --------------------------------------------------
boost::timer time;
time.restart();
try
{
// ================= Creation of the model =======================
// -> mind to set the initial conditions below.
// -------------------------
// --- Dynamical systems ---
// -------------------------
// --- DS: Simple Pendulum ---
// Initial position (angles in radian)
SP::SiconosVector q0(new SiconosVector(nDof));
SP::SiconosVector v0(new SiconosVector(nDof));
(*q0).zero();
(*v0).zero();
(*q0)(0) = L * sin(InitAngle);
(*q0)(1) = L * cos(InitAngle);
SP::SimpleMatrix Mass(new SimpleMatrix(nDof, nDof));
(*Mass)(0, 0) = m;
(*Mass)(1, 1) = m;
SP::LagrangianDS simplependulum(new LagrangianLinearTIDS(q0, v0, Mass));
SP::SiconosVector ForceExtern(new SiconosVector(nDof));
(*ForceExtern)(0) = 0.0;
(*ForceExtern)(1) = m * gravity;
simplependulum->setFExtPtr(ForceExtern);
// -------------------
// --- Interactions---
// -------------------
SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
SP::Relation relation(new LagrangianScleronomousR("SimplePendulumBilateralConstraintPlugin:h0", "SimplePendulumBilateralConstraintPlugin:G0", "SimplePendulumBilateralConstraintPlugin:G0dot"));
SP::Interaction inter(new Interaction(1, nslaw, relation));
// -------------
// --- Model ---
// -------------
SP::Model Pendulum(new Model(t0, T));
Pendulum->nonSmoothDynamicalSystem()->insertDynamicalSystem(simplependulum);
Pendulum->nonSmoothDynamicalSystem()->link(inter, simplependulum);
// ----------------
// --- Simulation ---
// ----------------
//1. Time discretization
SP::TimeDiscretisation TimeDiscret(new TimeDiscretisation(t0, h));
//2. Integration solver for one step
SP::OneStepIntegrator OSI(new LsodarOSI());
//3. Nonsmooth problem
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
//4. Simulation with (1), (2), (3)
SP::Simulation EDscheme(new EventDriven(TimeDiscret));
EDscheme->insertIntegrator(OSI);
EDscheme->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
EDscheme->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
Pendulum->setSimulation(EDscheme); // initialize the model
// =========================== End of model definition ===========================
// --- Simulation Initialization ---
cout << "====> Simulation initialisation ..." << endl << endl;
EDscheme->setPrintStat(true);
Pendulum->initialize(); // initialize the model
cout << "End of simulation initialisation" << endl;
// SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(OSI);
// lsodar->setMinMaxStepSizes(9.5e-4,1.0e-3);
// lsodar->setTol(1,1.0e-3,1.0e-6);
// lsodar->setMaxOrder(2, 2);
// ================================= Computation =================================
SP::EventsManager eventsManager = EDscheme->eventsManager(); // ponters point to the "eventsManager" object
SP::SiconosVector _q = simplependulum->q(); // pointer points to the position vector of the rocking block
SP::SiconosVector _qdot = simplependulum->velocity(); // pointer points to the velocity of the rocking block
SP::SiconosVector _qddot = simplependulum->acceleration(); // pointer points to the velocity of the rocking block
SP::SiconosVector _g = inter->y(0);
SP::SiconosVector _gdot = inter->y(1);
SP::SiconosVector _lambda = inter->lambda(2);
SP::InteractionsGraph indexSet0 = Pendulum->nonSmoothDynamicalSystem()->topology()->indexSet(0);
SP::InteractionsGraph indexSet1 = Pendulum->nonSmoothDynamicalSystem()->topology()->indexSet(1);
SP::InteractionsGraph indexSet2 = Pendulum->nonSmoothDynamicalSystem()->topology()->indexSet(2);
cout << "Size of IndexSet0: " << indexSet0->size() << endl;
cout << "Size of IndexSet1: " << indexSet1->size() << endl;
cout << "Size of IndexSet2: " << indexSet2->size() << endl;
//-------------------- Save the output during simulation ---------------------------------------------------------
SimpleMatrix DataPlot(N, 10);
//------------- At the initial time -----------------------------------------------------------------------------
DataPlot(0, 0) = Pendulum->t0();
DataPlot(0, 1) = (*_q)(0); // Position X
DataPlot(0, 2) = (*_q)(1); // Position Y
DataPlot(0, 3) = (*_qdot)(0); // Velocity Vx
DataPlot(0, 4) = (*_qdot)(1); // Velocity Vy
DataPlot(0, 5) = (*_qddot)(0); // Acceleration ax
DataPlot(0, 6) = (*_qddot)(1); // Acceleration ay
DataPlot(0, 7) = (*_g)(0); // Contraint in position
DataPlot(0, 8) = (*_gdot)(0); // Constraint in velocity
DataPlot(0, 9) = (*_lambda)(0); // Reaction force
//----------------------------------- Simulation starts ----------------------------------------------------------
cout << "====> Start computation ... " << endl << endl;
bool NSEvent = false;
unsigned int NumberNSEvent = 0;
unsigned int k = 1;
boost::progress_display show_progress(N);
while ((EDscheme->hasNextEvent()) && (k < N))
{
EDscheme->advanceToEvent(); // lead the simulation run from one event to the next
//---------- detect the statue of the current event ------------------------------------
if (eventsManager->nextEvent()->getType() == 2) // the current event is non-smooth
{
NSEvent = true;
};
EDscheme->processEvents(); // process the current event
//------------------- get data at the beginning of non-smooth events ---------------------------
if (NSEvent)
{
DataPlot(k, 0) = EDscheme->startingTime(); // instant at non-smooth event
SP::SiconosVector _qMemory = simplependulum->qMemory()->getSiconosVector(1);
SP::SiconosVector _qdotMemory = simplependulum->velocityMemory()->getSiconosVector(1);
DataPlot(k, 1) = (*_qMemory)(0);
DataPlot(k, 2) = (*_qMemory)(1);
DataPlot(k, 3) = (*_qdotMemory)(0);
DataPlot(k, 4) = (*_qdotMemory)(1);
k++;
++NumberNSEvent;
++show_progress;
NSEvent = false; // The next event is maybe smooth
};
//-------------------- get data at smooth events or at the end of non-smooth events ---------------
DataPlot(k, 0) = EDscheme->startingTime();
DataPlot(k, 1) = (*_q)(0); // Position X
DataPlot(k, 2) = (*_q)(1); // Position Y
DataPlot(k, 3) = (*_qdot)(0); // Velocity Vx
DataPlot(k, 4) = (*_qdot)(1); // Velocity Vy
DataPlot(k, 5) = (*_qddot)(0); // Acceleration ax
DataPlot(k, 6) = (*_qddot)(1); // Acceleration ay
DataPlot(k, 7) = (*_g)(0); // Contraint in position
DataPlot(k, 8) = (*_gdot)(0); // Constraint in velocity
DataPlot(k, 9) = (*_lambda)(0); // Reaction force
// go to the next time step
k++;
++show_progress;
}
//----------------------- At the end of the simulation --------------------------
cout << " " << endl;
cout << "End of the simulation" << endl;
cout << "Number of events processed during simulation: " << (k + 1) << endl;
cout << "Number of non-smooth events: " << NumberNSEvent << endl;
cout << "====> Output file writing ..." << endl << endl;
ioMatrix::write("result.dat", "ascii", DataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(DataPlot);
dataPlotRef.zero();
ioMatrix::read("result_LsodarOSI.ref", "ascii", dataPlotRef);
std::cout << (DataPlot - dataPlotRef).normInf() << std::endl;
if ((DataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught" << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}
int main(int argc, char* argv[])
{
boost::timer time;
time.restart();
try
{
// ================= Creation of the model =======================
// User-defined main parameters
unsigned int nDof = 3; // degrees of freedom for the ball
double t0 = 0; // initial computation time
double T = 8.5; // final computation time
double h = 0.005; // time step
double position_init = 1.0; // initial position for lowest bead.
double velocity_init = 0.0; // initial velocity for lowest bead.
double R = 0.1; // Ball radius
double m = 1; // Ball mass
double g = 9.81; // Gravity
// -------------------------
// --- Dynamical systems ---
// -------------------------
cout << "====> Model loading ..." << endl << endl;
SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof));
(*Mass)(0, 0) = m;
(*Mass)(1, 1) = m;
(*Mass)(2, 2) = 3. / 5 * m * R * R;
// -- Initial positions and velocities --
SP::SiconosVector q0(new SiconosVector(nDof));
SP::SiconosVector v0(new SiconosVector(nDof));
(*q0)(0) = position_init;
(*v0)(0) = velocity_init;
// -- The dynamical system --
SP::LagrangianLinearTIDS ball(new LagrangianLinearTIDS(q0, v0, Mass));
// -- Set external forces (weight) --
SP::SiconosVector weight(new SiconosVector(nDof));
(*weight)(0) = -m * g;
ball->setFExtPtr(weight);
// --------------------
// --- Interactions ---
// --------------------
// -- nslaw --
double e = 0.9;
// Interaction ball-floor
//
SP::SimpleMatrix H(new SimpleMatrix(1, nDof));
(*H)(0, 0) = 1.0;
SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
SP::Relation relation0(new LagrangianLinearTIR(H));
SP::Interaction inter(new Interaction(1, nslaw0, relation0));
// --------------------------------
// --- NonSmoothDynamicalSystem ---
// --------------------------------
// -------------
// --- Model ---
// -------------
SP::Model bouncingBall(new Model(t0, T));
// add the dynamical system in the non smooth dynamical system
bouncingBall->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);
// link the interaction and the dynamical system
bouncingBall->nonSmoothDynamicalSystem()->link(inter, ball);
// ----------------
// --- Simulation ---
// ----------------
// -- (1) OneStepIntegrators --
SP::OneStepIntegrator OSI(new LsodarOSI(ball));
// -- (2) Time discretisation --
SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
// -- (3) Non smooth problem --
SP::OneStepNSProblem impact(new LCP());
SP::OneStepNSProblem acceleration(new LCP());
// -- (4) Simulation setup with (1) (2) (3)
SP::EventDriven s(new EventDriven(t));
s->insertIntegrator(OSI);
s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
// =========================== End of model definition ===========================
// ================================= Computation =================================
// --- Simulation initialization ---
cout << "====> Simulation initialisation ..." << endl << endl;
s->setPrintStat(true);
bouncingBall->initialize(s);
int N = 1854; // Number of saved points: depends on the number of events ...
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 7;
SimpleMatrix dataPlot(N + 1, outputSize);
SP::SiconosVector q = ball->q();
SP::SiconosVector v = ball->velocity();
SP::SiconosVector p = ball->p(1);
SP::SiconosVector f = ball->p(2);
// SiconosVector * y = bouncingBall->nonSmoothDynamicalSystem()->interaction(0)->y(0);
SP::EventsManager eventsManager = s->eventsManager();
// For the initial time step:
// time
dataPlot(0, 0) = bouncingBall->t0();
dataPlot(0, 1) = (*q)(0);
dataPlot(0, 2) = (*v)(0);
dataPlot(0, 3) = (*p)(0);
dataPlot(0, 4) = (*f)(0);
// --- Time loop ---
cout << "====> Start computation ... " << endl << endl;
bool nonSmooth = false;
unsigned int numberOfEvent = 0 ;
int k = 0;
int kns = 0;
boost::progress_display show_progress(N);
while (s->hasNextEvent() && k < N)
{
s->advanceToEvent();
if (eventsManager->nextEvent()->getType() == 2)
nonSmooth = true;
s->processEvents();
// If the treated event is non smooth, the pre-impact state has been solved in memory vectors during process.
if (nonSmooth)
{
dataPlot(k, 0) = s->startingTime();
dataPlot(k, 1) = (*ball->qMemory()->getSiconosVector(1))(0);
dataPlot(k, 2) = (*ball->velocityMemory()->getSiconosVector(1))(0);
dataPlot(k, 3) = (*p)(0);
dataPlot(k, 4) = (*f)(0);
k++;
kns++;
nonSmooth = false;
++show_progress;
}
dataPlot(k, 0) = s->startingTime();
dataPlot(k, 1) = (*q)(0);
dataPlot(k, 2) = (*v)(0);
dataPlot(k, 3) = (*p)(0);
dataPlot(k, 4) = (*f)(0);
dataPlot(k, 5) = (*inter->lambda(1))(0);
dataPlot(k, 6) = (*inter->lambda(2))(0);
++k;
++numberOfEvent;
++show_progress;
}
// --- Output files ---
cout << endl;
cout << "===== End of Event Driven simulation. " << endl;
cout << numberOfEvent << " events have been processed. ==== " << endl;
cout << numberOfEvent- kns << " events are of time--discretization type ==== " << endl;
cout << kns << " events are of nonsmooth type ==== " << endl << endl;
cout << "====> Output file writing ..." << endl << endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("BouncingBallED.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The results is rather different from the reference file." << std::endl;
std::cout << "error = " << (dataPlot - dataPlotRef).normInf() << std::endl;
return 1;
}
}
catch (SiconosException e)
{
cout << e.report() << endl;
}
catch (...)
{
cout << "Exception caught." << endl;
}
cout << "Computation Time: " << time.elapsed() << endl;
}