// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChDoubleWishboneReduced::LogConstraintViolations(VehicleSide side) {
// Revolute joint
{
ChMatrix<>* C = m_revolute[side]->GetC();
GetLog() << "Spindle revolute ";
GetLog() << " " << C->GetElement(0, 0) << " ";
GetLog() << " " << C->GetElement(1, 0) << " ";
GetLog() << " " << C->GetElement(2, 0) << " ";
GetLog() << " " << C->GetElement(3, 0) << " ";
GetLog() << " " << C->GetElement(4, 0) << "\n";
}
// Distance constraints
GetLog() << "UCA front distance ";
GetLog() << " " << m_distUCA_F[side]->GetCurrentDistance() - m_distUCA_F[side]->GetImposedDistance() << "\n";
GetLog() << "UCA back distance ";
GetLog() << " " << m_distUCA_B[side]->GetCurrentDistance() - m_distUCA_B[side]->GetImposedDistance() << "\n";
GetLog() << "LCA front distance ";
GetLog() << " " << m_distLCA_F[side]->GetCurrentDistance() - m_distLCA_F[side]->GetImposedDistance() << "\n";
GetLog() << "LCA back distance ";
GetLog() << " " << m_distLCA_B[side]->GetCurrentDistance() - m_distLCA_B[side]->GetImposedDistance() << "\n";
GetLog() << "Tierod distance ";
GetLog() << " " << m_distTierod[side]->GetCurrentDistance() - m_distTierod[side]->GetImposedDistance() << "\n";
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPitmanArm::LogConstraintViolations() {
// Revolute joint
////{
//// ChMatrix<>* C = m_revolute->GetC();
//// GetLog() << "Revolute ";
//// GetLog() << " " << C->GetElement(0, 0) << " ";
//// GetLog() << " " << C->GetElement(1, 0) << " ";
//// GetLog() << " " << C->GetElement(2, 0) << " ";
//// GetLog() << " " << C->GetElement(3, 0) << " ";
//// GetLog() << " " << C->GetElement(4, 0) << "\n";
////}
// Universal joint
{
ChMatrix<>* C = m_universal->GetC();
GetLog() << "Universal ";
GetLog() << " " << C->GetElement(0, 0) << " ";
GetLog() << " " << C->GetElement(1, 0) << " ";
GetLog() << " " << C->GetElement(2, 0) << " ";
GetLog() << " " << C->GetElement(3, 0) << "\n";
}
// Revolute-spherical joint
{
ChMatrix<>* C = m_revsph->GetC();
GetLog() << "Revolute-spherical ";
GetLog() << " " << C->GetElement(0, 0) << " ";
GetLog() << " " << C->GetElement(1, 0) << "\n";
}
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChRoadWheel::LogConstraintViolations() {
ChMatrix<>* C = m_revolute->GetC();
GetLog() << " Road-wheel revolute\n";
GetLog() << " " << C->GetElement(0, 0) << " ";
GetLog() << " " << C->GetElement(1, 0) << " ";
GetLog() << " " << C->GetElement(2, 0) << " ";
GetLog() << " " << C->GetElement(3, 0) << " ";
GetLog() << " " << C->GetElement(4, 0) << "\n";
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChLinearDamperRWAssembly::LogConstraintViolations() {
ChMatrix<>* C = m_revolute->GetC();
GetLog() << " Arm-chassis revolute\n";
GetLog() << " " << C->GetElement(0, 0) << " ";
GetLog() << " " << C->GetElement(1, 0) << " ";
GetLog() << " " << C->GetElement(2, 0) << " ";
GetLog() << " " << C->GetElement(3, 0) << " ";
GetLog() << " " << C->GetElement(4, 0) << "\n";
m_road_wheel->LogConstraintViolations();
}
ChMapMatrix::ChMapMatrix(const ChMatrix<>& mat) {
m_num_rows = mat.GetRows();
m_num_cols = mat.GetColumns();
m_rows.resize(mat.GetRows());
for (int ir = 0; ir < m_num_rows; ir++) {
for (int ic = 0; ic < m_num_cols; ic++) {
double val = mat.GetElement(ir, ic);
if (val != 0) {
ChMapMatrix::SetElement(ir, ic, val);
}
}
}
m_CSR_current = false;
}
// =============================================================================
//
// Worker function for performing the simulation with specified parameters.
//
bool TestLinActuator(const ChQuaternion<>& rot, // translation along Z axis
double desiredSpeed, // imposed translation speed
double simTimeStep, // simulation time step
double outTimeStep, // output time step
const std::string& testName, // name of this test
bool animate) // if true, animate with Irrlich
{
std::cout << "TEST: " << testName << std::endl;
// Unit vector along translation axis, expressed in global frame
ChVector<> axis = rot.GetZaxis();
// Settings
//---------
double mass = 1.0; // mass of plate
ChVector<> inertiaXX(1, 1, 1); // mass moments of inertia of plate (centroidal frame)
double g = 9.80665;
double timeRecord = 5; // Stop recording to the file after this much simulated time
// Create the mechanical system
// ----------------------------
ChSystem my_system;
my_system.Set_G_acc(ChVector<>(0.0, 0.0, -g));
my_system.SetIntegrationType(ChSystem::INT_ANITESCU);
my_system.SetIterLCPmaxItersSpeed(100);
my_system.SetIterLCPmaxItersStab(100); //Tasora stepper uses this, Anitescu does not
my_system.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
my_system.SetTol(1e-6);
my_system.SetTolForce(1e-4);
// Create the ground body.
ChSharedBodyPtr ground(new ChBody);
my_system.AddBody(ground);
ground->SetBodyFixed(true);
// Add geometry to the ground body for visualizing the translational joint
ChSharedPtr<ChBoxShape> box_g(new ChBoxShape);
box_g->GetBoxGeometry().SetLengths(ChVector<>(0.1, 0.1, 5));
box_g->GetBoxGeometry().Pos = 2.5 * axis;
box_g->GetBoxGeometry().Rot = rot;
ground->AddAsset(box_g);
ChSharedPtr<ChColorAsset> col_g(new ChColorAsset);
col_g->SetColor(ChColor(0.6f, 0.2f, 0.2f));
ground->AddAsset(col_g);
// Create the plate body.
ChSharedBodyPtr plate(new ChBody);
my_system.AddBody(plate);
plate->SetPos(ChVector<>(0, 0, 0));
plate->SetRot(rot);
plate->SetPos_dt(desiredSpeed * axis);
plate->SetMass(mass);
plate->SetInertiaXX(inertiaXX);
// Add geometry to the plate for visualization
ChSharedPtr<ChBoxShape> box_p(new ChBoxShape);
box_p->GetBoxGeometry().SetLengths(ChVector<>(1, 1, 0.2));
plate->AddAsset(box_p);
ChSharedPtr<ChColorAsset> col_p(new ChColorAsset);
col_p->SetColor(ChColor(0.2f, 0.2f, 0.6f));
plate->AddAsset(col_p);
// Create prismatic (translational) joint between plate and ground.
// We set the ground as the "master" body (second one in the initialization
// call) so that the link coordinate system is expressed in the ground frame.
ChSharedPtr<ChLinkLockPrismatic> prismatic(new ChLinkLockPrismatic);
prismatic->Initialize(plate, ground, ChCoordsys<>(ChVector<>(0, 0, 0), rot));
my_system.AddLink(prismatic);
// Create a ramp function to impose constant speed. This function returns
// y(t) = 0 + t * desiredSpeed
// y'(t) = desiredSpeed
ChSharedPtr<ChFunction_Ramp> actuator_fun(new ChFunction_Ramp(0.0, desiredSpeed));
// Create the linear actuator, connecting the plate to the ground.
// Here, we set the plate as the master body (second one in the initialization
// call) so that the link coordinate system is expressed in the plate body
// frame.
ChSharedPtr<ChLinkLinActuator> actuator(new ChLinkLinActuator);
ChVector<> pt1 = ChVector<>(0, 0, 0);
ChVector<> pt2 = axis;
actuator->Initialize(ground, plate, false, ChCoordsys<>(pt1, rot), ChCoordsys<>(pt2, rot));
actuator->Set_lin_offset(1);
actuator->Set_dist_funct(actuator_fun);
my_system.AddLink(actuator);
// Perform the simulation (animation with Irrlicht)
// ------------------------------------------------
if (animate)
{
// Create the Irrlicht application for visualization
ChIrrApp application(&my_system, L"ChLinkRevolute demo", core::dimension2d<u32>(800, 600), false, true);
application.AddTypicalLogo();
application.AddTypicalSky();
application.AddTypicalLights();
application.AddTypicalCamera(core::vector3df(1, 2, -2), core::vector3df(0, 0, 2));
application.AssetBindAll();
application.AssetUpdateAll();
application.SetStepManage(true);
application.SetTimestep(simTimeStep);
// Simulation loop
while (application.GetDevice()->run())
{
application.BeginScene();
application.DrawAll();
// Draw an XZ grid at the global origin to add in visualization
ChIrrTools::drawGrid(
application.GetVideoDriver(), 1, 1, 20, 20,
ChCoordsys<>(ChVector<>(0, 0, 0), Q_from_AngX(CH_C_PI_2)),
video::SColor(255, 80, 100, 100), true);
application.DoStep(); //Take one step in time
application.EndScene();
}
return true;
}
// Perform the simulation (record results)
// ------------------------------------------------
// Create the CSV_Writer output objects (TAB delimited)
utils::CSV_writer out_pos = OutStream();
utils::CSV_writer out_vel = OutStream();
utils::CSV_writer out_acc = OutStream();
utils::CSV_writer out_quat = OutStream();
utils::CSV_writer out_avel = OutStream();
utils::CSV_writer out_aacc = OutStream();
utils::CSV_writer out_rfrcP = OutStream();
utils::CSV_writer out_rtrqP = OutStream();
utils::CSV_writer out_rfrcA = OutStream();
utils::CSV_writer out_rtrqA = OutStream();
utils::CSV_writer out_cnstrP = OutStream();
utils::CSV_writer out_cnstrA = OutStream();
// Write headers
out_pos << "Time" << "X_Pos" << "Y_Pos" << "Z_Pos" << std::endl;
out_vel << "Time" << "X_Vel" << "Y_Vel" << "Z_Vel" << std::endl;
out_acc << "Time" << "X_Acc" << "Y_Acc" << "Z_Acc" << std::endl;
out_quat << "Time" << "e0" << "e1" << "e2" << "e3" << std::endl;
out_avel << "Time" << "X_AngVel" << "Y_AngVel" << "Z_AngVel" << std::endl;
out_aacc << "Time" << "X_AngAcc" << "Y_AngAcc" << "Z_AngAcc" << std::endl;
out_rfrcP << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
out_rtrqP << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;
out_rfrcA << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
out_rtrqA << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;
out_cnstrP << "Time" << "Cnstr_1" << "Cnstr_2" << "Cnstr_3" << "Constraint_4" << "Cnstr_5" << std::endl;
out_cnstrA << "Time" << "Cnstr_1" << std::endl;
// Simulation loop
double simTime = 0;
double outTime = 0;
while (simTime <= timeRecord + simTimeStep / 2)
{
// Ensure that the final data point is recorded.
if (simTime >= outTime - simTimeStep / 2)
{
// CM position, velocity, and acceleration (expressed in global frame).
const ChVector<>& position = plate->GetPos();
const ChVector<>& velocity = plate->GetPos_dt();
out_pos << simTime << position << std::endl;
out_vel << simTime << velocity << std::endl;
out_acc << simTime << plate->GetPos_dtdt() << std::endl;
// Orientation, angular velocity, and angular acceleration (expressed in
// global frame).
out_quat << simTime << plate->GetRot() << std::endl;
out_avel << simTime << plate->GetWvel_par() << std::endl;
out_aacc << simTime << plate->GetWacc_par() << std::endl;
// Reaction Force and Torque in prismatic joint.
// These are expressed in the link coordinate system. We convert them to
// the coordinate system of Body2 (in our case this is the ground).
ChCoordsys<> linkCoordsysP = prismatic->GetLinkRelativeCoords();
ChVector<> reactForceP = prismatic->Get_react_force();
ChVector<> reactForceGlobalP = linkCoordsysP.TransformDirectionLocalToParent(reactForceP);
out_rfrcP << simTime << reactForceGlobalP << std::endl;
ChVector<> reactTorqueP = prismatic->Get_react_torque();
ChVector<> reactTorqueGlobalP = linkCoordsysP.TransformDirectionLocalToParent(reactTorqueP);
out_rtrqP << simTime << reactTorqueGlobalP << std::endl;
// Reaction force and Torque in linear actuator.
// These are expressed in the link coordinate system. We convert them to
// the coordinate system of Body2 (in our case this is the plate). As such,
// the reaction force represents the force that needs to be applied to the
// plate in order to maintain the prescribed constant velocity. These are
// then converted to the global frame for comparison to ADAMS
ChCoordsys<> linkCoordsysA = actuator->GetLinkRelativeCoords();
ChVector<> reactForceA = actuator->Get_react_force();
reactForceA = linkCoordsysA.TransformDirectionLocalToParent(reactForceA);
ChVector<> reactForceGlobalA = plate->TransformDirectionLocalToParent(reactForceA);
out_rfrcA << simTime << reactForceGlobalA << std::endl;
ChVector<> reactTorqueA = actuator->Get_react_torque();
reactTorqueA = linkCoordsysA.TransformDirectionLocalToParent(reactTorqueA);
ChVector<> reactTorqueGlobalA = plate->TransformDirectionLocalToParent(reactTorqueA);
out_rtrqA << simTime << reactTorqueGlobalA << std::endl;
// Constraint violations in prismatic joint
ChMatrix<>* CP = prismatic->GetC();
out_cnstrP << simTime
<< CP->GetElement(0, 0)
<< CP->GetElement(1, 0)
<< CP->GetElement(2, 0)
<< CP->GetElement(3, 0)
<< CP->GetElement(4, 0) << std::endl;
// Constraint violations in linear actuator
ChMatrix<>* CA = actuator->GetC();
out_cnstrA << simTime << CA->GetElement(0, 0) << std::endl;
// Increment output time
outTime += outTimeStep;
}
// Advance simulation by one step
my_system.DoStepDynamics(simTimeStep);
// Increment simulation time
simTime += simTimeStep;
}
// Write output files
out_pos.write_to_file(out_dir + testName + "_CHRONO_Pos.txt", testName + "\n\n");
out_vel.write_to_file(out_dir + testName + "_CHRONO_Vel.txt", testName + "\n\n");
out_acc.write_to_file(out_dir + testName + "_CHRONO_Acc.txt", testName + "\n\n");
out_quat.write_to_file(out_dir + testName + "_CHRONO_Quat.txt", testName + "\n\n");
out_avel.write_to_file(out_dir + testName + "_CHRONO_Avel.txt", testName + "\n\n");
out_aacc.write_to_file(out_dir + testName + "_CHRONO_Aacc.txt", testName + "\n\n");
out_rfrcP.write_to_file(out_dir + testName + "_CHRONO_RforceP.txt", testName + "\n\n");
out_rtrqP.write_to_file(out_dir + testName + "_CHRONO_RtorqueP.txt", testName + "\n\n");
out_rfrcA.write_to_file(out_dir + testName + "_CHRONO_RforceA.txt", testName + "\n\n");
out_rtrqA.write_to_file(out_dir + testName + "_CHRONO_RtorqueA.txt", testName + "\n\n");
out_cnstrP.write_to_file(out_dir + testName + "_CHRONO_ConstraintsP.txt", testName + "\n\n");
out_cnstrA.write_to_file(out_dir + testName + "_CHRONO_ConstraintsA.txt", testName + "\n\n");
return true;
}
int main(int argc, char* argv[]) {
// Create output directories.
if (ChFileutils::MakeDirectory(out_dir.c_str()) < 0) {
cout << "Error creating directory " << out_dir << endl;
return 1;
}
if (ChFileutils::MakeDirectory(pov_dir.c_str()) < 0) {
cout << "Error creating directory " << pov_dir << endl;
return 1;
}
// -------------
// Create system
// -------------
#ifdef USE_DEM
cout << "Create DEM system" << endl;
ChSystemParallelDEM* msystem = new ChSystemParallelDEM();
#else
cout << "Create DVI system" << endl;
ChSystemParallelDVI* msystem = new ChSystemParallelDVI();
#endif
msystem->Set_G_acc(ChVector<>(0, 0, -gravity));
// Set number of threads.
int max_threads = omp_get_num_procs();
if (threads > max_threads)
threads = max_threads;
msystem->SetParallelThreadNumber(threads);
omp_set_num_threads(threads);
cout << "Using " << threads << " threads" << endl;
msystem->GetSettings()->max_threads = threads;
msystem->GetSettings()->perform_thread_tuning = thread_tuning;
// Edit system settings
msystem->GetSettings()->solver.use_full_inertia_tensor = false;
msystem->GetSettings()->solver.tolerance = tolerance;
msystem->GetSettings()->solver.max_iteration_bilateral = max_iteration_bilateral;
msystem->GetSettings()->solver.clamp_bilaterals = clamp_bilaterals;
msystem->GetSettings()->solver.bilateral_clamp_speed = bilateral_clamp_speed;
#ifdef USE_DEM
msystem->GetSettings()->collision.narrowphase_algorithm = NARROWPHASE_R;
msystem->GetSettings()->solver.contact_force_model = contact_force_model;
msystem->GetSettings()->solver.tangential_displ_mode = tangential_displ_mode;
#else
msystem->GetSettings()->solver.solver_mode = SLIDING;
msystem->GetSettings()->solver.max_iteration_normal = max_iteration_normal;
msystem->GetSettings()->solver.max_iteration_sliding = max_iteration_sliding;
msystem->GetSettings()->solver.max_iteration_spinning = max_iteration_spinning;
msystem->GetSettings()->solver.alpha = 0;
msystem->GetSettings()->solver.contact_recovery_speed = contact_recovery_speed;
msystem->SetMaxPenetrationRecoverySpeed(contact_recovery_speed);
msystem->ChangeSolverType(APGDREF);
msystem->GetSettings()->collision.collision_envelope = 0.05 * r_g;
#endif
msystem->GetSettings()->collision.bins_per_axis = I3(10, 10, 10);
// --------------
// Problem set up
// --------------
// Depending on problem type:
// - Select end simulation time
// - Select output FPS
// - Create / load objects
double time_min = 0;
double time_end;
int out_fps;
ChSharedPtr<ChBody> ground;
ChSharedPtr<ChBody> loadPlate;
ChSharedPtr<ChLinkLockPrismatic> prismatic;
ChSharedPtr<ChLinkLinActuator> actuator;
switch (problem) {
case SETTLING: {
time_min = time_settling_min;
time_end = time_settling_max;
out_fps = out_fps_settling;
// Create the mechanism bodies (all fixed).
CreateMechanismBodies(msystem);
// Grab handles to mechanism bodies (must increase ref counts)
ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0));
loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1));
msystem->Get_bodylist()->at(0)->AddRef();
msystem->Get_bodylist()->at(1)->AddRef();
// Create granular material.
int num_particles = CreateGranularMaterial(msystem);
cout << "Granular material: " << num_particles << " particles" << endl;
break;
}
case PRESSING: {
time_min = time_pressing_min;
time_end = time_pressing_max;
out_fps = out_fps_pressing;
// Create bodies from checkpoint file.
cout << "Read checkpoint data from " << settled_ckpnt_file;
utils::ReadCheckpoint(msystem, settled_ckpnt_file);
cout << " done. Read " << msystem->Get_bodylist()->size() << " bodies." << endl;
// Grab handles to mechanism bodies (must increase ref counts)
ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0));
loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1));
msystem->Get_bodylist()->at(0)->AddRef();
msystem->Get_bodylist()->at(1)->AddRef();
// Move the load plate just above the granular material.
double highest, lowest;
FindHeightRange(msystem, lowest, highest);
ChVector<> pos = loadPlate->GetPos();
double z_new = highest + 1.01 * r_g;
loadPlate->SetPos(ChVector<>(pos.x, pos.y, z_new));
// Add collision geometry to plate
loadPlate->GetCollisionModel()->ClearModel();
utils::AddBoxGeometry(loadPlate.get_ptr(), ChVector<>(hdimX_p, hdimY_p, hdimZ_p), ChVector<>(0, 0, hdimZ_p));
loadPlate->GetCollisionModel()->BuildModel();
// If using an actuator, connect the load plate and get a handle to the actuator.
if (use_actuator) {
ConnectLoadPlate(msystem, ground, loadPlate);
prismatic = msystem->SearchLink("prismatic").StaticCastTo<ChLinkLockPrismatic>();
actuator = msystem->SearchLink("actuator").StaticCastTo<ChLinkLinActuator>();
}
// Release the load plate.
loadPlate->SetBodyFixed(!use_actuator);
break;
}
case TESTING: {
time_end = time_testing;
out_fps = out_fps_testing;
// For TESTING only, increse shearing velocity.
desiredVelocity = 0.5;
// Create the mechanism bodies (all fixed).
CreateMechanismBodies(msystem);
// Create the test ball.
CreateBall(msystem);
// Grab handles to mechanism bodies (must increase ref counts)
ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0));
loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1));
msystem->Get_bodylist()->at(0)->AddRef();
msystem->Get_bodylist()->at(1)->AddRef();
// Move the load plate just above the test ball.
ChVector<> pos = loadPlate->GetPos();
double z_new = 2.1 * radius_ball;
loadPlate->SetPos(ChVector<>(pos.x, pos.y, z_new));
// Add collision geometry to plate
loadPlate->GetCollisionModel()->ClearModel();
utils::AddBoxGeometry(loadPlate.get_ptr(), ChVector<>(hdimX_p, hdimY_p, hdimZ_p), ChVector<>(0, 0, hdimZ_p));
loadPlate->GetCollisionModel()->BuildModel();
// If using an actuator, connect the shear box and get a handle to the actuator.
if (use_actuator) {
ConnectLoadPlate(msystem, ground, loadPlate);
actuator = msystem->SearchLink("actuator").StaticCastTo<ChLinkLinActuator>();
}
// Release the shear box when using an actuator.
loadPlate->SetBodyFixed(!use_actuator);
break;
}
}
// ----------------------
// Perform the simulation
// ----------------------
// Set number of simulation steps and steps between successive output
int num_steps = (int)std::ceil(time_end / time_step);
int out_steps = (int)std::ceil((1.0 / time_step) / out_fps);
int write_steps = (int)std::ceil((1.0 / time_step) / write_fps);
// Initialize counters
double time = 0;
int sim_frame = 0;
int out_frame = 0;
int next_out_frame = 0;
double exec_time = 0;
int num_contacts = 0;
double max_cnstr_viol[2] = {0, 0};
// Circular buffer with highest particle location
// (only used for SETTLING or PRESSING)
int buffer_size = std::ceil(time_min / time_step);
std::valarray<double> hdata(0.0, buffer_size);
// Create output files
ChStreamOutAsciiFile statsStream(stats_file.c_str());
ChStreamOutAsciiFile sinkageStream(sinkage_file.c_str());
sinkageStream.SetNumFormat("%16.4e");
#ifdef CHRONO_PARALLEL_HAS_OPENGL
opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
gl_window.Initialize(1280, 720, "Pressure Sinkage Test", msystem);
gl_window.SetCamera(ChVector<>(0, -10 * hdimY, hdimZ), ChVector<>(0, 0, hdimZ), ChVector<>(0, 0, 1));
gl_window.SetRenderMode(opengl::WIREFRAME);
#endif
// Loop until reaching the end time...
while (time < time_end) {
// Current position and velocity of the shear box
ChVector<> pos_old = loadPlate->GetPos();
ChVector<> vel_old = loadPlate->GetPos_dt();
// Calculate minimum and maximum particle heights
double highest, lowest;
FindHeightRange(msystem, lowest, highest);
// If at an output frame, write PovRay file and print info
if (sim_frame == next_out_frame) {
cout << "------------ Output frame: " << out_frame + 1 << endl;
cout << " Sim frame: " << sim_frame << endl;
cout << " Time: " << time << endl;
cout << " Load plate pos: " << pos_old.z << endl;
cout << " Lowest point: " << lowest << endl;
cout << " Highest point: " << highest << endl;
cout << " Execution time: " << exec_time << endl;
// Save PovRay post-processing data.
if (write_povray_data) {
char filename[100];
sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), out_frame + 1);
utils::WriteShapesPovray(msystem, filename, false);
}
// Create a checkpoint from the current state.
if (problem == SETTLING || problem == PRESSING) {
cout << " Write checkpoint data " << flush;
if (problem == SETTLING)
utils::WriteCheckpoint(msystem, settled_ckpnt_file);
else
utils::WriteCheckpoint(msystem, pressed_ckpnt_file);
cout << msystem->Get_bodylist()->size() << " bodies" << endl;
}
// Increment counters
out_frame++;
next_out_frame += out_steps;
}
// Check for early termination of a settling phase.
if (problem == SETTLING) {
// Store maximum particle height in circular buffer
hdata[sim_frame % buffer_size] = highest;
// Check variance of data in circular buffer
if (time > time_min) {
double mean_height = hdata.sum() / buffer_size;
std::valarray<double> x = hdata - mean_height;
double var = std::sqrt((x * x).sum() / buffer_size);
// Consider the material settled when the variance is below the
// specified fraction of a particle radius
if (var < settling_tol * r_g) {
cout << "Granular material settled... time = " << time << endl;
break;
}
}
}
// Advance simulation by one step
#ifdef CHRONO_PARALLEL_HAS_OPENGL
if (gl_window.Active()) {
gl_window.DoStepDynamics(time_step);
gl_window.Render();
} else
break;
#else
msystem->DoStepDynamics(time_step);
#endif
TimingOutput(msystem);
// Record stats about the simulation
if (sim_frame % write_steps == 0) {
// write stat info
int numIters = msystem->data_manager->measures.solver.maxd_hist.size();
double residual = 0;
if (numIters)
residual = msystem->data_manager->measures.solver.residual;
statsStream << time << ", " << exec_time << ", " << num_contacts / write_steps << ", " << numIters << ", "
<< residual << ", " << max_cnstr_viol[0] << ", " << max_cnstr_viol[1] << ", \n";
statsStream.GetFstream().flush();
num_contacts = 0;
max_cnstr_viol[0] = 0;
max_cnstr_viol[1] = 0;
}
if (problem == PRESSING || problem == TESTING) {
// Get the current reaction force or impose load plate position
double cnstr_force = 0;
if (use_actuator) {
cnstr_force = actuator->Get_react_force().x;
} else {
double zpos_new = pos_old.z + desiredVelocity * time_step;
loadPlate->SetPos(ChVector<>(pos_old.x, pos_old.y, zpos_new));
loadPlate->SetPos_dt(ChVector<>(0, 0, desiredVelocity));
}
if (sim_frame % write_steps == 0) {
// std::cout << time << ", " << loadPlate->GetPos().z << ", " << cnstr_force << ", \n";
sinkageStream << time << ", " << loadPlate->GetPos().z << ", " << cnstr_force << ", \n";
sinkageStream.GetFstream().flush();
}
}
// Find maximum constraint violation
if (!prismatic.IsNull()) {
ChMatrix<>* C = prismatic->GetC();
for (int i = 0; i < 5; i++)
max_cnstr_viol[0] = std::max(max_cnstr_viol[0], std::abs(C->GetElement(i, 0)));
}
if (!actuator.IsNull()) {
ChMatrix<>* C = actuator->GetC();
max_cnstr_viol[1] = std::max(max_cnstr_viol[2], std::abs(C->GetElement(0, 0)));
}
// Increment counters
time += time_step;
sim_frame++;
exec_time += msystem->GetTimerStep();
num_contacts += msystem->GetNcontacts();
// If requested, output detailed timing information for this step
if (sim_frame == timing_frame)
msystem->PrintStepStats();
}
// ----------------
// Final processing
// ----------------
// Create a checkpoint from the last state
if (problem == SETTLING || problem == PRESSING) {
cout << " Write checkpoint data " << flush;
if (problem == SETTLING)
utils::WriteCheckpoint(msystem, settled_ckpnt_file);
else
utils::WriteCheckpoint(msystem, pressed_ckpnt_file);
cout << msystem->Get_bodylist()->size() << " bodies" << endl;
}
// Final stats
cout << "==================================" << endl;
cout << "Number of bodies: " << msystem->Get_bodylist()->size() << endl;
cout << "Simulation time: " << exec_time << endl;
cout << "Number of threads: " << threads << endl;
return 0;
}