void basicBayes::runRealWorld(){
bool realWorld = false;
if (realWorld == false){
//When you want to use a simulation for the real world
int realModel = 3;
std::cout << "this is the real world" << std::endl;
if (realModel == 0){
BasicDemo world;
realCurrentState_ = world.runSimulation(realPrevState_,realAction_);
}
else if (realModel == 1){
realCurrentState_ = realPrevState_;
}
else if (realModel == 2){
BasicDemoRev world;
std::vector<double> temp;
temp.push_back(atan2(realPrevState_[1],realPrevState_[0]-0.44));
realCurrentState_ = world.runSimulation(temp,realAction_);
}
else if (realModel == 3){
BasicDemoPris world;
std::vector<double> temp;
temp.push_back(realPrevState_[0]);
realCurrentState_ = world.runSimulation(temp,realAction_);
}
}
//When you want to use the robot for the real world
//translation of the coordinate systems is required
//bullet: +x forward, +y up, +z to the right, realAction_, realCurrentState_
//pr2: +x forward, +y to the left, +z up, translatedAction, translatedCurrentState
else if (realWorld == true){
std::vector<double> translatedAction;
translatedAction.push_back(realAction_[0]); //b to pr2: x+ to x+
translatedAction.push_back(-realAction_[1]); // +z to +y
//translatedAction.push_back(0.0);
//translatedAction.push_back(0.0);
translatedAction.push_back(0.0); //this is just +z for the pr2 which isn't read anyways
sendRequest(translatedAction);
//realCurrentState_.clear();
//realCurrentState_.shrink_to_fit();
std::vector<double> translatedCurrentState;
std::cout << "Now, waiting for a response from the robot" << std::endl;
while(!getResponse(translatedCurrentState)){
//std::cout << "waiting for a response from the robot" << std::endl;
}
realCurrentState_[0] = translatedCurrentState[0]; //pr2 to b: x+ to x+
realCurrentState_[1] = -translatedCurrentState[1]; // +y to +z
std::cout << "observation from robot: " << realCurrentState_[0] << " , " << realCurrentState_[1] << std::endl;
}
}
std::vector<double> basicBayes::stateTransition(std::vector<double> prevState, std::vector<double> action){
//this funciton is problem specific
//make this do something eventually
std::vector<double> nextState;
double sum = std::accumulate(action.begin(),action.end(),(double) 0);
if (sum == -100.0) {
//Didn't do anything kind of this has to change
nextState = prevState;
}
else {
//this is saying there is no probability to transition between models
//prevState[0] is the model. 0.0 is free, 1.0 is fixed, 2.0 is revolute, 3.0 is prismatic
//WOM means without model.
std::vector<double> prevStateWOM;
prevStateWOM.assign(prevState.begin()+1,prevState.end()); //state without the model which is always the first entry in the state
std::vector<double> tempWorldState;
if (prevState[0]==0.0) {
//model: free
BasicDemo world;
tempWorldState = world.runSimulation(prevStateWOM,action);
nextState.push_back(0.0); //this is saying you end up in the same state
for(size_t j=0;j<tempWorldState.size();j++){
nextState.push_back(tempWorldState[j]);
}
}
else if (prevState[0]==1.0) {
//model: fixed
nextState=prevState;
}
else if (prevState[0]==2.0) {
//model: revolute
BasicDemoRev world;
tempWorldState = world.runSimulation(prevStateWOM,action);
nextState.push_back(2.0); //this is saying you end up in the same state
for(size_t j=0;j<tempWorldState.size();j++){
nextState.push_back(tempWorldState[j]);
}
}
else if (prevState[0]==3.0) {
//model: prismatic
BasicDemoPris world;
tempWorldState = world.runSimulation(prevStateWOM,action);
nextState.push_back(3.0); //this is saying you end up in the same state
for(size_t j=0;j<tempWorldState.size();j++){
nextState.push_back(tempWorldState[j]);
}
}
//print out what the simulation returned each time
if (debug_print_){
std::cout << "the simulation returned:" << std::endl;
for(size_t j=0;j<tempWorldState.size();j++){
std::cout << tempWorldState[j] << "," << std::endl;
}
}
}
return nextState;
}