// 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);
}
}
MySimpleCar::MySimpleCar(ChSystem& my_system, ///< the chrono::engine physical system
ISceneManager* msceneManager, ///< the Irrlicht scene manager for 3d shapes
IVideoDriver* mdriver ///< the Irrlicht video driver
)
{
throttle = 0; // initially, gas throttle is 0.
conic_tau = 0.2;
gears[0] = -0.3; // rear gear<
gears[1] = 0.3; // 1st gear
gears[2] = 0.4; // 2nd gear
gears[3] = 0.6; // 3rd gear
gears[4] = 0.8; // 4th gear
gears[5] = 1.0; // 5th gear
actual_gear = 1;
gear_tau = gears[actual_gear];
// Define the torque curve with some polygonal approximation, entering
// a sequence of x-y pairs (speed-torque, with speed in [rad/s] and torque in [Nm])
// (Of course a real engine should have not-null torque only from some positive value, but
// here we start from -50 rad/s otherwise we cannot start - given we don't model the clutch)
torque_curve.AddPoint(-50, 35);
torque_curve.AddPoint(0, 45);
torque_curve.AddPoint(400,50);
torque_curve.AddPoint(700,54);
torque_curve.AddPoint(850, 0);
torque_curve.AddPoint(1000, -54); // finish in 4th quadrant
torque_curve.AddPoint(2000, -60); // finish in 4th quadrant
ChStreamOutAsciiFile myfiletq("data_torquecurve.dat");
torque_curve.FileAsciiPairsSave(myfiletq,-100,1000,200);
motorspeed= motortorque=0;
passo = 1.73162;
carr = 0.59963;
wanted_steer = 0;
actual_steer = 0;
max_steer_speed = 0.5;
convergenza_anteriore = 0;
convergenza_posteriore = 0;
max_brake_torque_post = 200;
max_brake_torque_ant = 300;
braking = 0;
// --- The car body ---
// Set the position of the center of gravity of the truss, respect to the auxiliary reference that
// we will use to measure all relative positions of joints on the truss.
truss_COG.Set( 0, 0, 0);
truss = (ChBodySceneNode*)addChBodySceneNode(
&my_system, msceneManager, 0,
150.0,
ChVector<>(0, 0.016 ,1.99) +truss_COG,
QUNIT);
truss->GetBody()->SetInertiaXX(ChVector<>(4.8, 4.5, 1));
truss->GetBody()->SetBodyFixed(false);
// Add some 'invisible' boxes which roughly define the collision shape of
// the car truss. Each colliding box must be defined in x y z half-sizes and position of
// box center respect to the truss COG.
truss->GetBody()->GetCollisionModel()->ClearModel();
truss->GetBody()->GetCollisionModel()->AddBox(0.3, 0.3, 1.5, &(ChVector<>(0,0.2,0)-truss_COG) ); // just example.. roughly the body
truss->GetBody()->GetCollisionModel()->AddBox(0.1, 0.3, 0.1, &(ChVector<>(0,0.4,0)-truss_COG) ); // just example.. roughly the rollbar
truss->GetBody()->GetCollisionModel()->BuildModel();
truss->GetBody()->SetCollide(true);
// Add a visible mesh for Irrlicht representation of the
// car - it must be found on disk with the relative path as indicated..
IAnimatedMesh* scocca_mesh = msceneManager->getMesh("../data/mycar.obj");
IAnimatedMeshSceneNode* scocca_node = msceneManager->addAnimatedMeshSceneNode( scocca_mesh, truss );
scocca_node->setPosition(vector3dfCH(-truss_COG)); // if COG is not the reference we use for all stuff in truss..
scocca_node->addShadowVolumeSceneNode();
// --- Right Front suspension ---
// ..the car right-front spindle
spindleRF = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,// mass of the spindle
ChVector<>(-(carr-0.05216), 0.02865 ,0.93534),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleRF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleRF->GetBody()->SetCollide(false);
// ..the car right-front wheel
wheelRF = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>(-carr ,0.02865 ,0.93534),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelRF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
wheelRF->GetBody()->SetCollide(true);
wheelRF->GetBody()->SetFriction(1.3);;
wheelRF->GetBody()->SetName("wheelRF");
video::ITexture* cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRF->setMaterialTexture(0, cylinderMap);
wheelRF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
// Note that we specify two ChCoordinates of two markers which will be added in the two bodies,
// but both rotated with Z their axes pointing laterally thank to the Q_from_AngAxis() 90 degrees
// rotations if necessary, because ChLinkLockRevolute will use the Z axes of markers as shafts.
link_revoluteRF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, front, upper, 1
link_revoluteRF->Initialize(wheelRF->GetBody(), spindleRF->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>(-0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteRF);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeRF = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteRF->GetMarker1()->AddRef();
link_revoluteRF->GetMarker2()->AddRef();
link_brakeRF->Initialize(ChSharedMarkerPtr(link_revoluteRF->GetMarker1()),
ChSharedMarkerPtr(link_revoluteRF->GetMarker2()) );
my_system.AddLink(link_brakeRF);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distRFU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, upper, 1
link_distRFU1->Initialize(truss->GetBody(), spindleRF->GetBody(),
true, // false = positions are relative to COG csystems of bodies, true = absolute positions
ChVector<>(-0.24708, 0.09313, -1.17789)-truss_COG, // position of 1st end rod in truss COG coordinate
ChVector<>( 0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFU1);
link_distRFU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, upper, 2
link_distRFU2->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.24708,0.09313,-0.71972)-truss_COG,
ChVector<>( 0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFU2);
link_distRFL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, lower, 1
link_distRFL1->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-1.17789)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFL1);
link_distRFL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, lower, 2
link_distRFL2->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-0.71972)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFL2);
// .. create the spring between the truss and the spindle
link_springRF = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springRF->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.31394,0.19399,-1.06243)-truss_COG,
ChVector<>( 0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springRF->Set_SpringK(28300);
link_springRF->Set_SpringR(80);
my_system.AddLink(link_springRF);
// .. create the rod for steering the wheel
link_distRSTEER = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right steer
link_distRSTEER->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.25457,0.10598,-1.24879)-truss_COG,
ChVector<>( 0.06661,0.09333,-0.15003),
false, 0.2330); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distRSTEER);
// --- Left Front suspension ---
// ..the car left-front spindle
spindleLF = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,// mass of the spindle
ChVector<>( (carr-0.05216), 0.02865 ,0.93534),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleLF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleLF->GetBody()->SetCollide(false);
// ..the car left-front wheel
wheelLF = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>( carr ,0.02865 ,0.93534),
chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelLF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
wheelLF->GetBody()->SetCollide(true);
wheelLF->GetBody()->SetFriction(1.3);
wheelLF->GetBody()->SetName("wheelLF");
wheelLF->setMaterialTexture(0, cylinderMap);
wheelLF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
link_revoluteLF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // left, front, upper, 1
link_revoluteLF->Initialize(wheelLF->GetBody(), spindleLF->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>( 0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteLF);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeLF = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteLF->GetMarker1()->AddRef();
link_revoluteLF->GetMarker2()->AddRef();
link_brakeLF->Initialize(ChSharedMarkerPtr(link_revoluteLF->GetMarker1()),
ChSharedMarkerPtr(link_revoluteLF->GetMarker2()) );
my_system.AddLink(link_brakeLF);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distLFU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, upper, 1
link_distLFU1->Initialize(truss->GetBody(), spindleLF->GetBody(),
true, // false = positions are relative to COG csystems of bodies, true = absolute positions
ChVector<>( 0.24708, 0.09313, -1.17789)-truss_COG, // position of 1st end rod in truss COG coordinate
ChVector<>(-0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFU1);
link_distLFU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, upper, 2
link_distLFU2->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.24708,0.09313, -0.71972)-truss_COG,
ChVector<>(-0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFU2);
link_distLFL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, lower, 1
link_distLFL1->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-1.17789)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFL1);
link_distLFL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, lower, 2
link_distLFL2->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-0.71972)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFL2);
// .. create the spring between the truss and the spindle
link_springLF = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springLF->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.31394,0.19399,-1.06243)-truss_COG,
ChVector<>(-0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springLF->Set_SpringK(28300);
link_springLF->Set_SpringR(80);
my_system.AddLink(link_springLF);
// .. create the rod for steering the wheel
link_distLSTEER = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right steer
link_distLSTEER->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.25457,0.10598,-1.24879)-truss_COG,
ChVector<>(-0.06661,0.09333,-0.15003),
false, 0.2330); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distLSTEER);
// --- Right Back suspension ---
// ..the car right-back spindle
spindleRB = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,
ChVector<>(-(carr-0.05216), 0.02865 ,(0.93534+passo)),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleRB->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleRB->GetBody()->SetCollide(false);
// ..the car right-back wheel
wheelRB = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>(-carr ,0.02865 ,(0.93534+passo)),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelRB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelRB->GetBody()->SetCollide(true);
wheelRB->GetBody()->SetFriction(1.3);
wheelRB->GetBody()->SetName("wheelRB");
cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRB->setMaterialTexture(0, cylinderMap);
wheelRB->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
link_revoluteRB = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, back, upper, 1
link_revoluteRB->Initialize(wheelRB->GetBody(), spindleRB->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>(-0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteRB);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeRB = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteRB->GetMarker1()->AddRef();
link_revoluteRB->GetMarker2()->AddRef();
link_brakeRB->Initialize(ChSharedMarkerPtr(link_revoluteRB->GetMarker1()),
ChSharedMarkerPtr(link_revoluteRB->GetMarker2()) );
my_system.AddLink(link_brakeRB);
// .. create the motor transmission joint between the wheel and the truss (assuming small changes of alignment)
link_engineR = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
link_engineR->Initialize(wheelRB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(-carr,0.02865,(0.93534+passo)) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
link_engineR->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_CARDANO); // approx as a double Rzeppa joint
link_engineR->Set_eng_mode(ChLinkEngine::ENG_MODE_TORQUE);
my_system.AddLink(link_engineR);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distRBU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, upper, 1
link_distRBU1->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708, 0.09313, -1.17789+passo)-truss_COG,
ChVector<>( 0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBU1);
link_distRBU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, upper, 2
link_distRBU2->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708,0.09313,-0.71972+passo)-truss_COG,
ChVector<>( 0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBU2);
link_distRBL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, lower, 1
link_distRBL1->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-1.17789+passo)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBL1);
link_distRBL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, lower, 2
link_distRBL2->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-0.71972+passo)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBL2);
// .. create the spring between the truss and the spindle
link_springRB = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springRB->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.31394,0.19399,-1.06243+passo)-truss_COG,
ChVector<>( 0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springRB->Set_SpringK(28300);
link_springRB->Set_SpringR(80);
my_system.AddLink(link_springRB);
// .. create the rod for avoid the steering of the wheel
link_distRBlat = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right rod
link_distRBlat->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.25457,0.10598,-1.24879+passo)-truss_COG,
ChVector<>( 0.06661,0.09333,-0.10003),
false, 0.2450); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distRBlat);
// --- Left Back suspension ---
// ..the car right-back spindle
spindleLB = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,
ChVector<>((carr-0.05216), 0.02865 ,(0.93534+passo)),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleLB->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleLB->GetBody()->SetCollide(false);
// ..the car left-back wheel
wheelLB = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>(carr ,0.02865 ,(0.93534+passo)),
chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelLB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelLB->GetBody()->SetCollide(true);
wheelLB->GetBody()->SetFriction(1.3);
wheelLB->GetBody()->SetName("wheelLB");
wheelLB->setMaterialTexture(0, cylinderMap);
wheelLB->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
link_revoluteLB = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // left, back, upper, 1
link_revoluteLB->Initialize(wheelLB->GetBody(), spindleLB->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>(0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteLB);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeLB = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteLB->GetMarker1()->AddRef();
link_revoluteLB->GetMarker2()->AddRef();
link_brakeLB->Initialize(ChSharedMarkerPtr(link_revoluteLB->GetMarker1()),
ChSharedMarkerPtr(link_revoluteLB->GetMarker2()) );
my_system.AddLink(link_brakeLB);
// .. create the motor transmission joint between the wheel and the truss (assuming small changes of alignment)
link_engineL = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
link_engineL->Initialize(wheelLB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(carr,0.02865,(0.93534+passo)) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
link_engineL->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_CARDANO); // approx as a double Rzeppa joint
link_engineL->Set_eng_mode(ChLinkEngine::ENG_MODE_TORQUE);
my_system.AddLink(link_engineL);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distLBU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, upper, 1
link_distLBU1->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708, 0.09313, -1.17789+passo)-truss_COG,
ChVector<>(-0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBU1);
link_distLBU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, upper, 2
link_distLBU2->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708,0.09313, -0.71972+passo)-truss_COG,
ChVector<>(-0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBU2);
link_distLBL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, lower, 1
link_distLBL1->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-1.17789+passo)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBL1);
link_distLBL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, lower, 2
link_distLBL2->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-0.71972+passo)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBL2);
// .. create the spring between the truss and the spindle
link_springLB = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springLB->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.31394,0.19399,-1.06243+passo)-truss_COG,
ChVector<>(-0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springLB->Set_SpringK(28300);
link_springLB->Set_SpringR(80);
my_system.AddLink(link_springLB);
// .. create the rod for avoid the steering of the wheel
link_distLBlat = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right
link_distLBlat->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.25457,0.10598,-1.24879+passo)-truss_COG,
ChVector<>(-0.06661,0.09333,-0.10003),
false, 0.2450); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distLBlat);
// Create also the motor sound
//
// start the sound engine with default parameters
sound_engine = irrklang::createIrrKlangDevice();
if (!sound_engine)
{
GetLog() << "Cannot start sound engine Irrklang \n";
}
// To play a sound, we only to call play2D(). The second parameter
// tells the engine to play it looped.
// play some sound stream, looped
if (sound_engine)
car_sound = sound_engine->play2D("../data/carsound.ogg", true, false, true);
else
car_sound = 0;
}
int main(int argc, char* argv[])
{
ChSystem system;
// Disable gravity
system.Set_G_acc(ChVector<>(0, 0, 0));
// Set the half-length of the two shafts
double hl = 2;
// Set the bend angle between the two shafts (positive rotation about the
// global X axis)
double angle = CH_C_PI / 6;
double cosa = std::cos(angle);
double sina = std::sin(angle);
ChQuaternion<> rot = Q_from_AngX(angle);
// Create the ground (fixed) body
// ------------------------------
ChSharedPtr<ChBody> ground(new ChBody);
system.AddBody(ground);
ground->SetIdentifier(-1);
ground->SetBodyFixed(true);
ground->SetCollide(false);
// attach visualization assets to represent the revolute and cylindrical
// joints that connect the two shafts to ground
{
ChSharedPtr<ChCylinderShape> cyl_1(new ChCylinderShape);
cyl_1->GetCylinderGeometry().p1 = ChVector<>(0, 0, -hl - 0.2);
cyl_1->GetCylinderGeometry().p2 = ChVector<>(0, 0, -hl + 0.2);
cyl_1->GetCylinderGeometry().rad = 0.3;
ground->AddAsset(cyl_1);
ChSharedPtr<ChCylinderShape> cyl_2(new ChCylinderShape);
cyl_2->GetCylinderGeometry().p1 = ChVector<>(0, -(hl - 0.2) * sina, (hl - 0.2) * cosa);
cyl_2->GetCylinderGeometry().p2 = ChVector<>(0, -(hl + 0.2) * sina, (hl + 0.2) * cosa);
cyl_2->GetCylinderGeometry().rad = 0.3;
ground->AddAsset(cyl_2);
}
// Create the first shaft body
// ---------------------------
ChSharedPtr<ChBody> shaft_1(new ChBody);
system.AddBody(shaft_1);
shaft_1->SetIdentifier(1);
shaft_1->SetBodyFixed(false);
shaft_1->SetCollide(false);
shaft_1->SetMass(1);
shaft_1->SetInertiaXX(ChVector<>(1, 1, 0.2));
shaft_1->SetPos(ChVector<>(0, 0, -hl));
shaft_1->SetRot(ChQuaternion<>(1, 0, 0, 0));
// Add visualization assets to represent the shaft (a box) and the arm of the
// universal joint's cross associated with this shaft (a cylinder)
{
ChSharedPtr<ChBoxShape> box_1(new ChBoxShape);
box_1->GetBoxGeometry().Size = ChVector<>(0.15, 0.15, 0.9 * hl);
shaft_1->AddAsset(box_1);
ChSharedPtr<ChCylinderShape> cyl_2(new ChCylinderShape);
cyl_2->GetCylinderGeometry().p1 = ChVector<>(-0.2, 0, hl);
cyl_2->GetCylinderGeometry().p2 = ChVector<>(0.2, 0, hl);
cyl_2->GetCylinderGeometry().rad = 0.05;
shaft_1->AddAsset(cyl_2);
ChSharedPtr<ChColorAsset> col(new ChColorAsset);
col->SetColor(ChColor(0.6f, 0, 0));
shaft_1->AddAsset(col);
}
// Create the second shaft body
// ----------------------------
// The second shaft is identical to the first one, but initialized at an angle
// equal to the specified bend angle.
ChSharedPtr<ChBody> shaft_2(new ChBody);
system.AddBody(shaft_2);
shaft_2->SetIdentifier(1);
shaft_2->SetBodyFixed(false);
shaft_2->SetCollide(false);
shaft_2->SetMass(1);
shaft_2->SetInertiaXX(ChVector<>(1, 1, 0.2));
shaft_2->SetPos(ChVector<>(0, -hl * sina, hl * cosa));
shaft_2->SetRot(rot);
// Add visualization assets to represent the shaft (a box) and the arm of the
// universal joint's cross associated with this shaft (a cylinder)
{
ChSharedPtr<ChBoxShape> box_1(new ChBoxShape);
box_1->GetBoxGeometry().Size = ChVector<>(0.15, 0.15, 0.9 * hl);
shaft_2->AddAsset(box_1);
ChSharedPtr<ChCylinderShape> cyl_2(new ChCylinderShape);
cyl_2->GetCylinderGeometry().p1 = ChVector<>(0, -0.2, -hl);
cyl_2->GetCylinderGeometry().p2 = ChVector<>(0, 0.2, -hl);
cyl_2->GetCylinderGeometry().rad = 0.05;
shaft_2->AddAsset(cyl_2);
ChSharedPtr<ChColorAsset> col(new ChColorAsset);
col->SetColor(ChColor(0, 0, 0.6f));
shaft_2->AddAsset(col);
}
// Connect the first shaft to ground
// ---------------------------------
// Use a ChLinkEngine to impose both the revolute joint constraints, as well
// as constant angular velocity. The joint is located at the origin of the
// first shaft.
ChSharedPtr<ChLinkEngine> motor(new ChLinkEngine);
system.AddLink(motor);
motor->Initialize(ground, shaft_1, ChCoordsys<>(ChVector<>(0, 0, -hl), ChQuaternion<>(1, 0, 0, 0)));
motor->Set_eng_mode(ChLinkEngine::ENG_MODE_ROTATION);
motor->Set_rot_funct(ChSharedPtr<ChFunction>(new ChFunction_Ramp(0, 1)));
// Connect the second shaft to ground through a cylindrical joint
// --------------------------------------------------------------
// Use a cylindrical joint so that we do not have redundant constraints
// (note that, technically Chrono could deal with a revolute joint here).
// the joint is located at the origin of the second shaft.
ChSharedPtr<ChLinkLockCylindrical> cyljoint(new ChLinkLockCylindrical);
system.AddLink(cyljoint);
cyljoint->Initialize(ground, shaft_2, ChCoordsys<>(ChVector<>(0, -hl * sina, hl * cosa), rot));
// Connect the two shafts through a universal joint
// ------------------------------------------------
// The joint is located at the global origin. Its kinematic constraints will
// enforce orthogonality of the associated cross.
ChSharedPtr<ChLinkUniversal> ujoint(new ChLinkUniversal);
system.AddLink(ujoint);
ujoint->Initialize(shaft_1, shaft_2, ChFrame<>(ChVector<>(0, 0, 0), rot));
// Create the Irrlicht application
// -------------------------------
ChIrrApp application(&system, L"ChBodyAuxRef demo", core::dimension2d<u32>(800, 600), false, true);
application.AddTypicalLogo();
application.AddTypicalSky();
application.AddTypicalLights();
application.AddTypicalCamera(core::vector3df(3, 1, -1.5));
application.AssetBindAll();
application.AssetUpdateAll();
// Simulation loop
application.SetTimestep(0.001);
int frame = 0;
while (application.GetDevice()->run())
{
application.BeginScene();
application.DrawAll();
application.DoStep();
application.EndScene();
frame++;
if (frame % 20 == 0) {
// Output the shaft angular velocities at the current time
double omega_1 = shaft_1->GetWvel_loc().z;
double omega_2 = shaft_2->GetWvel_loc().z;
GetLog() << system.GetChTime() << " " << omega_1 << " " << omega_2 << "\n";
}
}
return 0;
}
int main(int argc, char* argv[]) {
DLL_CreateGlobals();
ChSystem * system = new ChSystem();
system->SetMaxiter(100);
system->SetIterLCPmaxItersSpeed(100);
system->SetTol(0);
system->SetTolSpeeds(0);
ChLcpSystemDescriptor *mdescriptor = new ChLcpSystemDescriptor();
ChContactContainer *mcontactcontainer = new ChContactContainer();
//ChCollisionSystemBulletGPU *mcollisionengine = new ChCollisionSystemBulletGPU();
ChCollisionSystemBullet *mcollisionengine = new ChCollisionSystemBullet();
system->ChangeLcpSystemDescriptor(mdescriptor);
system->ChangeContactContainer(mcontactcontainer);
system->ChangeCollisionSystem(mcollisionengine);
system->SetIntegrationType(ChSystem::INT_ANITESCU);
system->SetLcpSolverType(ChSystem::LCP_ITERATIVE_APGD);
system->SetStep(timestep);
system->Set_G_acc(ChVector<>(0, gravity, 0));
ChSharedBodyPtr floor(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);
InitObject(floor, 1.0, ChVector<>(0, -3.5, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, true, -20, -20);
InitObject(chassis, 1, ChVector<>(0, 0, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 0, 1);
InitObject(axle_F, 1, ChVector<>(0, 0, chassisL / 2), Q_from_AngZ(CH_C_PI / 2.0), 1, 1, 0, false, false, -2, -2);
InitObject(axle_C, 1, ChVector<>(0, 0, 0), Q_from_AngZ(CH_C_PI / 2.0), 1, 1, 0, false, false, -2, -2);
InitObject(axle_R, 1, ChVector<>(0, 0, -chassisL / 2), Q_from_AngZ(CH_C_PI / 2.0), 1, 1, 0, false, false, -2, -2);
InitObject(leg_FR, 1, ChVector<>((axleL + legW) / 2.0, -legL / 2.0, chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_FL, 1, ChVector<>(-(axleL + legW) / 2.0, legL / 2.0, chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_CR, 1, ChVector<>(-(axleL + legW) / 2.0, -legL / 2.0, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_CL, 1, ChVector<>((axleL + legW) / 2.0, legL / 2.0, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_RR, 1, ChVector<>((axleL + legW) / 2.0, -legL / 2.0, -chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_RL, 1, ChVector<>(-(axleL + legW) / 2.0, legL / 2.0, -chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
AddCollisionGeometry(floor, BOX, ChVector<>(10, 1 / 2.0, 10), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(chassis, ELLIPSOID, ChVector<>(1, 1, 1), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(axle_F, ELLIPSOID, ChVector<>(0.5 / 2.0, axleL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(axle_C, ELLIPSOID, ChVector<>(0.5 / 2.0, axleL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(axle_R, ELLIPSOID, ChVector<>(0.5 / 2.0, axleL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_FR, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_FL, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_CR, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_CL, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_RR, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_RL, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
floor->SetInertiaXX(Vector(1,1,1));
chassis->SetInertiaXX(Vector(1,1,1));
axle_F->SetInertiaXX(Vector(1,1,1));
axle_C->SetInertiaXX(Vector(1,1,1));
axle_R->SetInertiaXX(Vector(1,1,1));
leg_FR->SetInertiaXX(Vector(1,1,1));
leg_FL->SetInertiaXX(Vector(1,1,1));
leg_CR->SetInertiaXX(Vector(1,1,1));
leg_CL->SetInertiaXX(Vector(1,1,1));
leg_RR->SetInertiaXX(Vector(1,1,1));
leg_RL->SetInertiaXX(Vector(1,1,1));
FinalizeObject(floor, (ChSystem *) system);
FinalizeObject(chassis, (ChSystem *) system);
FinalizeObject(axle_F, (ChSystem *) system);
FinalizeObject(axle_C, (ChSystem *) system);
FinalizeObject(axle_R, (ChSystem *) system);
FinalizeObject(leg_FR, (ChSystem *) system);
FinalizeObject(leg_FL, (ChSystem *) system);
FinalizeObject(leg_CR, (ChSystem *) system);
FinalizeObject(leg_CL, (ChSystem *) system);
FinalizeObject(leg_RR, (ChSystem *) system);
FinalizeObject(leg_RL, (ChSystem *) system);
ChSharedBodyPtr chassis_ptr = ChSharedBodyPtr(chassis);
ChSharedBodyPtr axle_F_ptr = ChSharedBodyPtr(axle_F);
ChSharedBodyPtr axle_C_ptr = ChSharedBodyPtr(axle_C);
ChSharedBodyPtr axle_R_ptr = ChSharedBodyPtr(axle_R);
ChSharedBodyPtr leg_FR_ptr = ChSharedBodyPtr(leg_FR);
ChSharedBodyPtr leg_FL_ptr = ChSharedBodyPtr(leg_FL);
ChSharedBodyPtr leg_CR_ptr = ChSharedBodyPtr(leg_CR);
ChSharedBodyPtr leg_CL_ptr = ChSharedBodyPtr(leg_CL);
ChSharedBodyPtr leg_RR_ptr = ChSharedBodyPtr(leg_RR);
ChSharedBodyPtr leg_RL_ptr = ChSharedBodyPtr(leg_RL);
// ChSharedPtr<ChLinkLockLock> axle_FR(new ChLinkLockLock);
// axle_FR->Initialize(leg_FR_ptr, axle_F_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_FR);
////
// ChSharedPtr<ChLinkLockLock> axle_FL(new ChLinkLockLock);
// axle_FL->Initialize(leg_FL_ptr, axle_F_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_FL);
//
// ChSharedPtr<ChLinkLockLock> axle_CR(new ChLinkLockLock);
// axle_CR->Initialize(leg_CR_ptr, axle_C_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_CR);
//
// ChSharedPtr<ChLinkLockLock> axle_CL(new ChLinkLockLock);
// axle_CL->Initialize(leg_CL_ptr, axle_C_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_CL);
//
// ChSharedPtr<ChLinkLockLock> axle_RR(new ChLinkLockLock);
// axle_RR->Initialize(leg_RR_ptr, axle_R_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_RR);
//
// ChSharedPtr<ChLinkLockLock> axle_RL(new ChLinkLockLock);
// axle_RL->Initialize(leg_RL_ptr, axle_R_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_RL);
// Create engine between axles and chassis
GetLog() << "Creating Motors\n";
// ChSharedPtr<ChLinkEngine> eng_F(new ChLinkEngine);
// eng_F->Initialize(axle_F_ptr, chassis_ptr, 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(1); // rad/s angular speed
// system->AddLink(eng_F);
ChSharedPtr<ChLinkEngine> eng_C(new ChLinkEngine);
eng_C->Initialize(axle_C_ptr, chassis_ptr, 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(1); // rad/s angular speed
system->AddLink(eng_C);
// ChSharedPtr<ChLinkEngine> eng_R(new ChLinkEngine);
// eng_R->Initialize(axle_R_ptr, chassis_ptr, 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(1); // rad/s angular speed
// system->AddLink(eng_R);
system->DoStepDynamics(timestep);
system->DoStepDynamics(timestep);
exit(0);
ChOpenGLManager * window_manager = new ChOpenGLManager();
ChOpenGL openGLView(window_manager, system, 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, -5, 0);
openGLView.render_camera->camera_scale = .5;
openGLView.SetCustomCallback(RunTimeStep);
openGLView.StartSpinning(window_manager);
window_manager->CallGlutMainLoop();
DLL_DeleteGlobals();
return 0;
}
