bool MyApp::OnInit() { wxCmdLineParser parser(argc,argv); parser.AddOption(wxT("c"), wxT("config-file")); parser.Parse(); simThread = new SimulationThread(); simThread->Create(); simThread->SetPriority(100); // Populate GUI { //cout << "Frame" << endl; frame = new MainFrame(wxT("Huboplus Control Test 1"), wxPoint(50,50), wxSize(600,400)); } frame->Show(); wxClientDC(frame->glPane); frame->glPane->SetCurrent(); // Load configurations { // load robot stuff const char *filename; wxString configFileName; if(parser.Found(wxT("c"), &configFileName)) { //filename = configFileName.mb_str(); //not working on Linux char cstring[256]; strncpy(cstring, (const char*)configFileName.mb_str(wxConvUTF8), 255); filename = (const char *) cstring; } else { cout<< "Error: cfg file not supplied !\n\n"; exit(1); } cout<<filename<<endl; ifstream cfg_ptr; cfg_ptr.open(filename); Float tmp; // Read simulation timing information. readConfigParameterLabel(cfg_ptr,"Integration_Stepsize"); cfg_ptr >> tmp; simThread->idt = tmp; if (simThread->idt <= 0.0) { cerr << "main error: invalid integration stepsize: " << simThread->idt << endl; exit(3); } readConfigParameterLabel(cfg_ptr,"Control_Stepsize"); cfg_ptr >> tmp; simThread->cdt = tmp; simThread->last_control_time = -2*simThread->cdt; if (simThread->cdt <= 0.0) { cerr << "main error: invalid control stepsize: " << simThread->cdt << endl; exit(3); } readConfigParameterLabel(cfg_ptr,"Display_Update_Rate"); cfg_ptr >> frame->glPane->render_rate; if (frame->glPane->render_rate < 1) frame->glPane->render_rate = 1; // ------------------------------------------------------------------ // Initialize DynaMechs environment - must occur before any linkage systems char env_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File"); readFilename(cfg_ptr, env_flname); dmEnvironment *environment = dmuLoadFile_env(env_flname); dmEnvironment::setEnvironment(environment); // ------------------------------------------------------------------ // Initialize a DynaMechs linkage system char robot_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File"); readFilename(cfg_ptr, robot_flname); cout<<"Robot parameter file: [\033[1;34m"<< robot_flname<< "\033[0m]"<<endl; G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname)); cout << "Robot created" << endl; huboCtrl = (HuboController *) new HuboBalanceController(G_robot); cout << "Robot controller created" << endl; // set status bar wxString s; s.Printf(wxT("idt = %1.1e s"), simThread->idt); frame->SetStatusText(s,1); s.Printf(_T("cdt = %f s" ), simThread->cdt); frame->SetStatusText(s,2); s.Printf(_T("refresh every %.2f ms"), 1000/(frame->glPane->render_rate) ); frame->SetStatusText(s,3); // ------------------------ // Read in data directory char data_dir[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr, "Data_Save_Directory"); readFilename(cfg_ptr, data_dir); // std::string(data_dir) // ... cout<<"Add robot to integrator "<<endl; simThread->G_integrator->addSystem(G_robot); } // ----------------------- { cout<<"Initilize scene..."<<endl; frame->glPane->camera->setRadius(8.0); frame->glPane->camera->setCOI(3.0, 3.0, 0.0); frame->glPane->camera->setTranslationScale(0.02f); frame->glPane->glInit(); dmEnvironment::getEnvironment()->drawInit(); frame->glPane->model_loaded = true; } cout << "Restart Timer" << endl; frame->glPane->restartTimer(); cout << "Start Sim Thread" << endl; simThread->Run(); return true; }
//---------------------------------------------------------------------------- // Summary: // Parameters: // Returns: //---------------------------------------------------------------------------- int main(int argc, char** argv) { int i, j; glutInit(&argc, argv); //========================= char *filename = "simulation.cfg"; if (argc > 1) { filename = argv[1]; } glutInitWindowSize(640,480); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); glutCreateWindow("DynaMechs Example"); myinit(); mouse = dmGLMouse::dmInitGLMouse(); for (i=0; i<4; i++) { for (j=0; j<4; j++) { view_mat[i][j] = 0.0; } view_mat[i][i] = 1.0; } //view_mat[3][2] = -10.0; camera = new dmGLPolarCamera_zup(); camera->setRadius(8.0); camera->setCOI(8.0, 10.0, 2.0); camera->setTranslationScale(0.02f); // load robot stuff ifstream cfg_ptr; cfg_ptr.open(filename); // Read simulation timing information. readConfigParameterLabel(cfg_ptr,"Integration_Stepsize"); cfg_ptr >> idt; if (idt <= 0.0) { cerr << "main error: invalid integration stepsize: " << idt << endl; exit(3); } motion_plan_rate = (int) (0.5 + 0.01/idt); // fixed rate of 100Hz readConfigParameterLabel(cfg_ptr,"Display_Update_Rate"); cfg_ptr >> render_rate; if (render_rate < 1) render_rate = 1; // =========================================================================== // Initialize DynaMechs environment - must occur before any linkage systems char env_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File"); readFilename(cfg_ptr, env_flname); dmEnvironment *environment = dmuLoadFile_env(env_flname); environment->drawInit(); dmEnvironment::setEnvironment(environment); // =========================================================================== // Initialize a DynaMechs linkage system char robot_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File"); readFilename(cfg_ptr, robot_flname); G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname)); G_integrator = new dmIntegRK4(); G_integrator->addSystem(G_robot); initAquaControl(G_robot); glutReshapeFunc(myReshape); glutKeyboardFunc(processKeyboard); glutSpecialFunc(processSpecialKeys); glutDisplayFunc(display); glutIdleFunc(updateSim); dmGetSysTime(&last_tv); cout << endl; cout << " p - toggles dynamic simulation" << endl; cout << " UP/DOWN ARROW - increases/decreases velocity command" << endl; cout << "RIGHT/LEFT ARROW - changes heading command" << endl << endl; glutMainLoop(); return 0; /* ANSI C requires main to return int. */ }
bool MyApp::OnInit() { wxCmdLineParser parser(argc,argv); parser.AddOption(wxT("c"), wxT("Config File")); parser.Parse(); simThread = new SimulationThread(); simThread->Create(); simThread->SetPriority(100); // Populate GUI { //cout << "Frame" << endl; frame = new MainFrame(wxT("Hello GL World"), wxPoint(50,50), wxSize(600,400)); } frame->Show(); wxClientDC(frame->glPane); frame->glPane->SetCurrent(); // Load DM File { // load robot stuff const char *filename; wxString configFileName; if(parser.Found(wxT("c"), &configFileName)) { filename = "humanoid.cfg";//configFileName.mb_str(); } else { filename = (char *) "config.cfg"; } cout<<filename<<endl; ifstream cfg_ptr; cfg_ptr.open(filename); Float tmp; // Read simulation timing information. readConfigParameterLabel(cfg_ptr,"Integration_Stepsize"); cfg_ptr >> tmp; simThread->idt = tmp; if (simThread->idt <= 0.0) { cerr << "main error: invalid integration stepsize: " << simThread->idt << endl; exit(3); } readConfigParameterLabel(cfg_ptr,"Control_Stepsize"); cfg_ptr >> tmp; simThread->cdt = tmp; simThread->last_control_time = -2*simThread->cdt; if (simThread->cdt <= 0.0) { cerr << "main error: invalid control stepsize: " << simThread->cdt << endl; exit(3); } readConfigParameterLabel(cfg_ptr,"Display_Update_Rate"); cfg_ptr >> frame->glPane->render_rate; if (frame->glPane->render_rate < 1) frame->glPane->render_rate = 1; // ------------------------------------------------------------------ // Initialize DynaMechs environment - must occur before any linkage systems char env_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File"); readFilename(cfg_ptr, env_flname); dmEnvironment *environment = dmuLoadFile_env(env_flname); dmEnvironment::setEnvironment(environment); // ------------------------------------------------------------------ // Initialize a DynaMechs linkage system char robot_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File"); readFilename(cfg_ptr, robot_flname); G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname)); humanoid = (HumanoidDataLogger *) new BalanceDemoStateMachine(G_robot); // -------- // Read in data directory char data_dir[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr, "Data_Save_Directory"); readFilename(cfg_ptr, data_dir); humanoid->dataSaveDirectory = std::string(data_dir); simThread->G_integrator->addSystem(G_robot); grfInfo.localContacts = 0; } // -- Project specific -- //initControl(); // ----------------------- { cout<<"initilize scene..."<<endl; frame->glPane->camera->setRadius(8.0); frame->glPane->camera->setCOI(3.0, 3.0, 0.0); frame->glPane->camera->setTranslationScale(0.02f); frame->glPane->glInit(); dmEnvironment::getEnvironment()->drawInit(); frame->glPane->model_loaded = true; } frame->glPane->restartTimer(); simThread->Run(); return true; }