00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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 )
00123 {
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
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
00154
00155 ros::WallTime expansion_start_time = ros::WallTime::now();
00156
00157 succ_idv->clear();
00158 cost_v->clear();
00159
00160
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
00180
00181 std::vector<int> succ_coord;
00182 std::vector<double> succ_joint_angles;
00183
00184
00185
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
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
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
00239
00240
00241
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
00258 ros::WallDuration dur(ros::WallTime::now()-before_coll);
00259
00260
00261 planning_statistics_.total_coll_check_time_ += dur;
00262 if(res.collision) {
00263
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
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300 bool succ_is_goal_state = false;
00301 if((planning_parameters_.use_bfs_ && dist == 0) || (!planning_parameters_.use_bfs_ && dist == 1)) {
00302
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
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
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
00322 continue;
00323 }
00324 }
00325 succ_hash_entry = planning_data_.getHashEntry(succ_coord, i);
00326 }
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
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
00361 generated_interpolations_map_[source_state_ID][succ_hash_entry->stateID] = interpolated_values;
00362 }
00363
00364
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
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
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
00489
00490
00491
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
00507
00508
00509
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
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
00581 if(planning_parameters_.use_bfs_) {
00582 bfs_->run(goal_xyz[0], goal_xyz[1], goal_xyz[2]);
00583
00584
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
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
00644 if(dist > max_dist) {
00645 max_dist = dist;
00646 }
00647 }
00648 maximum_distance_for_motion_ = ceil(max_dist/angle_discretization_);
00649
00650 }
00651
00652 int EnvironmentChain3D::calculateCost(EnvChain3DHashEntry* HashEntry1, EnvChain3DHashEntry* HashEntry2)
00653 {
00654
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
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675 }
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702 int EnvironmentChain3D::getBFSCostToGoal(int x, int y, int z) const
00703 {
00704
00705
00706 int cost = (floor(bfs_->getDistance(x,y,z)/(maximum_distance_for_motion_)))*JOINT_DIST_MULT;
00707
00708 return cost;
00709
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
00755 dist += jdist;
00756 }
00757 return dist;
00758 }
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
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
00795
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
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
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
00881
00882
00883
00884
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
00898
00899
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
00909 trajectory_msgs::JointTrajectoryPoint statep;
00910 statep.positions = angle_vector.back();
00911
00912
00913
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
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
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
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
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
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
01076 } else {
01077
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
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 }