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