environment_chain3d.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Maxim Likhachev
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Maxim Likhachev nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Benjamin Cohen, E. Gil Jones */
00036 
00037 #include <sbpl_interface/environment_chain3d.h>
00038 #include <collision_detection/collision_common.h>
00039 #include <planning_models/conversions.h>
00040 #include <boost/timer.hpp>
00041 #include <planning_models/angle_utils.h>
00042 
00043 static const unsigned int DEBUG_OVER = 1;
00044 static const unsigned int PRINT_HEURISTIC_UNDER = 1;
00045 
00046 static const double JOINT_DIST_MULT=1000.0;
00047 
00048 namespace sbpl_interface
00049 {
00050 
00051 EnvironmentChain3D::EnvironmentChain3D(const planning_scene::PlanningSceneConstPtr& planning_scene) :
00052   planning_scene_(planning_scene),
00053   bfs_(NULL),
00054   state_(planning_scene->getCurrentState()),
00055   planning_data_(StateID2IndexMapping),
00056   goal_constraint_set_(planning_scene->getRobotModel(),
00057                        planning_scene->getTransforms()),
00058   path_constraint_set_(planning_scene->getRobotModel(),
00059                        planning_scene->getTransforms()),
00060   interpolation_state_1_(planning_scene->getCurrentState()),
00061   interpolation_state_2_(planning_scene->getCurrentState()),
00062   interpolation_state_temp_(planning_scene->getCurrentState()),
00063   closest_to_goal_(DBL_MAX)
00064 {
00065 }
00066 
00067 
00068 EnvironmentChain3D::~EnvironmentChain3D()
00069 {
00070   if(bfs_ != NULL) {
00071     delete bfs_;
00072   }
00073 }
00074 
00076 //                      SBPL Planner Interface
00078 
00079 bool EnvironmentChain3D::InitializeMDPCfg(MDPConfig *MDPCfg)
00080 {
00081   if(planning_data_.goal_hash_entry_ &&
00082      planning_data_.start_hash_entry_) {
00083     MDPCfg->goalstateid = planning_data_.goal_hash_entry_->stateID;
00084     MDPCfg->startstateid = planning_data_.start_hash_entry_->stateID;
00085     return true;
00086   }
00087   return false;
00088 }
00089 
00090 bool EnvironmentChain3D::InitializeEnv(const char* sEnvFile)
00091 {
00092   ROS_INFO("[env] InitializeEnv is not implemented right now.");
00093   return true;
00094 }
00095 
00096 int EnvironmentChain3D::GetFromToHeuristic(int FromStateID, int ToStateID)
00097 {
00098   //std::cerr << "Getting heuristic" << std::endl;
00099   return getEndEffectorHeuristic(FromStateID,ToStateID);
00100 }
00101 
00102 int EnvironmentChain3D::GetGoalHeuristic(int stateID)
00103 {
00104   if(planning_data_.state_ID_to_coord_table_.size() < DEBUG_OVER) {
00105     std::cerr << "Getting heur distance from " << stateID << " to "
00106               << planning_data_.goal_hash_entry_->stateID << " "
00107               << GetFromToHeuristic(stateID, planning_data_.goal_hash_entry_->stateID) << std::endl;
00108   }
00109   return GetFromToHeuristic(stateID, planning_data_.goal_hash_entry_->stateID);
00110 }
00111 
00112 int EnvironmentChain3D::GetStartHeuristic(int stateID)
00113 {
00114   return GetFromToHeuristic(stateID, planning_data_.start_hash_entry_->stateID);
00115 }
00116 
00117 int EnvironmentChain3D::SizeofCreatedEnv()
00118 {
00119   return planning_data_.state_ID_to_coord_table_.size();
00120 }
00121 
00122 void EnvironmentChain3D::PrintState(int stateID, bool bVerbose, FILE* fOut /*=NULL*/)
00123 {
00124   // if(fOut == NULL)
00125   //   fOut = stdout;
00126 
00127   // EnvChain3DHashEntry* HashEntry = EnvChain.StateID2CoordTable[stateID];
00128 
00129   // bool bGoal = false;
00130   // if(stateID == EnvChain.goalHashEntry->stateID)
00131   //   bGoal = true;
00132 
00133   // if(stateID == EnvChain.goalHashEntry->stateID && bVerbose)
00134   // {
00135   //   //ROS_DEBUG_NAMED(fOut, "the state is a goal state\n");
00136   //   bGoal = true;
00137   // }
00138 
00139   // printJointArray(fOut, HashEntry, bGoal, bVerbose);
00140 }
00141 
00142 void EnvironmentChain3D::PrintEnv_Config(FILE* fOut)
00143 {
00144   std::cerr <<("ERROR in EnvChain... function: PrintEnv_Config is undefined\n");
00145   throw new SBPL_Exception();
00146 }
00147 
00148 void EnvironmentChain3D::GetSuccs(int source_state_ID,
00149                                   std::vector<int>* succ_idv,
00150                                   std::vector<int>* cost_v)
00151 {
00152   boost::this_thread::interruption_point();
00153   //std::cerr << "Calling get succ state " << source_state_ID << std::endl;
00154 
00155   ros::WallTime expansion_start_time = ros::WallTime::now();
00156 
00157   succ_idv->clear();
00158   cost_v->clear();
00159 
00160   //From environment_robarm3d.cpp -- //goal state should be absorbing
00161   if(source_state_ID == planning_data_.goal_hash_entry_->stateID) {
00162     std::cerr << "Think we have goal" << std::endl;
00163     return;
00164   }
00165 
00166   if(source_state_ID > (int)planning_data_.state_ID_to_coord_table_.size()-1) {
00167     ROS_WARN_STREAM("source id too large");
00168     std::cerr << "Source id too large" << std::endl;
00169     return;
00170   }
00171 
00172   if(planning_data_.state_ID_to_coord_table_.size() < DEBUG_OVER) {
00173     std::cerr << "Expanding " << source_state_ID << std::endl;
00174   }
00175 
00176   EnvChain3DHashEntry* hash_entry = planning_data_.state_ID_to_coord_table_[source_state_ID];
00177 
00178   std::vector<double> source_joint_angles = hash_entry->angles;
00179   //convertCoordToJointAngles(hash_entry->coord, source_joint_angles);
00180 
00181   std::vector<int> succ_coord;
00182   std::vector<double> succ_joint_angles;
00183 
00184   // for(unsigned int i = 0; i < source_joint_angles.size(); i++) {
00185   //   std::cerr << "Source " << i << " " << source_joint_angles[i] << std::endl;
00186   // }
00187 
00188   planning_statistics_.total_expansions_++;
00189 
00190   for(unsigned int i = 0; i < possible_actions_.size(); i++) {
00191     if(!possible_actions_[i]->generateSuccessorState(source_joint_angles, succ_joint_angles)) {
00192       continue;
00193     }
00194 
00195     // for(unsigned int j = 0; j < planning_data_.goal_hash_entry_->angles.size(); j++) {
00196     //   if(joint_is_continuous_[j]) {
00197     //     if(source_joint_angles[j] < M_PI && succ_joint_angles[j] > M_PI) {
00198     //       succ_joint_angles[j] -= 2*M_PI;
00199     //     } else if(source_joint_angles[j] > -M_PI && succ_joint_angles[j] < -M_PI) {
00200     //       succ_joint_angles[j] += 2*M_PI;
00201     //     }
00202     //   }
00203     // }
00204     // double dist = 0.0;
00205     // for(unsigned int j = 0; j < planning_data_.goal_hash_entry_->angles.size(); j++) {
00206     //   dist += fabs(planning_data_.goal_hash_entry_->angles[j]-succ_joint_angles[j]);
00207     //   //std::cerr << "Succ " << i << " " << j << " " << succ_joint_angles[j] << std::endl;
00208     // }
00209     // if(dist < closest_to_goal_) {
00210     //   //std::cerr << "Got dist " << dist << std::endl;
00211     //   closest_to_goal_ = dist;
00212     // }
00213 
00214     // int max_dist = getJointDistanceIntegerMax(succ_joint_angles,
00215     //                                           planning_data_.goal_hash_entry_->angles);
00216     // if(max_dist*1.0 < closest_to_goal_) {
00217     //   std::cerr << "Max integer distance is " << max_dist << std::endl;
00218     //   closest_to_goal_ = max_dist*1.0;
00219     //   // for(unsigned int j = 0; j < joint_motion_wrappers_.size(); j++) {
00220     //   //   if(joint_motion_wrappers_[j]->canGetCloser(succ_joint_angles[j],
00221     //   //                                              planning_data_.goal_hash_entry_->angles[j],
00222     //   //                                              LONG_RANGE_JOINT_DIFF)) {
00223     //   //     std::cerr << "Joint " << j << " succ " << succ_joint_angles[j] << " "
00224     //   //               << planning_data_.goal_hash_entry_->angles[j] << " can get closer\n";
00225     //   //     break;
00226     //   //   }
00227     //   // }
00228     // }
00229     convertJointAnglesToCoord(succ_joint_angles, succ_coord);
00230 
00231     joint_state_group_->setStateValues(succ_joint_angles);
00232 
00233     kinematic_constraints::ConstraintEvaluationResult con_res = path_constraint_set_.decide(state_);
00234     if(!con_res.satisfied) {
00235       ROS_INFO_STREAM("State violates path constraints");
00236     }
00237 
00238     // if(!joint_state_group_->satisfiesBounds()) {
00239     //   std::cerr << "ERROR - successor doesn't satisfy bounds" << std::endl;
00240     //   //std::cerr << "Successor doesn't satisfy bounds" << std::endl;
00241     //   continue;
00242     // }
00243     ros::WallTime before_coll = ros::WallTime::now();
00244     collision_detection::CollisionRequest req;
00245     collision_detection::CollisionResult res;
00246     req.group_name = planning_group_;
00247     if(!planning_parameters_.use_standard_collision_checking_) {
00248       hy_world_->checkCollisionDistanceField(req,
00249                                              res,
00250                                              *hy_robot_->getCollisionRobotDistanceField().get(),
00251                                              state_,
00252                                              gsr_);
00253     } else {
00254       planning_scene_->checkCollision(req, res, state_);
00255     }
00256     planning_statistics_.coll_checks_++;
00257     //std::cerr << "Elapsed " << t.elapsed() << std::endl;
00258     ros::WallDuration dur(ros::WallTime::now()-before_coll);
00259     //std::cerr << dur.toSec() << std::endl;
00260     //ROS_DEBUG_STREAM("Time " << ros::WallTime::now()-before_coll);
00261     planning_statistics_.total_coll_check_time_ += dur;
00262     if(res.collision) {
00263       //std::cerr << "Successor in collision" << std::endl;
00264       continue;
00265     }
00266 
00267     Eigen::Affine3d pose = tip_link_state_->getGlobalLinkTransform();
00268 
00269     int xyz[3];
00270     if(!planning_parameters_.use_standard_collision_checking_) {
00271       if(!getGridXYZInt(pose, xyz)) {
00272         std::cerr << "Can't get successor x y z" << std::endl;
00273         continue;
00274       }
00275     }
00276     int dist;
00277     if(planning_parameters_.use_bfs_) {
00278       dist = getBFSCostToGoal(xyz[0],
00279                               xyz[1],
00280                               xyz[2]);
00281     } else {
00282       dist = getJointDistanceIntegerMax(succ_joint_angles,
00283                                         planning_data_.goal_hash_entry_->angles,
00284                                         planning_parameters_.joint_motion_primitive_distance_);
00285     }
00286 
00287     EnvChain3DHashEntry* succ_hash_entry = NULL;
00288     // bool can_get_closer = false;
00289     // for(unsigned int j = 0; j < joint_motion_wrappers_.size(); j++) {
00290     //   if(joint_motion_wrappers_[j]->canGetCloser(succ_joint_angles[j],
00291     //                                              planning_data_.goal_hash_entry_->angles[j],
00292     //                                              LONG_RANGE_JOINT_DIFF)) {
00293     //     can_get_closer = true;
00294     //     // std::cerr << "Joint " << j << " succ " << succ_joint_angles[j] << " "
00295     //     //           << planning_data_.goal_hash_entry_->angles[j] << " can get closer\n";
00296     //     break;
00297     //   }
00298     // }
00299     //if(!can_get_closer) {
00300     bool succ_is_goal_state = false;
00301     if((planning_parameters_.use_bfs_ && dist == 0) || (!planning_parameters_.use_bfs_ && dist == 1)) {
00302       //std::cerr << "Joint distance for goal move " << getJointDistanceDoubleSum(planning_data_.goal_hash_entry_->angles, succ_joint_angles) << std::endl;
00303       std::vector<std::vector<double> > interpolated_values;
00304       if(interpolateAndCollisionCheck(source_joint_angles,
00305                                       planning_data_.goal_hash_entry_->angles,
00306                                       interpolated_values)) {
00307         //std::cerr << "Interpolation generated from id " << source_state_ID << " is " << interpolated_values.size() << " states\n";
00308         generated_interpolations_map_[source_state_ID][planning_data_.goal_hash_entry_->stateID] = interpolated_values;
00309         succ_hash_entry = planning_data_.goal_hash_entry_;
00310         succ_is_goal_state = true;
00311       } else {
00312         //std::cerr << "Interpolation in collision\n";
00313       }
00314     }
00315     std::vector<std::vector<double> > interpolated_values;
00316     if(!succ_is_goal_state) {
00317       if(planning_parameters_.interpolation_distance_ < planning_parameters_.joint_motion_primitive_distance_) {
00318         if(!interpolateAndCollisionCheck(source_joint_angles,
00319                                          succ_joint_angles,
00320                                          interpolated_values)) {
00321           //std::cerr << "Interpolation failed" << std::endl;
00322           continue;
00323         }
00324       }
00325       succ_hash_entry = planning_data_.getHashEntry(succ_coord, i);
00326     }
00327     // double max_dist = getJointDistanceMax(planning_data_.goal_hash_entry_->angles, succ_joint_angles);
00328     // if(max_dist < closest_to_goal_) {
00329     //   std::cerr << "State " << source_state_ID << " sum dist " << getJointDistanceSum(planning_data_.goal_hash_entry_->angles, succ_joint_angles) << " max "
00330     //             << " " << getJointDistanceMax(planning_data_.goal_hash_entry_->angles, succ_joint_angles) << std::endl;
00331     //   for(unsigned int i = 0; i < planning_data_.goal_hash_entry_->angles.size(); i++) {
00332     //     std::cerr << "Joint " << i << " " << planning_data_.goal_hash_entry_->angles[i] << " " << succ_joint_angles[i] << std::endl;
00333     //   }
00334     //   closest_to_goal_ = max_dist;
00335     // }
00336     // std::cerr << "Dist " << dist << " source state id " << source_state_ID << std::endl;
00337     // if(max_dist <= LONG_RANGE_JOINT_DIFF/2.0+.001) {
00338     //   succ_hash_entry = planning_data_.goal_hash_entry_;
00339     // } else {
00340     //   succ_hash_entry = planning_data_.getHashEntry(succ_coord, i);
00341     // }
00342 
00343     // kinematic_constraints::ConstraintEvaluationResult con_res = goal_constraint_set_.decide(state_);
00344     // if(con_res.satisfied) {
00345     //   std::cerr << "Constraints satisfied, dist is " << dist << std::endl;
00346     //   succ_hash_entry = planning_data_.goal_hash_entry_;
00347     // } else {
00348     //   succ_hash_entry = planning_data_.getHashEntry(succ_coord, i);
00349     // }
00350     if(!succ_hash_entry) {
00351       succ_hash_entry = planning_data_.addHashEntry(succ_coord, succ_joint_angles, xyz, i);
00352     } else {
00353       if(planning_data_.state_ID_to_coord_table_.size() < DEBUG_OVER) {
00354         std::cerr << "Already have hash entry " << succ_hash_entry->stateID << std::endl;
00355       }
00356     }
00357 
00358     if(planning_parameters_.interpolation_distance_ < planning_parameters_.joint_motion_primitive_distance_
00359        && !succ_is_goal_state) {
00360       //std::cerr << "Adding segment from " << source_state_ID << " to " << succ_hash_entry->stateID << std::endl;
00361       generated_interpolations_map_[source_state_ID][succ_hash_entry->stateID] = interpolated_values;
00362     }
00363 
00364     //std::cerr << "Adding hash entry" << std::endl;
00365     if(planning_data_.state_ID_to_coord_table_.size() < DEBUG_OVER) {
00366       std::cerr << std::endl;
00367       std::cerr << "Adding " << succ_hash_entry->stateID << std::endl;
00368       for(unsigned int j = 0; j < planning_data_.goal_hash_entry_->angles.size(); j++) {
00369         std::cerr << "Succ " << j << " " << succ_joint_angles[j] << std::endl;
00370       }
00371     }
00372     succ_idv->push_back(succ_hash_entry->stateID);
00373     cost_v->push_back(calculateCost(hash_entry, succ_hash_entry));
00374   }
00375   planning_statistics_.total_expansion_time_ += ros::WallTime::now()-expansion_start_time;
00376 }
00377 
00378 void EnvironmentChain3D::GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* cost_v)
00379 {
00380   std::cerr <<("ERROR in EnvChain... function: GetPreds is undefined\n");
00381   throw new SBPL_Exception();
00382 }
00383 
00384 bool EnvironmentChain3D::AreEquivalent(int StateID1, int StateID2)
00385 {
00386   std::cerr <<("ERROR in EnvChain... function: AreEquivalent is undefined\n");
00387   throw new SBPL_Exception();
00388 }
00389 
00390 void EnvironmentChain3D::SetAllActionsandAllOutcomes(CMDPSTATE* state)
00391 {
00392   std::cerr <<("ERROR in EnvChain..function: SetAllActionsandOutcomes is undefined\n");
00393   throw new SBPL_Exception();
00394 }
00395 
00396 void EnvironmentChain3D::SetAllPreds(CMDPSTATE* state)
00397 {
00398   std::cerr <<("ERROR in EnvChain... function: SetAllPreds is undefined\n");
00399   throw new SBPL_Exception();
00400 }
00401 
00403 //                      End of SBPL Planner Interface
00405 
00406 bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00407                                             const moveit_msgs::GetMotionPlan::Request &mreq,
00408                                             moveit_msgs::GetMotionPlan::Response &mres,
00409                                             const PlanningParameters& params)
00410 {
00411   std::cerr << "really here " << std::endl;
00412   planning_scene_ = planning_scene;
00413 
00414   planning_group_ = mreq.motion_plan_request.group_name;
00415   planning_parameters_ = params;
00416 
00417   if(!planning_parameters_.use_standard_collision_checking_) {
00418     hy_world_ = dynamic_cast<const collision_detection::CollisionWorldHybrid*>(planning_scene->getCollisionWorld().get());
00419     if(!hy_world_) {
00420       ROS_WARN_STREAM("Could not initialize hybrid collision world from planning scene");
00421       mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE;
00422       return false;
00423     }
00424 
00425     hy_robot_ = dynamic_cast<const collision_detection::CollisionRobotHybrid*>(planning_scene->getCollisionRobot().get());
00426     if(!hy_robot_) {
00427       ROS_WARN_STREAM("Could not initialize hybrid collision robot from planning scene");
00428       mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE;
00429       return false;
00430     }
00431   }
00432 
00433   state_ = planning_scene->getCurrentState();
00434   interpolation_state_1_ = planning_scene->getCurrentState();
00435   interpolation_state_2_ = planning_scene->getCurrentState();
00436   interpolation_state_temp_ = planning_scene->getCurrentState();
00437 
00438   planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), mreq.motion_plan_request.start_state, state_);
00439   joint_state_group_ = state_.getJointStateGroup(planning_group_);
00440   interpolation_joint_state_group_1_ = interpolation_state_1_.getJointStateGroup(planning_group_);
00441   interpolation_joint_state_group_2_ = interpolation_state_2_.getJointStateGroup(planning_group_);
00442   interpolation_joint_state_group_temp_ = interpolation_state_temp_.getJointStateGroup(planning_group_);
00443   tip_link_state_ = state_.getLinkState(joint_state_group_->getJointModelGroup()->getLinkModelNames().back());
00444 
00445   collision_detection::CollisionRequest req;
00446   collision_detection::CollisionResult res;
00447   req.group_name = planning_group_;
00448   if(!planning_parameters_.use_standard_collision_checking_) {
00449     hy_world_->checkCollisionDistanceField(req,
00450                                            res,
00451                                            *hy_robot_->getCollisionRobotDistanceField().get(),
00452                                            state_,
00453                                            planning_scene_->getAllowedCollisionMatrix(),
00454                                            gsr_);
00455   } else {
00456     planning_scene->checkCollision(req, res, state_);
00457   }
00458   if(res.collision) {
00459     ROS_WARN_STREAM("Start state is in collision.  Can't plan");
00460     mres.error_code.val = moveit_msgs::MoveItErrorCodes::START_STATE_IN_COLLISION;
00461     return false;
00462   }
00463   if(!planning_parameters_.use_standard_collision_checking_) {
00464     angle_discretization_ = gsr_->dfce_->distance_field_->getResolution();
00465   } else {
00466     angle_discretization_ = .02;
00467   }
00468   setMotionPrimitives(planning_group_);
00469 
00470   //can only do bfs if not using standard collison checking
00471   if(planning_parameters_.use_standard_collision_checking_) {
00472     planning_parameters_.use_bfs_ = false;
00473   }
00474   if(!planning_parameters_.use_standard_collision_checking_ && planning_parameters_.use_bfs_) {
00475     bfs_ = new BFS_3D(gsr_->dfce_->distance_field_->getXNumCells(),
00476                       gsr_->dfce_->distance_field_->getYNumCells(),
00477                       gsr_->dfce_->distance_field_->getZNumCells());
00478 
00479     boost::shared_ptr<const distance_field::DistanceField> world_distance_field = hy_world_->getCollisionWorldDistanceField()->getDistanceField();
00480     if(world_distance_field->getXNumCells() != gsr_->dfce_->distance_field_->getXNumCells() ||
00481        world_distance_field->getYNumCells() != gsr_->dfce_->distance_field_->getYNumCells() ||
00482        world_distance_field->getZNumCells() != gsr_->dfce_->distance_field_->getZNumCells()) {
00483       ROS_WARN_STREAM("Size mismatch between world and self distance fields");
00484       std::cerr << "Size mismatch between world and self distance fields" << std::endl;
00485       mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE;
00486       return false;
00487     }
00488     // std::cerr << "BFS dimensions are "
00489     //           << world_distance_field->getXNumCells() << " "
00490     //           << world_distance_field->getYNumCells() << " "
00491     //           << world_distance_field->getZNumCells() << std::endl;
00492     unsigned int wall_count = 0;
00493     for(int i = 0; i < gsr_->dfce_->distance_field_->getXNumCells()-2; i++) {
00494       for(int j = 0; j < gsr_->dfce_->distance_field_->getYNumCells()-2; j++) {
00495         for(int k = 0; k < gsr_->dfce_->distance_field_->getZNumCells()-2; k++) {
00496           boost::this_thread::interruption_point();
00497           if(gsr_->dfce_->distance_field_->getDistanceFromCell(i+1, j+1, k+1) == 0.0 ||
00498              world_distance_field->getDistanceFromCell(i+1, j+1, k+1) == 0.0) {
00499             bfs_->setWall(i+1, j+1, k+1);
00500             wall_count++;
00501           }
00502         }
00503       }
00504     }
00505   }
00506   // std::cerr << "Wall cells are " << wall_count << " of " <<
00507   //   world_distance_field->getXNumCells()*world_distance_field->getYNumCells()*world_distance_field->getZNumCells() << std::endl;
00508 
00509   //setting start position
00510   std::vector<double> start_joint_values;
00511   joint_state_group_->getGroupStateValues(start_joint_values);
00512   std::vector<int> start_coords;
00513   convertJointAnglesToCoord(start_joint_values, start_coords);
00514   Eigen::Affine3d start_pose = tip_link_state_->getGlobalLinkTransform();
00515 
00516   int start_xyz[3];
00517   if(!planning_parameters_.use_standard_collision_checking_) {
00518     if(!getGridXYZInt(start_pose, start_xyz)) {
00519       std::cerr << "Bad start pose" << std::endl;
00520       mres.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
00521       return false;
00522     }
00523   } else {
00524     start_xyz[0] = 0.0;
00525     start_xyz[1] = 0.0;
00526     start_xyz[2] = 0.0;
00527   }
00528   planning_data_.start_hash_entry_ = planning_data_.addHashEntry(start_coords,
00529                                                                  start_joint_values,
00530                                                                  start_xyz,
00531                                                                  0);
00532 
00533   //setting goal position
00534   planning_models::RobotState *goal_state(state_);
00535   std::map<std::string, double> goal_vals;
00536   for(unsigned int i = 0; i < mreq.motion_plan_request.goal_constraints[0].joint_constraints.size(); i++) {
00537     goal_vals[mreq.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name] = mreq.motion_plan_request.goal_constraints[0].joint_constraints[i].position;
00538   }
00539   goal_state.setStateValues(goal_vals);
00540   if(!planning_parameters_.use_standard_collision_checking_) {
00541     hy_world_->checkCollisionDistanceField(req,
00542                                            res,
00543                                            *hy_robot_->getCollisionRobotDistanceField().get(),
00544                                            goal_state,
00545                                            planning_scene_->getAllowedCollisionMatrix(),
00546                                            gsr_);
00547   } else {
00548     planning_scene->checkCollision(req, res, goal_state);
00549   }
00550   if(res.collision) {
00551     ROS_WARN_STREAM("Goal state is in collision.  Can't plan");
00552     mres.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_IN_COLLISION;
00553     return false;
00554   }
00555   std::vector<double> goal_joint_values;
00556   planning_models::RobotState *::JointStateGroup* goal_joint_state_group = goal_state.getJointStateGroup(planning_group_);
00557   goal_joint_state_group->getGroupStateValues(goal_joint_values);
00558   std::vector<int> goal_coords;
00559   convertJointAnglesToCoord(goal_joint_values, goal_coords);
00560   goal_pose_ = goal_state.getLinkState(tip_link_state_->getName())->getGlobalLinkTransform();
00561   int goal_xyz[3];
00562   if(!planning_parameters_.use_standard_collision_checking_) {
00563     if(!getGridXYZInt(goal_pose_, goal_xyz)) {
00564       std::cerr << "Bad goal pose" << std::endl;
00565       mres.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
00566       return false;
00567     }
00568   } else {
00569     goal_xyz[0] = 0.0;
00570     goal_xyz[1] = 0.0;
00571     goal_xyz[2] = 0.0;
00572   }
00573   std::vector<std::string> goal_dofs = goal_joint_state_group->getJointModelGroup()->getActiveDOFNames();
00574   for(unsigned int i = 0; i < goal_dofs.size(); i++) {
00575     ROS_DEBUG_STREAM("Start " << goal_dofs[i] << " pos " << start_joint_values[i]);
00576   }
00577   for(unsigned int i = 0; i < goal_dofs.size(); i++) {
00578     ROS_DEBUG_STREAM("Goal " << goal_dofs[i] << " pos " << goal_joint_values[i]);
00579   }
00580   //std::cerr << "Running bfs with goal " << goal_xyz[0] << " " <<  goal_xyz[1] << " " << goal_xyz[2] << std::endl;
00581   if(planning_parameters_.use_bfs_) {
00582     bfs_->run(goal_xyz[0], goal_xyz[1], goal_xyz[2]);
00583     // std::cerr << "Got start " << start_xyz[0] << " " <<  start_xyz[1] << " " << start_xyz[2] << " cost "
00584     //           << getBFSCostToGoal(start_xyz[0], start_xyz[1], start_xyz[2]) << std::endl;
00585   }
00586   goal_constraint_set_.clear();
00587   goal_constraint_set_.add(mreq.motion_plan_request.goal_constraints[0]);
00588   planning_data_.goal_hash_entry_ = planning_data_.addHashEntry(goal_coords,
00589                                                                 goal_joint_values,
00590                                                                 goal_xyz,
00591                                                                 0);
00592   path_constraint_set_.clear();
00593   path_constraint_set_.add(mreq.motion_plan_request.path_constraints);
00594   return true;
00595 }
00596 
00597 void EnvironmentChain3D::setMotionPrimitives(const std::string& group_name) {
00598   possible_actions_.clear();
00599   if(!joint_state_group_) {
00600     ROS_ERROR_STREAM("Can't set motion primitives as joint state group not set");
00601   }
00602   const planning_models::RobotModel::JointModelGroup* jmg = joint_state_group_->getJointModelGroup();
00603   for(unsigned int i = 0; i < jmg->getActiveDOFNames().size(); i++) {
00604     const planning_models::RobotModel::JointModel* joint = jmg->getJointModel(jmg->getActiveDOFNames()[i]);
00605     boost::shared_ptr<JointMotionWrapper> jmw(new JointMotionWrapper(joint));
00606     joint_motion_wrappers_.push_back(jmw);
00607     //TODO - figure out which DOFs have something to do with end effector position
00608     if(!planning_parameters_.use_bfs_ || i < 4) {
00609       boost::shared_ptr<SingleJointMotionPrimitive> sing_pos(new SingleJointMotionPrimitive(jmw, i, planning_parameters_.joint_motion_primitive_distance_));
00610       boost::shared_ptr<SingleJointMotionPrimitive> sing_neg(new SingleJointMotionPrimitive(jmw, i, -planning_parameters_.joint_motion_primitive_distance_));
00611       possible_actions_.push_back(sing_pos);
00612       possible_actions_.push_back(sing_neg);
00613     }
00614   }
00615   determineMaximumEndEffectorTravel();
00616 }
00617 
00618 void EnvironmentChain3D::determineMaximumEndEffectorTravel() {
00619   planning_models::RobotState *def(state_);
00620   def.setToDefaultValues();
00621   planning_models::RobotState *::JointStateGroup* jsg = def.getJointStateGroup(planning_group_);
00622   planning_models::RobotState *::LinkState* tip_link_state = def.getLinkState(jsg->getJointModelGroup()->getLinkModelNames().back());
00623   std::vector<double> default_values;
00624   jsg->getGroupStateValues(default_values);
00625   Eigen::Affine3d default_pose = tip_link_state->getGlobalLinkTransform();
00626   double default_x = default_pose.translation().x();
00627   double default_y = default_pose.translation().y();
00628   double default_z = default_pose.translation().z();
00629   double max_dist = 0.0;
00630   for(unsigned int i = 0; i < possible_actions_.size(); i++) {
00631     std::vector<double> succ_joint_angles;
00632     if(!possible_actions_[i]->generateSuccessorState(default_values, succ_joint_angles)) {
00633       ROS_ERROR_STREAM("Can't move from default state");
00634       continue;
00635     }
00636     jsg->setStateValues(succ_joint_angles);
00637     Eigen::Affine3d motion_pose = tip_link_state->getGlobalLinkTransform();
00638     double motion_x = motion_pose.translation().x();
00639     double motion_y = motion_pose.translation().y();
00640     double motion_z = motion_pose.translation().z();
00641     double dist = getEuclideanDistance(default_x, default_y, default_z,
00642                                        motion_x, motion_y, motion_z);
00643     //std::cerr << "Motion " << i << " dist " << dist << std::endl;
00644     if(dist > max_dist) {
00645       max_dist = dist;
00646     }
00647   }
00648   maximum_distance_for_motion_ = ceil(max_dist/angle_discretization_);
00649   //std::cerr << "Maximum distance is " << maximum_distance_for_motion_ << std::endl;
00650 }
00651 
00652 int EnvironmentChain3D::calculateCost(EnvChain3DHashEntry* HashEntry1, EnvChain3DHashEntry* HashEntry2)
00653 {
00654   //if(prms_.use_uniform_cost_)
00655   if(planning_parameters_.use_bfs_) {
00656     return JOINT_DIST_MULT*(maximum_distance_for_motion_*1.0);
00657   } else {
00658     return JOINT_DIST_MULT;
00659   }
00660   //return .05;//prms_.cost_multiplier_;
00661   //  else
00662   //   {
00663   //     // Max's suggestion is to just put a high cost on being close to
00664   //     // obstacles but don't provide some sort of gradient
00665   //     if(int(HashEntry2->dist) < 7) // in cells
00666   //       return prms_.cost_multiplier_ * prms_.range1_cost_;
00667   //     else if(int(HashEntry2->dist) < 12)
00668   //       return prms_.cost_multiplier_ * prms_.range2_cost_;
00669   //     else if(int(HashEntry2->dist) < 17)
00670   //       return prms_.cost_multiplier_ * prms_.range3_cost_;
00671   //     else
00672   //       return prms_.cost_multiplier_;
00673   //   }
00674   // }
00675 }
00676 
00677 // double EnvironmentChain3D::getEpsilon()
00678 // {
00679 //   return 10.0;
00680 //   //printf("%0.3f\n",prms_.epsilon_);fflush(stdout);
00681 //   //return prms_.epsilon_;
00682 // }
00683 
00684 // int EnvironmentChain3D::getEdgeCost(int FromStateID, int ToStateID)
00685 // {
00686 // #if DEBUG
00687 //   if(FromStateID >= (int)EnvChain.StateID2CoordTable.size()
00688 //       || ToStateID >= (int)EnvChain.StateID2CoordTable.size())
00689 //   {
00690 //     std::cerr <<("ERROR in EnvChain... function: stateID illegal\n");
00691 //     throw new SBPL_Exception();
00692 //   }
00693 // #endif
00694 
00695 //   //get X, Y for the state
00696 //   EnvChain3DHashEntry* FromHashEntry = EnvChain.StateID2CoordTable[FromStateID];
00697 //   EnvChain3DHashEntry* ToHashEntry = EnvChain.StateID2CoordTable[ToStateID];
00698 
00699 //   return cost(FromHashEntry, ToHashEntry, false);
00700 // }
00701 
00702 int EnvironmentChain3D::getBFSCostToGoal(int x, int y, int z) const
00703 {
00704   //std::cerr << "Getting cost for " << x << " " << y << " " << z << std::endl;
00705   //TODO - deal with cost_per_cell
00706   int cost = (floor(bfs_->getDistance(x,y,z)/(maximum_distance_for_motion_)))*JOINT_DIST_MULT;
00707   //std::cerr << "Cost is " << cost << std::endl;
00708   return cost;
00709   //* .05;//prms_.cost_per_cell_;
00710 }
00711 
00712 int EnvironmentChain3D::getJointDistanceIntegerSum(const std::vector<double>& angles1,
00713                                                    const std::vector<double>& angles2,
00714                                                    double delta) const
00715 {
00716   if(angles1.size() != angles2.size()) {
00717     std::cerr << "Angles aren't the same size!!!" << std::endl;
00718     return INT_MAX;
00719   }
00720   int dist = 0;
00721   for(unsigned int i = 0; i < angles1.size(); i++) {
00722     dist += joint_motion_wrappers_[i]->getIntegerDistance(angles1[i], angles2[i], delta);
00723   }
00724   return dist;
00725 }
00726 
00727 int EnvironmentChain3D::getJointDistanceIntegerMax(const std::vector<double>& angles1,
00728                                                    const std::vector<double>& angles2,
00729                                                    double delta) const
00730 {
00731   if(angles1.size() != angles2.size()) {
00732     std::cerr << "Angles aren't the same size!!!" << std::endl;
00733     return INT_MAX;
00734   }
00735   int max_dist = 0;
00736   for(unsigned int i = 0; i < angles1.size(); i++) {
00737     int dist = joint_motion_wrappers_[i]->getIntegerDistance(angles1[i], angles2[i], delta);
00738     if(dist > max_dist) {
00739       max_dist = dist;
00740     }
00741   }
00742   return max_dist;
00743 }
00744 
00745 double EnvironmentChain3D::getJointDistanceDoubleSum(const std::vector<double>& angles1,
00746                                                      const std::vector<double>& angles2) const
00747 {
00748   if(angles1.size() != angles2.size()) {
00749     return DBL_MAX;
00750   }
00751   double dist = 0.0;
00752   for(unsigned int i = 0; i < angles1.size(); i++) {
00753     double jdist = joint_motion_wrappers_[i]->getDoubleDistance(angles1[i], angles2[i]);
00754     //std::cerr << "Joint " << i << " angle1 " << angles1[i] << " " << angles2[i] << " distance " << jdist << std::endl;
00755     dist += jdist;
00756   }
00757   return dist;
00758 }
00759 
00760 // double EnvironmentChain3D::getJointDistanceMax(const std::vector<double>& angles1,
00761 //                                                const std::vector<double>& angles2)
00762 // {
00763 //   if(angles1.size() != angles2.size()) {
00764 //     return DBL_MAX;
00765 //   }
00766 //   double max_dist = 0.0;
00767 //   for(unsigned int i = 0; i < angles1.size(); i++) {
00768 //     double dist;
00769 //     if(joint_is_continuous_[i]) {
00770 //       if(planning_data_.state_ID_to_coord_table_.size() < PRINT_HEURISTIC_UNDER) {
00771 //         ROS_INFO_STREAM("Dist for cont joint " << i << " from " << angles1[i] << " to " << angles2[i] << " is " <<
00772 //                         planning_models::shortestAngularDistance(angles1[i],angles2[i]));
00773 //       }
00774 //       dist = fabs(planning_models::shortestAngularDistance(angles1[i],angles2[i]));
00775 //     } else {
00776 //       if(planning_data_.state_ID_to_coord_table_.size() < PRINT_HEURISTIC_UNDER) {
00777 //         ROS_INFO_STREAM("Dist for reg joint " << i << " from " << angles1[i] << " to " << angles2[i] << " is " <<
00778 //                         fabs(angles1[i]-angles2[i]));
00779 //       }
00780 //       dist = fabs(angles1[i]-angles2[i]);
00781 //     }
00782 //     if(max_dist < dist) {
00783 //       max_dist = dist;
00784 //     }
00785 //   }
00786 //   return max_dist;
00787 // }
00788 
00789 int EnvironmentChain3D::getEndEffectorHeuristic(int from_stateID, int to_stateID)
00790 {
00791   boost::this_thread::interruption_point();
00792   EnvChain3DHashEntry* from_hash_entry = planning_data_.state_ID_to_coord_table_[from_stateID];
00793   EnvChain3DHashEntry* to_hash_entry = planning_data_.state_ID_to_coord_table_[to_stateID];
00794   //if(planning_data_.state_ID_to_coord_table_.size() < PRINT_HEURISTIC_UNDER) {
00795   //std::cerr << " Dist " << dist << " heur " << getBFSCostToGoal(from_hash_entry->xyz[0], from_hash_entry->xyz[1], from_hash_entry->xyz[2]) << std::endl;
00796   if(planning_parameters_.use_bfs_) {
00797     return getBFSCostToGoal(from_hash_entry->xyz[0],
00798                             from_hash_entry->xyz[1],
00799                             from_hash_entry->xyz[2]);
00800   } else {
00801     return getJointDistanceIntegerSum(from_hash_entry->angles,
00802                                       to_hash_entry->angles,
00803                                       planning_parameters_.joint_motion_primitive_distance_)*JOINT_DIST_MULT;
00804   }
00805   //return getBFSCostToGoal(from_hash_entry->xyz[0], from_hash_entry->xyz[1], from_hash_entry->xyz[2]);
00806   //else
00807   //{
00808   // double x, y, z;
00809   // if(!gsr_->dfce_->distance_field_->gridToWorld(from_hash_entry->xyz[0],
00810   //                                               from_hash_entry->xyz[1],
00811   //                                               from_hash_entry->xyz[2],
00812   //                                               x,y,z)) {
00813   //   std::cerr << "problem" << std::endl;
00814   //   return 1000000;
00815   // }
00816   // return getEuclideanDistance(x, y, z, goal_pose_.translation().x(), goal_pose_.translation().y(), goal_pose_.translation().z())*1000.0;
00817   // heur =  getEuclideanDistance(x, y, z, env_chain_config_.goal.xyz[0],env_chain_config_.goal.xyz[1], env_chain_config_.goal.xyz[2]) * prms_.cost_per_meter_;
00818   //}
00819 }
00820 
00821 bool EnvironmentChain3D::getGridXYZInt(const Eigen::Affine3d& pose,
00822                                        int(&xyz)[3]) const
00823 {
00824   if(!gsr_ || !gsr_->dfce_->distance_field_) {
00825     ROS_WARN_STREAM("No distance field cache entry available");
00826     return false;
00827   }
00828   if(!gsr_->dfce_->distance_field_->worldToGrid(pose.translation().x(),
00829                                                 pose.translation().y(),
00830                                                 pose.translation().z(),
00831                                                 xyz[0],
00832                                                 xyz[1],
00833                                                 xyz[2])) {
00834     ROS_WARN_STREAM("Pose out of bounds");
00835     return false;
00836   }
00837   return true;
00838 }
00839 
00840 bool EnvironmentChain3D::populateTrajectoryFromStateIDSequence(const std::vector<int>& state_ids,
00841                                                                trajectory_msgs::JointTrajectory& traj) const
00842 {
00843   traj.joint_names = joint_state_group_->getJointModelGroup()->getActiveDOFNames();
00844   std::vector<std::vector<double> > angle_vector;
00845   if(!planning_data_.convertFromStateIDsToAngles(state_ids,
00846                                                  angle_vector)) {
00847     return false;
00848   }
00849   if(planning_parameters_.interpolation_distance_ >= planning_parameters_.joint_motion_primitive_distance_) {
00850     std::map<int, std::map<int, std::vector<std::vector<double> > > >::const_iterator it = generated_interpolations_map_.find(*(state_ids.end()-2));
00851     std::vector< std::vector<double> > end_points;
00852     if(it != generated_interpolations_map_.end()) {
00853       std::map<int, std::vector<std::vector<double> > >::const_iterator it2 = it->second.find(state_ids.back());
00854       if(it2 == it->second.end()) {
00855         std::cerr << "No interpolated segment connecting state id " << (*state_ids.end()-2) << " and goal " << state_ids.back() << std::endl;
00856       } else {
00857         end_points = it2->second;
00858       }
00859     } else {
00860       std::cerr << "No interpolated segment connecting state id " << (*state_ids.end()-2) << " and goal " << state_ids.back() << std::endl;
00861     }
00862     traj.points.resize(end_points.size()+angle_vector.size());
00863     for(unsigned int i = 0; i < angle_vector.size()-1; i++) {
00864       traj.points[i].positions = angle_vector[i];
00865     }
00866     for(unsigned int i = 0; i < end_points.size(); i++) {
00867       traj.points[i+angle_vector.size()-1].positions = end_points[i];
00868     }
00869     traj.points.back().positions = angle_vector.back();
00870     for(unsigned int i = 0; i < traj.points.back().positions.size(); i++) {
00871       ROS_DEBUG_STREAM("Last " << i << " " << traj.points.back().positions[i]);
00872     }
00873     std::cerr << "Original path " << angle_vector.size() << " end path " << end_points.size() << std::endl;
00874   } else {
00875     std::cerr << "Num states " << state_ids.size() << std::endl;
00876     for(unsigned int i = 0; i < state_ids.size()-1; i++) {
00877       trajectory_msgs::JointTrajectoryPoint statep;
00878       statep.positions = angle_vector[i];
00879       ROS_DEBUG_STREAM("State id " << state_ids[i]);
00880       //if(traj.points.size() > 0) {
00881         // std::cerr << "State " << i << " id " << state_ids[i] << " dist "
00882         //           <<  getJointDistanceIntegerMax(traj.points.back().positions,
00883         //                                          statep.positions,
00884         //                                          INTERPOLATION_DISTANCE) << std::endl;
00885       //}
00886       traj.points.push_back(statep);
00887       std::map<int, std::map<int, std::vector<std::vector<double> > > >::const_iterator it = generated_interpolations_map_.find(state_ids[i]);
00888       if(it != generated_interpolations_map_.end()) {
00889         std::map<int, std::vector<std::vector<double> > >::const_iterator it2 = it->second.find(state_ids[i+1]);
00890         if(it2 == it->second.end()) {
00891           std::cerr << "No interpolated segment connecting state id " << state_ids[i] << " and state " << state_ids[i+1] << std::endl;
00892           continue;
00893         } else {
00894           for(unsigned int j = 0; j < it2->second.size(); j++) {
00895             trajectory_msgs::JointTrajectoryPoint p;
00896             p.positions = it2->second[j];
00897             // std::cerr << "Interp " << getJointDistanceIntegerMax(traj.points.back().positions,
00898             //                                                      p.positions,
00899             //                                                      INTERPOLATION_DISTANCE) << std::endl;
00900             traj.points.push_back(p);
00901           }
00902         }
00903       } else {
00904         std::cerr << "No interpolated segment connecting state id " << state_ids[i] << " and state " << state_ids[i+1] << std::endl;
00905         continue;
00906       }
00907     }
00908     //last point
00909     trajectory_msgs::JointTrajectoryPoint statep;
00910     statep.positions = angle_vector.back();
00911     // std::cerr << "Last " << getJointDistanceIntegerMax(traj.points.back().positions,
00912     //                                                    statep.positions,
00913     //                                                    INTERPOLATION_DISTANCE) << std::endl;
00914 
00915     traj.points.push_back(statep);
00916   }
00917   std::cerr << "Resulting path is " << traj.points.size() << std::endl;
00918   return true;
00919 }
00920 
00921 bool EnvironmentChain3D::getPlaneBFSMarker(visualization_msgs::Marker& plane_marker,
00922                                            double z_val)
00923 {
00924   if(!gsr_ || !gsr_->dfce_ || !gsr_->dfce_->distance_field_) {
00925     return false;
00926   }
00927   plane_marker.header.frame_id = planning_scene_->getPlanningFrame();
00928   plane_marker.header.stamp = ros::Time::now();
00929   plane_marker.ns = "bfs_plane";
00930   plane_marker.id = 0;
00931   plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
00932   plane_marker.action = visualization_msgs::Marker::ADD;
00933   plane_marker.points.resize(1);
00934   std_msgs::ColorRGBA wall_color;
00935   wall_color.r = wall_color.a = 1.0;
00936   plane_marker.colors.resize(1);
00937   plane_marker.colors[0] = wall_color;
00938   plane_marker.points[0].x = 1.0;
00939   plane_marker.pose.orientation.w = 1.0;
00940   plane_marker.color = wall_color;
00941   plane_marker.scale.x = plane_marker.scale.y = plane_marker.scale.z = gsr_->dfce_->distance_field_->getResolution();
00942   plane_marker.points.resize((gsr_->dfce_->distance_field_->getXNumCells()-2)*(gsr_->dfce_->distance_field_->getYNumCells()-2));
00943   plane_marker.colors.resize((gsr_->dfce_->distance_field_->getXNumCells()-2)*(gsr_->dfce_->distance_field_->getYNumCells()-2));
00944   //TODO - deal if 0,0 is not valid for x and y
00945   int x_val_temp, y_val_temp;
00946   int z_val_int;
00947   gsr_->dfce_->distance_field_->worldToGrid(0.0, 0.0, z_val, x_val_temp, y_val_temp, z_val_int);
00948   std_msgs::ColorRGBA dist_color;
00949   dist_color.g = dist_color.a = 1.0;
00950   unsigned int count = 0;
00951   for(int i = 0; i < gsr_->dfce_->distance_field_->getXNumCells()-2; i++) {
00952     for(int j = 0; j < gsr_->dfce_->distance_field_->getYNumCells()-2; j++, count++) {
00953       gsr_->dfce_->distance_field_->gridToWorld(i, j, z_val_int,
00954                                                 plane_marker.points[count].x,
00955                                                 plane_marker.points[count].y,
00956                                                 plane_marker.points[count].z);
00957       //ROS_INFO_STREAM("Point " << count << " point " << plane_marker.points[count]);
00958       if(bfs_->isWall(i, j, z_val_int)) {
00959         plane_marker.colors[count] = wall_color;
00960         } else {
00961         int dist = bfs_->getDistance(i,j, z_val_int);
00962         if(dist < 40) {
00963           plane_marker.colors[count] = dist_color;
00964           plane_marker.colors[count].g = (40-dist)/40.0;
00965         }
00966       }
00967     }
00968   }
00969   return true;
00970 }
00971 
00972 bool EnvironmentChain3D::interpolateAndCollisionCheck(const std::vector<double> angles1,
00973                                                       const std::vector<double> angles2,
00974                                                       std::vector<std::vector<double> >& state_values)
00975 {
00976   static bool print_first = false;
00977   state_values.clear();
00978   interpolation_joint_state_group_1_->setStateValues(angles1);
00979   interpolation_joint_state_group_2_->setStateValues(angles2);
00980 
00981   interpolation_joint_state_group_temp_->setStateValues(angles1);
00982 
00983   collision_detection::CollisionRequest req;
00984   req.group_name = planning_group_;
00985 
00986   int maximum_moves = getJointDistanceIntegerMax(angles1, angles2, planning_parameters_.interpolation_distance_);
00987   if(print_first) {
00988     std::cerr << "Maximum moves " << maximum_moves << std::endl;
00989     for(unsigned int i = 0; i < angles1.size(); i++) {
00990       std::cerr << "Start " << i << " " << angles1[i] << std::endl;
00991     }
00992     for(unsigned int i = 0; i < angles2.size(); i++) {
00993       std::cerr << "End " << i << " " << angles2[i] << std::endl;
00994     }
00995   }
00996   for(int i = 1; i < maximum_moves; i++) {
00997     interpolation_joint_state_group_1_->interpolate(interpolation_joint_state_group_2_,
00998                                                     (1.0/(maximum_moves*1.0))*i,
00999                                                     interpolation_joint_state_group_temp_);
01000     ros::WallTime before_coll = ros::WallTime::now();
01001     collision_detection::CollisionResult res;
01002     if(!planning_parameters_.use_standard_collision_checking_) {
01003       hy_world_->checkCollisionDistanceField(req,
01004                                              res,
01005                                              *hy_robot_->getCollisionRobotDistanceField().get(),
01006                                              interpolation_state_temp_,
01007                                              gsr_);
01008     } else {
01009       planning_scene_->checkCollision(req, res, interpolation_state_temp_);
01010     }
01011     planning_statistics_.coll_checks_++;
01012     ros::WallDuration dur(ros::WallTime::now()-before_coll);
01013     planning_statistics_.total_coll_check_time_ += dur;
01014     if(res.collision) {
01015       return false;
01016     }
01017     state_values.resize(state_values.size()+1);
01018     interpolation_joint_state_group_temp_->getGroupStateValues(state_values.back());
01019     if(print_first) {
01020       for(unsigned int j = 0; j < state_values.back().size(); j++) {
01021         std::cerr << "Interp " << i << " " << j << " " << state_values.back()[j] << std::endl;
01022       }
01023     }
01024   }
01025   print_first = false;
01026   return true;
01027 }
01028 
01029 void EnvironmentChain3D::attemptShortcut(const trajectory_msgs::JointTrajectory& traj_in,
01030                                          trajectory_msgs::JointTrajectory& traj_out)
01031 {
01032   unsigned int last_point_ind = 0;
01033   unsigned int current_point_ind = 1;
01034   unsigned int last_good_start_ind = 0;
01035   unsigned int last_good_end_ind = 1;
01036   traj_out = traj_in;
01037   traj_out.points.clear();
01038   traj_out.points.push_back(traj_in.points.front());
01039   std::vector<std::vector<double> > last_good_segment_values;
01040   if(traj_in.points.size() == 1) {
01041     traj_out = traj_in;
01042     return;
01043   }
01044   if(planning_parameters_.attempt_full_shortcut_) {
01045     std::vector<std::vector<double> > full_shortcut;
01046     //std::cerr << "Checking" << std::endl;
01047     if(interpolateAndCollisionCheck(traj_in.points.front().positions,
01048                                     traj_in.points.back().positions,
01049                                     full_shortcut)) {
01050       std::cerr << "Full shortcut has " << full_shortcut.size() << " points " << std::endl;
01051       for(unsigned int i = 0; i < full_shortcut.size(); i++) {
01052         trajectory_msgs::JointTrajectoryPoint jtp;
01053         jtp.positions = full_shortcut[i];
01054         traj_out.points.push_back(jtp);
01055       }
01056       traj_out.points.push_back(traj_in.points.back());
01057       std::cerr << "Full shortcut worked" << std::endl;
01058       return;
01059     }
01060   }
01061 
01062   while(1) {
01063     //std::cerr << "Checking from " << last_point_ind << " to " << current_point_ind << std::endl;
01064     const trajectory_msgs::JointTrajectoryPoint& start_point = traj_in.points[last_point_ind];
01065     const trajectory_msgs::JointTrajectoryPoint& end_point = traj_in.points[current_point_ind];
01066     std::vector<std::vector<double> > segment_values;
01067     //if we can go from start to end then keep going
01068     if(interpolateAndCollisionCheck(start_point.positions,
01069                                     end_point.positions,
01070                                     segment_values)) {
01071       last_good_start_ind = last_point_ind;
01072       last_good_end_ind = current_point_ind;
01073       last_good_segment_values = segment_values;
01074       current_point_ind++;
01075       //std::cerr << "Interpolation ok" << std::endl;
01076     } else {
01077       //first case - start and end are separated by single point, so we copy the end in
01078       if(last_good_end_ind-last_good_start_ind == 1) {
01079         traj_out.points.push_back(traj_in.points[last_good_end_ind]);
01080       } else {
01081         for(unsigned int i = 0; i < last_good_segment_values.size(); i++) {
01082           trajectory_msgs::JointTrajectoryPoint jtp;
01083           jtp.positions = last_good_segment_values[i];
01084           traj_out.points.push_back(jtp);
01085         }
01086       }
01087       last_good_start_ind = last_good_end_ind;
01088       last_point_ind = last_good_end_ind;
01089       current_point_ind = last_good_end_ind+1;
01090       last_good_segment_values.clear();
01091       //std::cerr << "Interpolation not ok" << std::endl;
01092     }
01093     if(current_point_ind >= traj_in.points.size()) {
01094       if(last_good_segment_values.size() > 0) {
01095         for(unsigned int i = 0; i < last_good_segment_values.size(); i++) {
01096           trajectory_msgs::JointTrajectoryPoint jtp;
01097           jtp.positions = last_good_segment_values[i];
01098           traj_out.points.push_back(jtp);
01099         }
01100       }
01101       traj_out.points.push_back(traj_in.points.back());
01102       break;
01103     }
01104   }
01105 }
01106 
01107 }


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03