int main(int argc, char* argv[]) {
save = atoi(argv[1]);
cout << save << endl;
if (save) {
seconds_to_simulate = 5;
num_steps = seconds_to_simulate / timestep;
} else {
seconds_to_simulate = 300;
num_steps = seconds_to_simulate / timestep;
}
if (argc > 2) {
particle_slide_friction = atof(argv[2]);
particle_roll_friction = atof(argv[3]);
particle_cohesion = atof(argv[4]);
data_folder = argv[5];
cout << particle_slide_friction << " " << particle_roll_friction << " " << particle_roll_friction << endl;
}
//=========================================================================================================
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
//=========================================================================================================
//system_gpu->SetMinThreads(32);
//system_gpu->SetMaxiter(max_iter);
//system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChSystemParallelDVI*) system_gpu)->DoThreadTuning(true);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationBilateral(0);
system_gpu->SetTol(tolerance);
system_gpu->SetTolSpeeds(tolerance);
system_gpu->SetMaxPenetrationRecoverySpeed(1e9);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance(0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(100);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
//((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->DoStabilization(true);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .05);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(30, 60, 30));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(100, 50);
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
((ChSystemParallel*) system_gpu)->SetAABB(R3(-250, -600, -250), R3(250, 600, 250));
//=========================================================================================================
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(container_friction);
material->SetRollingFriction(container_friction);
material->SetSpinningFriction(container_friction);
material->SetCompliance(0);
material->SetCohesion(-100);
CONTAINER = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(CONTAINER, 100000, Vector(0, 0, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
if (save) {
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_thickness, container_size.y, container_size.z),
Vector(-container_size.x + container_thickness, container_height - container_thickness, 0),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_thickness, container_size.y, container_size.z),
Vector(container_size.x - container_thickness, container_height - container_thickness, 0),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_size.x, container_size.y, container_thickness),
Vector(0, container_height - container_thickness, -container_size.z + container_thickness),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_size.x, container_size.y, container_thickness),
Vector(0, container_height - container_thickness, container_size.z - container_thickness),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_size.x, container_thickness, container_size.z),
Vector(0, container_height - container_size.y, 0),
Quaternion(1, 0, 0, 0));
//AddCollisionGeometry(CONTAINER, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, container_height + container_size.y, 0), Quaternion(1, 0, 0, 0));
}
CONTAINER->GetMaterialSurface()->SetCohesion(container_cohesion);
FinalizeObject(CONTAINER, (ChSystemParallel *) system_gpu);
if (save == false) {
real anchor_length = 100;
real anchor_r = 35 / 2.0;
real anchor_R = 150 / 4.0;
real anchor_h = 50;
real anchor_thickness = 6 / 2.0;
real anchor_blade_width = 8;
ChVector<> p1(0, 0, 0);
ChVector<> p2(0, anchor_length, 0);
real anchor_mass = 6208;
real number_sections = 30;
REFERENCE = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(REFERENCE, anchor_mass, Vector(0, 200, 0), Quaternion(1, 0, 0, 0), material, false, false, -15, -15);
// AddCollisionGeometry(REFERENCE, SPHERE, ChVector<>(anchor_r, 0, 0), p1, Quaternion(1, 0, 0, 0));
// AddCollisionGeometry(REFERENCE, CYLINDER, Vector(anchor_r, anchor_length, anchor_r), p2, Quaternion(1, 0, 0, 0));
// for (int i = 0; i < number_sections; i++) {
// ChQuaternion<> quat, quat2;
// quat.Q_from_AngAxis(i / number_sections * 2 * PI, ChVector<>(0, 1, 0));
// quat2.Q_from_AngAxis(6.5 * 2 * PI / 360.0, ChVector<>(0, 0, 1));
// quat = quat % quat2;
// ChVector<> pos(sin(i / number_sections * 2 * PI) * anchor_R, i / number_sections * anchor_h, cos(i / number_sections * 2 * PI) * anchor_R);
// //ChMatrix33<> mat(quat);
// AddCollisionGeometry(REFERENCE, BOX, ChVector<>(anchor_blade_width, anchor_thickness, anchor_R), pos, quat);
// }
ANCHOR = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(ANCHOR, anchor_mass, Vector(0, 200, 0), Quaternion(1, 0, 0, 0), material, true, false, -15, -15);
AddCollisionGeometry(ANCHOR, SPHERE, ChVector<>(anchor_r, 0, 0), p1, Quaternion(1, 0, 0, 0));
AddCollisionGeometry(ANCHOR, CYLINDER, Vector(anchor_r, anchor_length, anchor_r), p2, Quaternion(1, 0, 0, 0));
//
for (int i = 0; i < number_sections; i++) {
ChQuaternion<> quat, quat2;
quat.Q_from_AngAxis(i / number_sections * 2 * PI, ChVector<>(0, 1, 0));
quat2.Q_from_AngAxis(6.5 * 2 * PI / 360.0, ChVector<>(0, 0, 1));
quat = quat % quat2;
ChVector<> pos(sin(i / number_sections * 2 * PI) * anchor_R, i / number_sections * anchor_h, cos(i / number_sections * 2 * PI) * anchor_R);
//ChMatrix33<> mat(quat);
AddCollisionGeometry(ANCHOR, BOX, ChVector<>(anchor_blade_width, anchor_thickness, anchor_R), pos, quat);
}
FinalizeObject(ANCHOR, (ChSystemParallel *) system_gpu);
FinalizeObject(REFERENCE, (ChSystemParallel *) system_gpu);
// real vol = ((ChCollisionModelParallel *) ANCHOR->GetCollisionModel())->getVolume();
// real den = .00785;
// anchor_mass = den*vol;
// cout<<vol<<" "<<anchor_mass<<endl;
// ANCHOR->SetMass(anchor_mass);
ANCHOR->SetInertiaXX(ChVector<>(12668786.72, 5637598.31, 12682519.69));
REFERENCE->SetInertiaXX(ChVector<>(12668786.72, 5637598.31, 12682519.69));
// ANCHOR->SetInertiaXX(
// ChVector<>(
// 1 / 12.0 * anchor_mass * (1 * 1 + anchor_R * anchor_R),
// 1 / 12.0 * anchor_mass * (anchor_R * anchor_R + anchor_R * anchor_R),
// 1 / 12.0 * anchor_mass * (anchor_R * anchor_R + 1 * 1)));
//BLOCK = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
//InitObject(BLOCK, anchor_mass/2, Vector(0, 300, 0), Quaternion(1, 0, 0, 0), material, false, false, -20, -20);
//AddCollisionGeometry(BLOCK, BOX, ChVector<>(1, 1, 1), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
//FinalizeObject(BLOCK, (ChSystemParallel *) system_gpu);
//actuator_anchor = ChSharedPtr<ChLinkLockLock>(new ChLinkLockLock());
//actuator_anchor->Initialize(CONTAINER, BLOCK, ChCoordsys<>(ChVector<>(0, 0, 0), QUNIT));
//system_gpu->AddLink(actuator_anchor);
// apply motion
//motionFunc1 = new ChFunction_Ramp(0, -anchor_vel);
//actuator_anchor->SetMotion_Y(motionFunc1);
//actuator_anchor->SetMotion_axis(ChVector<>(0, 1, 0));
engine_anchor = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
engine_anchor->Initialize(CONTAINER, ANCHOR, ChCoordsys<>(ANCHOR->GetPos(), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_X)));
engine_anchor->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_PRISM); // also works as revolute support
engine_anchor->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
system_gpu->AddLink(engine_anchor);
reference_engine = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
reference_engine->Initialize(CONTAINER, REFERENCE, ChCoordsys<>(REFERENCE->GetPos(), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_X)));
reference_engine->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_PRISM); // also works as revolute support
reference_engine->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
system_gpu->AddLink(reference_engine);
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(engine_anchor->Get_spe_funct())) {
mfun->Set_yconst(anchor_rot * 1 / 60.0 * 2 * CH_C_PI); // rad/s angular speed
}
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(reference_engine->Get_spe_funct())) {
mfun->Set_yconst(anchor_rot * 1 / 60.0 * 2 * CH_C_PI); // rad/s angular speed
}
ReadAllObjectsWithGeometryChrono(system_gpu, "data/anchor/anchor.dat");
ChSharedPtr<ChMaterialSurface> material_read;
material_read = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material_read->SetFriction(particle_slide_friction);
material_read->SetRollingFriction(particle_roll_friction);
material_read->SetSpinningFriction(particle_roll_friction);
material_read->SetCompliance(0);
material_read->SetCohesion(particle_cohesion * timestep);
for (int i = 0; i < system_gpu->Get_bodylist()->size(); i++) {
system_gpu->Get_bodylist()->at(i)->SetMaterialSurface(material_read);
}
} else {
layer_gen = new ParticleGenerator<ChSystemParallelDVI>((ChSystemParallelDVI *) system_gpu);
layer_gen->SetDensity(particle_density);
layer_gen->SetRadius(R3(particle_radius, particle_radius * .5, particle_radius));
layer_gen->SetNormalDistribution(particle_radius, particle_std_dev, 1);
layer_gen->material->SetFriction(particle_slide_friction);
layer_gen->material->SetCohesion(particle_cohesion);
layer_gen->material->SetRollingFriction(0);
layer_gen->material->SetSpinningFriction(0);
layer_gen->AddMixtureType(MIX_SPHERE);
}
//layer_gen->AddMixtureType(MIX_ELLIPSOID);
//layer_gen->AddMixtureType(MIX_DOUBLESPHERE);
//layer_gen->addPerturbedVolumeMixture(R3(0, 0, 0), I3(64, 1, 64), R3(0, 0, 0), R3(0, 0, 0));
//=========================================================================================================
//Rendering specific stuff:
// ChOpenGLManager * window_manager = new ChOpenGLManager();
// ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
// openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
// openGLView.render_camera->camera_look_at = glm::vec3(0, 0, 0);
// openGLView.render_camera->camera_scale = 2;
// openGLView.SetCustomCallback(RunTimeStep);
// openGLView.StartSpinning(window_manager);
// window_manager->CallGlutMainLoop();
//=========================================================================================================
int file = 0;
ofstream reactionfile;
if (save == false) {
stringstream ss;
ss << data_folder << "reactions.txt";
reactionfile.open(ss.str());
}
for (int i = 0; i < num_steps; i++) {
system_gpu->DoStepDynamics(timestep);
double TIME = system_gpu->GetChTime();
double STEP = system_gpu->GetTimerStep();
double BROD = system_gpu->GetTimerCollisionBroad();
double NARR = system_gpu->GetTimerCollisionNarrow();
double LCP = system_gpu->GetTimerLcp();
double SHUR = ((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->solver.time_shurcompliment;
double UPDT = system_gpu->GetTimerUpdate();
double RESID = ((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->GetResidual();
int BODS = system_gpu->GetNbodies();
int CNTC = system_gpu->GetNcontacts();
int REQ_ITS = ((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->GetTotalIterations();
printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d|%7.4f\n", TIME, STEP, BROD, NARR, LCP, SHUR, UPDT, BODS, CNTC, REQ_ITS, RESID);
system_gpu->gpu_data_manager->system_timer.PrintReport();
int save_every = 1.0 / timestep / 60.0; //save data every n steps
if (i % save_every == 0) {
stringstream ss;
cout << "Frame: " << file << endl;
ss << data_folder << "/" << file << ".txt";
if (save) {
DumpAllObjectsWithGeometryChrono(system_gpu, "data/anchor/anchor.dat");
} else {
DumpAllObjectsWithGeometryPovray(system_gpu, ss.str());
}
file++;
}
if (save == false) {
Vector force = engine_anchor->Get_react_force();
Vector torque = engine_anchor->Get_react_torque();
double motor_torque = engine_anchor->Get_mot_torque();
reactionfile << force.x << " " << force.y << " " << force.z << " " << torque.x << " " << torque.y << " " << torque.z << " " << motor_torque << " " << ANCHOR->GetPos().y
<< " " << ANCHOR->GetPos_dt().y << endl;
}
RunTimeStep(system_gpu, i);
}
if (save == false) {
reactionfile.close();
}
}
// Build and initialize the tank, creating all bodies corresponding to
// the various parts and adding them to the physical system - also creating
// and adding constraints to the system.
MySimpleTank(ChSystem& my_system, ///< the chrono::engine physical system
ISceneManager* msceneManager, ///< the Irrlicht scene manager for 3d shapes
IVideoDriver* mdriver ///< the Irrlicht video driver
)
{
throttleL = throttleR = 0; // initially, gas throttle is 0.
max_motor_speed = 10;
double my = 0.5; // left back hub pos
double mx = 0;
double shoelength = 0.2;
double shoethickness = 0.06;
double shoewidth = 0.3;
double shoemass = 2;
double radiustrack = 0.31;
double wheeldiameter = 0.280*2;
int nwrap = 6;
int ntiles = 7;
double rlwidth = 1.20;
double passo = (ntiles+1)*shoelength;
ChVector<> cyl_displA(0, 0.075+0.02,0);
ChVector<> cyl_displB(0, -0.075-0.02,0);
double cyl_hthickness = 0.045;
// --- The tank body ---
IAnimatedMesh* bulldozer_bodyMesh = msceneManager->getMesh("../data/bulldozerB10.obj");
truss = (ChBodySceneNode*)addChBodySceneNode(
&my_system, msceneManager, bulldozer_bodyMesh,
350.0,
ChVector<>(mx + passo/2, my + radiustrack , rlwidth/2),
QUNIT);
truss->GetBody()->SetInertiaXX(ChVector<>(13.8, 13.5, 10));
truss->GetBody()->SetBodyFixed(false);
//truss->addShadowVolumeSceneNode();
// --- Right Front suspension ---
// Load a triangle mesh for wheel visualization
IAnimatedMesh* irmesh_wheel_view = msceneManager->getMesh("../data/wheel_view.obj");
// ..the tank right-front wheel
wheelRF = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx + passo, my+ radiustrack , 0),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelRF->GetBody()->GetCollisionModel()->ClearModel();
wheelRF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelRF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelRF->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelRF->GetBody()->SetCollide(true);
wheelRF->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelRF->GetBody()->SetFriction(1.0);
video::ITexture* cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRF->setMaterialTexture(0, cylinderMap);
wheelRF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the truss
link_revoluteRF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, front, upper, 1
link_revoluteRF->Initialize(wheelRF->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx + passo, my+ radiustrack , 0) , QUNIT ) );
my_system.AddLink(link_revoluteRF);
// --- Left Front suspension ---
// ..the tank left-front wheel
wheelLF = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx + passo, my+ radiustrack , rlwidth),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelLF->GetBody()->GetCollisionModel()->ClearModel();
wheelLF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelLF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelLF->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelLF->GetBody()->SetCollide(true);
wheelLF->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelLF->GetBody()->SetFriction(1.0);
wheelLF->setMaterialTexture(0, cylinderMap);
wheelLF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the truss
link_revoluteLF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // left, front, upper, 1
link_revoluteLF->Initialize(wheelLF->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx + passo, my+ radiustrack , rlwidth) , QUNIT ) );
my_system.AddLink(link_revoluteLF);
// --- Right Back suspension ---
// ..the tank right-back wheel
wheelRB = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx , my+ radiustrack , 0),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelRB->GetBody()->GetCollisionModel()->ClearModel();
wheelRB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelRB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelRB->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelRB->GetBody()->SetCollide(true);
wheelRB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelRB->GetBody()->SetFriction(1.0);
cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRB->setMaterialTexture(0, cylinderMap);
wheelRB->addShadowVolumeSceneNode();
// .. create the motor joint between the wheel and the truss (simplified motor model: just impose speed..)
link_revoluteRB = ChSharedPtr<ChLinkEngine>(new ChLinkEngine); // right, back, upper, 1
link_revoluteRB->Initialize(wheelRB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx , my+ radiustrack , 0) , QUNIT ) );
link_revoluteRB->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
my_system.AddLink(link_revoluteRB);
// --- Left Back suspension ---
// ..the tank left-back wheel
wheelLB = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx , my+ radiustrack , rlwidth),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelLB->GetBody()->GetCollisionModel()->ClearModel();
wheelLB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelLB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelLB->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelLB->GetBody()->SetCollide(true);
wheelLB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelLB->GetBody()->SetFriction(1.0);
wheelLB->setMaterialTexture(0, cylinderMap);
wheelLB->addShadowVolumeSceneNode();
// .. create the motor joint between the wheel and the truss (simplified motor model: just impose speed..)
link_revoluteLB = ChSharedPtr<ChLinkEngine>(new ChLinkEngine); // left, back, upper, 1
link_revoluteLB->Initialize(wheelLB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx , my+ radiustrack , rlwidth) , QUNIT ) );
link_revoluteLB->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
my_system.AddLink(link_revoluteLB);
//--- TRACKS ---
// Load a triangle mesh for visualization
IAnimatedMesh* irmesh_shoe_view = msceneManager->getMesh("../data/shoe_view.obj");
// Load a triangle mesh for collision
IAnimatedMesh* irmesh_shoe_collision = msceneManager->getMesh("../data/shoe_collision.obj");
ChTriangleMeshSoup temp_trianglemesh;
fillChTrimeshFromIrlichtMesh(&temp_trianglemesh, irmesh_shoe_collision->getMesh(0));
chrono::ChVector<> mesh_displacement(shoelength*0.5,0,0); // since mesh origin is not in body center of mass
chrono::ChVector<> joint_displacement(-shoelength*0.5,0,0); // position of shoe-shoe constraint, relative to COG.
chrono::ChVector<> position;
chrono::ChQuaternion<> rotation;
for (int side = 0; side <2; side ++)
{
mx = 0;
mx += shoelength;
double mz = 0;
if (side == 0)
mz = 0;
else
mz = rlwidth;
position.Set(mx, my, mz);
rotation = QUNIT;
// Create sample body (with empty collision shape; later create the collision model by adding the coll.shapes)
ChBodySceneNode* firstBodyShoe = (ChBodySceneNode*)addChBodySceneNode(
&my_system, msceneManager, 0,
shoemass,
position,
rotation);
// Creates the collision model with a (simplified) mesh
ChVector<> pin_displacement = mesh_displacement + ChVector<>(0,0.05,0);
firstBodyShoe->GetBody()->GetCollisionModel()->SetSafeMargin(0.004); // inward safe margin
firstBodyShoe->GetBody()->GetCollisionModel()->SetEnvelope(0.010); // distance of the outward "collision envelope"
firstBodyShoe->GetBody()->GetCollisionModel()->ClearModel();
// ...try to use a concave (simplified) mesh plus automatic convex decomposition?? ...
firstBodyShoe->GetBody()->GetCollisionModel()->AddTriangleMesh(temp_trianglemesh, false, false, &mesh_displacement); // not static, not convex
// .. or rather use few 'primitive' shapes to approximate with cubes/etc??
//firstBodyShoe->GetBody()->GetCollisionModel()->AddBox(shoelength/2, shoethickness/2, shoewidth/2, &mesh_displacement);
//firstBodyShoe->GetBody()->GetCollisionModel()->AddBox(0.04, 0.02, 0.02, &pin_displacement);
firstBodyShoe->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
firstBodyShoe->GetBody()->SetCollide(true);
/* // alternative: use box as a collision geometry (but wheels will slip..)
ChBodySceneNode* firstBodyShoe = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
shoemass,
position,
rotation,
ChVector<>(shoelength, 0.02, 0.2) );
*/
// Add a mesh for visualization purposes
msceneManager->addAnimatedMeshSceneNode(irmesh_shoe_view, firstBodyShoe,-1, vector3dfCH(mesh_displacement));
// Avoid creation of contacts with neighbouring shoes, using
// a collision family (=3) that does not collide with itself
firstBodyShoe->GetBody()->GetCollisionModel()->SetFamily(3);
firstBodyShoe->GetBody()->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(3);
// Other settings
firstBodyShoe->GetBody()->SetInertiaXX(ChVector<>(0.1, 0.1, 0.1));
ChBodySceneNode* previous_rigidBodyShoe = firstBodyShoe;
for (int nshoe = 1; nshoe < ntiles; nshoe++)
{
mx += shoelength;
position.Set(mx, my, mz);
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
}
for (int nshoe = 0; nshoe < nwrap; nshoe++)
{
double alpha = (CH_C_PI/((double)(nwrap-1.0)))*((double)nshoe);
double lx = mx + shoelength + radiustrack * sin(alpha);
double ly = my + radiustrack - radiustrack * cos(alpha);
position.Set(lx, ly, mz);
rotation = chrono::Q_from_AngAxis(alpha,ChVector<>(0,0,1));
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
}
for (int nshoe = (ntiles-1); nshoe >= 0; nshoe--)
{
position.Set(mx, my+2*radiustrack, mz);
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
mx -= shoelength;
}
for (int nshoe = 0; nshoe < nwrap; nshoe++)
{
double alpha = CH_C_PI + (CH_C_PI/((double)(nwrap-1.0)))*((double)nshoe);
double lx = mx + 0 + radiustrack * sin(alpha);
double ly = my + radiustrack - radiustrack * cos(alpha);
position.Set(lx, ly, mz);
rotation = chrono::Q_from_AngAxis(alpha,ChVector<>(0,0,1));
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
}
// close track
ChVector<> linkpos = firstBodyShoe->GetBody()->Point_Body2World(&joint_displacement);
ChSharedPtr<ChLinkLockRevolute> link_revolute_shoeshoe(new ChLinkLockRevolute);
link_revolute_shoeshoe->Initialize(firstBodyShoe->GetBody(), previous_rigidBodyShoe->GetBody(),
ChCoordsys<>( linkpos , QUNIT) );
my_system.AddLink(link_revolute_shoeshoe);
}
}
// Build and initialize the forklift, creating all bodies corresponding to
// the various parts and adding them to the physical system - also creating
// and adding constraints to the system.
MySimpleForklift(ChIrrAppInterface* app, ChVector<> offset = ChVector<>(0,0,0))
{
throttle = 0; // initially, gas throttle is 0.
steer = 0;
lift =0;
ChVector<> COG_truss(0, 0.4, 0.5);
ChVector<> COG_wheelRF( 0.566, 0.282, 1.608);
ChVector<> COG_wheelLF(-0.566, 0.282, 1.608);
ChVector<> COG_arm(0, 1.300, 1.855);
ChVector<> COG_fork(0, 0.362, 2.100);
ChVector<> COG_wheelB(0, 0.282, 0.003);
ChVector<> POS_pivotarm(0, 0.150, 1.855);
ChVector<> POS_prismatic(0, 0.150, 1.855);
double RAD_back_wheel =0.28;
double RAD_front_wheel = 0.28;
forkliftTiremap = app->GetVideoDriver()->getTexture("../data/tire_truck.png");
// --- The car body ---
IAnimatedMesh* forklift_bodyMesh = app->GetSceneManager()->getMesh("../data/forklift_body.obj");
truss = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), forklift_bodyMesh,
200.0,
COG_truss,
QUNIT);
truss->GetBody()->SetInertiaXX(ChVector<>(100, 100, 100));
truss->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
truss->setMaterialTexture(0, forkliftTiremap);
truss->GetChildMesh()->setPosition(-vector3df(COG_truss.x, COG_truss.y, COG_truss.z));// offset the mesh
truss->addShadowVolumeSceneNode();
truss->GetBody()->GetCollisionModel()->ClearModel();
truss->GetBody()->GetCollisionModel()->AddBox(1.227/2., 1.621/2., 1.864/2., &ChVector<>(-0.003, 1.019, 0.192));
truss->GetBody()->GetCollisionModel()->AddBox(0.187/2., 0.773/2., 1.201/2., &ChVector<>( 0.486 , 0.153,-0.047));
truss->GetBody()->GetCollisionModel()->AddBox(0.187/2., 0.773/2., 1.201/2., &ChVector<>(-0.486 , 0.153,-0.047));
truss->GetBody()->GetCollisionModel()->BuildModel();
truss->GetBody()->SetCollide(true);
// ..the right-front wheel
IAnimatedMesh* forklift_wheelMesh = app->GetSceneManager()->getMesh("../data/wheel.obj");
wheelRF = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), forklift_wheelMesh,
20.0,
COG_wheelRF,
QUNIT);
wheelRF->GetBody()->SetInertiaXX(ChVector<>(2, 2, 2));
wheelRF->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
wheelRF->setMaterialTexture(0, forkliftTiremap);
wheelRF->GetChildMesh()->setPosition(-vector3df(COG_wheelRF.x, COG_wheelRF.y, COG_wheelRF.z));// offset the mesh
// Describe the (invisible) colliding shape
ChMatrix33<>Arot(chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Z));
wheelRF->GetBody()->GetCollisionModel()->ClearModel();
wheelRF->GetBody()->GetCollisionModel()->AddCylinder(RAD_front_wheel,RAD_front_wheel, 0.1, &ChVector<>(0,0,0), &Arot);
wheelRF->GetBody()->GetCollisionModel()->BuildModel();
wheelRF->GetBody()->SetCollide(true);
// .. create the revolute joint between the wheel and the truss
link_revoluteRF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, front, upper, 1
link_revoluteRF->Initialize(wheelRF->GetBody(), truss->GetBody(),
ChCoordsys<>(COG_wheelRF , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
app->GetSystem()->AddLink(link_revoluteRF);
// ..the left-front wheel
wheelLF = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), forklift_wheelMesh,
20.0,
COG_wheelLF,
chrono::Q_from_AngAxis(CH_C_PI, VECT_Y)); // reuse RF wheel shape, flipped
wheelLF->GetBody()->SetInertiaXX(ChVector<>(2, 2, 2));
wheelLF->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
wheelLF->setMaterialTexture(0, forkliftTiremap);
wheelLF->GetChildMesh()->setPosition(-vector3df(COG_wheelRF.x, COG_wheelRF.y, COG_wheelRF.z));// offset the mesh (reuse RF)
// Describe the (invisible) colliding shape
wheelLF->GetBody()->GetCollisionModel()->ClearModel();
wheelLF->GetBody()->GetCollisionModel()->AddCylinder(RAD_front_wheel,RAD_front_wheel, 0.1, &ChVector<>(0,0,0), &Arot);
wheelLF->GetBody()->GetCollisionModel()->BuildModel();
wheelLF->GetBody()->SetCollide(true);
// .. create the revolute joint between the wheel and the truss
link_revoluteLF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, front, upper, 1
link_revoluteLF->Initialize(wheelLF->GetBody(), truss->GetBody(),
ChCoordsys<>(COG_wheelLF , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
app->GetSystem()->AddLink(link_revoluteLF);
// ..the back steering spindle (invisible, no mesh)
spindleB = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), 0,
10.0,
COG_wheelB,
QUNIT);
spindleB->GetBody()->SetInertiaXX(ChVector<>(1, 1, 1));
spindleB->GetBody()->SetCollide(false);
// .. create the vertical steering link between the spindle structure and the truss
link_steer_engineB = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
link_steer_engineB->Initialize(spindleB->GetBody(), truss->GetBody(),
ChCoordsys<>(COG_wheelB , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X)) ); // vertical axis
link_steer_engineB->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK);
link_steer_engineB->Set_eng_mode(ChLinkEngine::ENG_MODE_ROTATION);
app->GetSystem()->AddLink(link_steer_engineB);
// ..the back wheel
wheelB = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), forklift_wheelMesh,
20.0,
COG_wheelB,
QUNIT);
wheelB->GetBody()->SetInertiaXX(ChVector<>(2, 2, 2));
wheelB->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
wheelB->setMaterialTexture(0, forkliftTiremap);
wheelB->GetChildMesh()->setPosition(-vector3df(COG_wheelRF.x, COG_wheelRF.y, COG_wheelRF.z));// offset the mesh (reuse RF wheel)
double rescale = RAD_back_wheel/RAD_front_wheel;
// Describe the (invisible) colliding shape
wheelB->GetBody()->GetCollisionModel()->ClearModel();
wheelB->GetBody()->GetCollisionModel()->AddCylinder(RAD_back_wheel,RAD_back_wheel, 0.1, &ChVector<>(0,0,0), &Arot);
wheelB->GetBody()->GetCollisionModel()->BuildModel();
wheelB->GetBody()->SetCollide(true);
// .. create the motor between the back wheel and the steering spindle structure
link_engineB = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
link_engineB->Initialize(wheelB->GetBody(), spindleB->GetBody(),
ChCoordsys<>(COG_wheelB , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
link_engineB->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK);
link_engineB->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
app->GetSystem()->AddLink(link_engineB);
// ..the arm
IAnimatedMesh* forklift_armMesh = app->GetSceneManager()->getMesh("../data/forklift_arm.obj");
arm = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), forklift_armMesh,
100.0,
COG_arm,
QUNIT);
arm->GetBody()->SetInertiaXX(ChVector<>(30, 30, 30));
arm->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
arm->GetChildMesh()->setPosition(-vector3df(COG_arm.x, COG_arm.y, COG_arm.z));// offset the mesh
// .. create the revolute joint between the arm and the truss
link_engineArm = ChSharedPtr<ChLinkEngine>(new ChLinkEngine); // right, front, upper, 1
link_engineArm->Initialize(arm->GetBody(), truss->GetBody(),
ChCoordsys<>(POS_pivotarm , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
link_engineArm->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK);
link_engineArm->Set_eng_mode(ChLinkEngine::ENG_MODE_ROTATION);
app->GetSystem()->AddLink(link_engineArm);
// ..the fork
IAnimatedMesh* forklift_forkMesh = app->GetSceneManager()->getMesh("../data/forklift_forks.obj");
fork = (ChBodySceneNode*)addChBodySceneNode(
app->GetSystem(), app->GetSceneManager(), forklift_forkMesh,
60.0,
COG_fork,
QUNIT);
fork->GetBody()->SetInertiaXX(ChVector<>(15, 15, 15));
//fork->GetBody()->SetCollide(false);
fork->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
//fork->setMaterialTexture(0, forkliftTiremap);
fork->GetChildMesh()->setPosition(-vector3df(COG_fork.x, COG_fork.y, COG_fork.z));// offset the mesh
// Describe the (invisible) colliding shapes - two cubes for the two fingers, etc
fork->GetBody()->GetCollisionModel()->ClearModel();
fork->GetBody()->GetCollisionModel()->AddBox(0.1/2., 0.032/2., 1.033/2., &ChVector<>(-0.352, -0.312, 0.613));
fork->GetBody()->GetCollisionModel()->AddBox(0.1/2., 0.032/2., 1.033/2., &ChVector<>( 0.352, -0.312, 0.613));
fork->GetBody()->GetCollisionModel()->AddBox(0.344/2., 1.134/2., 0.101/2., &ChVector<>(-0.000, 0.321, -0.009));
fork->GetBody()->GetCollisionModel()->BuildModel();
fork->GetBody()->SetCollide(true);
// .. create the prismatic joint between the fork and arm
link_prismaticFork = ChSharedPtr<ChLinkLockPrismatic>(new ChLinkLockPrismatic);
link_prismaticFork->Initialize(fork->GetBody(), arm->GetBody(),
ChCoordsys<>(POS_prismatic , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X)) ); // set prism as vertical (default would be aligned to z, horizontal
app->GetSystem()->AddLink(link_prismaticFork);
// .. create the linear actuator that pushes upward the fork
link_actuatorFork = ChSharedPtr<ChLinkLinActuator>(new ChLinkLinActuator);
link_actuatorFork->Initialize(fork->GetBody(), arm->GetBody(), false,
ChCoordsys<>(POS_prismatic+ChVector<>(0,0.01,0) , QUNIT),
ChCoordsys<>(POS_prismatic , QUNIT) );
app->GetSystem()->AddLink(link_actuatorFork);
// ..a pallet
IAnimatedMesh* palletMesh = app->GetSceneManager()->getMesh("../data/pallet.obj");
video::ITexture* palletmap = app->GetVideoDriver()->getTexture("../data/cubetexture.png");
ChBodySceneNode* pallet = (ChBodySceneNode*)addChBodySceneNode_easyConcaveMesh(
app->GetSystem(), app->GetSceneManager(), "../data/pallet.obj",
15.0,
ChVector<>(0,1,3),
QUNIT);
pallet->GetBody()->SetInertiaXX(ChVector<>(3, 3, 3));
pallet->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
pallet->setMaterialTexture(0, palletmap);
//
// Move the forklift to initial offset position
//
// pallet->GetBody()->Move(offset);
truss->GetBody()->Move(offset);
wheelRF->GetBody()->Move(offset);
wheelLF->GetBody()->Move(offset);
wheelB->GetBody()->Move(offset);
spindleB->GetBody()->Move(offset);
arm->GetBody()->Move(offset);
fork->GetBody()->Move(offset);
}
