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