state_space.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planner/state_space/state_space.h>
00002 
00003 #include <vigir_footstep_planning_lib/math.h>
00004 
00005 #include <vigir_footstep_planning_plugins/plugin_aggregators/robot_model.h>
00006 #include <vigir_footstep_planning_plugins/plugin_aggregators/post_processor.h>
00007 #include <vigir_footstep_planning_plugins/plugin_aggregators/step_cost_estimator.h>
00008 #include <vigir_footstep_planning_plugins/plugin_aggregators/heuristic.h>
00009 
00010 
00011 
00012 namespace vigir_footstep_planning
00013 {
00014 StateSpace::StateSpace(const EnvironmentParameters& params, std::vector<int*>& state_ID2_index_mapping)
00015   : params(params)
00016   , frame_id("/world")
00017   , ivIdPlanningStart(-1)
00018   , ivIdPlanningGoal(-1)
00019   , ivIdStartFootLeft(-1)
00020   , ivIdStartFootRight(-1)
00021   , ivIdGoalFootLeft(-1)
00022   , ivIdGoalFootRight(-1)
00023   , state_ID2_index_mapping(state_ID2_index_mapping)
00024   , ivpStateHash2State(new std::vector<PlanningState*>[params.hash_table_size])
00025   , ivHeuristicExpired(false)
00026   , ivRandomNodeDist(params.random_node_distance / params.cell_size)
00027 {
00028 }
00029 
00030 StateSpace::~StateSpace()
00031 {
00032   if (ivpStateHash2State)
00033   {
00034     delete[] ivpStateHash2State;
00035     ivpStateHash2State = NULL;
00036   }
00037 }
00038 
00039 void StateSpace::reset()
00040 {
00041   for(unsigned int i = 0; i < ivStateId2State.size(); ++i)
00042   {
00043     if (ivStateId2State[i])
00044     {
00045       delete ivStateId2State[i];
00046     }
00047   }
00048   ivStateId2State.clear();
00049 
00050   if (ivpStateHash2State)
00051   {
00052     for(int i = 0; i < params.hash_table_size; ++i)
00053       ivpStateHash2State[i].clear();
00054   }
00055 
00056   state_ID2_index_mapping.clear();
00057 
00058   ivRandomStates.clear();
00059 
00060   ivIdPlanningStart = -1;
00061   ivIdPlanningGoal = -1;
00062 
00063   ivIdGoalFootLeft = -1;
00064   ivIdGoalFootRight = -1;
00065   ivIdStartFootLeft = -1;
00066   ivIdStartFootRight = -1;
00067 
00068   ivHeuristicExpired = true;
00069 }
00070 
00071 void StateSpace::setFrameId(const std::string& frame_id)
00072 {
00073   this->frame_id = frame_id;
00074 }
00075 
00076 std::pair<int, int> StateSpace::updateGoal(const State& foot_left, const State& foot_right)
00077 {
00078   // keep the old IDs
00079   int goal_foot_id_left = ivIdGoalFootLeft;
00080   int goal_foot_id_right = ivIdGoalFootRight;
00081 
00082   // update the states for both feet (if necessary)
00083   goal_foot_left = getHashEntry(foot_left);
00084   if (goal_foot_left == NULL)
00085     goal_foot_left = createNewHashEntry(foot_left);
00086   goal_foot_right = getHashEntry(foot_right);
00087   if (goal_foot_right == NULL)
00088     goal_foot_right = createNewHashEntry(foot_right);
00089   ivIdGoalFootLeft = goal_foot_left->getId();
00090   ivIdGoalFootRight = goal_foot_right->getId();
00091   // check if everything has been set correctly
00092   assert(ivIdGoalFootLeft != -1);
00093   assert(ivIdGoalFootRight != -1);
00094 
00095   // if using the forward search a change of the goal states involves an
00096   // update of the heuristic
00097   if (params.forward_search)
00098   {
00099     // check if the goal states have been changed
00100     if (goal_foot_id_left != ivIdGoalFootLeft || goal_foot_id_right != ivIdGoalFootRight)
00101     {
00102       ivHeuristicExpired = true;
00103       //setStateArea(*goal_foot_left, *goal_foot_right);
00104     }
00105   }
00106 
00107   return std::pair<int, int>(ivIdGoalFootLeft, ivIdGoalFootRight);
00108 }
00109 
00110 std::pair<int, int> StateSpace::updateStart(const State& foot_left, const State& foot_right)
00111 {
00112   // keep the old IDs
00113   int start_foot_id_left = ivIdStartFootLeft;
00114   int start_foot_id_right = ivIdStartFootRight;
00115 
00116   // update the states for both feet (if necessary)
00117   start_foot_left = getHashEntry(foot_left);
00118   if (start_foot_left == NULL)
00119     start_foot_left = createNewHashEntry(foot_left);
00120   start_foot_right = getHashEntry(foot_right);
00121   if (start_foot_right == NULL)
00122     start_foot_right = createNewHashEntry(foot_right);
00123   ivIdStartFootLeft = start_foot_left->getId();
00124   ivIdStartFootRight = start_foot_right->getId();
00125   // check if everything has been set correctly
00126   assert(ivIdStartFootLeft != -1);
00127   assert(ivIdStartFootRight != -1);
00128 
00129   // if using the backward search a change of the start states involves an
00130   // update of the heuristic
00131   if (!params.forward_search)
00132   {
00133     // check if the start states have been changed
00134     if (start_foot_id_left != ivIdStartFootLeft || start_foot_id_right != ivIdStartFootRight)
00135     {
00136       ivHeuristicExpired = true;
00137       //setStateArea(*start_foot_left, *start_foot_right);
00138     }
00139   }
00140 
00141   return std::pair<int, int>(ivIdStartFootLeft, ivIdStartFootRight);
00142 }
00143 
00144 void StateSpace::setPlannerStartAndGoal(unsigned int start_foot_selection)
00145 {
00146   if (start_foot_selection == msgs::StepPlanRequest::AUTO)
00147   {
00148     State robot_start;
00149     getStartState(robot_start);
00150 
00151     State robot_goal;
00152     getGoalState(robot_goal);
00153 
00154     tf::Transform direction = robot_start.getPose().inverse() * robot_goal.getPose();
00155     if (direction.getOrigin().getY() > 0.0)
00156       start_foot_selection = msgs::StepPlanRequest::RIGHT;
00157     else
00158       start_foot_selection = msgs::StepPlanRequest::LEFT;
00159   }
00160 
00161   if (start_foot_selection == msgs::StepPlanRequest::LEFT) // move first right foot
00162   {
00163     ivIdPlanningStart = ivIdStartFootLeft;
00164     start_foot_right->setSuccState(start_foot_left);
00165     start_foot_left->setPredState(start_foot_right);
00166 
00167     ivIdPlanningGoal = ivIdGoalFootRight;
00168     goal_foot_right->setSuccState(goal_foot_left);
00169     goal_foot_left->setPredState(goal_foot_right);
00170   }
00171   else if (start_foot_selection == msgs::StepPlanRequest::RIGHT) // move first left foot
00172   {
00173     ivIdPlanningStart = ivIdStartFootRight;
00174     start_foot_left->setSuccState(start_foot_right);
00175     start_foot_right->setPredState(start_foot_left);
00176 
00177     ivIdPlanningGoal = ivIdGoalFootLeft;
00178     goal_foot_left->setSuccState(goal_foot_right);
00179     goal_foot_right->setPredState(goal_foot_left);
00180   }
00181   else
00182     ROS_ERROR("[setPlannerStartAndGoal] Unknown selection mode: %u", start_foot_selection);
00183 }
00184 
00185 bool StateSpace::getState(unsigned int id, State &s) const
00186 {
00187   if (id >= ivStateId2State.size())
00188     return false;
00189 
00190   s = ivStateId2State[id]->getState();
00191   return true;
00192 }
00193 
00194 bool StateSpace::getStartState(State &left, State &right) const
00195 {
00196   if (!getState(ivIdStartFootLeft, left))
00197     return false;
00198   if (!getState(ivIdStartFootRight, right))
00199     return false;
00200   return true;
00201 }
00202 
00203 bool StateSpace::getStartState(State &robot) const
00204 {
00205   State left;
00206   State right;
00207 
00208   if (!getState(ivIdStartFootLeft, left))
00209     return false;
00210   if (!getState(ivIdStartFootRight, right))
00211     return false;
00212 
00213   robot.setX(0.5*(left.getX()+right.getX()));
00214   robot.setY(0.5*(left.getY()+right.getY()));
00215   robot.setZ(0.5*(left.getZ()+right.getZ()));
00216   robot.setYaw(0.5*(left.getYaw()+right.getYaw()));
00217   return true;
00218 }
00219 
00220 bool StateSpace::getGoalState(State &left, State &right) const
00221 {
00222   if (!getState(ivIdGoalFootLeft, left))
00223     return false;
00224   if (!getState(ivIdGoalFootRight, right))
00225     return false;
00226   return true;
00227 }
00228 
00229 bool StateSpace::getGoalState(State &robot) const
00230 {
00231   State left;
00232   State right;
00233 
00234   if (!getState(ivIdGoalFootLeft, left))
00235     return false;
00236   if (!getState(ivIdGoalFootRight, right))
00237     return false;
00238 
00239   robot.setX(0.5*(left.getX()+right.getX()));
00240   robot.setY(0.5*(left.getY()+right.getY()));
00241   robot.setZ(0.5*(left.getZ()+right.getZ()));
00242   robot.setYaw(0.5*(left.getYaw()+right.getYaw()));
00243   return true;
00244 }
00245 
00246 PlanningState *StateSpace::createNewHashEntry(const State& s)
00247 {
00248   PlanningState tmp(s, params.cell_size, params.angle_bin_size, params.hash_table_size);
00249   return createNewHashEntry(tmp);
00250 }
00251 
00252 PlanningState* StateSpace::createNewHashEntry(const PlanningState& s)
00253 {
00254   unsigned int state_hash = s.getHashTag();
00255   PlanningState* new_state = new PlanningState(s);
00256 
00257   boost::unique_lock<boost::shared_mutex> lock(hash_table_shared_mutex);
00258 
00259   size_t state_id = ivStateId2State.size();
00260   assert(state_id < (size_t)std::numeric_limits<int>::max());
00261 
00262   // insert the ID of the new state into the corresponding map
00263   new_state->setId(state_id);
00264   ivStateId2State.push_back(new_state);
00265 
00266   // insert the new state into the hash map at the corresponding position
00267   ivpStateHash2State[state_hash].push_back(new_state);
00268 
00269   int* entry = new int[NUMOFINDICES_STATEID2IND];
00270   state_ID2_index_mapping.push_back(entry);
00271 
00272   for(int i = 0; i < NUMOFINDICES_STATEID2IND; ++i)
00273     state_ID2_index_mapping[state_id][i] = -1;
00274 
00275   assert(state_ID2_index_mapping.size() - 1 == state_id);
00276 
00277   return new_state;
00278 }
00279 
00280 PlanningState* StateSpace::getHashEntry(const State& s)
00281 {
00282   PlanningState tmp(s, params.cell_size, params.angle_bin_size, params.hash_table_size);
00283   return getHashEntry(tmp);
00284 }
00285 
00286 PlanningState* StateSpace::getHashEntry(const PlanningState& s)
00287 {
00288   unsigned int state_hash = s.getHashTag();
00289   std::vector<PlanningState*>::const_iterator state_iter;
00290   boost::shared_lock<boost::shared_mutex> lock(hash_table_shared_mutex);
00291   for (state_iter = ivpStateHash2State[state_hash].begin(); state_iter != ivpStateHash2State[state_hash].end(); ++state_iter)
00292   {
00293     if (*(*state_iter) == s)
00294       return *state_iter;
00295   }
00296 
00297   return NULL;
00298 }
00299 
00300 PlanningState* StateSpace::createHashEntryIfNotExists(const PlanningState& s)
00301 {
00302   PlanningState* hash_entry = getHashEntry(s);
00303   if (hash_entry == NULL)
00304     hash_entry = createNewHashEntry(s);
00305 
00306   return hash_entry;
00307 }
00308 
00309 bool StateSpace::closeToStart(const PlanningState& from) const
00310 {
00311   if (from.getLeg() == ivStateId2State[ivIdPlanningStart]->getLeg())
00312     return false;
00313 
00314   assert(from.getSuccState() != nullptr);
00315 
00316   // check if first goal pose can be reached
00317   State left_foot = (from.getLeg() == LEFT) ? from.getState() : from.getSuccState()->getState();
00318   State right_foot = (from.getLeg() == RIGHT) ? from.getState() : from.getSuccState()->getState();
00319   State start_foot = ivStateId2State[ivIdPlanningStart]->getState();
00320 
00321   PostProcessor::instance().postProcessBackward(left_foot, right_foot, start_foot);
00322   if (!RobotModel::instance().isReachable(left_foot, right_foot, start_foot))
00323     return false;
00324 
00325   // check if second (final) goal can be reached
00326   if (start_foot.getLeg() == LEFT)
00327   {
00328     left_foot = start_foot;
00329     start_foot = start_foot_right->getState();
00330   }
00331   else
00332   {
00333     right_foot = start_foot;
00334     start_foot = start_foot_left->getState();
00335   }
00336 
00337   PostProcessor::instance().postProcessBackward(left_foot, right_foot, start_foot);
00338   start_foot.setBodyVelocity(geometry_msgs::Vector3()); // set velocity to zero
00339   if (!RobotModel::instance().isReachable(left_foot, right_foot, start_foot))
00340     return false;
00341 
00342   return true;
00343 }
00344 
00345 bool StateSpace::closeToGoal(const PlanningState& from) const
00346 {
00347   if (from.getLeg() == ivStateId2State[ivIdPlanningGoal]->getLeg())
00348     return false;
00349 
00350   assert(from.getPredState() != nullptr);
00351 
00352   // check if first goal pose can be reached
00353   State left_foot = (from.getLeg() == LEFT) ? from.getState() : from.getPredState()->getState();
00354   State right_foot = (from.getLeg() == RIGHT) ? from.getState() : from.getPredState()->getState();
00355   State goal_foot = ivStateId2State[ivIdPlanningGoal]->getState();
00356 
00357   PostProcessor::instance().postProcessForward(left_foot, right_foot, goal_foot);
00358   if (!RobotModel::instance().isReachable(left_foot, right_foot, goal_foot))
00359     return false;
00360 
00361   // check if second (final) goal can be reached
00362   if (goal_foot.getLeg() == LEFT)
00363   {
00364     left_foot = goal_foot;
00365     goal_foot = goal_foot_right->getState();
00366   }
00367   else
00368   {
00369     right_foot = goal_foot;
00370     goal_foot = goal_foot_left->getState();
00371   }
00372 
00373   PostProcessor::instance().postProcessForward(left_foot, right_foot, goal_foot);
00374   goal_foot.setBodyVelocity(geometry_msgs::Vector3()); // set velocity to zero
00375   if (!RobotModel::instance().isReachable(left_foot, right_foot, goal_foot))
00376     return false;
00377 
00378   return true;
00379 }
00380 
00381 bool StateSpace::getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, double& cost, double& risk) const
00382 {
00383   cost = 0.0;
00384   risk = 0.0;
00385 
00386   if (stand_foot.getLeg() == swing_foot_before.getLeg())
00387   {
00388     ROS_ERROR("Can't compute step cost: No standing foot is same leg as swing foot!");
00389     return false;
00390   }
00391 
00392   if (swing_foot_before.getLeg() != swing_foot_after.getLeg())
00393   {
00394     ROS_ERROR("Can't compute step cost: Swing foot states have not same leg!");
00395     return false;
00396   }
00397 
00398   const State& left_foot  = stand_foot.getLeg() == LEFT  ? stand_foot : swing_foot_before;
00399   const State& right_foot = stand_foot.getLeg() == RIGHT ? stand_foot : swing_foot_before;
00400 
00401   if (!StepCostEstimator::instance().getCost(left_foot, right_foot, swing_foot_after, cost, risk))
00402     return false;
00403 
00404   if (risk >= params.max_risk)
00405     return false;
00406 
00407   return true;
00408 }
00409 
00410 bool StateSpace::getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, int& cost, int& risk) const
00411 {
00412   double cost_d, risk_d;
00413   if (!getStepCost(stand_foot, swing_foot_before, swing_foot_after, cost_d, risk_d))
00414     return false;
00415 
00416   cost = static_cast<int>(cvMmScale * cost_d + 0.5);
00417   risk = static_cast<int>(cvMmScale * risk_d + 0.5);
00418 
00419   return true;
00420 }
00421 
00422 bool StateSpace::getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, int& cost) const
00423 {
00424   int risk = 0;
00425   return getStepCost(stand_foot, swing_foot_before, swing_foot_after, cost, risk);
00426 }
00427 }


vigir_footstep_planner
Author(s): Alexander Stumpf
autogenerated on Fri Apr 7 2017 02:59:36