$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/FootstepPlannerEnvironment.cpp $ 00002 // SVN $Id: FootstepPlannerEnvironment.cpp 3979 2013-02-26 14:13:46Z garimort@informatik.uni-freiburg.de $ 00003 00004 /* 00005 * A footstep planner for humanoid robots 00006 * 00007 * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg 00008 * http://www.ros.org/wiki/footstep_planner 00009 * 00010 * 00011 * This program is free software: you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation, version 3. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU General Public License for more details. 00019 * 00020 * You should have received a copy of the GNU General Public License 00021 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00022 */ 00023 00024 #include <footstep_planner/FootstepPlannerEnvironment.h> 00025 00026 00027 namespace footstep_planner 00028 { 00029 FootstepPlannerEnvironment::FootstepPlannerEnvironment( 00030 const environment_params& params) 00031 : DiscreteSpaceInformation(), 00032 ivIdStartFootLeft(-1), 00033 ivIdStartFootRight(-1), 00034 ivIdGoalFootLeft(-1), 00035 ivIdGoalFootRight(-1), 00036 ivpStateHash2State( 00037 new std::vector<const PlanningState*>[params.hash_table_size]), 00038 ivFootstepSet(params.footstep_set), 00039 ivHeuristicConstPtr(params.heuristic), 00040 ivFootsizeX(params.footsize_x), 00041 ivFootsizeY(params.footsize_y), 00042 ivOriginFootShiftX(params.foot_origin_shift_x), 00043 ivOriginFootShiftY(params.foot_origin_shift_y), 00044 ivMaxFootstepX(disc_val(params.max_footstep_x, params.cell_size)), 00045 ivMaxFootstepY(disc_val(params.max_footstep_y, params.cell_size)), 00046 ivMaxFootstepTheta( 00047 angle_state_2_cell(params.max_footstep_theta, params.num_angle_bins)), 00048 ivMaxInvFootstepX(disc_val(params.max_inverse_footstep_x, params.cell_size)), 00049 ivMaxInvFootstepY(disc_val(params.max_inverse_footstep_y, params.cell_size)), 00050 ivMaxInvFootstepTheta( 00051 angle_state_2_cell(params.max_inverse_footstep_theta, 00052 params.num_angle_bins)), 00053 ivStepCost(cvMmScale * params.step_cost), 00054 ivCollisionCheckAccuracy(params.collision_check_accuracy), 00055 ivHashTableSize(params.hash_table_size), 00056 ivCellSize(params.cell_size), 00057 ivNumAngleBins(params.num_angle_bins), 00058 ivForwardSearch(params.forward_search), 00059 ivMaxStepWidth(double(disc_val(params.max_step_width, params.cell_size))), 00060 ivNumRandomNodes(params.num_random_nodes), 00061 ivRandomNodeDist(params.random_node_distance / ivCellSize), 00062 ivHeuristicScale(params.heuristic_scale), 00063 ivHeuristicExpired(true), 00064 ivNumExpandedStates(0) 00065 { 00066 int num_angle_bins_half = ivNumAngleBins / 2; 00067 if (ivMaxFootstepTheta >= num_angle_bins_half) 00068 ivMaxFootstepTheta -= ivNumAngleBins; 00069 if (ivMaxInvFootstepTheta >= num_angle_bins_half) 00070 ivMaxInvFootstepTheta -= ivNumAngleBins; 00071 00072 int num_x = ivMaxFootstepX - ivMaxInvFootstepX + 1; 00073 ivStepRange = new bool[num_x * (ivMaxFootstepY - ivMaxInvFootstepY + 1)]; 00074 00075 for (int j = ivMaxInvFootstepY; j <= ivMaxFootstepY; ++j) 00076 { 00077 for (int i = ivMaxInvFootstepX; i <= ivMaxFootstepX; ++i) 00078 { 00079 ivStepRange[(j - ivMaxInvFootstepY) * num_x + (i - ivMaxInvFootstepX)] = 00080 pointWithinPolygon(std::pair<int, int>(i, j), params.step_range); 00081 } 00082 } 00083 } 00084 00085 00086 FootstepPlannerEnvironment::~FootstepPlannerEnvironment() 00087 { 00088 reset(); 00089 if (ivpStateHash2State) 00090 { 00091 delete[] ivpStateHash2State; 00092 ivpStateHash2State = NULL; 00093 } 00094 00095 delete[] ivStepRange; 00096 } 00097 00098 00099 void 00100 FootstepPlannerEnvironment::updateGoal(const State& foot_left, 00101 const State& foot_right) 00102 { 00103 // keep the old IDs 00104 int goal_foot_id_left = ivIdGoalFootLeft; 00105 int goal_foot_id_right = ivIdGoalFootRight; 00106 00107 // update the states for both feet (if necessary) 00108 const PlanningState* p_foot_left = getHashEntry(foot_left); 00109 if (p_foot_left == NULL) 00110 { 00111 p_foot_left = createNewHashEntry(foot_left); 00112 ivIdGoalFootLeft = p_foot_left->getId(); 00113 } 00114 else 00115 { 00116 ivIdGoalFootLeft = p_foot_left->getId(); 00117 } 00118 const PlanningState* p_foot_right = getHashEntry(foot_right); 00119 if (p_foot_right == NULL) 00120 { 00121 p_foot_right = createNewHashEntry(foot_right); 00122 ivIdGoalFootRight = p_foot_right->getId(); 00123 } 00124 else 00125 { 00126 ivIdGoalFootRight = p_foot_right->getId(); 00127 } 00128 00129 // check if everything has been set correctly 00130 assert(ivIdGoalFootLeft != -1); 00131 assert(ivIdGoalFootRight != -1); 00132 00133 // if using the forward search a change of the goal states involves an 00134 // update of the heuristic 00135 if (ivForwardSearch) 00136 { 00137 // check if the goal states have been changed 00138 if (goal_foot_id_left != ivIdGoalFootLeft && 00139 goal_foot_id_right != ivIdGoalFootRight) 00140 { 00141 ivHeuristicExpired = true; 00142 } 00143 } 00144 } 00145 00146 00147 void 00148 FootstepPlannerEnvironment::updateStart(const State& foot_left, 00149 const State& foot_right) 00150 { 00151 // keep the old IDs 00152 int start_foot_id_left = ivIdStartFootLeft; 00153 int start_foot_id_right = ivIdStartFootRight; 00154 00155 // update the states for both feet (if necessary) 00156 const PlanningState* p_foot_left = getHashEntry(foot_left); 00157 if (p_foot_left == NULL) 00158 { 00159 p_foot_left = createNewHashEntry(foot_left); 00160 ivIdStartFootLeft = p_foot_left->getId(); 00161 } 00162 else 00163 { 00164 ivIdStartFootLeft = p_foot_left->getId(); 00165 } 00166 const PlanningState* p_foot_right = getHashEntry(foot_right); 00167 if (p_foot_right == NULL) 00168 { 00169 p_foot_right = createNewHashEntry(foot_right); 00170 ivIdStartFootRight = p_foot_right->getId(); 00171 } 00172 else 00173 { 00174 ivIdStartFootRight = p_foot_right->getId(); 00175 } 00176 00177 // check if everything has been set correctly 00178 assert(ivIdStartFootLeft != -1); 00179 assert(ivIdStartFootRight != -1); 00180 00181 // if using the backward search a change of the start states involves an 00182 // update of the heuristic 00183 if (!ivForwardSearch) 00184 { 00185 // check if the start states have been changed 00186 if (start_foot_id_left != ivIdStartFootLeft && 00187 start_foot_id_right != ivIdStartFootRight) 00188 { 00189 ivHeuristicExpired = true; 00190 } 00191 } 00192 } 00193 00194 00195 const PlanningState* 00196 FootstepPlannerEnvironment::createNewHashEntry(const State& s) 00197 { 00198 PlanningState tmp(s, ivCellSize, ivNumAngleBins, ivHashTableSize); 00199 return createNewHashEntry(tmp); 00200 } 00201 00202 00203 const PlanningState* 00204 FootstepPlannerEnvironment::createNewHashEntry(const PlanningState& s) 00205 { 00206 unsigned int state_hash = s.getHashTag(); 00207 PlanningState* new_state = new PlanningState(s); 00208 00209 size_t state_id = ivStateId2State.size(); 00210 assert(state_id < (size_t)std::numeric_limits<int>::max()); 00211 00212 // insert the ID of the new state into the corresponding map 00213 new_state->setId(state_id); 00214 ivStateId2State.push_back(new_state); 00215 00216 // insert the new state into the hash map at the corresponding position 00217 ivpStateHash2State[state_hash].push_back(new_state); 00218 00219 int* entry = new int[NUMOFINDICES_STATEID2IND]; 00220 StateID2IndexMapping.push_back(entry); 00221 for(int i = 0; i < NUMOFINDICES_STATEID2IND; i++) 00222 { 00223 StateID2IndexMapping[state_id][i] = -1; 00224 } 00225 00226 assert(StateID2IndexMapping.size() - 1 == state_id); 00227 00228 return new_state; 00229 } 00230 00231 00232 const PlanningState* 00233 FootstepPlannerEnvironment::getHashEntry(const State& s) 00234 { 00235 PlanningState tmp(s, ivCellSize, ivNumAngleBins, ivHashTableSize); 00236 return getHashEntry(tmp); 00237 } 00238 00239 00240 const PlanningState* 00241 FootstepPlannerEnvironment::getHashEntry(const PlanningState& s) 00242 { 00243 unsigned int state_hash = s.getHashTag(); 00244 std::vector<const PlanningState*>::const_iterator state_iter; 00245 for (state_iter = ivpStateHash2State[state_hash].begin(); 00246 state_iter != ivpStateHash2State[state_hash].end(); 00247 state_iter++) 00248 { 00249 if (*(*state_iter) == s) 00250 return *state_iter; 00251 } 00252 00253 return NULL; 00254 } 00255 00256 const PlanningState* 00257 FootstepPlannerEnvironment::createHashEntryIfNotExists( 00258 const PlanningState& s) 00259 { 00260 const PlanningState* hash_entry = getHashEntry(s); 00261 if (hash_entry == NULL) 00262 hash_entry = createNewHashEntry(s); 00263 00264 return hash_entry; 00265 00266 } 00267 00268 00269 int 00270 FootstepPlannerEnvironment::stepCost(const PlanningState& a, 00271 const PlanningState& b) 00272 { 00273 if (a == b) 00274 return 0; 00275 00276 // NOTE: instead of using cont_val() the calculation is done directly 00277 // here because cont_val() truncates the input length to int 00278 double dist = euclidean_distance( 00279 a.getX(), a.getY(), b.getX(), b.getY()) * ivCellSize; 00280 00281 return int(cvMmScale * dist) + ivStepCost; 00282 } 00283 00284 00285 bool 00286 FootstepPlannerEnvironment::occupied(const State& s) 00287 { 00288 return occupied(PlanningState(s, ivCellSize, ivNumAngleBins, 00289 ivHashTableSize)); 00290 } 00291 00292 00293 bool 00294 FootstepPlannerEnvironment::occupied(const PlanningState& s) 00295 { 00296 double x = cell_2_state(s.getX(), ivCellSize); 00297 double y = cell_2_state(s.getY(), ivCellSize); 00298 // collision check for the planning state 00299 if (ivMapPtr->isOccupiedAt(x,y)) 00300 return true; 00301 double theta = angle_cell_2_state(s.getTheta(), ivNumAngleBins); 00302 double theta_cos = cos(theta); 00303 double theta_sin = sin(theta); 00304 00305 // transform the planning state to the foot center 00306 x += theta_cos*ivOriginFootShiftX - theta_sin*ivOriginFootShiftY; 00307 if (s.getLeg() == LEFT) 00308 y += theta_sin*ivOriginFootShiftX + theta_cos*ivOriginFootShiftY; 00309 else // leg == RLEG 00310 y += theta_sin*ivOriginFootShiftX - theta_cos*ivOriginFootShiftY; 00311 00312 // collision check for the foot center 00313 return collision_check(x, y, theta, ivFootsizeX, ivFootsizeY, 00314 ivCollisionCheckAccuracy, *ivMapPtr); 00315 } 00316 00317 00318 bool 00319 FootstepPlannerEnvironment::getState(unsigned int id, State* s) 00320 { 00321 if (id >= ivStateId2State.size()) 00322 return false; 00323 00324 const PlanningState* planning_state = ivStateId2State[id]; 00325 s->setX(cell_2_state(planning_state->getX(), ivCellSize)); 00326 s->setY(cell_2_state(planning_state->getY(), ivCellSize)); 00327 s->setTheta(angles::normalize_angle(angle_cell_2_state( 00328 planning_state->getTheta(), ivNumAngleBins))); 00329 s->setLeg(planning_state->getLeg()); 00330 00331 return true; 00332 } 00333 00334 00335 void 00336 FootstepPlannerEnvironment::updateMap(gridmap_2d::GridMap2DPtr map) 00337 { 00338 ivMapPtr.reset(); 00339 ivMapPtr = map; 00340 00341 if (ivHeuristicConstPtr->getHeuristicType() == Heuristic::PATH_COST) 00342 { 00343 boost::shared_ptr<PathCostHeuristic> h = 00344 boost::dynamic_pointer_cast<PathCostHeuristic>( 00345 ivHeuristicConstPtr); 00346 h->updateMap(map); 00347 } 00348 } 00349 00350 00351 void 00352 FootstepPlannerEnvironment::updateHeuristicValues() 00353 { 00354 // check if start and goal have been set 00355 assert(ivIdGoalFootLeft != -1 && ivIdGoalFootRight != -1); 00356 assert(ivIdStartFootLeft != -1 && ivIdStartFootRight != -1); 00357 00358 if (!ivHeuristicExpired) 00359 return; 00360 00361 ROS_INFO("Updating the heuristic values."); 00362 00363 if (ivHeuristicConstPtr->getHeuristicType() == Heuristic::PATH_COST) 00364 { 00365 boost::shared_ptr<PathCostHeuristic> h = 00366 boost::dynamic_pointer_cast<PathCostHeuristic>( 00367 ivHeuristicConstPtr); 00368 MDPConfig MDPCfg; 00369 InitializeMDPCfg(&MDPCfg); 00370 const PlanningState* start = ivStateId2State[MDPCfg.startstateid]; 00371 const PlanningState* goal = ivStateId2State[MDPCfg.goalstateid]; 00372 00373 // NOTE: start/goal state are set to left leg 00374 bool success; 00375 if (ivForwardSearch) 00376 success = h->calculateDistances(*start, *goal); 00377 else 00378 success = h->calculateDistances(*goal, *start); 00379 if (!success) 00380 { 00381 ROS_ERROR("Failed to calculate path cost heuristic."); 00382 exit(1); 00383 } 00384 } 00385 00386 ROS_DEBUG("Finished updating the heuristic values."); 00387 ivHeuristicExpired = false; 00388 } 00389 00390 00391 void 00392 FootstepPlannerEnvironment::reset() 00393 { 00394 for(unsigned int i = 0; i < ivStateId2State.size(); ++i) 00395 { 00396 if (ivStateId2State[i]) 00397 { 00398 delete ivStateId2State[i]; 00399 } 00400 } 00401 ivStateId2State.clear(); 00402 00403 if (ivpStateHash2State) 00404 { 00405 for(int i = 0; i < ivHashTableSize; ++i) 00406 ivpStateHash2State[i].clear(); 00407 } 00408 00409 for(unsigned int i = 0; i < StateID2IndexMapping.size(); ++i) 00410 { 00411 if (StateID2IndexMapping[i]) 00412 { 00413 delete[] StateID2IndexMapping[i]; 00414 } 00415 } 00416 StateID2IndexMapping.clear(); 00417 00418 ivExpandedStates.clear(); 00419 ivNumExpandedStates = 0; 00420 ivRandomStates.clear(); 00421 00422 ivIdGoalFootLeft = -1; 00423 ivIdGoalFootRight = -1; 00424 ivIdStartFootLeft = -1; 00425 ivIdStartFootRight = -1; 00426 00427 ivHeuristicExpired = true; 00428 } 00429 00430 00431 bool 00432 FootstepPlannerEnvironment::closeToStart(const PlanningState& from) 00433 { 00434 // NOTE: "goal check" for backward planning 00435 const PlanningState* start; 00436 if (from.getLeg() == RIGHT) 00437 start = ivStateId2State[ivIdStartFootLeft]; 00438 else 00439 start = ivStateId2State[ivIdStartFootRight]; 00440 00441 return reachable(*start, from); 00442 } 00443 00444 00445 bool 00446 FootstepPlannerEnvironment::closeToGoal(const PlanningState& from) 00447 { 00448 // NOTE: "goal check" for forward planning 00449 const PlanningState* goal; 00450 if (from.getLeg() == RIGHT) 00451 goal = ivStateId2State[ivIdGoalFootLeft]; 00452 else 00453 goal = ivStateId2State[ivIdGoalFootRight]; 00454 00455 // TODO: check step if reachable == True 00456 return reachable(from, *goal); 00457 } 00458 00459 00460 bool 00461 FootstepPlannerEnvironment::reachable(const PlanningState& from, 00462 const PlanningState& to) 00463 { 00464 if (euclidean_distance(from.getX(), from.getY(), to.getX(), to.getY()) > 00465 ivMaxStepWidth) 00466 { 00467 return false; 00468 } 00469 00470 tf::Transform step = 00471 tf::Pose( 00472 tf::createQuaternionFromYaw( 00473 angle_cell_2_state(from.getTheta(), ivNumAngleBins)), 00474 tf::Point(cell_2_state(from.getX(), ivCellSize), 00475 cell_2_state(from.getY(), ivCellSize), 00476 0.0)).inverse() * 00477 tf::Pose( 00478 tf::createQuaternionFromYaw( 00479 angle_cell_2_state(from.getTheta(), ivNumAngleBins)), 00480 tf::Point(cell_2_state(to.getX(), ivCellSize), 00481 cell_2_state(to.getY(), ivCellSize), 00482 0.0)); 00483 int footstep_x = disc_val(step.getOrigin().x(), ivCellSize); 00484 int footstep_y = disc_val(step.getOrigin().y(), ivCellSize); 00485 00486 // calculate the footstep rotation 00487 int footstep_theta = to.getTheta() - from.getTheta(); 00488 // transform the value into [-ivNumAngleBins/2..ivNumAngleBins/2) 00489 int num_angle_bins_half = ivNumAngleBins / 2; 00490 if (footstep_theta >= num_angle_bins_half) 00491 footstep_theta -= ivNumAngleBins; 00492 else if (footstep_theta < -num_angle_bins_half) 00493 footstep_theta += ivNumAngleBins; 00494 00495 // adjust for the left foot 00496 if (from.getLeg() == LEFT) 00497 { 00498 footstep_y = -footstep_y; 00499 footstep_theta = -footstep_theta; 00500 } 00501 00502 // check if footstep_x is not within the executable range 00503 if (footstep_x > ivMaxFootstepX || footstep_x < ivMaxInvFootstepX) 00504 return false; 00505 // check if footstep_y is not within the executable range 00506 if (footstep_y > ivMaxFootstepY || footstep_y < ivMaxInvFootstepY) 00507 return false; 00508 // check if footstep_theta is not within the executable range 00509 if (footstep_theta > ivMaxFootstepTheta || 00510 footstep_theta < ivMaxInvFootstepTheta) 00511 return false; 00512 return ivStepRange[(footstep_y - ivMaxInvFootstepY) * 00513 (ivMaxFootstepX - ivMaxInvFootstepX + 1) + 00514 (footstep_x - ivMaxInvFootstepX)]; 00515 00516 // // get the (continuous) orientation of state 'from' 00517 // double orient = -(angle_cell_2_state(from.getTheta(), ivNumAngleBins)); 00518 // double orient_cos = cos(orient); 00519 // double orient_sin = sin(orient); 00520 // 00521 // // calculate the footstep shift and rotate it into the 'from'-view 00522 // int footstep_x = to.getX() - from.getX(); 00523 // int footstep_y = to.getY() - from.getY(); 00524 // double shift_x = footstep_x * orient_cos - footstep_y * orient_sin; 00525 // double shift_y = footstep_x * orient_sin + footstep_y * orient_cos; 00526 // footstep_x = round(shift_x); 00527 // footstep_y = round(shift_y); 00528 // 00529 // // calculate the footstep rotation 00530 // int footstep_theta = to.getTheta() - from.getTheta(); 00531 // 00532 // // transform the value into [-ivNumAngleBins/2..ivNumAngleBins/2) 00533 // int num_angle_bins_half = ivNumAngleBins / 2; 00534 // if (footstep_theta >= num_angle_bins_half) 00535 // footstep_theta -= ivNumAngleBins; 00536 // else if (footstep_theta < -num_angle_bins_half) 00537 // footstep_theta += ivNumAngleBins; 00538 // 00539 // // adjust for the left foot 00540 // if (from.getLeg() == LEFT) 00541 // { 00542 // footstep_y = -footstep_y; 00543 // footstep_theta = -footstep_theta; 00544 // } 00545 // 00546 // return (footstep_x <= ivMaxFootstepX && 00547 // footstep_x >= ivMaxInvFootstepX && 00548 // footstep_y <= ivMaxFootstepY && 00549 // footstep_y >= ivMaxInvFootstepY && 00550 // footstep_theta <= ivMaxFootstepTheta && 00551 // footstep_theta >= ivMaxInvFootstepTheta); 00552 } 00553 00554 00555 void 00556 FootstepPlannerEnvironment::getPredsOfGridCells( 00557 const std::vector<State>& changed_states, 00558 std::vector<int>* pred_ids) 00559 { 00560 pred_ids->clear(); 00561 00562 std::vector<State>::const_iterator state_iter; 00563 for (state_iter = changed_states.begin(); 00564 state_iter != changed_states.end(); 00565 state_iter++) 00566 { 00567 PlanningState s(*state_iter, ivCellSize, ivNumAngleBins, 00568 ivHashTableSize); 00569 // generate predecessor planning states 00570 std::vector<Footstep>::const_iterator footstep_set_iter; 00571 for(footstep_set_iter = ivFootstepSet.begin(); 00572 footstep_set_iter != ivFootstepSet.end(); 00573 footstep_set_iter++) 00574 { 00575 PlanningState pred = footstep_set_iter->reverseMeOnThisState(s); 00576 // check if predecessor exists 00577 const PlanningState* pred_hash_entry = getHashEntry(pred); 00578 if (pred_hash_entry == NULL) 00579 continue; 00580 pred_ids->push_back(pred_hash_entry->getId()); 00581 } 00582 } 00583 } 00584 00585 00586 void 00587 FootstepPlannerEnvironment::getSuccsOfGridCells( 00588 const std::vector<State>& changed_states, 00589 std::vector<int>* succ_ids) 00590 { 00591 succ_ids->clear(); 00592 00593 std::vector<State>::const_iterator state_iter; 00594 for (state_iter = changed_states.begin(); 00595 state_iter != changed_states.end(); 00596 state_iter++) 00597 { 00598 PlanningState s(*state_iter, ivCellSize, ivNumAngleBins, 00599 ivHashTableSize); 00600 // generate successors 00601 std::vector<Footstep>::const_iterator footstep_set_iter; 00602 for(footstep_set_iter = ivFootstepSet.begin(); 00603 footstep_set_iter != ivFootstepSet.end(); 00604 footstep_set_iter++) 00605 { 00606 PlanningState succ = footstep_set_iter->performMeOnThisState(s); 00607 // check if successor exists 00608 const PlanningState* succ_hash_entry = getHashEntry(succ); 00609 if (succ_hash_entry == NULL) 00610 continue; 00611 succ_ids->push_back(succ_hash_entry->getId()); 00612 } 00613 } 00614 } 00615 00616 00617 int 00618 FootstepPlannerEnvironment::GetFromToHeuristic(int FromStateID, 00619 int ToStateID) 00620 { 00621 assert(FromStateID >= 0 && (unsigned int) FromStateID < ivStateId2State.size()); 00622 assert(ToStateID >= 0 && (unsigned int) ToStateID < ivStateId2State.size()); 00623 00624 if ((FromStateID == ivIdGoalFootLeft && ToStateID == ivIdGoalFootRight) 00625 || (FromStateID == ivIdGoalFootRight && ToStateID == ivIdGoalFootLeft)){ 00626 return 0; 00627 } 00628 00629 const PlanningState* from = ivStateId2State[FromStateID]; 00630 const PlanningState* to = ivStateId2State[ToStateID]; 00631 // if (ivHeuristicConstPtr->getHeuristicType() == Heuristic::PATH_COST){ 00632 // boost::shared_ptr<PathCostHeuristic> pathCostHeuristic = boost::dynamic_pointer_cast<PathCostHeuristic>(ivHeuristicConstPtr); 00633 // pathCostHeuristic->calculateDistances(*from, *to); 00634 // } 00635 return GetFromToHeuristic(*from, *to); 00636 } 00637 00638 int 00639 FootstepPlannerEnvironment::GetFromToHeuristic(const PlanningState& from, 00640 const PlanningState& to) 00641 { 00642 return cvMmScale * ivHeuristicScale * 00643 ivHeuristicConstPtr->getHValue(from, to); 00644 } 00645 00646 00647 int 00648 FootstepPlannerEnvironment::GetGoalHeuristic(int stateID) 00649 { 00650 return GetFromToHeuristic(stateID, ivIdGoalFootLeft); 00651 } 00652 00653 00654 void 00655 FootstepPlannerEnvironment::GetPreds(int TargetStateID, 00656 std::vector<int> *PredIDV, 00657 std::vector<int> *CostV) 00658 { 00659 PredIDV->clear(); 00660 CostV->clear(); 00661 00662 assert(TargetStateID >= 0 && 00663 (unsigned int) TargetStateID < ivStateId2State.size()); 00664 00665 // make goal state absorbing (only left!) 00666 if (TargetStateID == ivIdStartFootLeft) 00667 return; 00668 00669 const PlanningState* current = ivStateId2State[TargetStateID]; 00670 00671 // add cheap transition from right to left, so right becomes an equivalent goal 00672 if (TargetStateID == ivIdStartFootRight && current->getLeg() == RIGHT) 00673 { 00674 PredIDV->push_back(ivIdStartFootLeft); 00675 CostV->push_back(ivStepCost); 00676 return; 00677 } 00678 00679 ivExpandedStates.insert(std::pair<int,int>(current->getX(), 00680 current->getY())); 00681 ivNumExpandedStates++; 00682 00683 if (closeToStart(*current)) 00684 { 00685 int start_id; 00686 if (current->getLeg() == RIGHT) 00687 start_id = ivIdStartFootLeft; 00688 else 00689 start_id = ivIdStartFootRight; 00690 00691 const PlanningState* start = ivStateId2State[start_id]; 00692 PredIDV->push_back(start_id); 00693 CostV->push_back(stepCost(*current, *start)); 00694 00695 return; 00696 } 00697 00698 PredIDV->reserve(ivFootstepSet.size()); 00699 CostV->reserve(ivFootstepSet.size()); 00700 std::vector<Footstep>::const_iterator footstep_set_iter; 00701 for(footstep_set_iter = ivFootstepSet.begin(); 00702 footstep_set_iter != ivFootstepSet.end(); 00703 footstep_set_iter++) 00704 { 00705 const PlanningState predecessor = 00706 footstep_set_iter->reverseMeOnThisState(*current); 00707 if (occupied(predecessor)) 00708 continue; 00709 00710 const PlanningState* predecessor_hash = createHashEntryIfNotExists( 00711 predecessor); 00712 00713 int cost = stepCost(*current, *predecessor_hash); 00714 PredIDV->push_back(predecessor_hash->getId()); 00715 CostV->push_back(cost); 00716 } 00717 } 00718 00719 00720 int 00721 FootstepPlannerEnvironment::GetStartHeuristic(int stateID) 00722 { 00723 return GetFromToHeuristic(stateID, ivIdStartFootLeft); 00724 } 00725 00726 00727 void 00728 FootstepPlannerEnvironment::GetSuccs(int SourceStateID, 00729 std::vector<int> *SuccIDV, 00730 std::vector<int> *CostV) 00731 { 00732 SuccIDV->clear(); 00733 CostV->clear(); 00734 00735 assert(SourceStateID >= 0 && 00736 unsigned(SourceStateID) < ivStateId2State.size()); 00737 00738 // make goal state absorbing (only left!) 00739 if (SourceStateID == ivIdGoalFootLeft) 00740 { 00741 return; 00742 } 00743 00744 const PlanningState* current = ivStateId2State[SourceStateID]; 00745 00746 // add cheap transition from right to left, so right becomes an 00747 // equivalent goal 00748 if (SourceStateID == ivIdGoalFootRight && current->getLeg() == RIGHT) 00749 { 00750 SuccIDV->push_back(ivIdGoalFootLeft); 00751 CostV->push_back(ivStepCost); 00752 return; 00753 } 00754 00755 ivExpandedStates.insert(std::pair<int,int>(current->getX(), 00756 current->getY())); 00757 ivNumExpandedStates++; 00758 00759 if (closeToGoal(*current)) 00760 { 00761 int goal_id; 00762 assert(current->getLeg() != NOLEG); 00763 if (current->getLeg() == RIGHT) 00764 goal_id = ivIdGoalFootLeft; 00765 else 00766 goal_id = ivIdGoalFootRight; 00767 00768 const PlanningState* goal = ivStateId2State[goal_id]; 00769 SuccIDV->push_back(goal_id); 00770 CostV->push_back(stepCost(*current, *goal)); 00771 00772 return; 00773 } 00774 00775 SuccIDV->reserve(ivFootstepSet.size()); 00776 CostV->reserve(ivFootstepSet.size()); 00777 std::vector<Footstep>::const_iterator footstep_set_iter; 00778 for(footstep_set_iter = ivFootstepSet.begin(); 00779 footstep_set_iter != ivFootstepSet.end(); 00780 footstep_set_iter++) 00781 { 00782 PlanningState successor = 00783 footstep_set_iter->performMeOnThisState(*current); 00784 if (occupied(successor)) 00785 continue; 00786 00787 const PlanningState* successor_hash_entry = 00788 createHashEntryIfNotExists(successor); 00789 00790 int cost = stepCost(*current, *successor_hash_entry); 00791 SuccIDV->push_back(successor_hash_entry->getId()); 00792 CostV->push_back(cost); 00793 } 00794 } 00795 00796 void 00797 FootstepPlannerEnvironment::GetSuccsTo(int SourceStateID, int goalStateId, 00798 std::vector<int> *SuccIDV, 00799 std::vector<int> *CostV) 00800 { 00801 //return GetSuccs(SourceStateID, SuccIDV, CostV); 00802 00803 SuccIDV->clear(); 00804 CostV->clear(); 00805 00806 assert(SourceStateID >= 0 && 00807 unsigned(SourceStateID) < ivStateId2State.size()); 00808 00809 // make goal state absorbing 00810 if (SourceStateID == ivIdGoalFootLeft ){ 00811 return; 00812 } 00813 00814 const PlanningState* current = ivStateId2State[SourceStateID]; 00815 ivExpandedStates.insert(std::pair<int,int>(current->getX(), current->getY())); 00816 ivNumExpandedStates++; 00817 00818 //ROS_INFO("GetSuccsTo %d -> %d: %f", SourceStateID, goalStateId, euclidean_distance(current->getX(), current->getY(), ivStateId2State[goalStateId]->getX(), ivStateId2State[goalStateId]->getY())); 00819 00820 // add cheap transition from right to left, so right becomes an equivalent goal 00821 if (goalStateId== ivIdGoalFootLeft && SourceStateID == ivIdGoalFootRight && current->getLeg() == RIGHT){ 00822 SuccIDV->push_back(ivIdGoalFootLeft); 00823 CostV->push_back(ivStepCost); 00824 return; 00825 } 00826 00827 if (closeToGoal(*current)) 00828 { 00829 int goal_id; 00830 assert(current->getLeg() != NOLEG); 00831 if (current->getLeg() == RIGHT){ 00832 goal_id = ivIdGoalFootLeft; 00833 } else { 00834 goal_id = ivIdGoalFootRight; 00835 } 00836 00837 const PlanningState* goal = ivStateId2State[goal_id]; 00838 int cost = stepCost(*current, *goal); 00839 SuccIDV->push_back(goal_id); 00840 CostV->push_back(cost); 00841 00842 return; 00843 } 00844 00845 // intermediate goal reachable (R*)? 00846 assert(goalStateId >= 0 && unsigned(goalStateId) < ivStateId2State.size()); 00847 const PlanningState* randomGoal = ivStateId2State[goalStateId]; 00848 if (randomGoal->getLeg() != current->getLeg() && reachable(*current, *randomGoal)){ 00849 int cost = stepCost(*current, *randomGoal); 00850 SuccIDV->push_back(goalStateId); 00851 CostV->push_back(cost); 00852 // ROS_INFO("%d %d", goalStateId, cost); 00853 00854 // return; 00855 } 00856 00857 00858 SuccIDV->reserve(ivFootstepSet.size()); 00859 CostV->reserve(ivFootstepSet.size()); 00860 std::vector<Footstep>::const_iterator footstep_set_iter; 00861 for(footstep_set_iter = ivFootstepSet.begin(); 00862 footstep_set_iter != ivFootstepSet.end(); 00863 footstep_set_iter++) 00864 { 00865 PlanningState successor = 00866 footstep_set_iter->performMeOnThisState(*current); 00867 if (occupied(successor)) 00868 continue; 00869 00870 const PlanningState* successor_hash = createHashEntryIfNotExists(successor); 00871 00872 int cost = stepCost(*current, *successor_hash); 00873 SuccIDV->push_back(successor_hash->getId()); 00874 CostV->push_back(cost); 00875 } 00876 } 00877 00878 00879 void 00880 FootstepPlannerEnvironment::GetRandomSuccsatDistance(int SourceStateID, 00881 std::vector<int>* SuccIDV, std::vector<int>* CLowV) 00882 { 00883 00884 assert(SourceStateID >= 0 && unsigned(SourceStateID) < ivStateId2State.size()); 00885 //goal state should be absorbing 00886 if (SourceStateID == ivIdGoalFootLeft || SourceStateID == ivIdGoalFootRight ) 00887 return; 00888 00889 00890 const PlanningState* currentState = ivStateId2State[SourceStateID]; 00891 // TODO: closeToGoal? 00892 // 00893 // if (closeToGoal(*currentState)) 00894 // return; 00895 00896 //get the successors 00897 GetRandomNeighs(currentState, SuccIDV, CLowV, ivNumRandomNodes, 00898 ivRandomNodeDist, true); 00899 } 00900 00901 void 00902 FootstepPlannerEnvironment::GetRandomPredsatDistance(int TargetStateID, 00903 std::vector<int>* PredIDV, std::vector<int>* CLowV) 00904 { 00905 00906 assert(TargetStateID >= 0 && 00907 unsigned(TargetStateID) < ivStateId2State.size()); 00908 00909 // start state should be absorbing 00910 if (TargetStateID == ivIdStartFootLeft || TargetStateID == ivIdStartFootRight) 00911 return; 00912 00913 const PlanningState* currentState = ivStateId2State[TargetStateID]; 00914 00915 // TODO: ??? 00916 // if(closeToStart(*currentState)) 00917 // return; 00918 00919 //get the predecessors 00920 GetRandomNeighs(currentState, PredIDV, CLowV, ivNumRandomNodes, 00921 ivRandomNodeDist, false); 00922 00923 } 00924 00925 //generates nNumofNeighs random neighbors of cell <X,Y> at distance nDist_c (measured in cells) 00926 //it will also generate goal if within this distance as an additional neighbor 00927 //bSuccs is set to true if we are computing successor states, otherwise it is Preds 00928 // (see fct. implemented in environment_nav2D) 00929 void FootstepPlannerEnvironment::GetRandomNeighs(const PlanningState* currentState, std::vector<int>* NeighIDV, std::vector<int>* CLowV, int nNumofNeighs, int nDist_c, bool bSuccs) 00930 { 00931 00932 //clear the successor array 00933 NeighIDV->clear(); 00934 CLowV->clear(); 00935 00936 00937 //get X, Y for the states 00938 int X = currentState->getX(); 00939 int Y = currentState->getY(); 00940 //int theta = currentState->getTheta(); 00941 00942 //see if the goal/start belongs to the inside area and if yes then add it to Neighs as well 00943 // NOTE: "goal check" for backward planning 00944 const PlanningState* goal_left = NULL; 00945 const PlanningState* goal_right = NULL; 00946 if (bSuccs){ 00947 goal_left = ivStateId2State[ivIdGoalFootLeft]; 00948 goal_right = ivStateId2State[ivIdGoalFootRight]; 00949 } else { 00950 goal_left = ivStateId2State[ivIdStartFootLeft]; 00951 goal_right = ivStateId2State[ivIdStartFootRight]; 00952 } 00953 00954 int nDist_sq = nDist_c*nDist_c; 00955 00956 //add left if within the distance 00957 if (euclidean_distance_sq(X, Y, goal_left->getX(), goal_left->getY()) <= nDist_sq) 00958 { 00959 //compute clow 00960 int clow; 00961 if(bSuccs) 00962 clow = GetFromToHeuristic(*currentState, *goal_left); 00963 else 00964 clow = GetFromToHeuristic(*goal_left, *currentState); 00965 00966 NeighIDV->push_back(goal_left->getId()); 00967 CLowV->push_back(clow); 00968 ivRandomStates.push_back(goal_left->getId()); 00969 } 00970 00971 //add right if within the distance 00972 if(euclidean_distance_sq(X, Y, goal_right->getX(), goal_right->getY()) <= nDist_sq) 00973 { 00974 //compute clow 00975 int clow; 00976 if(bSuccs) 00977 clow = GetFromToHeuristic(*currentState, *goal_right); 00978 else 00979 clow = GetFromToHeuristic(*goal_right, *currentState); 00980 00981 NeighIDV->push_back(goal_right->getId()); 00982 CLowV->push_back(clow); 00983 ivRandomStates.push_back(goal_right->getId()); 00984 } 00985 00986 //iterate through random actions 00987 int nAttempts = 0; 00988 for (int i = 0; i < nNumofNeighs && nAttempts < 5*nNumofNeighs; i++, nAttempts++) 00989 { 00990 00991 // pick goal in random direction 00992 float fDir = (float)(TWO_PI*(((double)rand())/RAND_MAX)); 00993 00994 int dX = (int)(nDist_c*cos(fDir)); 00995 int dY = (int)(nDist_c*sin(fDir)); 00996 00997 //get the coords of the state 00998 int newX = X + dX; 00999 int newY = Y + dY; 01000 01001 // TODO / FIXME x,y, can be negative! need offset 01002 // check if outside of map: 01003 // if (newX < 0 || newY < 0 || unsigned(newX) >= ivMapPtr->getInfo().width || unsigned(newY) >= ivMapPtr->getInfo().height){ 01004 // i--; 01005 // ROS_INFO("Outside of map: %d %d", newX, newY); 01006 // continue; 01007 // } 01008 01009 // direction of random exploration (facing forward): 01010 int newTheta = angle_state_2_cell(fDir, ivNumAngleBins); 01011 01012 // random left/right 01013 Leg newLeg = Leg(rand() % 2); 01014 01015 PlanningState randomState(newX, newY, newTheta, newLeg, ivHashTableSize); 01016 01017 // add both left and right if available: 01018 // int sep = disc_val(0.07, ivCellSize); 01019 // int ddX = int(-sin(fDir) * sep); 01020 // int ddY = int(cos(fDir) * sep); 01021 // PlanningState randomState(newX+ddX, newY+ddY, newTheta, LEFT, ivHashTableSize); 01022 // 01023 // PlanningState randomStateR(newX-ddX, newY-ddY, newTheta, RIGHT, ivHashTableSize); 01024 01025 if(!occupied(randomState)) 01026 { 01027 const PlanningState* random_hash_entry = getHashEntry(randomState); 01028 if (random_hash_entry == NULL){ 01029 random_hash_entry = createNewHashEntry(randomState); 01030 ivRandomStates.push_back(random_hash_entry->getId()); 01031 } 01032 01033 //compute clow 01034 int clow; 01035 if(bSuccs) 01036 clow = GetFromToHeuristic(currentState->getId(), random_hash_entry->getId()); 01037 01038 else 01039 clow = GetFromToHeuristic(random_hash_entry->getId(), currentState->getId()); 01040 01041 NeighIDV->push_back(random_hash_entry->getId()); 01042 CLowV->push_back(clow); 01043 01044 }else{ 01045 i--; 01046 } 01047 01048 // if(!occupied(randomStateR)) 01049 // { 01050 // const PlanningState* random_hash_entry = getHashEntry(randomStateR); 01051 // if (random_hash_entry == NULL){ 01052 // random_hash_entry = createNewHashEntry(randomStateR); 01053 // ivRandomStates.push_back(random_hash_entry->getId()); 01054 // } 01055 // 01056 // //compute clow 01057 // int clow; 01058 // if(bSuccs) 01059 // clow = GetFromToHeuristic(currentState->getId(), random_hash_entry->getId()); 01060 // else 01061 // clow = GetFromToHeuristic(random_hash_entry->getId(), currentState->getId()); 01062 // 01063 // NeighIDV->push_back(random_hash_entry->getId()); 01064 // CLowV->push_back(clow); 01065 // 01066 // }else{ 01067 // i--; 01068 // } 01069 01070 01071 } 01072 01073 if (NeighIDV->size() == 0){ 01074 ROS_WARN("Could not create any random neighbor nodes (%d attempts) from id %d (%d %d)", 01075 nAttempts, currentState->getId(), X, Y); 01076 } else 01077 01078 ROS_DEBUG("Created %zu random neighbors (%d attempts) from id %d " 01079 "(%d %d)", NeighIDV->size(), nAttempts, currentState->getId(), 01080 X, Y); 01081 } 01082 01083 bool 01084 FootstepPlannerEnvironment::AreEquivalent(int StateID1, int StateID2) 01085 { 01086 assert(StateID1 >= 0 && StateID2 >= 0 01087 && unsigned(StateID1) < ivStateId2State.size() && unsigned(StateID2) < ivStateId2State.size()); 01088 01089 01090 if (StateID1 == StateID2) 01091 return true; 01092 01093 const PlanningState* s1 = ivStateId2State[StateID1]; 01094 const PlanningState* s2 = ivStateId2State[StateID2]; 01095 01096 // // approximately compare, ignore theta: 01097 return (std::abs(s1->getX() - s2->getX()) < 1 01098 && std::abs(s1->getY() - s2->getY()) < 1 01099 // && std::abs(s1->getTheta() - s2->getTheta()) < 3 01100 && s1->getLeg() == s2->getLeg() 01101 ); 01102 01103 01104 // compare the actual values (exact comparison) 01105 // return (*s1 == *s2); 01106 } 01107 01108 01109 01110 bool 01111 FootstepPlannerEnvironment::InitializeEnv(const char *sEnvFile) 01112 { 01113 // ROS_ERROR("FootstepPlanerEnvironment::InitializeEnv: Hit " 01114 // "unimplemented function. Check this!"); 01115 return true; 01116 } 01117 01118 01119 bool 01120 FootstepPlannerEnvironment::InitializeMDPCfg(MDPConfig *MDPCfg) 01121 { 01122 // NOTE: The internal start and goal ids are set here to the left foot 01123 // (this affects the calculation of the heuristic values) 01124 MDPCfg->goalstateid = ivIdGoalFootLeft; 01125 MDPCfg->startstateid = ivIdStartFootLeft; 01126 01127 assert(ivIdGoalFootLeft != -1); 01128 assert(ivIdStartFootLeft != -1); 01129 01130 return true; 01131 } 01132 01133 01134 void 01135 FootstepPlannerEnvironment::PrintEnv_Config(FILE *fOut) 01136 { 01137 // NOTE: implement this if the planner needs to print out configurations 01138 ROS_ERROR("FootstepPlanerEnvironment::PrintEnv_Config: Hit " 01139 "unimplemented function. Check this!"); 01140 } 01141 01142 01143 void 01144 FootstepPlannerEnvironment::PrintState(int stateID, bool bVerbose, 01145 FILE *fOut) 01146 { 01147 if(fOut == NULL) 01148 { 01149 fOut = stdout; 01150 } 01151 01152 if(stateID == ivIdGoalFootLeft && bVerbose) 01153 { 01154 SBPL_FPRINTF(fOut, "the state is a goal state\n"); 01155 } 01156 01157 const PlanningState* s = ivStateId2State[stateID]; 01158 01159 if(bVerbose) 01160 { 01161 SBPL_FPRINTF(fOut, "X=%i Y=%i THETA=%i FOOT=%i\n", 01162 s->getX(), s->getY(), s->getTheta(), s->getLeg()); 01163 } 01164 else 01165 { 01166 SBPL_FPRINTF(fOut, "%i %i %i %i\n", 01167 s->getX(), s->getY(), s->getTheta(), s->getLeg()); 01168 } 01169 } 01170 01171 01172 void 01173 FootstepPlannerEnvironment::SetAllActionsandAllOutcomes(CMDPSTATE *state) 01174 { 01175 // NOTE: not implemented so far 01176 // Description: Some searches may also use SetAllActionsandAllOutcomes 01177 // or SetAllPreds functions if they keep the pointers to successors 01178 // (predecessors) but most searches do not require this, so it is not 01179 // necessary to support this 01180 01181 ROS_ERROR("FootstepPlannerEnvironment::SetAllActionsandAllOutcomes: Hit" 01182 " unimplemented function. Check this!"); 01183 } 01184 01185 01186 void 01187 FootstepPlannerEnvironment::SetAllPreds(CMDPSTATE *state) 01188 { 01189 // NOTE: not implemented so far 01190 // Description: Some searches may also use SetAllActionsandAllOutcomes 01191 // or SetAllPreds functions if they keep the pointers to successors 01192 // (predecessors) but most searches do not require this, so it is not 01193 // necessary to support this 01194 01195 ROS_ERROR("FootstepPlannerEnvironment::SetAllPreds: Hit unimplemented " 01196 "function. Check this!"); 01197 } 01198 01199 01200 int 01201 FootstepPlannerEnvironment::SizeofCreatedEnv() 01202 { 01203 return ivStateId2State.size(); 01204 } 01205 01206 01207 bool 01208 FootstepPlannerEnvironment::pointWithinPolygon( 01209 const std::pair<int, int>& point, 01210 const std::vector<std::pair<int, int> >& edges) 01211 { 01212 int cn = 0; 01213 01214 // loop through all edges of the polygon 01215 for(unsigned int i = 0; i < edges.size() - 1; ++i) 01216 { 01217 if ((edges[i].second <= point.second && edges[i + 1].second > point.second) 01218 || 01219 (edges[i].second > point.second && edges[i + 1].second <= point.second)) 01220 { 01221 float vt = (float)(point.second - edges[i].second) / 01222 (edges[i + 1].second - edges[i].second); 01223 if (point.first < 01224 edges[i].first + vt * (edges[i + 1].first - edges[i].first)) 01225 { 01226 ++cn; 01227 } 01228 } 01229 } 01230 return cn & 1; 01231 } 01232 01233 01234 bool 01235 FootstepPlannerEnvironment::less::operator ()(const PlanningState* a, 01236 const PlanningState* b) 01237 const 01238 { 01239 if (a->getX() < b->getX()) 01240 return true; 01241 else if (a->getY() < b->getY()) 01242 return true; 01243 else 01244 return false; 01245 } 01246 } 01247