void SRADiagnosticsView :: init ()
{
char buf [ 256 ];
rc_t rc = KConfig_Get_Home ( config, buf, sizeof ( buf ), nullptr );
if ( rc != 0 )
qDebug () << "Failed to get home path";
else
homeDir = QString ( buf );
init_metadata_view ();
init_tree_view ();
init_controls ();
}
static long file_dialog(object wind, unsigned id)
{
object dlg;
dlg = mNewDialog(ModelessDialog, DL1, NULL);
gSetTag(dlg, wind);
init_controls(dlg);
gCompletionFunction(dlg, displayValues);
gPerform(dlg);
return 0L;
}
void WorldSQP::solve() {
#if TIMER_ENABLED
StartClock();
#endif
#if COLOCATION_METHOD
assert(current_states.size() == _num_traj);
cout << current_controls.size() << " " << _num_worlds - 1 << endl;
assert(current_controls.size() == _num_worlds-1);
for (int i = 0; i < current_states.size(); i++) {
assert(current_states[i].size() == _num_worlds); // memory leak if false
}
#else
assert(false); //solver not implemented for SHOOTING_METHOD
#endif
//vector to contain the new states
VectorXd new_states(_num_traj*(_num_worlds-2)*_size_each_state + (_num_worlds-1)*_size_each_control);
//setup filenames
char filename_goalvec[256];
sprintf(filename_goalvec, "%s/%s_%s", SQP_BASE_FOLDER, _namestring, FILENAME_GOALVEC);
char filename_alltrans[256];
sprintf(filename_alltrans, "%s/%s_%s", SQP_BASE_FOLDER, _namestring, FILENAME_ALLTRANS);
char filename_initctrls[256];
sprintf(filename_initctrls, "%s/%s_%s", SQP_BASE_FOLDER, _namestring, FILENAME_INIT_CTRLS);
//compute Jacobians
compute_all_jacobians();
double t0 = GetClock();
//write out all Jacobians to file
SparseMatrix<double> all_trans_sparse(_all_trans);
Matrix_To_File(all_trans_sparse, filename_alltrans);
//write out all states to file
VectorXd goal_vector(_size_each_state*_num_worlds*_num_traj);
VectorXd state_for_file(_size_each_state);
for (int j=0; j < current_states.size(); j++) {
for (int i=0; i < current_states[j].size(); i++) {
world_to_state(current_states[j][i], state_for_file);
goal_vector.segment((_num_worlds)*_size_each_state*j + i*_size_each_state, _size_each_state) = state_for_file;
}
}
Vector_To_File(goal_vector, filename_goalvec);
//write out all controls to file
VectorXd init_controls(_size_each_control*(_num_worlds-1));
for (int j = 0; j < current_controls.size(); j++)
{
vector<Control*> cu = current_controls[j];
VectorXd vu;
current_states[0][j]->ControlToVectorXd(cu, vu);
vu = current_states[0][j]->JacobianControlStripper(vu);
assert(vu.size() == _size_each_control);
init_controls.segment(j*_size_each_control, _size_each_control) = vu;
}
Vector_To_File(init_controls, filename_initctrls);
//prepare to execute solver (MATLAB)
char filename_statevec_thisiter[256];
sprintf(filename_statevec_thisiter, "%s/%s_%s%d.txt", SQP_BASE_FOLDER, _namestring, FILENAME_STATEVEC_BASE, 1);
//char matlab_command[1024];
//sprintf(matlab_command, "%s -nodisplay -nodesktop -nojvm -r \"solve_sparse(%d, %d, \'%s\', %d, %d, \'%s\', \'%s\', %d, %d, %d)\"", MATLAB_INSTALL, _all_trans.rows(), _all_trans.cols(), filename_alltrans, goal_vector.rows(), goal_vector.cols(), filename_goalvec, filename_statevec_thisiter, _num_worlds, _size_each_state, _size_each_control);
//char matlab_command[1024];
//sprintf(matlab_command, "java -jar MatlabClient.jar \"solve_sparse(%d, %d, \'%s\', %d, %d, \'%s\', \'%s\', %d, %d, %d)\"", _all_trans.rows(), _all_trans.cols(), filename_alltrans, goal_vector.rows(), goal_vector.cols(), filename_goalvec, filename_statevec_thisiter, _num_worlds, _size_each_state, _size_each_control);
char python_command[1024];
sprintf(python_command, "python ../MotionPlanning/SQPSolver.py solver %d %d \'%s\' %d %d \'%s\' \'%s\' \'%s\' %d %d %d %d %f %f %f %f %f %f %f", _all_trans.rows(), _all_trans.cols(), filename_alltrans, goal_vector.rows(), goal_vector.cols(), filename_goalvec, filename_initctrls, filename_statevec_thisiter, _num_traj, _num_worlds, _size_each_state, _size_each_control, LAMBDA_U, LAMBDA_U_DOT, LAMBDA_DIST_FROM_GOAL, LAMBDA_DIST_FROM_PAIR, STATE_INIT_CONSTRAINT, TRANSL_INIT_CONSTRAINT, ROT_INIT_CONSTRAINT );
std::cout << "command: " << python_command << std::endl;
#if TIMER_ENABLED
double JCT = GetClock();
StartClock();
#endif
//run solver (MATLAB) // NONO! ITS PYTHON/MOSEK!
//int return_value = system(matlab_command);
int return_value = system(python_command);
#if TIMER_ENABLED
double MST = GetClock();
StartClock();
#endif
//Read solver output
File_To_Vector(filename_statevec_thisiter, new_states);
vector<vector<VectorXd> > sqp_intermediate_states;
sqp_intermediate_states.resize(_num_traj);
//copy out new states
for (int j = 0; j < sqp_intermediate_states.size(); j++) {
sqp_intermediate_states[j].resize(_num_worlds-2);
for (int i = 0; i < _num_worlds-2; i++) {
sqp_intermediate_states[j][i] = new_states.segment((_num_worlds-2)*j + _size_each_state*i, _size_each_state);
}
}
//update controls
vector<VectorXd> new_controls;
new_controls.resize(_num_worlds-1);
for (int i=0; i < _num_worlds-1; i++)
{
new_controls[i] = new_states.segment(_num_traj*_size_each_state*(_num_worlds-2) + i*_size_each_control, _size_each_control);
}
vector<vector<Control*> > c_new_controls;
for (int i = 0; i < current_controls.size(); i++) {
vector<Control*> cu ;
cu.resize(2);
VectorXd vu = current_states[0][i]->JacobianControlWrapper(new_controls[i]);
current_states[0][i]->VectorXdToControl(vu, cu);
for (int j = 0; j < 2; j++) {
cu[j]->setButton(UP, current_controls[i][j]->getButton(UP));
delete current_controls[i][j];
}
current_controls[i].clear();
c_new_controls.push_back(cu);
}
current_controls = c_new_controls;
#if COLOCATION_METHOD
//take sqp takes, transform to real states, and iterate
/*vector< vector<World*> > newStates;
newStates.resize(current_states.size());
for (int j = 0; j < newStates.size(); j++) {
newStates[j].resize(current_states[j].size());
for (int i = 0; i < current_states[j].size(); i++) {
newStates[j][i] = new World(*current_states[j][i]);
delete current_states[j][i]; // TODO: Dependent on memory leak above
current_states[j][i] = NULL;
}
current_states[j].resize(0);
}
current_states.resize(0);*/
//TODO: MOVE THIS OUT OF COLOCATION METHOD!!!
for (int j = 0; j < current_states.size(); j++) {
vector<World*> openLoopWorlds;
openLoopController(current_states[j], current_controls, openLoopWorlds);
openLoopWorlds.pop_back();
openLoopWorlds.push_back(new World(*current_states[j].back()));
for (int i = 0; i < current_states[j].size(); i++) {
delete current_states[j][i];
}
current_states[j] = openLoopWorlds;
}
/*cout << "Projecting SQP States into Legal States" << endl;
boost::thread_group group;
for (int j = 0; j < sqp_intermediate_states.size(); j++) {
for (int i = 0; i < sqp_intermediate_states[j].size(); i++) {
group.create_thread(boost::bind(setWorldFromState,
newStates[j][i+1], &sqp_intermediate_states[j][i]));
}
}
group.join_all();
current_states = newStates;
*/
for (int j = 0; j < current_jacobians.size(); j++) {
for (int i = 0; i < current_jacobians[j].size(); i++) {
current_jacobians[j][i] = MatrixXd();
}
}
#else
assert(false); // not implemented for SHOOTING_METHOD
#endif
#if TIMER_ENABLED
cout << "Jacobian Computation Time: " << JCT << endl;
cout << "MATLAB Solver Time: " << MST << endl;
cout << "Total Solver Time: " << JCT + MST + GetClock() << endl;
#endif
}
/* Constructor for CARBON_GUI class. */
CARBON_GUI::CARBON_GUI(int argc, char **argv, Stream_mixer *mix)
: GUI(argc,argv,mix)
{
/* initialization stuff */
jmix = mix;
memset(myLcd,0,sizeof(myLcd));
memset(myPos,0,sizeof(myPos));
vumeter=0;
vuband=0;
selectedChannel=NULL;
memset(channel,0,sizeof(channel));
playlistManager=new PlaylistManager();
msgList=new Linklist();
/* init mutex used when accessing the statusbox buffer ...
* this is needed because other threads can try to write status messages concurrently
*/
if(pthread_mutex_init(&_statusLock,NULL) == -1) {
error("error initializing POSIX thread mutex creating a new CarbonChannel");
QuitApplicationEventLoop();
}
// Create a Nib reference
err = CreateNibReference(CFSTR("main"), &nibRef);
if(err != noErr) error("Can't get NIB reference to obtain gui controls!!");
// Create the MainWindow using nib resource file
err = CreateWindowFromNib(nibRef, CFSTR("MainWindow"), &window);
if(err != noErr) {
error("Can't create MainWindow!!");
QuitApplicationEventLoop();
}
else {
msg = new CarbonMessage(nibRef);
/* make the main window also the frontmost one */
BringToFront(window);
init_controls();
/* now create the menu to use for the menubar ... it's stored in nib */
err=CreateMenuFromNib(nibRef,CFSTR("MenuBar"),&mainMenu);
if(err!=noErr) {
msg->error("Can't create main menu (%d)!!",err);
}
/* install vumeter controls */
setupVumeters();
/* and the status box */
setupStatusWindow();
bufferInspector = new BufferInspector(window,nibRef,jmix);
/* now we have to group windows together so if all are visible they will also be layered together */
err=CreateWindowGroup(kWindowGroupAttrLayerTogether,&mainGroup);
err=SetWindowGroup(window,mainGroup);
err=SetWindowGroup(vumeterWindow,mainGroup);
err=SetWindowGroup(statusWindow,mainGroup);
err=SetWindowGroup(bufferInspector->window,mainGroup);
SetWindowGroupOwner(mainGroup,window);
/* let's create a channel window for each active input channel */
unsigned int i;
bool cc=false;
for (i=0;i<MAX_CHANNELS;i++) {
strcpy(ch_lcd[i],"00:00:00");
if(jmix->chan[i]) {
CarbonChannel *newChan = new CarbonChannel(jmix,this,nibRef,i);
channel[i] = newChan;
/*
if(i > 0) {
RepositionWindow(channel[i]->window,channel[i-1]->window,kWindowCascadeOnParentWindow);
}
else {
RepositionWindow(channel[i],window,kWindowCascadeOnParentWindow);
}
*/
cc=true;
}
else {
channel[i] = NULL;
}
}
/* Ok, once MainWindow has been created and we have instantiated all acrive input channels,
* we need an instance of CarbonStream to control the stream option window */
streamHandler = new CarbonStream(jmix,window,nibRef);
/* by default we want at leat one active channel */
if(!cc) new_channel();
aboutWindow = new AboutWindow(window,nibRef);
// The window was created hidden so show it.
ShowWindow( window );
}
}
int main( int argc, char* args[] )
{
// this is the last time (SDL_Getticks()) that beasts were evaluated
int beastLastEval=0;
// this is how many times per second the beasts should be evaluated
int beastEvalsPerSec = 30;
paused = 0;
//get a random seed.
sgenrand(time(NULL));
//mouse variables and cell types
int x, y, sleepTime = 0, countVar = 0;
//mouse is held variables
int mouseStatusLeft = 0, mouseStatusRight = 0;
// these keep track of the position of the right mouse button when it was clicked down
int mouseRDx;
int mouseRDy;
//make sure the program waits for a quit
int quit = false;
init_mats();
init_graphics();
init_controls();
//Initialize
if( init() == false ) return 1;
//Load the files
if( load_files() == false ) return 2;
//Update the screen
if( SDL_Flip( screen ) == -1 ) return 3;
gen_world();
// these keep track of the WASD keys.
int keyw=0, keya=0, keys=0, keyd=0;
int key; // used as a stand-int for the verbose event.key.keysym.sym
bool keyF3=true;
//these are used to calculating and keeping track of the FPS
int ticksSinceLastFPSUpdate = 0;
int cumulativeFrames = 0;
int currentTicks = 0;
SDL_Rect screenRect;
screenRect.x = screenRect.y = 0;
screenRect.w = SCREEN_WIDTH;
screenRect.h = SCREEN_HEIGHT;
//While the user hasn't quit
while(1){
//While there's an event to handle
while( SDL_PollEvent( &event ) ){
//If the user has Xed out the window
if( event.type == SDL_QUIT || quit == true ){
//Quit the program
clean_up();
return 0;
}
if( event.type == SDL_MOUSEBUTTONDOWN ){ /// mouse down
x = event.motion.x;
y = event.motion.y;
if( event.button.button == SDL_BUTTON_LEFT ){
mouseStatusLeft = 1;
}
else if( event.button.button == SDL_BUTTON_RIGHT ){
mouseStatusRight = 1;
mouseRDx = x;
mouseRDy = y;
}
else if( event.button.button == SDL_BUTTON_WHEELUP )
;//zoom_in(x,y);
else if( event.button.button == SDL_BUTTON_WHEELDOWN )
;//zoom_out(x,y);
}
else if(event.type == SDL_MOUSEBUTTONUP){ /// mouse up
x = event.motion.x;
y = event.motion.y;
if( event.button.button == SDL_BUTTON_LEFT ){
mouseStatusLeft = 0;
}
else if( event.button.button == SDL_BUTTON_RIGHT ){
mouseStatusRight = 0;
}
}
else if( event.type == SDL_MOUSEMOTION ){ /// mouse motion
x = event.motion.x;
y = event.motion.y;
/*
// if the alt key (camera panning key) is down and the coordinates have changed, then let the screen be panned!
if(alt && x != mouse_x_when_pan && y != mouse_y_when_pan){
// this adjusts the x-axis camera (this scales with the CELL_SIZE)
camera_x += (x-mouse_x_when_pan+remainder_panning_motion_x)/CELL_SIZE;
camera_y += (y-mouse_y_when_pan+remainder_panning_motion_y)/CELL_SIZE;
//calculate the remainders of the mouse motion.
// these values represent the motion of the mouse that is not utilized by the discrete nature of the operation on the camera_x and camera_y values.
remainder_panning_motion_x = (x-mouse_x_when_pan+remainder_panning_motion_x) - (int)((x-mouse_x_when_pan+remainder_panning_motion_x)/CELL_SIZE)*CELL_SIZE;
remainder_panning_motion_y = (y-mouse_y_when_pan+remainder_panning_motion_y) - (int)((y-mouse_y_when_pan+remainder_panning_motion_y)/CELL_SIZE)*CELL_SIZE;
// make sure the camera is not out of bounds.
verify_camera();
//reset the user's curcor position to the original position the curcor was in when the user started panning the camera
SDL_WarpMouse(mouse_x_when_pan, mouse_y_when_pan);
}
*/
}
else if(event.type == SDL_VIDEORESIZE){ /// window resize
// don't resize the grid.
//float new_cell_size = CELL_SIZE * event.resize.h/((float)SCREEN_HEIGHT); // adjust the pixel size.
//if(new_cell_size - ((int)new_cell_size) >= 0.5f) CELL_SIZE = new_cell_size + 1;
//else CELL_SIZE = new_cell_size;
screenRect.w = SCREEN_WIDTH = event.resize.w;
screenRect.h = SCREEN_HEIGHT = event.resize.h;
set_window_size(event.resize.w, event.resize.h); // set window to correct dimensions
}
if( event.type == SDL_KEYDOWN ){ ///keyboard event
key = event.key.keysym.sym;
if (key == controlsGame.pan[d_up]) camera_pan(d_up); // pan up
else if(key == controlsGame.pan[d_down]) camera_pan(d_down); // pan down
else if(key == controlsGame.pan[d_left]) camera_pan(d_left); // pan left
else if(key == controlsGame.pan[d_right]) camera_pan(d_right); // pan right
else if(key == controlsGame.debug) keyF3 ^= 1; // toggle the keyF3 state
}
if( event.type == SDL_KEYUP ){ ///keyboard event
// nothing here yet
}
} // end while(event)
//no more events to handle at the moment.
if(mouseStatusLeft==true && beast_find_at_cell(x/CELL_SIZE+cameraX,y/CELL_SIZE+cameraY) == NULL){
// add beast function
struct beastData *myBeast;
myBeast = beast_create(NULL);
if(myBeast != NULL ){
myBeast->x = x/CELL_SIZE + cameraX;
myBeast->y = y/CELL_SIZE + cameraY;
}
}
if(SDL_GetTicks()-beastLastEval > 1000/beastEvalsPerSec){
// evaluate all beasts
beasts_evaluate();
beastLastEval = SDL_GetTicks();
}
// print the grid data
print_grid(screen);
print_beasts(screen);
print_beasts_targets(screen);
// print debugging information
if(keyF3) print_debugging_information(screen);
//updates the screen
SDL_Flip( screen );
SDL_FillRect(screen, &screenRect, 0);
//----------------------------------------------------
// FPS calculation and variable handling
//----------------------------------------------------
currentTicks = SDL_GetTicks();
// it is officially the next second
if(currentTicks >= ticksSinceLastFPSUpdate + 1000){
// calculate the FPS
FPS = cumulativeFrames;//(cumulativeFrames*1000 ) / (currentTicks-ticksSinceLastFPSUpdate);
cumulativeFrames=0; // reset cumulative amount of frames
ticksSinceLastFPSUpdate = currentTicks; // reset the last FPS update to the number of ticks now.
}
cumulativeFrames++;
}// end while(quit == false)
//Free the surface and quit SDL
clean_up();
return 0;
}
