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
00079 int goal_foot_id_left = ivIdGoalFootLeft;
00080 int goal_foot_id_right = ivIdGoalFootRight;
00081
00082
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
00092 assert(ivIdGoalFootLeft != -1);
00093 assert(ivIdGoalFootRight != -1);
00094
00095
00096
00097 if (params.forward_search)
00098 {
00099
00100 if (goal_foot_id_left != ivIdGoalFootLeft || goal_foot_id_right != ivIdGoalFootRight)
00101 {
00102 ivHeuristicExpired = true;
00103
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
00113 int start_foot_id_left = ivIdStartFootLeft;
00114 int start_foot_id_right = ivIdStartFootRight;
00115
00116
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
00126 assert(ivIdStartFootLeft != -1);
00127 assert(ivIdStartFootRight != -1);
00128
00129
00130
00131 if (!params.forward_search)
00132 {
00133
00134 if (start_foot_id_left != ivIdStartFootLeft || start_foot_id_right != ivIdStartFootRight)
00135 {
00136 ivHeuristicExpired = true;
00137
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)
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)
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
00263 new_state->setId(state_id);
00264 ivStateId2State.push_back(new_state);
00265
00266
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
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
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());
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
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
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());
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 }