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