QSimTestGUI::QSimTestGUI(QKlamptDisplay* _display,SimTestBackend *_backend) :
QtGUIBase(_backend), display(_display)
{
//BaseT::backend = _backend;
// BaseT::width = w;
// BaseT::height = h;
sim = &_backend->sim;
assert(_backend != NULL);
assert(sim != NULL);
assert(sim->world != NULL);
_backend->gui = this;
driver_tool=new DriverEdit(sim->world);
connect(driver_tool,SIGNAL(SetDriverValue(int,float)),this,SLOT(SendDriverValue(int,float)));
//driver_tool->show();
log_options=new LogOptions();
connect(log_options,SIGNAL(ShowSensor(int)),this,SLOT(ShowSensor(int)));
connect(log_options,SIGNAL(HideSensor(int)),this,SLOT(HideSensor(int)));
connect(log_options,SIGNAL(toggle_measurement(int,int,bool)),this,SLOT(SendMeasurement(int,int,bool)));
connect(log_options,SIGNAL(toggle_measurement(int,int,bool)),this,SLOT(SendMeasurement(int,int,bool)));
if(sim->controlSimulators.size() > 0) {
RobotSensors sensors=sim->controlSimulators[0].sensors;
log_options->robotsensors=sensors;
}
log_options->GetSensors();
//log_options->show();
//UpdateMeasurements();
controller_dialog=new ControllerDialog(sim);
connect(controller_dialog,SIGNAL(SendControllerSetting(int,string,string)),this,SLOT(SendControllerSetting(int,string,string)));
connect(controller_dialog,SIGNAL(ControllerCommand(int,string,string)),this,SLOT(SendControllerCommand(int,string,string)));
connect(controller_dialog,SIGNAL(MakeConnect(int,QString,int,int)),this,SLOT(SendConnection(int,QString,int,int)));
UpdateGUI();
string appdataPath = AppUtils::GetApplicationDataPath("Klampt");
string viewFile = appdataPath + string("/simtest_view.txt");
const static int NR = 9;
const static char* rules [NR*3]= {"{type:key_down,key:c}","constrain_link","",
"{type:key_down,key:C}","constrain_link_point","",
"{type:key_down,key:d}","delete_constraint","",
"{type:key_down,key:p}","print_config","",
"{type:key_down,key:a}","advance","",
"{type:key_down,key:\" \"}","command_pose","",
"{type:key_down,key:v}","save_view",viewFile.c_str(),
"{type:key_down,key:V}","load_view",viewFile.c_str(),
"{type:button_toggle,button:simulate,checked:_0}","toggle_simulate","",
};
for(int i=0;i<NR;i++) {
AnyCollection c;
bool res=c.read(rules[i*3]);
Assert(res == true);
AddCommandRule(c,rules[i*3+1],rules[i*3+2]);
}
connect(&idle_timer, SIGNAL(timeout()),this,SLOT(OnIdleTimer()));
idle_timer.start(0);
}
bool GLUIRealTimePlannerGUI::Initialize()
{
if(!BaseT::Initialize()) return false;
glui = GLUI_Master.create_glui_subwindow(main_window,GLUI_SUBWINDOW_RIGHT);
glui->set_main_gfx_window(main_window);
AddControl(glui->add_button("New target"),"new_target");
AddControl(glui->add_checkbox("Draw desired"),"draw_desired");
AddControl(glui->add_checkbox("Draw commanded"),"draw_desired");
AddControl(glui->add_checkbox("Draw UI"),"draw_ui");
AddControl(glui->add_checkbox("Draw path"),"draw_path");
AddControl(glui->add_checkbox("Draw contacts"),"draw_contacts");
GLUI_Spinner* spinner = glui->add_spinner("Collision margin",GLUI_SPINNER_FLOAT);
spinner->set_float_limits(0.0,1.0);
AddControl(spinner,"collision_margin");
AnyCollection c;
bool res=c.read("{type:button_press,button:new_target}");
Assert(res == true);
AddCommandRule(c,"new_target","");
res=c.read("{type:widget_value,widget:collision_margin,value:_1}");
Assert(res == true);
AddCommandRule(c,"set_collision_margin","_1");
printf("Done initializing...\n");
return true;
}
bool read(const char* fn) {
ifstream in(fn,ios::in);
if(!in) return false;
AnyCollection newEntries;
if(!newEntries.read(in)) return false;
merge(newEntries);
return true;
}
bool ProgramSettings::read(const char* fn)
{
string appDataPath = GetApplicationDataPath(applicationName.c_str(),version.c_str());
ifstream in(((appDataPath+"/")+fn).c_str(),ios::in);
if(!in) return false;
AnyCollection newEntries;
if(!newEntries.read(in)) return false;
merge(newEntries);
return true;
}
bool HaltingCondition::LoadJSON(const string& str)
{
AnyCollection items;
if(!items.read(str.c_str())) return false;
items["foundSolution"].as(foundSolution);
items["maxIters"].as(maxIters);
items["timeLimit"].as(timeLimit);
items["costThreshold"].as(costThreshold);
items["costImprovementPeriod"].as(costImprovementPeriod);
items["costImprovementThreshold"].as(costImprovementThreshold);
return true;
}
bool Service::WaitForMessage(AnyCollection& message,double timeout)
{
if(!worker) {
fprintf(stderr,"%s::WaitForMessage(): Not connected\n",Name());
return false;
}
Timer timer;
while(timer.ElapsedTime() < timeout) {
if(!worker->initialized) {
fprintf(stderr,"%s::WaitForMessage(): Abnormal disconnection\n",Name());
return false;
}
//read new messages
if(worker->UnreadCount() > 0) {
if(onlyProcessNewest) {
string str = worker->Newest();
stringstream ss(str);
AnyCollection msg;
if(!msg.read(ss)) {
fprintf(stderr,"%s::WaitForMessage(): Got an improperly formatted string\n",Name());
cout<<"String = \""<<str<<"\""<<endl;
return false;
}
if(!OnMessage(msg)) {
fprintf(stderr,"%s::WaitForMessage(): OnMessage returned false\n",Name());
return false;
}
message = msg;
return true;
}
else {
vector<string> msgs = worker->New();
for(size_t i=0;i<msgs.size();i++) {
stringstream ss(msgs[i]);
AnyCollection msg;
if(!msg.read(ss)) {
fprintf(stderr,"%s::WaitForMessage(): Got an improperly formatted string\n",Name());
return false;
}
if(!OnMessage(msg)) {
fprintf(stderr,"%s::WaitForMessage(): OnMessage returned false\n",Name());
return false;
}
}
message = msgs[0];
return true;
}
ThreadSleep(SSPP_MESSAGE_WAIT_TIME);
}
}
return false;
}
string HaltingCondition::SaveJSON() const
{
AnyCollection items;
items["foundSolution"] = foundSolution;
items["maxIters"] = maxIters;
items["timeLimit"] = timeLimit;
items["costThreshold"] = costThreshold;
items["costImprovementPeriod"] = costImprovementPeriod;
items["costImprovementThreshold"] = costImprovementThreshold;
stringstream ss;
items.write_inline(ss);
return ss.str();
}
int Service::Process()
{
if(!worker) {
fprintf(stderr,"%s::Process(): Not connected\n",Name());
return -1;
}
if(!worker->initialized) {
fprintf(stderr,"%s::Process(): Abnormal disconnection\n",Name());
return -1;
}
//read new messages
if(worker->UnreadCount() > 0) {
if(onlyProcessNewest) {
string str = worker->Newest();
stringstream ss(str);
AnyCollection msg;
if(!msg.read(ss)) {
fprintf(stderr,"%s::Process(): Got an improperly formatted string\n",Name());
cerr<<" Offending string: \""<<str<<"\""<<endl;
return -1;
}
if(!OnMessage(msg)) {
fprintf(stderr,"%s::Process(): OnMessage returned false\n",Name());
return -1;
}
return 1;
}
else {
vector<string> msgs = worker->New();
for(size_t i=0;i<msgs.size();i++) {
stringstream ss(msgs[i]);
AnyCollection msg;
if(!msg.read(ss)) {
fprintf(stderr,"%s::Process(): Got an improperly formatted string\n",Name());
cerr<<" Offending string: \""<<msgs[i]<<"\""<<endl;
return -1;
}
if(!OnMessage(msg)) {
fprintf(stderr,"%s::Process(): OnMessage returned false\n",Name());
return -1;
}
}
return (int)msgs.size();
}
}
return 0;
}
bool SavePlannerObjective(PlannerObjectiveBase* obj,AnyCollection& msg)
{
msg.clear();
string type = string(obj->TypeString());
msg["type"] = type;
if(type == "time") {
}
else if(type == "term_time") {
msg["data"] = dynamic_cast<TerminalTimeObjective*>(obj)->tgoal;
}
else if(type == "config") {
msg["data"] = vector<Real>(dynamic_cast<ConfigObjective*>(obj)->qgoal);
}
else if(type == "velocity") {
msg["data"] = vector<Real>(dynamic_cast<VelocityObjective*>(obj)->vgoal);
}
else if(type == "composite") {
CompositeObjective* cobj = dynamic_cast<CompositeObjective*>(obj);
msg["norm"] = cobj->norm;
for(size_t i=0;i<cobj->components.size();i++) {
if(!SavePlannerObjective(cobj->components[i],msg["components"][i])) {
fprintf(stderr,"SavePlannerObjective: error saving component %d of composite objective\n",(int)i);
return false;
}
}
msg["weights"] = cobj->weights;
}
else if(type == "cartesian") {
CartesianObjective* cobj = dynamic_cast<CartesianObjective*>(obj);
msg["link"] = cobj->ikGoal.link;
Convert(cobj->ikGoal.localPosition,msg["plocal"]);
Convert(cobj->ikGoal.endPosition,msg["pworld"]);
}
else if(type == "ik") {
IKObjective* cobj = dynamic_cast<IKObjective*>(obj);
Convert(cobj->ikGoal,msg["data"]);
}
else {
fprintf(stderr,"SavePlannerObjective: unknown objective type %s\n",type.c_str());
return false;
}
return true;
}
template <> inline bool isMatchingElement(const AnyCollection& collection, const Element& element)
{
return collection.elementMatches(element);
}
PlannerObjectiveBase* LoadPlannerObjective(AnyCollection& msg,Robot* robot)
{
string type;
bool res = msg["type"].as<string>(type);
if(!res) {
fprintf(stderr,"LoadPlannerObjective: message didn't contain 'type' member\n");
return NULL;
}
if(type == "time") {
return new TimeObjective();
}
else if(type == "term_time") {
return new TerminalTimeObjective((Real)msg["data"]);
}
else if(type == "config") {
vector<Real> q;
if(!msg["data"].asvector(q)) {
fprintf(stderr,"LoadPlannerObjective: config message didn't contain 'data' member\n");
return NULL;
}
if(robot && q.size() != robot->links.size()) {
fprintf(stderr,"LoadPlannerObjective: config message contains desired configuration of incorrect length %d vs %d\n",(int)q.size(),(int)robot->links.size());
return NULL;
}
return new ConfigObjective(Vector(q));
}
else if(type == "velocity") {
vector<Real> v;
if(!msg["data"].asvector(v)) {
fprintf(stderr,"LoadPlannerObjective: velocity message didn't contain 'data' member\n");
return NULL;
}
if(robot && v.size() != robot->links.size()) {
fprintf(stderr,"LoadPlannerObjective: velocity message contains desired velocity of incorrect length %d vs %d\n",(int)v.size(),(int)robot->links.size());
return NULL;
}
return new VelocityObjective(Vector(v));
}
else if(type == "composite") {
vector<SmartPointer<AnyCollection> > items;
AnyCollection msgcomp = msg["components"];
if(msgcomp.depth() == 0) {
fprintf(stderr,"LoadPlannerObjective: composite message didn't contain 'components' member\n");
return NULL;
}
msgcomp.enumerate(items);
vector<SmartPointer<PlannerObjectiveBase> > components;
for(size_t i=0;i<items.size();i++) {
components.push_back(LoadPlannerObjective(*items[i],robot));
if(components.back()==NULL) return NULL;
}
CompositeObjective* obj = new CompositeObjective;
if(msg["norm"].as<Real>(obj->norm)) {}
if(msg["weights"].asvector(obj->weights)) {}
else obj->weights.resize(components.size(),1);
obj->components=components;
return obj;
}
else {
fprintf(stderr,"LoadPlannerObjective: message of unknown type %s\n",type.c_str());
return NULL;
}
}
