// -----------------------------------------------------------------------------
// CreateBoxContainer
// CreateBoxContainerDEM
//
// Create a fixed body with contact and asset geometry representing a box with 5
// walls (no top).
// These functions assume a coordinate frame with Z up.
// The first version uses a ChBody; the second version uses a ChBodyDEM.
// -----------------------------------------------------------------------------
void CreateBoxContainer(ChSystem* system,
int id,
ChSharedPtr<ChMaterialSurface>& mat,
const ChVector<>& hdim,
double hthick,
const ChVector<>& pos,
const ChQuaternion<>& rot,
bool collide)
{
// Create the body and set material
ChBody* body;
body = new ChBody();
body->SetMaterialSurface(mat);
// Set body properties and geometry.
body->SetIdentifier(id);
body->SetMass(1);
body->SetPos(pos);
body->SetRot(rot);
body->SetCollide(collide);
body->SetBodyFixed(true);
body->GetCollisionModel()->ClearModel();
AddBoxGeometry(body, ChVector<>(hdim.x, hdim.y, hthick), ChVector<>(0, 0, -hthick));
AddBoxGeometry(body, ChVector<>(hthick, hdim.y, hdim.z), ChVector<>(-hdim.x-hthick, 0, hdim.z));
AddBoxGeometry(body, ChVector<>(hthick, hdim.y, hdim.z), ChVector<>( hdim.x+hthick, 0, hdim.z));
AddBoxGeometry(body, ChVector<>(hdim.x, hthick, hdim.z), ChVector<>(0, -hdim.y-hthick, hdim.z));
AddBoxGeometry(body, ChVector<>(hdim.x, hthick, hdim.z), ChVector<>(0, hdim.y+hthick, hdim.z));
body->GetCollisionModel()->BuildModel();
// Attach the body to the system.
system->AddBody(ChSharedPtr<ChBody>(body));
}
int GetNumParticlesAboveHeight(ChSystemParallel* sys, double value) {
int count = 0;
for (int i = 0; i < sys->GetNumBodies(); ++i) {
ChBody* body = (ChBody*)sys->Get_bodylist()->at(i);
if (body->GetIdentifier() > 0 && body->GetPos().z > value)
count++;
}
return count;
}
// -----------------------------------------------------------------------------
// Find and return the body with specified identifier.
// -----------------------------------------------------------------------------
ChBody* FindBodyById(ChSystemParallel* sys, int id) {
for (int i = 0; i < sys->GetNumBodies(); ++i) {
ChBody* body = (ChBody*)sys->Get_bodylist()->at(i);
if (body->GetIdentifier() == id)
return body;
}
return NULL;
}
// -----------------------------------------------------------------------------
// Return true if all bodies in the granular mix have a linear velocity whose
// magnitude is below the specified value.
// -----------------------------------------------------------------------------
bool CheckSettled(ChSystemParallel* sys, double threshold) {
double t2 = threshold * threshold;
for (int i = 0; i < sys->GetNumBodies(); ++i) {
ChBody* body = (ChBody*)sys->Get_bodylist()->at(i);
if (body->GetIdentifier() > 0) {
double vel2 = body->GetPos_dt().Length2();
if (vel2 > t2)
return false;
}
}
return true;
}
void FindHeightRange(ChSystemParallel* sys, double& lowest, double& highest) {
highest = -1000;
lowest = 1000;
for (int i = 0; i < sys->Get_bodylist()->size(); ++i) {
ChBody* body = (ChBody*)sys->Get_bodylist()->at(i);
if (body->GetIdentifier() <= 0)
continue;
if (fabs(body->GetPos().x) <= hdimX_p && fabs(body->GetPos().y) <= hdimY_p) {
double h = body->GetPos().z;
if (h < lowest)
lowest = h;
else if (h > highest)
highest = h;
}
}
}
void setBulkDensity(ChSystem* sys, double bulkDensity) {
double vol_g = (4.0 / 3) * CH_C_PI * r_g * r_g * r_g;
double normalPlateHeight = sys->Get_bodylist()->at(1)->GetPos().z - hdimZ;
double bottomHeight = 0;
int numBodies = sys->Get_bodylist()->size();
double boxVolume = hdimX * 2 * hdimX * 2 * (normalPlateHeight - bottomHeight);
double granularVolume = (numBodies - 3) * vol_g;
double reqDensity = bulkDensity * boxVolume / granularVolume;
for (int i = 0; i < sys->Get_bodylist()->size(); ++i) {
ChBody* body = (ChBody*)sys->Get_bodylist()->at(i);
if (body->GetIdentifier() > 1) {
body->SetMass(reqDensity * vol_g);
}
}
cout << "N Bodies: " << numBodies << endl;
cout << "Box Volume: " << boxVolume << endl;
cout << "Granular Volume: " << granularVolume << endl;
cout << "Desired bulk density = " << bulkDensity << ", Required Body Density = " << reqDensity << endl;
}
// -----------------------------------------------------------------------------
// ReadCheckpoint
//
//
// -----------------------------------------------------------------------------
void ReadCheckpoint(ChSystem* system,
const std::string& filename)
{
// Open input file stream
std::ifstream ifile(filename.c_str());
std::string line;
while (std::getline(ifile, line)) {
std::istringstream iss1(line);
// Read body type, Id, flags
int btype, bid, bfixed, bcollide;
iss1 >> btype >> bid >> bfixed >> bcollide;
// Read body mass and inertia
double mass;
ChVector<> inertiaXX;
iss1 >> mass >> inertiaXX.x >> inertiaXX.y >> inertiaXX.z;
// Read body position, orientation, and their time derivatives
ChVector<> bpos, bpos_dt;
ChQuaternion<> brot, brot_dt;
iss1 >> bpos.x >> bpos.y >> bpos.z >> brot.e0 >> brot.e1 >> brot.e2 >> brot.e3;
iss1 >> bpos_dt.x >> bpos_dt.y >> bpos_dt.z >> brot_dt.e0 >> brot_dt.e1 >> brot_dt.e2 >> brot_dt.e3;
// Get the next line in the file (material properties)
std::getline(ifile, line);
std::istringstream iss2(line);
// Create a body of the appropriate type, read and apply material properties
ChBody* body;
if (btype == 0) {
body = new ChBody(ChBody::DVI);
ChSharedPtr<ChMaterialSurface> mat = body->GetMaterialSurface();
iss2 >> mat->static_friction >> mat->sliding_friction >> mat->rolling_friction >> mat->spinning_friction;
iss2 >> mat->restitution >> mat->cohesion >> mat->dampingf;
iss2 >> mat->compliance >> mat->complianceT >> mat->complianceRoll >> mat->complianceSpin;
} else {
// -----------------------------------------------------------------------------
int main(int argc, char* argv[]) {
// 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
// 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()->perform_thread_tuning = thread_tuning;
// Set gravitational acceleration
msystem->Set_G_acc(ChVector<>(0, 0, -gravity));
// Edit system settings
msystem->GetSettings()->solver.max_iteration_bilateral = max_iteration_bilateral;
msystem->GetSettings()->solver.tolerance = tolerance;
msystem->GetSettings()->solver.use_full_inertia_tensor = false;
#ifdef USE_DEM
msystem->GetSettings()->collision.narrowphase_algorithm = NARROWPHASE_R;
#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->ChangeSolverType(APGDREF);
msystem->GetSettings()->collision.collision_envelope = 0.05 * r_g;
#endif
msystem->GetSettings()->collision.bins_per_axis = I3(10, 10, 10);
// Set simulation duration and create bodies (depending on problem type).
double time_end;
int out_fps;
ChBody* insert;
switch (problem) {
case SETTLING:
time_end = time_settling_max;
out_fps = out_fps_settling;
insert = CreateMechanism(msystem);
CreateParticles(msystem);
break;
case DROPPING:
time_end = time_dropping_max;
out_fps = out_fps_dropping;
utils::ReadCheckpoint(msystem, checkpoint_file);
insert = FindBodyById(msystem, 0);
break;
}
// Number of steps
int num_steps = std::ceil(time_end / time_step);
int out_steps = std::ceil((1.0 / time_step) / out_fps);
// Zero velocity level for settling check
double zero_v = 2 * r_g;
// 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;
}
// Perform the simulation
double time = 0;
int sim_frame = 0;
int out_frame = 0;
int next_out_frame = 0;
double exec_time = 0;
int num_contacts = 0;
ChStreamOutAsciiFile sfile(stats_file.c_str());
ChStreamOutAsciiFile ffile(flow_file.c_str());
#ifdef CHRONO_PARALLEL_HAS_OPENGL
opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
gl_window.Initialize(1280, 720, "Mass Flow Test", msystem);
gl_window.SetCamera(ChVector<>(0, -width, height), ChVector<>(0, 0, height), ChVector<>(0, 0, 1));
gl_window.SetRenderMode(opengl::WIREFRAME);
#endif
while (time < time_end) {
// Output data
if (sim_frame == next_out_frame) {
char filename[100];
sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), out_frame + 1);
utils::WriteShapesPovray(msystem, filename);
cout << "------------ Output frame: " << out_frame << endl;
cout << " Sim frame: " << sim_frame << endl;
cout << " Time: " << time << endl;
cout << " Avg. contacts: " << num_contacts / out_steps << endl;
cout << " Execution time: " << exec_time << endl;
double opening = insert->GetPos().x + 0.5 * height + delta;
int count = GetNumParticlesBelowHeight(msystem, 0);
cout << " Gap: " << -opening << endl;
cout << " Flow: " << count << endl;
sfile << time << " " << exec_time << " " << num_contacts / out_steps << "\n";
sfile.GetFstream().sync();
switch (problem) {
case SETTLING:
// Create a checkpoint from the current state.
utils::WriteCheckpoint(msystem, checkpoint_file);
cout << " Checkpoint: " << msystem->Get_bodylist()->size() << " bodies" << endl;
break;
case DROPPING:
// Save current gap opening and number of dropped particles.
ffile << time << " " << -opening << " " << count << "\n";
ffile.GetFstream().sync();
break;
}
out_frame++;
next_out_frame += out_steps;
num_contacts = 0;
}
// Check for early termination of settling phase.
if (problem == SETTLING && time > time_settling_min && CheckSettled(msystem, zero_v)) {
cout << "Granular material settled... time = " << time << endl;
break;
}
// Check for early termination of dropping phase.
if (problem == DROPPING && time > time_opening && GetNumParticlesAboveHeight(msystem, -pos_collector / 2) == 0) {
cout << "Granular material exhausted... time = " << time << endl;
break;
}
// Advance system state by one step.
// 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
// Open the gate until it reaches the specified gap distance.
if (problem == DROPPING && time <= time_opening) {
insert->SetPos(ChVector<>(-0.5 * height - delta - time * speed, 0, 0.5 * height - delta));
insert->SetPos_dt(ChVector<>(-speed, 0, 0));
}
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();
}
// Create a checkpoint from the last state
if (problem == SETTLING) {
cout << "Write checkpoint data to " << checkpoint_file;
utils::WriteCheckpoint(msystem, checkpoint_file);
cout << " done. Wrote " << msystem->Get_bodylist()->size() << " bodies." << endl;
}
// Final stats
cout << "==================================" << endl;
cout << "Number of bodies: " << msystem->GetNumBodies() << endl;
cout << "Simulation time: " << exec_time << endl;
cout << "Number of threads: " << threads << endl;
return 0;
}
int main(int argc, char* argv[])
{
// The DLL_CreateGlobals() - DLL_DeleteGlobals(); pair is needed if
// global functions are needed.
DLL_CreateGlobals();
{
// The physical system: it contains all physical objects.
ChSystem my_system;
ChCollisionModel::SetDefaultSuggestedEnvelope(0.01);
// Create rigid bodies
GetLog() << "Creating Bodies...";
// create pointers to bodies
ChSharedBodyPtr floor(new ChBody);
ChSharedBodyPtr wallL(new ChBody);
ChSharedBodyPtr wallR(new ChBody);
ChSharedBodyPtr wallF(new ChBody);
ChSharedBodyPtr wallB(new ChBody);
ChSharedBodyPtr chassis(new ChBody);
ChSharedBodyPtr axle_F(new ChBody);
ChSharedBodyPtr axle_C(new ChBody);
ChSharedBodyPtr axle_R(new ChBody);
ChSharedBodyPtr leg_FR(new ChBody);
ChSharedBodyPtr leg_FL(new ChBody);
ChSharedBodyPtr leg_CR(new ChBody);
ChSharedBodyPtr leg_CL(new ChBody);
ChSharedBodyPtr leg_RR(new ChBody);
ChSharedBodyPtr leg_RL(new ChBody);
ChSharedBodyPtr foot_FR(new ChBody);
ChSharedBodyPtr foot_FL(new ChBody);
ChSharedBodyPtr foot_CR(new ChBody);
ChSharedBodyPtr foot_CL(new ChBody);
ChSharedBodyPtr foot_RR(new ChBody);
ChSharedBodyPtr foot_RL(new ChBody);
// Set initial position of the bodies (center of mass)
floor->SetPos(ChVector<>(0, -3-boxDrop, boxL/2.0-(chassisL/2.0)-2.0) );
wallL->SetPos(ChVector<>(-boxW/2.0-0.5, boxH/2.0-boxDrop, boxL/2.0-(chassisL/2.0)-2.0) );
wallR->SetPos(ChVector<>( boxW/2.0+0.5, boxH/2.0-boxDrop, boxL/2.0-(chassisL/2.0)-2.0) );
wallF->SetPos(ChVector<>(0, boxH/2.0-boxDrop, boxL-chassisL/2.0-2.0+0.5) );
wallB->SetPos(ChVector<>(0, boxH/2.0-boxDrop, -chassisL/2.0-2.0-0.5) );
floor->SetBodyFixed(true);
wallL->SetBodyFixed(true);
wallR->SetBodyFixed(true);
wallF->SetBodyFixed(true);
wallB->SetBodyFixed(true);
chassis->SetPos(VNULL);
axle_F->SetPos(ChVector<>(0,0,chassisL/2));
axle_C->SetPos(ChVector<>(0,0,0));
axle_R->SetPos(ChVector<>(0,0,-chassisL/2));
leg_FR->SetPos(ChVector<>( (axleL+legW)/2.0, -legL/2.0, chassisL/2));
leg_FL->SetPos(ChVector<>(-(axleL+legW)/2.0, legL/2.0, chassisL/2));
leg_CR->SetPos(ChVector<>(-(axleL+legW)/2.0, -legL/2.0, 0));
leg_CL->SetPos(ChVector<>( (axleL+legW)/2.0, legL/2.0, 0));
leg_RR->SetPos(ChVector<>( (axleL+legW)/2.0, -legL/2.0, -chassisL/2));
leg_RL->SetPos(ChVector<>(-(axleL+legW)/2.0, legL/2.0, -chassisL/2));
foot_FR->SetPos(ChVector<>( (axleL+footW)/2.0, -(legL-footH/2.0), chassisL/2+1));
foot_FL->SetPos(ChVector<>(-(axleL+footW)/2.0, (legL-footH/2.0), chassisL/2-1));
foot_CR->SetPos(ChVector<>(-(axleL+footW)/2.0, -(legL-footH/2.0), 1));
foot_CL->SetPos(ChVector<>( (axleL+footW)/2.0, (legL-footH/2.0), -1));
foot_RR->SetPos(ChVector<>( (axleL+footW)/2.0, -(legL-footH/2.0), -chassisL/2+1));
foot_RL->SetPos(ChVector<>(-(axleL+footW)/2.0, (legL-footH/2.0), -chassisL/2-1));
// Set masses
chassis->SetMass(4.0);
axle_F->SetMass(0.1);
axle_C->SetMass(0.1);
axle_R->SetMass(0.1);
leg_FR->SetMass(0.1);
leg_FL->SetMass(0.1);
leg_CR->SetMass(0.1);
leg_CL->SetMass(0.1);
leg_RR->SetMass(0.1);
leg_RL->SetMass(0.1);
foot_FR->SetMass(0.1);
foot_FL->SetMass(0.1);
foot_CR->SetMass(0.1);
foot_CL->SetMass(0.1);
foot_RR->SetMass(0.1);
foot_RL->SetMass(0.1);
//
// Create collision models
//
// box
floor->GetCollisionModel()->AddBox(50,3/2.0,200, &VNULL);
floor->SetCollide(true);
wallL->GetCollisionModel()->AddBox(1, boxH, boxL);
wallR->GetCollisionModel()->AddBox(1, boxH, boxL);
wallF->GetCollisionModel()->AddBox(boxW, boxH, 1);
wallB->GetCollisionModel()->AddBox(boxW, boxH, 1);
wallL->SetCollide(true);
wallR->SetCollide(true);
wallF->SetCollide(true);
wallB->SetCollide(true);
/*
floor->GetCollisionModel()->SetFamily(0);
wallL->GetCollisionModel()->SetFamily(0);
wallR->GetCollisionModel()->SetFamily(0);
wallF->GetCollisionModel()->SetFamily(0);
wallB->GetCollisionModel()->SetFamily(0);
floor->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(0);
wallL->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(0);
wallR->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(0);
wallF->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(0);
wallB->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(0);
*/
chassis->GetCollisionModel()->AddBox(chassisW/2.0,1.0/2.0,chassisL/2.0, &VNULL);
chassis->SetCollide(true);
chassis->GetCollisionModel()->SetFamily(1);
// Do not collide axles with chassis
chassis->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(2);
axle_F->GetCollisionModel()->AddCylinder(0.5, axleL, 0.5, &VNULL, &ChMatrix33<double>(Q_from_AngZ(CH_C_PI/2.0)) );
axle_C->GetCollisionModel()->AddCylinder(0.5, axleL, 0.5, &VNULL, &ChMatrix33<double>(Q_from_AngZ(CH_C_PI/2.0)) );
axle_R->GetCollisionModel()->AddCylinder(0.5, axleL, 0.5, &VNULL, &ChMatrix33<double>(Q_from_AngZ(CH_C_PI/2.0)) );
axle_F->SetCollide(true);
axle_C->SetCollide(true);
axle_R->SetCollide(true);
// Chassis will not collide with the family of axles
axle_F->GetCollisionModel()->SetFamily(2);
axle_C->GetCollisionModel()->SetFamily(2);
axle_R->GetCollisionModel()->SetFamily(2);
leg_FR->GetCollisionModel()->AddBox(legW/2.0, legL/2.0, 0.5/2.0, &VNULL);
leg_FL->GetCollisionModel()->AddBox(legW/2.0, legL/2.0, 0.5/2.0, &VNULL);
leg_CR->GetCollisionModel()->AddBox(legW/2.0, legL/2.0, 0.5/2.0, &VNULL);
leg_CL->GetCollisionModel()->AddBox(legW/2.0, legL/2.0, 0.5/2.0, &VNULL);
leg_RR->GetCollisionModel()->AddBox(legW/2.0, legL/2.0, 0.5/2.0, &VNULL);
leg_RL->GetCollisionModel()->AddBox(legW/2.0, legL/2.0, 0.5/2.0, &VNULL);
leg_FR->SetCollide(true);
leg_FL->SetCollide(true);
leg_CR->SetCollide(true);
leg_CL->SetCollide(true);
leg_RR->SetCollide(true);
leg_RL->SetCollide(true);
foot_FR->GetCollisionModel()->AddBox(footW/2.0, footH/2.0, footL/2.0, &VNULL);
foot_FL->GetCollisionModel()->AddBox(footW/2.0, footH/2.0, footL/2.0, &VNULL);
foot_CR->GetCollisionModel()->AddBox(footW/2.0, footH/2.0, footL/2.0, &VNULL);
foot_CL->GetCollisionModel()->AddBox(footW/2.0, footH/2.0, footL/2.0, &VNULL);
foot_RR->GetCollisionModel()->AddBox(footW/2.0, footH/2.0, footL/2.0, &VNULL);
foot_RL->GetCollisionModel()->AddBox(footW/2.0, footH/2.0, footL/2.0, &VNULL);
foot_FR->SetCollide(true);
foot_FL->SetCollide(true);
foot_CR->SetCollide(true);
foot_CL->SetCollide(true);
foot_RR->SetCollide(true);
foot_RL->SetCollide(true);
// Add bodies to system
my_system.AddBody(floor);
my_system.AddBody(wallR);
my_system.AddBody(wallL);
my_system.AddBody(wallF);
my_system.AddBody(wallB);
my_system.AddBody(chassis);
my_system.AddBody(axle_F);
my_system.AddBody(axle_C);
my_system.AddBody(axle_R);
my_system.AddBody(leg_FR);
my_system.AddBody(leg_FL);
my_system.AddBody(leg_CR);
my_system.AddBody(leg_CL);
my_system.AddBody(leg_RR);
my_system.AddBody(leg_RL);
my_system.AddBody(foot_FR);
my_system.AddBody(foot_FL);
my_system.AddBody(foot_CR);
my_system.AddBody(foot_CL);
my_system.AddBody(foot_RR);
my_system.AddBody(foot_RL);
// Create lattice of spheres as gruanular terrain
createTerrain(my_system, particleDiameter, boxW, boxH, boxL);
GetLog() << "DONE\n";
// Create links between bodies
GetLog() << "Creating Links...";
// attach feet to legs
ChSharedPtr<ChLinkLockLock> link_FR(new ChLinkLockLock);
link_FR->Initialize(foot_FR, leg_FR, ChCoordsys<>(VNULL));
my_system.AddLink(link_FR);
ChSharedPtr<ChLinkLockLock> link_FL(new ChLinkLockLock);
link_FL->Initialize(foot_FL, leg_FL, ChCoordsys<>(VNULL));
my_system.AddLink(link_FL);
ChSharedPtr<ChLinkLockLock> link_CR(new ChLinkLockLock);
link_CR->Initialize(foot_CR, leg_CR, ChCoordsys<>(VNULL));
my_system.AddLink(link_CR);
ChSharedPtr<ChLinkLockLock> link_CL(new ChLinkLockLock);
link_CL->Initialize(foot_CL, leg_CL, ChCoordsys<>(VNULL));
my_system.AddLink(link_CL);
ChSharedPtr<ChLinkLockLock> link_RR(new ChLinkLockLock);
link_RR->Initialize(foot_RR, leg_RR, ChCoordsys<>(VNULL));
my_system.AddLink(link_RR);
ChSharedPtr<ChLinkLockLock> link_RL(new ChLinkLockLock);
link_RL->Initialize(foot_RL, leg_RL, ChCoordsys<>(VNULL));
my_system.AddLink(link_RL);
// attach legs to axles
ChSharedPtr<ChLinkLockLock> axle_FR(new ChLinkLockLock);
axle_FR->Initialize(leg_FR, axle_F, ChCoordsys<>(VNULL));
my_system.AddLink(axle_FR);
ChSharedPtr<ChLinkLockLock> axle_FL(new ChLinkLockLock);
axle_FL->Initialize(leg_FL, axle_F, ChCoordsys<>(VNULL));
my_system.AddLink(axle_FL);
ChSharedPtr<ChLinkLockLock> axle_CR(new ChLinkLockLock);
axle_CR->Initialize(leg_CR, axle_C, ChCoordsys<>(VNULL));
my_system.AddLink(axle_CR);
ChSharedPtr<ChLinkLockLock> axle_CL(new ChLinkLockLock);
axle_CL->Initialize(leg_CL, axle_C, ChCoordsys<>(VNULL));
my_system.AddLink(axle_CL);
ChSharedPtr<ChLinkLockLock> axle_RR(new ChLinkLockLock);
axle_RR->Initialize(leg_RR, axle_R, ChCoordsys<>(VNULL));
my_system.AddLink(axle_RR);
ChSharedPtr<ChLinkLockLock> axle_RL(new ChLinkLockLock);
axle_RL->Initialize(leg_RL, axle_R, ChCoordsys<>(VNULL));
my_system.AddLink(axle_RL);
GetLog() << "DONE\n";
// Create engine between axles and chassis
GetLog() << "Creating Motors...";
ChSharedPtr<ChLinkEngine> eng_F(new ChLinkEngine);
eng_F->Initialize(axle_F, chassis,
ChCoordsys<>(ChVector<>(0, 0, chassisL/2) , Q_from_AngY(CH_C_PI/2) ) );
eng_F->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK); // also works as revolute support
eng_F->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(eng_F->Get_spe_funct()))
mfun->Set_yconst(speed); // rad/s angular speed
my_system.AddLink(eng_F);
ChSharedPtr<ChLinkEngine> eng_C(new ChLinkEngine);
eng_C->Initialize(axle_C, chassis,
ChCoordsys<>(ChVector<>(0, 0, 0) , Q_from_AngY(CH_C_PI/2) ) );
eng_C->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK); // also works as revolute support
eng_C->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(eng_C->Get_spe_funct()))
mfun->Set_yconst(speed); // rad/s angular speed
my_system.AddLink(eng_C);
ChSharedPtr<ChLinkEngine> eng_R(new ChLinkEngine);
eng_R->Initialize(axle_R, chassis,
ChCoordsys<>(ChVector<>(0, 0, -chassisL/2) , Q_from_AngY(CH_C_PI/2) ) );
eng_R->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK); // also works as revolute support
eng_R->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(eng_R->Get_spe_funct()))
mfun->Set_yconst(speed); // rad/s angular speed
my_system.AddLink(eng_R);
GetLog() << "DONE\n";
/*
// View system hierarchy and constraints
GetLog() << "\n\nSystem Hierarchy:\n";
my_system.ShowHierarchy(GetLog());
GetLog() << "\n\nConstraints:\n";
ChSystem::IteratorLinks myiter = my_system.IterBeginLinks();
while (myiter != my_system.IterEndLinks())
{
GetLog() << " Link class: " << (*myiter)->GetRTTI()->GetName() << " , leaves n.DOFs: " << (*myiter)->GetLeftDOF() << "\n";
++myiter;
}
*/
// Prepare the physical system for the simulation
my_system.SetIntegrationType(ChSystem::INT_ANITESCU);
my_system.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR_MULTITHREAD);
my_system.SetMaxiter(50);
my_system.SetIterLCPmaxItersSpeed(50);
// OK! NOW GET READY FOR THE DYNAMICAL SIMULATION!
GetLog() << "Running Simulation...\n";
// A very simple simulation loop..
double simTime(0);
int currFrame(0);
while (simTime < simDuration)
{
simTime += timestep;
// PERFORM SIMULATION UP TO simTime
my_system.DoFrameDynamics(simTime);
if (currFrame%everyFrame == 0 )
{
// Output data to files
char padnumber[256];
sprintf(padnumber, "%d", (currFrame/everyFrame + 10000));
char filename[256];
// filepath (/walkerdata) must exist before executing
sprintf(filename, "%s/pos%s.txt", "walkerdata", padnumber + 1);
// create output file
ChStreamOutAsciiFile output(filename);
// i=5 => skip floor and walls
for (int i = 5; i < my_system.Get_bodylist()->size(); i++) {
ChBody* abody = (ChBody*) my_system.Get_bodylist()->at(i);
ChVector<> bodPos = abody->GetPos();
ChQuaternion<> bodRot = abody->GetRot();
output << (float)bodPos.x << ",";
output << (float)bodPos.y << ",";
output << (float)bodPos.z << ",";
output << (float)bodRot.e0 << ",";
output << (float)bodRot.e1 << ",";
output << (float)bodRot.e2 << ",";
output << (float)bodRot.e3 << ",\n";
}
}
currFrame++;
GetLog() << "Completed Step " << currFrame << "\n";
}
GetLog() << "DONE\n";
}
// Remember this at the end of the program, if you started
// with DLL_CreateGlobals();
DLL_DeleteGlobals();
return 0;
}
