$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 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 the Willow Garage 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 00037 #include <visualization_msgs/Marker.h> 00038 #include <visualization_msgs/MarkerArray.h> 00039 #include <planning_environment/models/model_utils.h> 00040 #include <collision_proximity/collision_proximity_space.h> 00041 #include <tf/tf.h> 00042 00043 using collision_proximity::CollisionProximitySpace; 00044 00045 // static double gen_rand(double min, double max) 00046 // { 00047 // int rand_num = rand()%100+1; 00048 // double result = min + (double)((max-min)*rand_num)/101.0; 00049 // return result; 00050 // } 00051 00052 static std::string makeStringFromUnsignedInt(unsigned int j) 00053 { 00054 std::stringstream ss; 00055 ss << j; 00056 return ss.str(); 00057 } 00058 00059 static std::string makeAttachedObjectId(std::string link, std::string object) 00060 { 00061 return link+"_"+object; 00062 } 00063 00064 CollisionProximitySpace::CollisionProximitySpace(const std::string& robot_description_name, 00065 bool register_with_environment_server, bool use_signed_environment_field , bool use_signed_self_field) : 00066 priv_handle_("~") 00067 { 00068 collision_models_interface_ = new planning_environment::CollisionModelsInterface(robot_description_name, 00069 register_with_environment_server); 00070 00071 priv_handle_.param("size_x", size_x_, 3.0); 00072 priv_handle_.param("size_y", size_y_, 3.0); 00073 priv_handle_.param("size_z", size_z_, 4.0); 00074 priv_handle_.param("origin_x", origin_x_, -1.0); 00075 priv_handle_.param("origin_y", origin_y_, -1.5); 00076 priv_handle_.param("origin_z", origin_z_, -2.0); 00077 priv_handle_.param("resolution", resolution_, 0.02); 00078 priv_handle_.param("collision_tolerance", tolerance_, 0.00); 00079 priv_handle_.param("max_environment_distance", max_environment_distance_, 0.25); 00080 priv_handle_.param("max_self_distance", max_self_distance_, 0.1); 00081 priv_handle_.param("undefined_distance", undefined_distance_, 1.0); 00082 00083 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("collision_proximity_body_spheres", 128); 00084 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("collision_proximity_body_spheres_array", 128); 00085 00086 if(use_signed_self_field) 00087 { 00088 self_distance_field_ = (distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>*)(new distance_field::SignedPropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_self_distance_)); 00089 } 00090 else 00091 { 00092 self_distance_field_ = new distance_field::PropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_self_distance_); 00093 } 00094 if(use_signed_environment_field) 00095 { 00096 environment_distance_field_ = (distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>*)(new distance_field::SignedPropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_environment_distance_)); 00097 } 00098 else 00099 { 00100 environment_distance_field_ = new distance_field::PropagationDistanceField(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_, origin_z_, max_environment_distance_); 00101 } 00102 00103 collision_models_interface_->addSetPlanningSceneCallback(boost::bind(&CollisionProximitySpace::setPlanningSceneCallback, this, _1)); 00104 collision_models_interface_->addRevertPlanningSceneCallback(boost::bind(&CollisionProximitySpace::revertPlanningSceneCallback, this)); 00105 00106 loadRobotBodyDecompositions(); 00107 00108 loadDefaultCollisionOperations(); 00109 00110 /* 00111 shapes::Box base_box(1.0,1.0,.3); 00112 BodyDecomposition* bd = new BodyDecomposition("base_box", &base_box, resolution_); 00113 btTransform trans; 00114 trans.setIdentity(); 00115 trans.setOrigin(btVector3(0.0,0.0,.15)); 00116 bd->updatePose(trans); 00117 BodyDecompositionVector* bdv = new BodyDecompositionVector(); 00118 bdv->addToVector(bd); 00119 static_object_map_["base_box"] = bdv; 00120 */ 00121 } 00122 00123 CollisionProximitySpace::~CollisionProximitySpace() 00124 { 00125 delete collision_models_interface_; 00126 delete self_distance_field_; 00127 delete environment_distance_field_; 00128 for(std::map<std::string, BodyDecomposition*>::iterator it = body_decomposition_map_.begin(); 00129 it != body_decomposition_map_.end(); 00130 it++) { 00131 delete it->second; 00132 } 00133 deleteAllStaticObjectDecompositions(); 00134 deleteAllAttachedObjectDecompositions(); 00135 } 00136 00137 void CollisionProximitySpace::deleteAllStaticObjectDecompositions() 00138 { 00139 for(std::map<std::string, BodyDecompositionVector*>::iterator it = static_object_map_.begin(); 00140 it != static_object_map_.end(); 00141 it++) { 00142 delete it->second; 00143 } 00144 static_object_map_.clear(); 00145 } 00146 00147 void CollisionProximitySpace::deleteAllAttachedObjectDecompositions() 00148 { 00149 for(std::map<std::string, BodyDecompositionVector*>::iterator it = attached_object_map_.begin(); 00150 it != attached_object_map_.end(); 00151 it++) { 00152 delete it->second; 00153 } 00154 attached_object_map_.clear(); 00155 current_attached_body_decompositions_.clear(); 00156 current_attached_body_names_.clear(); 00157 current_attached_body_indices_.clear(); 00158 } 00159 00160 void CollisionProximitySpace::loadDefaultCollisionOperations() 00161 { 00162 std::map<std::string, bool> all_true_map; 00163 const planning_models::KinematicModel* kmodel = collision_models_interface_->getKinematicModel(); 00164 00165 for(unsigned int i = 0; i < kmodel->getLinkModels().size(); i++) { 00166 all_true_map[kmodel->getLinkModels()[i]->getName()] = true; 00167 } 00168 00169 for(unsigned int i = 0; i < kmodel->getLinkModels().size(); i++) { 00170 intra_group_collision_links_[kmodel->getLinkModels()[i]->getName()] = all_true_map; 00171 } 00172 00173 //creating lists for each group in planning groups 00174 const std::map<std::string, planning_models::KinematicModel::JointModelGroup*>& jmgm = 00175 collision_models_interface_->getKinematicModel()->getJointModelGroupMap(); 00176 for(std::map<std::string, planning_models::KinematicModel::JointModelGroup*>::const_iterator it = jmgm.begin(); 00177 it != jmgm.end(); 00178 it++) { 00179 enabled_self_collision_links_[it->first] = all_true_map; 00180 std::vector<std::string> link_names = it->second->getUpdatedLinkModelNames(); 00181 for(unsigned int i = 0; i < link_names.size(); i++) { 00182 enabled_self_collision_links_[it->first][link_names[i]] = false; 00183 } 00184 } 00185 00186 XmlRpc::XmlRpcValue coll_ops; 00187 if(!root_handle_.hasParam("/robot_description_planning/distance_collision_operations")) { 00188 ROS_WARN("No distance collision operations specified"); 00189 return; 00190 } 00191 root_handle_.getParam("/robot_description_planning/distance_collision_operations", coll_ops); 00192 00193 if(coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray) { 00194 ROS_WARN("distance_collision_operations is not an array"); 00195 return; 00196 } 00197 00198 if(coll_ops.size() == 0) { 00199 ROS_WARN("No collision operations in distance collision operations"); 00200 return; 00201 } 00202 00203 for(int i = 0; i < coll_ops.size(); i++) { 00204 if(!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation")) { 00205 ROS_WARN("All collision operations must have two objects and an operation"); 00206 continue; 00207 } 00208 std::string object1 = std::string(coll_ops[i]["object1"]); 00209 std::string object2 = std::string(coll_ops[i]["object2"]); 00210 std::string operation = std::string(coll_ops[i]["operation"]); 00211 00212 if(jmgm.find(object1) == jmgm.end()) { 00213 if(object1 == object2) { 00214 //using for disabling self checks 00215 self_excludes_[object1] = true; 00216 } else { 00217 //must be intra_collision 00218 if(jmgm.find(object2) == jmgm.end()) { 00219 intra_group_collision_links_[object1][object2] = (operation == "enable"); 00220 intra_group_collision_links_[object2][object1] = (operation == "enable"); 00221 } else { 00222 std::vector<std::string> group_links = jmgm.find(object2)->second->getUpdatedLinkModelNames(); 00223 for(unsigned int j = 0; j < group_links.size(); j++) { 00224 intra_group_collision_links_[object1][group_links[j]] = (operation == "enable"); 00225 intra_group_collision_links_[group_links[j]][object1] = (operation == "enable"); 00226 } 00227 } 00228 } 00229 } else if(jmgm.find(object2) == jmgm.end()) { 00230 std::vector<std::string> group_links = jmgm.find(object1)->second->getUpdatedLinkModelNames(); 00231 for(unsigned int j = 0; j < group_links.size(); j++) { 00232 intra_group_collision_links_[object2][group_links[j]] = (operation == "enable"); 00233 intra_group_collision_links_[group_links[j]][object2] = (operation == "enable"); 00234 } 00235 } else { 00236 std::vector<std::string> group_links_1 = jmgm.find(object1)->second->getUpdatedLinkModelNames(); 00237 std::vector<std::string> group_links_2 = jmgm.find(object2)->second->getUpdatedLinkModelNames(); 00238 for(unsigned int j = 0; j < group_links_1.size(); j++) { 00239 for(unsigned int k = 0; k < group_links_2.size(); k++) { 00240 intra_group_collision_links_[group_links_1[j]][group_links_2[k]] = (operation == "enable"); 00241 intra_group_collision_links_[group_links_2[k]][group_links_1[j]] = (operation == "enable"); 00242 } 00243 } 00244 } 00245 } 00246 } 00247 00248 void CollisionProximitySpace::loadRobotBodyDecompositions() 00249 { 00250 const planning_models::KinematicModel* kmodel = collision_models_interface_->getKinematicModel(); 00251 00252 for(unsigned int i = 0; i < kmodel->getLinkModels().size(); i++) { 00253 if(kmodel->getLinkModels()[i]->getLinkShape() != NULL) { 00254 double padding = collision_models_interface_->getDefaultPadding(); 00255 00256 if(collision_models_interface_->getDefaultLinkPaddingMap().find(kmodel->getLinkModels()[i]->getName()) != 00257 collision_models_interface_->getDefaultLinkPaddingMap().end()) 00258 { 00259 padding = collision_models_interface_->getDefaultLinkPaddingMap().at(kmodel->getLinkModels()[i]->getName()); 00260 } 00261 00262 body_decomposition_map_[kmodel->getLinkModels()[i]->getName()] = new BodyDecomposition(kmodel->getLinkModels()[i]->getName(), 00263 kmodel->getLinkModels()[i]->getLinkShape(), 00264 resolution_/2.0, padding); 00265 } 00266 } 00267 } 00268 00269 bool CollisionProximitySpace::setPlanningScene(const arm_navigation_msgs::PlanningScene& scene) { 00270 return(collision_models_interface_->setPlanningSceneWithCallbacks(scene)); 00271 } 00272 00273 void CollisionProximitySpace::setPlanningSceneCallback(const arm_navigation_msgs::PlanningScene& scene) 00274 { 00275 ros::WallTime n1 = ros::WallTime::now(); 00276 deleteAllStaticObjectDecompositions(); 00277 deleteAllAttachedObjectDecompositions(); 00278 00279 syncObjectsWithCollisionSpace(*collision_models_interface_->getPlanningSceneState()); 00280 00281 prepareEnvironmentDistanceField(*collision_models_interface_->getPlanningSceneState()); 00282 ros::WallTime n2 = ros::WallTime::now(); 00283 ROS_DEBUG_STREAM("Setting environment took " << (n2-n1).toSec()); 00284 } 00285 00286 void CollisionProximitySpace::setupForGroupQueries(const std::string& group_name, 00287 const arm_navigation_msgs::RobotState& rob_state, 00288 std::vector<std::string>& link_names, 00289 std::vector<std::string>& attached_body_names) 00290 { 00291 ros::WallTime n1 = ros::WallTime::now(); 00292 //setting up current info 00293 current_group_name_ = group_name; 00294 planning_environment::setRobotStateAndComputeTransforms(rob_state, 00295 *collision_models_interface_->getPlanningSceneState()); 00296 getGroupLinkAndAttachedBodyNames(current_group_name_, 00297 *collision_models_interface_->getPlanningSceneState(), 00298 current_link_names_, 00299 current_link_indices_, 00300 current_attached_body_names_, 00301 current_attached_body_indices_); 00302 link_names = current_link_names_; 00303 attached_body_names = current_attached_body_names_; 00304 setupGradientStructures(current_link_names_, 00305 current_attached_body_names_, 00306 current_gradients_); 00307 00308 current_link_body_decompositions_.clear(); 00309 current_attached_body_decompositions_.clear(); 00310 current_self_excludes_.clear(); 00311 for(unsigned int i = 0; i < current_link_names_.size(); i++) { 00312 current_link_body_decompositions_.push_back(body_decomposition_map_[current_link_names_[i]]); 00313 if(self_excludes_.find(current_link_names_[i]) != self_excludes_.end()) { 00314 current_self_excludes_.push_back(false); 00315 } else { 00316 current_self_excludes_.push_back(true); 00317 } 00318 } 00319 for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) { 00320 current_attached_body_decompositions_.push_back(attached_object_map_[current_attached_body_names_[i]]); 00321 } 00322 current_intra_group_collision_links_.clear(); 00323 unsigned int num_links = current_link_names_.size(); 00324 unsigned int num_attached = current_attached_body_names_.size(); 00325 unsigned int tot = num_links+num_attached; 00326 current_intra_group_collision_links_.resize(tot); 00327 for(unsigned int i = 0; i < tot; i++) { 00328 current_intra_group_collision_links_[i].resize(tot); 00329 for(unsigned int j = 0; j < tot; j++) { 00330 if(i < num_links && j < num_links) { 00331 current_intra_group_collision_links_[i][j] = intra_group_collision_links_.find(current_link_names_[i])->second.find(current_link_names_[j])->second; 00332 //ROS_INFO_STREAM("Setting intra_group_collision_links for " << current_link_names_[i] << " and " << current_link_names_[j] << " to " << current_intra_group_collision_links_[i][j]); 00333 } else { 00334 std::string name1; 00335 std::string name2; 00336 bool attached1 = false; 00337 bool attached2 = false; 00338 if(i < num_links) { 00339 name1 = current_link_names_[i]; 00340 } else { 00341 attached1 = true; 00342 name1 = current_attached_body_names_[i-num_links]; 00343 } 00344 if(j < num_links) { 00345 name2 = current_link_names_[j]; 00346 } else { 00347 attached2 = true; 00348 name2 = current_attached_body_names_[j-num_links]; 00349 } 00350 current_intra_group_collision_links_[i][j] = true; 00351 if(attached1) { 00352 if(attached_object_collision_links_.find(name1)->second.find(name2) != attached_object_collision_links_.find(name1)->second.end() && !attached_object_collision_links_.find(name1)->second.find(name2)->second) { 00353 current_intra_group_collision_links_[i][j] = false; 00354 } 00355 } 00356 if(attached2) { 00357 if(attached_object_collision_links_.find(name2)->second.find(name1) != attached_object_collision_links_.find(name2)->second.end() && !attached_object_collision_links_.find(name2)->second.find(name1)->second) { 00358 current_intra_group_collision_links_[i][j] = false; 00359 } 00360 } 00361 } 00362 } 00363 } 00364 setBodyPosesGivenKinematicState(*collision_models_interface_->getPlanningSceneState()); 00365 setDistanceFieldForGroupQueries(current_group_name_, *collision_models_interface_->getPlanningSceneState()); 00366 ros::WallTime n2 = ros::WallTime::now(); 00367 ROS_DEBUG_STREAM("Setting self for group " << current_group_name_ << " took " << (n2-n1).toSec()); 00368 //visualizeDistanceField(self_distance_field_); 00369 } 00370 00371 void CollisionProximitySpace::revertPlanningSceneCallback() { 00372 00373 collision_models_interface_->bodiesLock(); 00374 current_group_name_ = ""; 00375 00376 deleteAllStaticObjectDecompositions(); 00377 deleteAllAttachedObjectDecompositions(); 00378 collision_models_interface_->bodiesUnlock(); 00379 } 00380 00381 void CollisionProximitySpace::setCurrentGroupState(const planning_models::KinematicState& state) 00382 { 00383 ros::WallTime n1 = ros::WallTime::now(); 00384 if(current_group_name_.empty()) { 00385 return; 00386 } 00387 btTransform inv = getInverseWorldTransform(state); 00388 for(unsigned int i = 0; i < current_link_indices_.size(); i++) { 00389 const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[current_link_indices_[i]]; 00390 current_link_body_decompositions_[i]->updateSpheresPose(inv*ls->getGlobalCollisionBodyTransform()); 00391 } 00392 for(unsigned int i = 0; i < current_attached_body_indices_.size(); i++) { 00393 const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[current_attached_body_indices_[i]]; 00394 for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) { 00395 const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j]; 00396 for(unsigned int k = 0; k < att_state->getGlobalCollisionBodyTransforms().size(); k++) { 00397 btTransform test = inv*att_state->getGlobalCollisionBodyTransforms()[k]; 00398 current_attached_body_decompositions_[i]->updateSpheresPose(k, inv*att_state->getGlobalCollisionBodyTransforms()[k]); 00399 } 00400 } 00401 } 00402 updateSphereLocations(current_link_names_, current_attached_body_names_, current_gradients_); 00403 ROS_DEBUG_STREAM("Group state update took " << (ros::WallTime::now()-n1).toSec()); 00404 } 00405 00406 void CollisionProximitySpace::setBodyPosesGivenKinematicState(const planning_models::KinematicState& state) 00407 { 00408 btTransform inv = getInverseWorldTransform(state); 00409 for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) { 00410 const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i]; 00411 if(body_decomposition_map_.find(ls->getName()) == body_decomposition_map_.end()) { 00412 //ROS_WARN_STREAM("Can't find body decomposition for link state " << ls->getName()); 00413 continue; 00414 } 00415 body_decomposition_map_[ls->getName()]->updatePose(inv*ls->getGlobalCollisionBodyTransform()); 00416 for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) { 00417 const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j]; 00418 std::string id = makeAttachedObjectId(ls->getName(), att_state->getName()); 00419 if(attached_object_map_.find(id) == attached_object_map_.end()) { 00420 ROS_WARN_STREAM("Updating pose but no attached object for id " << id); 00421 continue; 00422 } 00423 if(attached_object_map_[id]->getSize() != att_state->getGlobalCollisionBodyTransforms().size()) { 00424 ROS_WARN_STREAM("For attached object bodies number " << attached_object_map_[id]->getSize() 00425 << " not equal to state number " << att_state->getGlobalCollisionBodyTransforms().size() << ". Not updating"); 00426 continue; 00427 } 00428 for(unsigned int k = 0; k < att_state->getGlobalCollisionBodyTransforms().size(); k++) { 00429 btTransform test = inv*att_state->getGlobalCollisionBodyTransforms()[k]; 00430 attached_object_map_[id]->updateBodyPose(k, inv*att_state->getGlobalCollisionBodyTransforms()[k]); 00431 } 00432 } 00433 } 00434 } 00435 00436 btTransform CollisionProximitySpace::getInverseWorldTransform(const planning_models::KinematicState& state) const { 00437 const planning_models::KinematicState::JointState* world_state = state.getJointStateVector()[0];//(monitor_->getKinematicModel()->getRoot()->getName()); 00438 if(world_state == NULL) { 00439 ROS_WARN_STREAM("World state " << collision_models_interface_->getKinematicModel()->getRoot()->getName() << " not found"); 00440 } 00441 const btTransform& world_trans = world_state->getVariableTransform(); 00442 btTransform ret(world_trans); 00443 return ret.inverse(); 00444 } 00445 00446 void CollisionProximitySpace::syncObjectsWithCollisionSpace(const planning_models::KinematicState& state) 00447 { 00448 btTransform inv = getInverseWorldTransform(state); 00449 const collision_space::EnvironmentObjects *eo = collision_models_interface_->getCollisionSpace()->getObjects(); 00450 std::vector<std::string> ns = eo->getNamespaces(); 00451 for(unsigned int i = 0; i < ns.size(); i++) { 00452 if(ns[i] == COLLISION_MAP_NAME) continue; 00453 const collision_space::EnvironmentObjects::NamespaceObjects &no = eo->getObjects(ns[i]); 00454 BodyDecompositionVector* bdv = new BodyDecompositionVector(); 00455 for(unsigned int j = 0; j < no.shape.size(); j++) { 00456 BodyDecomposition* bd = new BodyDecomposition(ns[i]+"_"+makeStringFromUnsignedInt(j), no.shape[j], resolution_); 00457 bd->updatePose(inv*no.shape_pose[j]); 00458 btTransform trans = bd->getBody()->getPose(); 00459 bdv->addToVector(bd); 00460 } 00461 static_object_map_[ns[i]] = bdv; 00462 } 00463 00464 const std::vector<planning_models::KinematicState::LinkState*> link_states = state.getLinkStateVector(); 00465 for(unsigned int i = 0; i < link_states.size(); i++) { 00466 const planning_models::KinematicState::LinkState* ls = link_states[i]; 00467 for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) { 00468 BodyDecompositionVector* bdv = new BodyDecompositionVector(); 00469 const planning_models::KinematicState::AttachedBodyState* abs = ls->getAttachedBodyStateVector()[j]; 00470 std::string id = makeAttachedObjectId(ls->getName(),abs->getName()); 00471 for(unsigned int k = 0; k < abs->getAttachedBodyModel()->getShapes().size(); k++) { 00472 BodyDecomposition* bd = new BodyDecomposition(id+makeStringFromUnsignedInt(j), abs->getAttachedBodyModel()->getShapes()[k], resolution_); 00473 bd->updatePose(inv*abs->getGlobalCollisionBodyTransforms()[k]); 00474 bdv->addToVector(bd); 00475 } 00476 attached_object_map_[id] = bdv; 00477 for(unsigned int k = 0; k < abs->getAttachedBodyModel()->getTouchLinks().size(); k++) { 00478 attached_object_collision_links_[id][abs->getAttachedBodyModel()->getTouchLinks()[k]] = false; 00479 } 00480 } 00481 } 00482 } 00483 00484 void CollisionProximitySpace::setDistanceFieldForGroupQueries(const std::string& group_name, 00485 const planning_models::KinematicState& state) 00486 { 00487 if(enabled_self_collision_links_.find(group_name) == enabled_self_collision_links_.end()) { 00488 ROS_WARN_STREAM("No group named " << group_name << " in planning groups"); 00489 return; 00490 } 00491 std::vector<std::string> df_links; 00492 for(std::map<std::string,bool>::iterator it = enabled_self_collision_links_[group_name].begin(); 00493 it != enabled_self_collision_links_[group_name].end(); 00494 it++) { 00495 if(it->second) { 00496 df_links.push_back(it->first); 00497 } 00498 } 00499 prepareSelfDistanceField(df_links, state); 00500 } 00501 00502 void CollisionProximitySpace::prepareEnvironmentDistanceField(const planning_models::KinematicState& state) 00503 { 00504 environment_distance_field_->reset(); 00505 btTransform inv = getInverseWorldTransform(state); 00506 std::vector<btVector3> all_points; 00507 for(std::map<std::string, BodyDecompositionVector*>::iterator it = static_object_map_.begin(); 00508 it != static_object_map_.end(); 00509 it++) { 00510 for(unsigned int i = 0; i < it->second->getSize(); i++) { 00511 std::vector<btVector3> obj_points = it->second->getBodyDecomposition(i)->getCollisionPoints(); 00512 all_points.insert(all_points.end(),obj_points.begin(), obj_points.end()); 00513 } 00514 } 00515 for(unsigned int i = 0; i < collision_models_interface_->getCollisionMapPoses().size(); i++) { 00516 all_points.push_back(inv*collision_models_interface_->getCollisionMapPoses()[i].getOrigin()); 00517 } 00518 environment_distance_field_->addPointsToField(all_points); 00519 //ROS_INFO_STREAM("Adding points took " << (n2-n1).toSec()); 00520 } 00521 00522 void CollisionProximitySpace::prepareSelfDistanceField(const std::vector<std::string>& link_names, 00523 const planning_models::KinematicState& state) 00524 { 00525 self_distance_field_->reset(); 00526 btTransform inv = getInverseWorldTransform(state); 00527 std::vector<btVector3> all_points; 00528 for(unsigned int i = 0; i < link_names.size(); i++) { 00529 if(body_decomposition_map_.find(link_names[i]) == body_decomposition_map_.end()) { 00530 //there is no collision geometry as far as we can tell 00531 continue; 00532 } 00533 //ROS_INFO_STREAM("Adding link " << link_names[i]); 00534 const BodyDecomposition* bd = body_decomposition_map_.find(link_names[i])->second; 00535 all_points.insert(all_points.end(), bd->getCollisionPoints().begin(), bd->getCollisionPoints().end()); 00536 const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_names[i]); 00537 for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) { 00538 std::string id = makeAttachedObjectId(ls->getName(),ls->getAttachedBodyStateVector()[j]->getName()); 00539 if(attached_object_map_.find(id) == attached_object_map_.end()) { 00540 ROS_WARN_STREAM("Have no attached object body for attached object " << id); 00541 continue; 00542 } 00543 const BodyDecompositionVector* att = attached_object_map_.find(id)->second; 00544 const std::vector<btVector3>& att_points = att->getCollisionPoints(); 00545 all_points.insert(all_points.end(), att_points.begin(), att_points.end()); 00546 } 00547 } 00548 self_distance_field_->addPointsToField(all_points); 00549 } 00550 00551 /* 00552 bool CollisionProximitySpace::getEnvironmentProximity(ProximityInfo& prox) const 00553 { 00554 updating_objects_lock_.lock(); 00555 prox.proximity = DBL_MAX; 00556 for(unsigned int i = 0; i < link_names.size(); i++) { 00557 if(body_decomposition_map_.find(link_names[i]) == body_decomposition_map_.end()) { 00558 ROS_WARN_STREAM("No link " << link_names[i] << " for getEnvironmentProximity"); 00559 continue; 00560 } 00561 const BodyDecomposition* bd = body_decomposition_map_.find(link_names[i])->second; 00562 unsigned int lc; 00563 btVector3 grad; 00564 double dist = getCollisionSphereProximity(bd->getCollisionSpheres(), lc, grad); 00565 if(dist == 0.0) { 00566 //ROS_WARN_STREAM("Zero distance on link " << link_names[i]); 00567 } else { 00568 //ROS_INFO_STREAM("Link " << link_names[i] << " proximity " << dist); 00569 if(dist < prox.proximity) { 00570 prox.proximity = dist; 00571 prox.link_name = link_names[i]; 00572 prox.sphere_index = lc; 00573 prox.closest_point = bd->getCollisionSpheres()[lc].center_; 00574 prox.closest_gradient = grad; 00575 } 00576 } 00577 const planning_models::KinematicModel::LinkModel* lm = monitor_->getKinematicModel()->getLinkModel(link_names[i]); 00578 for(unsigned int j = 0; j < lm->getAttachedBodyModels().size(); j++) { 00579 std::string id = makeAttachedObjectId(lm->getName(),lm->getAttachedBodyModels()[j]->getName()); 00580 if(attached_object_map_.find(id) == attached_object_map_.end()) { 00581 ROS_WARN_STREAM("Have no attached object body for attached object " << id); 00582 continue; 00583 } 00584 const BodyDecompositionVector* att = attached_object_map_.find(id)->second; 00585 for(unsigned int k = 0; k < att->getSize(); k++) { 00586 double dist = getCollisionSphereProximity(att->getBodyDecomposition(k)->getCollisionSpheres(), lc, grad); 00587 if(dist < prox.proximity) { 00588 prox.proximity = dist; 00589 prox.link_name = lm->getName(); 00590 prox.attached_object_name = id; 00591 prox.att_index = j; 00592 prox.sphere_index = lc; 00593 prox.closest_point = att->getBodyDecomposition(k)->getCollisionSpheres()[lc].center_; 00594 prox.closest_gradient = grad; 00595 } 00596 } 00597 } 00598 } 00599 updating_objects_lock_.unlock(); 00600 return true; 00601 } 00602 00603 double CollisionProximitySpace::getCollisionSphereProximity(const std::vector<CollisionSphere>& sphere_list, unsigned int& closest, btVector3& gradient) const { 00604 double distance = DBL_MAX; 00605 for(unsigned int i = 0; i < sphere_list.size(); i++) { 00606 btVector3 p = sphere_list[i].center_; 00607 double gx, gy, gz; 00608 double dist = distance_field_->getDistanceGradient(p.x(), p.y(), p.z(), gx, gy, gz);//-sphere_list[i].radius_; 00609 //ROS_INFO_STREAM("Point " << p.x() << " " << p.y() << " " << p.z() << " dist " << dist); 00610 00611 if(dist < distance) { 00612 distance = dist; 00613 closest = i; 00614 gradient = btVector3(gx,gy,gz); 00615 } 00616 } 00617 return distance; 00618 } 00619 00620 bool CollisionProximitySpace::getEnvironmentCollision(const std::vector<std::string>& link_names) 00621 { 00622 updating_objects_lock_.lock(); 00623 bool any_link_in_coll = false; 00624 for(unsigned int i = 0; i < link_names.size(); i++) { 00625 if(body_decomposition_map_.find(link_names[i]) == body_decomposition_map_.end()) { 00626 ROS_WARN_STREAM("No link " << link_names[i] << " for getEnvironmentCollision"); 00627 continue; 00628 } 00629 const BodyDecomposition* bd = body_decomposition_map_.find(link_names[i])->second; 00630 bool coll = getCollisionSphereCollision(bd->getCollisionSpheres()); 00631 if(coll) { 00632 ROS_INFO_STREAM("Link " << link_names[i] << " in collision"); 00633 any_link_in_coll = true; 00634 } 00635 const planning_models::KinematicModel::LinkModel* lm = monitor_->getKinematicModel()->getLinkModel(link_names[i]); 00636 for(unsigned int j = 0; j < lm->getAttachedBodyModels().size(); j++) { 00637 std::string id = makeAttachedObjectId(lm->getName(),lm->getAttachedBodyModels()[j]->getName()); 00638 if(attached_object_map_.find(id) == attached_object_map_.end()) { 00639 ROS_WARN_STREAM("Have no attached object body for attached object " << id); 00640 continue; 00641 } 00642 const BodyDecompositionVector* att = attached_object_map_[id]; 00643 for(unsigned int k = 0; k < att->getSize(); k++) { 00644 coll = getCollisionSphereCollision(att->getBodyDecomposition(k)->getCollisionSpheres()); 00645 if(coll) { 00646 ROS_INFO_STREAM("Attached body " << id << " in collision"); 00647 any_link_in_coll = true; 00648 } 00649 } 00650 } 00651 } 00652 updating_objects_lock_.unlock(); 00653 return any_link_in_coll; 00654 } 00655 */ 00656 bool CollisionProximitySpace::getGroupLinkAndAttachedBodyNames(const std::string& group_name, 00657 const planning_models::KinematicState& state, 00658 std::vector<std::string>& link_names, 00659 std::vector<unsigned int>& link_indices, 00660 std::vector<std::string>& attached_body_names, 00661 std::vector<unsigned int>& attached_body_link_indices) const 00662 { 00663 ros::WallTime n1 = ros::WallTime::now(); 00664 link_names = collision_models_interface_->getKinematicModel()->getModelGroup(group_name)->getUpdatedLinkModelNames(); 00665 if(link_names.size() == 0) { 00666 return false; 00667 } 00668 00669 //making sure that we have bodies for the links - otherwise they don't have collision geometries 00670 std::vector<std::string>::iterator it = link_names.begin(); 00671 while(it != link_names.end()) { 00672 if(body_decomposition_map_.find(*it) == body_decomposition_map_.end()) { 00673 it = link_names.erase(it); 00674 } else { 00675 it++; 00676 } 00677 } 00678 00679 attached_body_names.clear(); 00680 link_indices.clear(); 00681 attached_body_link_indices.clear(); 00682 00683 const std::vector<planning_models::KinematicState::LinkState*>& lsv = state.getLinkStateVector(); 00684 for(unsigned int i = 0; i < link_names.size(); i++) { 00685 bool found = false; 00686 for(unsigned int j = 0; j < lsv.size(); j++) { 00687 if(lsv[j]->getName() == link_names[i]) { 00688 link_indices.push_back(j); 00689 found = true; 00690 break; 00691 } 00692 } 00693 if(!found) { 00694 ROS_INFO_STREAM("No link state found for link " << link_names[i]); 00695 } 00696 if(collision_models_interface_->getLinkAttachedObjects().find(link_names[i]) != collision_models_interface_->getLinkAttachedObjects().end()) { 00697 unsigned int j = 0; 00698 for(std::map<std::string, bodies::BodyVector*>::const_iterator it = collision_models_interface_->getLinkAttachedObjects().find(link_names[i])->second.begin(); 00699 it != collision_models_interface_->getLinkAttachedObjects().find(link_names[i])->second.end(); 00700 it++, j++) { 00701 std::string id = makeAttachedObjectId(link_names[i],it->first); 00702 attached_body_names.push_back(id); 00703 attached_body_link_indices.push_back(link_indices[i]); 00704 } 00705 } 00706 } 00707 return true; 00708 } 00709 00710 bool CollisionProximitySpace::updateSphereLocations(const std::vector<std::string>& link_names, 00711 const std::vector<std::string>& attached_body_names, 00712 std::vector<GradientInfo>& gradients) 00713 { 00714 if(link_names.size()+attached_body_names.size() != gradients.size()) { 00715 ROS_WARN_STREAM("Updating sphere locations with improperly sized gradients"); 00716 return false; 00717 } 00718 for(unsigned int i = 0; i < link_names.size(); i++) { 00719 const std::vector<CollisionSphere>& lcs1 = body_decomposition_map_.find(link_names[i])->second->getCollisionSpheres(); 00720 gradients[i].sphere_locations.resize(lcs1.size()); 00721 gradients[i].sphere_radii.resize(lcs1.size()); 00722 for(unsigned int j = 0; j < lcs1.size(); j++) { 00723 gradients[i].sphere_locations[j] = lcs1[j].center_; 00724 gradients[i].sphere_radii[j] = lcs1[j].radius_; 00725 } 00726 } 00727 unsigned int att_index = link_names.size(); 00728 unsigned int att_count = attached_body_names.size(); 00729 for(unsigned int i = 0; i < att_count; i++) { 00730 if(attached_object_map_.find(attached_body_names[i]) == attached_object_map_.end()) { 00731 ROS_WARN_STREAM("No attached object " << attached_body_names[i]); 00732 return false; 00733 } 00734 const std::vector<CollisionSphere>& att_vec = attached_object_map_.find(attached_body_names[i])->second->getCollisionSpheres(); 00735 gradients[att_index].sphere_locations.resize(att_vec.size()); 00736 gradients[att_index].sphere_radii.resize(att_vec.size()); 00737 for(unsigned int j = 0; j < att_vec.size(); j++) { 00738 gradients[att_index].sphere_locations[j] = att_vec[j].center_; 00739 gradients[att_index].sphere_radii[j] = att_vec[j].radius_; 00740 } 00741 } 00742 return true; 00743 } 00744 00745 bool CollisionProximitySpace::setupGradientStructures(const std::vector<std::string>& link_names, 00746 const std::vector<std::string>& attached_body_names, 00747 std::vector<GradientInfo>& gradients) const 00748 { 00749 if(link_names.size() == 0) { 00750 ROS_WARN_STREAM("No link names in setup gradient structure"); 00751 return false; 00752 } 00753 gradients.clear(); 00754 unsigned int att_count = attached_body_names.size(); 00755 00756 gradients.resize(link_names.size()+att_count); 00757 00758 for(unsigned int i = 0; i < link_names.size(); i++) { 00759 const std::vector<CollisionSphere>& lcs1 = body_decomposition_map_.find(link_names[i])->second->getCollisionSpheres(); 00760 gradients[i].sphere_locations.resize(lcs1.size()); 00761 gradients[i].sphere_radii.resize(lcs1.size()); 00762 gradients[i].joint_name = collision_models_interface_->getKinematicModel()->getLinkModel(link_names[i])->getParentJointModel()->getName(); 00763 for(unsigned int j = 0; j < lcs1.size(); j++) { 00764 gradients[i].sphere_locations[j] = lcs1[j].center_; 00765 gradients[i].sphere_radii[j] = lcs1[j].radius_; 00766 } 00767 gradients[i].distances.resize(lcs1.size(), DBL_MAX); 00768 gradients[i].gradients.resize(lcs1.size()); 00769 } 00770 unsigned int att_index = link_names.size(); 00771 for(unsigned int i = 0; i < att_count; i++) { 00772 if(attached_object_map_.find(attached_body_names[i]) == attached_object_map_.end()) { 00773 ROS_WARN_STREAM("No attached object " << attached_body_names[i]); 00774 return false; 00775 } 00776 const std::vector<CollisionSphere>& att_vec = attached_object_map_.find(attached_body_names[i])->second->getCollisionSpheres(); 00777 gradients[att_index].sphere_locations.resize(att_vec.size()); 00778 gradients[att_index].sphere_radii.resize(att_vec.size()); 00779 gradients[att_index].joint_name = collision_models_interface_->getKinematicModel()->getLinkModel(link_names[link_names.size() - 1])->getParentJointModel()->getName(); 00780 for(unsigned int j = 0; j < att_vec.size(); j++) { 00781 gradients[att_index].sphere_locations[j] = att_vec[j].center_; 00782 gradients[att_index].sphere_radii[j] = att_vec[j].radius_; 00783 } 00784 gradients[att_index].distances.resize(att_vec.size(), DBL_MAX); 00785 gradients[att_index].gradients.resize(att_vec.size()); 00786 att_index++; 00787 } 00788 return true; 00789 } 00790 00791 bool CollisionProximitySpace::isStateInCollision() const 00792 { 00793 if(isEnvironmentCollision()) return true; 00794 if(isSelfCollision()) return true; 00795 return isIntraGroupCollision(); 00796 } 00797 00798 bool CollisionProximitySpace::getStateCollisions(bool& in_collision, 00799 std::vector<CollisionType>& collisions) const 00800 { 00801 collisions.clear(); 00802 collisions.resize(current_link_names_.size()+current_attached_body_names_.size()); 00803 std::vector<bool> env_collisions, intra_collisions, self_collisions; 00804 env_collisions.resize(collisions.size(), false); 00805 intra_collisions = self_collisions = env_collisions; 00806 bool env_collision = getEnvironmentCollisions(env_collisions, false); 00807 bool intra_group_collision = getIntraGroupCollisions(intra_collisions, false); 00808 bool self_collision = getSelfCollisions(intra_collisions, false); 00809 for(unsigned int i = 0; i < current_link_names_.size()+current_attached_body_names_.size(); i++) { 00810 collisions[i].environment = env_collisions[i]; 00811 collisions[i].self = self_collisions[i]; 00812 collisions[i].intra = intra_collisions[i]; 00813 } 00814 if(env_collision || intra_group_collision || self_collision) { 00815 in_collision = true; 00816 } else { 00817 in_collision = false; 00818 } 00819 return in_collision; 00820 } 00821 00822 bool CollisionProximitySpace::getStateGradients(std::vector<GradientInfo>& gradients, 00823 bool subtract_radii) const 00824 { 00825 gradients = current_gradients_; 00826 00827 std::vector<GradientInfo> intra_gradients; 00828 std::vector<GradientInfo> self_gradients; 00829 std::vector<GradientInfo> env_gradients; 00830 00831 bool env_coll = getEnvironmentProximityGradients(env_gradients, subtract_radii); 00832 bool self_coll = getSelfProximityGradients(self_gradients, subtract_radii); 00833 bool intra_coll = getIntraGroupProximityGradients(intra_gradients, subtract_radii); 00834 00835 for(unsigned int i = 0; i < gradients.size(); i++) { 00836 if(i < current_link_names_.size()) { 00837 ROS_DEBUG_STREAM("Link " << current_link_names_[i] 00838 << " env " << env_gradients[i].closest_distance 00839 << " self " << self_gradients[i].closest_distance 00840 << " intra " << intra_gradients[i].closest_distance); 00841 } 00842 { 00843 bool env_at_max = env_gradients[i].closest_distance >= max_environment_distance_; 00844 bool self_at_max = self_gradients[i].closest_distance >= max_self_distance_; 00845 if(env_at_max) { 00846 if(self_at_max || intra_gradients[i].closest_distance < self_gradients[i].closest_distance) { 00847 if(intra_gradients[i].closest_distance == DBL_MAX) { 00848 gradients[i].closest_distance = undefined_distance_; 00849 } else { 00850 gradients[i].closest_distance = intra_gradients[i].closest_distance; 00851 } 00852 ROS_DEBUG_STREAM("Intra gradient is closest"); 00853 } else { 00854 ROS_DEBUG_STREAM("Self gradient is closest"); 00855 gradients[i].closest_distance = self_gradients[i].closest_distance; 00856 } 00857 } else if(self_at_max) { 00858 //don't need to check env_at_max, as the previous condition should take care of it 00859 if(intra_gradients[i].closest_distance < env_gradients[i].closest_distance) { 00860 gradients[i].closest_distance = intra_gradients[i].closest_distance; 00861 ROS_DEBUG_STREAM("Intra gradient is closest"); 00862 } else { 00863 gradients[i].closest_distance = env_gradients[i].closest_distance; 00864 ROS_DEBUG_STREAM("Env gradient is closest"); 00865 } 00866 } else if(self_gradients[i].closest_distance < env_gradients[i].closest_distance) { 00867 gradients[i].closest_distance = self_gradients[i].closest_distance; 00868 ROS_DEBUG_STREAM("Self gradient is closest"); 00869 } else { 00870 gradients[i].closest_distance = env_gradients[i].closest_distance; 00871 ROS_DEBUG_STREAM("Env gradient is closest"); 00872 } 00873 } 00874 00875 if(i < current_link_names_.size() && gradients[i].closest_distance < 0.0) { 00876 ROS_DEBUG_STREAM("Link " << current_link_names_[i] 00877 << " env " << env_gradients[i].closest_distance 00878 << " self " << self_gradients[i].closest_distance 00879 << " intra " << intra_gradients[i].closest_distance); 00880 } 00881 00882 00883 for(unsigned int j = 0; j < gradients[i].distances.size(); j++) { 00884 bool env_at_max = env_gradients[i].distances[j] >= max_environment_distance_; 00885 bool self_at_max = self_gradients[i].distances[j] >= max_self_distance_; 00886 if(env_at_max) { 00887 if(self_at_max || intra_gradients[i].distances[j] < self_gradients[i].distances[j]) { 00888 if(intra_gradients[i].distances[j] == DBL_MAX) { 00889 gradients[i].distances[j] = undefined_distance_; 00890 } else { 00891 gradients[i].distances[j] = intra_gradients[i].distances[j]; 00892 } 00893 gradients[i].gradients[j] = intra_gradients[i].gradients[j]; 00894 } else { 00895 gradients[i].distances[j] = self_gradients[i].distances[j]; 00896 gradients[i].gradients[j] = self_gradients[i].gradients[j]; 00897 } 00898 } else if(self_at_max) { 00899 if(intra_gradients[i].distances[j] < env_gradients[i].distances[j]) { 00900 gradients[i].distances[j] = intra_gradients[i].distances[j]; 00901 gradients[i].gradients[j] = intra_gradients[i].gradients[j]; 00902 } else { 00903 gradients[i].distances[j] = env_gradients[i].distances[j]; 00904 gradients[i].gradients[j] = env_gradients[i].gradients[j]; 00905 } 00906 } else if(self_gradients[i].distances[j] < env_gradients[i].distances[j]) { 00907 gradients[i].distances[j] = self_gradients[i].distances[j]; 00908 gradients[i].gradients[j] = self_gradients[i].gradients[j]; 00909 } else { 00910 gradients[i].distances[j] = env_gradients[i].distances[j]; 00911 gradients[i].gradients[j] = env_gradients[i].gradients[j]; 00912 } 00913 } 00914 } 00915 return (env_coll || intra_coll || self_coll); 00916 } 00917 00918 bool CollisionProximitySpace::isIntraGroupCollision() const 00919 { 00920 std::vector<bool> collisions; 00921 return(getIntraGroupCollisions(collisions, true)); 00922 } 00923 00924 bool CollisionProximitySpace::getIntraGroupCollisions(std::vector<bool>& collisions, bool stop_at_first_collision) const { 00925 bool in_collision = false; 00926 unsigned int num_links = current_link_names_.size(); 00927 unsigned int num_attached = current_attached_body_names_.size(); 00928 unsigned int tot = num_links+num_attached; 00929 for(unsigned int i = 0; i < tot; i++) { 00930 for(unsigned int j = i; j < tot; j++) { 00931 if(i == j) continue; 00932 if(!current_intra_group_collision_links_[i][j]) continue; 00933 const std::vector<CollisionSphere>* lcs1; 00934 const std::vector<CollisionSphere>* lcs2; 00935 if(i < num_links) { 00936 lcs1 = &(current_link_body_decompositions_[i]->getCollisionSpheres()); 00937 } else { 00938 lcs1 = &(current_attached_body_decompositions_[i-num_links]->getCollisionSpheres()); 00939 } 00940 if(j < num_links) { 00941 lcs2 = &(current_link_body_decompositions_[j]->getCollisionSpheres()); 00942 } else { 00943 lcs2 = &(current_attached_body_decompositions_[j-num_links]->getCollisionSpheres()); 00944 } 00945 for(unsigned int k = 0; k < lcs1->size(); k++) { 00946 for(unsigned int l = 0; l < lcs2->size(); l++) { 00947 double dist = (*lcs1)[k].center_.distance((*lcs2)[l].center_); 00948 dist += -(*lcs1)[k].radius_-(*lcs2)[l].radius_; 00949 if(dist <= tolerance_) { 00950 if(stop_at_first_collision) { 00951 return true; 00952 } 00953 collisions[i] = true; 00954 collisions[j] = true; 00955 in_collision = true; 00956 } 00957 } 00958 } 00959 } 00960 } 00961 return in_collision; 00962 } 00963 00964 bool CollisionProximitySpace::getIntraGroupProximityGradients(std::vector<GradientInfo>& gradients, 00965 bool subtract_radii) const { 00966 gradients = current_gradients_; 00967 bool in_collision = false; 00968 unsigned int count = 0; 00969 unsigned int num_links = current_link_names_.size(); 00970 unsigned int num_attached = current_attached_body_names_.size(); 00971 unsigned int tot = num_links+num_attached; 00972 std::vector<std::string> all_names = current_link_names_; 00973 all_names.insert(all_names.end(), current_attached_body_names_.begin(), current_attached_body_names_.end()); 00974 for(unsigned int i = 0; i < tot; i++) { 00975 for(unsigned int j = 0; j < tot; j++) { 00976 if(i == j) continue; 00977 if(!current_intra_group_collision_links_[i][j]) { 00978 continue; 00979 } 00980 const std::vector<CollisionSphere>* lcs1; 00981 const std::vector<CollisionSphere>* lcs2; 00982 if(i < num_links) { 00983 lcs1 = &(current_link_body_decompositions_[i]->getCollisionSpheres()); 00984 } else { 00985 lcs1 = &(current_attached_body_decompositions_[i-num_links]->getCollisionSpheres()); 00986 } 00987 if(j < num_links) { 00988 lcs2 = &(current_link_body_decompositions_[j]->getCollisionSpheres()); 00989 } else { 00990 lcs2 = &(current_attached_body_decompositions_[j-num_links]->getCollisionSpheres()); 00991 } 00992 for(unsigned int k = 0; k < lcs1->size(); k++) { 00993 for(unsigned int l = 0; l < lcs2->size(); l++) { 00994 double dist = (*lcs1)[k].center_.distance((*lcs2)[l].center_); 00995 if(subtract_radii) { 00996 dist += -(*lcs1)[k].radius_-(*lcs2)[l].radius_; 00997 if(dist <= tolerance_) { 00998 in_collision = true; 00999 } 01000 } 01001 count++; 01002 if(dist < gradients[i].distances[k]) { 01003 gradients[i].distances[k] = dist; 01004 gradients[i].gradients[k] = (*lcs1)[k].center_-(*lcs2)[l].center_; 01005 } 01006 if(dist < gradients[i].closest_distance) { 01007 gradients[i].closest_distance = dist; 01008 } 01009 if(dist < gradients[j].distances[l]) { 01010 gradients[j].distances[l] = dist; 01011 gradients[j].gradients[l] = (*lcs2)[l].center_-(*lcs1)[k].center_; 01012 } 01013 if(dist < gradients[j].closest_distance) { 01014 gradients[j].closest_distance = dist; 01015 } 01016 } 01017 } 01018 } 01019 } 01020 return in_collision; 01021 } 01022 01023 bool CollisionProximitySpace::isSelfCollision() const 01024 { 01025 std::vector<bool> collisions; 01026 return(getSelfCollisions(collisions, true)); 01027 } 01028 01029 bool CollisionProximitySpace::getSelfCollisions(std::vector<bool>& collisions, 01030 bool stop_at_first_collision) const 01031 { 01032 bool in_collision = false; 01033 for(unsigned int i = 0; i < current_link_names_.size(); i++) { 01034 const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres(); 01035 bool coll = getCollisionSphereCollision(self_distance_field_, body_spheres, tolerance_); 01036 if(coll) { 01037 if(stop_at_first_collision) { 01038 return true; 01039 } 01040 in_collision = true; 01041 collisions[i] = true; 01042 } 01043 } 01044 for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) { 01045 const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres(); 01046 bool coll = getCollisionSphereCollision(self_distance_field_, body_spheres, tolerance_); 01047 if(coll) { 01048 if(stop_at_first_collision) { 01049 return true; 01050 } 01051 in_collision = true; 01052 collisions[i+current_link_names_.size()] = true; 01053 } 01054 } 01055 return in_collision; 01056 } 01057 01058 bool CollisionProximitySpace::getSelfProximityGradients(std::vector<GradientInfo>& gradients, 01059 bool subtract_radii) const { 01060 gradients = current_gradients_; 01061 bool in_collision = false; 01062 for(unsigned int i = 0; i < current_link_names_.size(); i++) { 01063 if(!current_self_excludes_[i]) continue; 01064 const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres(); 01065 if(gradients[i].distances.size() != body_spheres.size()) { 01066 ROS_INFO_STREAM("Wrong size for closest distances for link " << current_link_names_[i]); 01067 } 01068 bool coll = getCollisionSphereGradients(self_distance_field_, body_spheres, gradients[i], tolerance_, subtract_radii, max_self_distance_, false); 01069 if(coll) { 01070 in_collision = true; 01071 } 01072 } 01073 for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) { 01074 const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres(); 01075 bool coll = getCollisionSphereGradients(self_distance_field_, body_spheres, gradients[i+current_link_names_.size()], 01076 tolerance_, subtract_radii, max_self_distance_, false); 01077 if(coll) { 01078 in_collision = true; 01079 } 01080 } 01081 return in_collision; 01082 } 01083 01084 bool CollisionProximitySpace::isEnvironmentCollision() const 01085 { 01086 std::vector<bool> collisions; 01087 return(getEnvironmentCollisions(collisions, true)); 01088 } 01089 01090 bool CollisionProximitySpace::getEnvironmentCollisions(std::vector<bool>& collisions, 01091 bool stop_at_first_collision) const 01092 { 01093 bool in_collision = false; 01094 for(unsigned int i = 0; i < current_link_names_.size(); i++) { 01095 const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres(); 01096 bool coll = getCollisionSphereCollision(environment_distance_field_, body_spheres, tolerance_); 01097 if(coll) { 01098 if(stop_at_first_collision) { 01099 return true; 01100 } 01101 in_collision = true; 01102 collisions[i] = true; 01103 } 01104 } 01105 for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) { 01106 const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres(); 01107 bool coll = getCollisionSphereCollision(environment_distance_field_, body_spheres, tolerance_); 01108 if(coll) { 01109 if(stop_at_first_collision) { 01110 return true; 01111 } 01112 in_collision = true; 01113 collisions[i+current_link_names_.size()] = true; 01114 } 01115 } 01116 return in_collision; 01117 } 01118 01119 bool CollisionProximitySpace::getEnvironmentProximityGradients(std::vector<GradientInfo>& gradients, 01120 bool subtract_radii) const { 01121 gradients = current_gradients_; 01122 bool in_collision = false; 01123 for(unsigned int i = 0; i < current_link_names_.size(); i++) { 01124 const std::vector<CollisionSphere>& body_spheres = current_link_body_decompositions_[i]->getCollisionSpheres(); 01125 if(gradients[i].distances.size() != body_spheres.size()) { 01126 ROS_INFO_STREAM("Wrong size for closest distances for link " << current_link_names_[i]); 01127 } 01128 bool coll = getCollisionSphereGradients(environment_distance_field_, body_spheres, gradients[i], tolerance_, subtract_radii, max_environment_distance_, false); 01129 if(coll) { 01130 in_collision = true; 01131 } 01132 } 01133 for(unsigned int i = 0; i < current_attached_body_names_.size(); i++) { 01134 const std::vector<CollisionSphere>& body_spheres = current_attached_body_decompositions_[i]->getCollisionSpheres(); 01135 bool coll = getCollisionSphereGradients(environment_distance_field_, body_spheres, gradients[i+current_link_names_.size()], tolerance_, subtract_radii, max_environment_distance_, false); 01136 if(coll) { 01137 in_collision = true; 01138 } 01139 } 01140 return in_collision; 01141 } 01142 01143 void CollisionProximitySpace::getProximityGradientMarkers(const std::vector<std::string>& link_names, 01144 const std::vector<std::string>& attached_body_names, 01145 const std::vector<GradientInfo>& gradients, 01146 const std::string& ns, 01147 visualization_msgs::MarkerArray& arr) const 01148 { 01149 for(unsigned int i = 0; i < gradients.size(); i++) { 01150 01151 const std::vector<CollisionSphere>* lcs; 01152 std::string name; 01153 if(i < link_names.size()) { 01154 name = link_names[i]; 01155 lcs = &(body_decomposition_map_.find(name)->second->getCollisionSpheres()); 01156 } else { 01157 name = attached_body_names[i-link_names.size()]; 01158 lcs = &(attached_object_map_.find(name)->second->getCollisionSpheres()); 01159 } 01160 for(unsigned int j = 0; j < gradients[i].distances.size(); j++) { 01161 visualization_msgs::Marker arrow_mark; 01162 arrow_mark.header.frame_id = collision_models_interface_->getRobotFrameId(); 01163 arrow_mark.header.stamp = ros::Time::now(); 01164 if(ns.empty()) { 01165 arrow_mark.ns = "self_coll_gradients"; 01166 } else { 01167 arrow_mark.ns = ns; 01168 } 01169 arrow_mark.id = i*1000+j; 01170 double xscale = 0.0; 01171 double yscale = 0.0; 01172 double zscale = 0.0; 01173 if(gradients[i].distances[j] > 0.0) { 01174 if(gradients[i].gradients[j].length() > 0.0) { 01175 xscale = gradients[i].gradients[j].x()/gradients[i].gradients[j].length(); 01176 yscale = gradients[i].gradients[j].y()/gradients[i].gradients[j].length(); 01177 zscale = gradients[i].gradients[j].z()/gradients[i].gradients[j].length(); 01178 } else { 01179 ROS_DEBUG_STREAM("Negative length for " << name << " " << arrow_mark.id << " " << gradients[i].gradients[j].length()); 01180 } 01181 } else { 01182 ROS_DEBUG_STREAM("Negative dist for " << name << " " << arrow_mark.id); 01183 } 01184 arrow_mark.points.resize(2); 01185 arrow_mark.points[1].x = (*lcs)[j].center_.x(); 01186 arrow_mark.points[1].y = (*lcs)[j].center_.y(); 01187 arrow_mark.points[1].z = (*lcs)[j].center_.z(); 01188 arrow_mark.points[0] = arrow_mark.points[1]; 01189 arrow_mark.points[0].x -= xscale*gradients[i].distances[j]; 01190 arrow_mark.points[0].y -= yscale*gradients[i].distances[j]; 01191 arrow_mark.points[0].z -= zscale*gradients[i].distances[j]; 01192 arrow_mark.scale.x = 0.01; 01193 arrow_mark.scale.y = 0.03; 01194 arrow_mark.color.r = 1.0; 01195 arrow_mark.color.g = 0.2; 01196 arrow_mark.color.b = .5; 01197 arrow_mark.color.a = 1.0; 01198 arr.markers.push_back(arrow_mark); 01199 } 01200 } 01201 } 01202 01203 bool CollisionProximitySpace::isTrajectorySafe(const trajectory_msgs::JointTrajectory& trajectory, 01204 const arm_navigation_msgs::Constraints& goal_constraints, 01205 const arm_navigation_msgs::Constraints& path_constraints, 01206 const std::string& groupName) 01207 { 01208 01209 if(!collision_models_interface_->getPlanningSceneState()->hasJointStateGroup(groupName)) 01210 { 01211 ROS_ERROR("Group %s does not exist. Cannot evaluate trajectory safety.", groupName.c_str()); 01212 return false; 01213 } 01214 planning_models::KinematicState::JointStateGroup* stateGroup = collision_models_interface_->getPlanningSceneState()-> 01215 getJointStateGroup(groupName); 01216 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 01217 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes; 01218 std::map<std::string, double> stateMap; 01219 collision_models_interface_->getPlanningSceneState()->getKinematicStateValues(stateMap); 01220 01221 // If the trajectory is already valid and has no mesh to mesh collisions, we know it's safe. 01222 if(collision_models_interface_->isJointTrajectoryValid(*(collision_models_interface_->getPlanningSceneState()), 01223 trajectory, goal_constraints, path_constraints, error_code, error_codes, false)) 01224 { 01225 return true; 01226 } 01227 // Otherwise we have to do something more clever. We know it may have mesh to mesh collisions, but we might 01228 // want to plan into collision or out of collision. We will still avoid planning through obstacles. 01229 else 01230 { 01231 std::vector<double> lastDistances; 01232 TrajectoryPointType type = None; 01233 // For each trajectory point, get gradients and assert that the collision cost of start points 01234 // is monotonically decreasing, and that the collision cost of end points is monotonically increasing. 01235 // Midpoints must never have collisions. 01236 //+--------------------------------------------------------------------------------------------------------------+ 01237 //| NONE | START | MIDDLE | END | 01238 //| Starting Phase| First collision to first non-collision | First non-collision to collision | collision to end | 01239 //| First point. | Monotonically increasing distance | No collisions allowed | Monoton. decrease| 01240 //+--------------------------------------------------------------------------------------------------------------+ 01241 for(size_t i = 0; i < trajectory.points.size(); i++) 01242 { 01243 std::vector<GradientInfo> gradients; 01244 std::vector<double> distances; 01245 const trajectory_msgs::JointTrajectoryPoint& trajectoryPoint = trajectory.points[i]; 01246 stateGroup->setKinematicState(trajectoryPoint.positions); 01247 setCurrentGroupState(*(collision_models_interface_->getPlanningSceneState())); 01248 01249 getStateGradients(gradients, true); 01250 bool in_collision = false; 01251 for(size_t j = 0; j < gradients.size(); j++) 01252 { 01253 GradientInfo& gradient = gradients[j]; 01254 in_collision = in_collision || gradient.collision; 01255 01256 for(size_t k = 0; k < gradient.distances.size(); k++) 01257 { 01258 distances.push_back(gradient.distances[k]); 01259 in_collision = in_collision || gradient.distances[k] < tolerance_; 01260 } 01261 } 01262 01263 // If the first point is in collision, we're in the start collision phase 01264 if(type == None && in_collision && i == 0) 01265 { 01266 type = StartCollision; 01267 ROS_DEBUG("In starting collision phase at index 0"); 01268 } 01269 // If the first point is not in collision, then the first collision we encounter is the end phase. 01270 else if(type == None && in_collision && i != 0) 01271 { 01272 type = EndCollision; 01273 ROS_DEBUG("In ending collision phase at index %lu", (long unsigned int)i); 01274 } 01275 // If the first point is not in collision, and the next point is not in collision, then we've entered the 01276 // middle phase. 01277 else if(type == None && !in_collision && i != 0) 01278 { 01279 type = Middle; 01280 ROS_DEBUG("In middle collision phase at index %lu", (long unsigned int)i); 01281 } 01282 // If we've exited collision from the start, then we're in the middle phase. 01283 else if (type == StartCollision && !in_collision) 01284 { 01285 type = Middle; 01286 ROS_DEBUG("In middle collision phase at index %lu", (long unsigned int)i); 01287 } 01288 // If we're in the middle phase, and we encounter a collision, we're in the end phase. 01289 else if(type == Middle && in_collision) 01290 { 01291 type = EndCollision; 01292 ROS_DEBUG("In end collision phase at index %lu", (long unsigned int)i); 01293 } 01294 // If the end phase has collisions, then there should be no point in which it is out of collision. 01295 // We want to avoid passing through obstacles, but not going into contact with obstacles. 01296 else if(type == EndCollision && !in_collision) 01297 { 01298 ROS_DEBUG("Got point in the end collision phase that was not in collision : %lu", (long unsigned int) i); 01299 return false; 01300 } 01301 01302 01303 for(size_t j = 0; j < distances.size(); j++) 01304 { 01305 double dist = distances[j]; 01306 double lastDist = -10000; 01307 01308 if(lastDistances.size() > j) 01309 { 01310 lastDist = lastDistances[j]; 01311 } 01312 else 01313 { 01314 lastDistances.push_back(dist); 01315 } 01316 01317 // Only consider spheres in collision. 01318 if(dist > 0.02) 01319 { 01320 continue; 01321 } 01322 01323 // Switch occurs in start phase during collision, in end phase during collision, or middle phase 01324 // during non-collision. 01325 switch(type) 01326 { 01327 case StartCollision: 01328 // Assert monotonically increasing. 01329 if(dist < lastDist) 01330 { 01331 ROS_DEBUG("Start phase was not monotonically increasing in distance. Reason: point %lu sphere %lu", (long unsigned int)i, (long unsigned int)j); 01332 return false; 01333 } 01334 break; 01335 case EndCollision: 01336 // Assert monotonically decreasing. 01337 if(dist > lastDist) 01338 { 01339 ROS_DEBUG("End phase was not monotonically decreasing in distance. Reason: point %lu sphere %lu", (long unsigned int)i, (long unsigned int)j); 01340 return false; 01341 } 01342 break; 01343 case Middle: 01344 // Assert no collisions. 01345 if(in_collision) 01346 { 01347 ROS_DEBUG("Middle phase had a collision. Reason: point %lu sphere %lu", (long unsigned int)i, (long unsigned int)j); 01348 return false; 01349 } 01350 break; 01351 case None: 01352 // Nothing. 01353 break; 01354 } 01355 01356 lastDistances[j] = dist; 01357 } 01358 } 01359 // Trajectory passed all tests. 01360 ROS_DEBUG("Trajectory passed all safety tests."); 01361 return true; 01362 } 01363 01364 01365 } 01366 01368 // Visualization functions 01370 01371 void CollisionProximitySpace::visualizeDistanceField(distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field) const 01372 { 01373 btTransform ident; 01374 ident.setIdentity(); 01375 distance_field->visualize(0.0, 0.0, collision_models_interface_->getWorldFrameId(), ident, ros::Time::now()); 01376 } 01377 01378 void CollisionProximitySpace::visualizeDistanceFieldPlane(distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field) const 01379 { 01380 double length = distance_field->getSize(distance_field::PropagationDistanceField::DIM_X); 01381 double width = distance_field->getSize(distance_field::PropagationDistanceField::DIM_Y); 01382 double height = distance_field->getSize(distance_field::PropagationDistanceField::DIM_Z); 01383 btVector3 origin(distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_X) + length / 2.0, 01384 distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_Y) + width / 2.0, 01385 distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_Z) + height / 2.0); 01386 01387 for(double z = distance_field->getOrigin(distance_field::PropagationDistanceField::DIM_Z); 01388 z < distance_field->getSize(distance_field::PropagationDistanceField::DIM_Z); 01389 z += distance_field->getResolution(distance_field::PropagationDistanceField::DIM_Z)) 01390 { 01391 distance_field->visualizePlane(distance_field::XYPlane, length, width, 01392 z, origin, collision_models_interface_->getWorldFrameId(), ros::Time::now()); 01393 ros::Time::sleepUntil(ros::Time::now() + ros::Duration(0.02)); 01394 ros::spinOnce(); 01395 } 01396 } 01397 /* 01398 void CollisionProximitySpace::visualizeClosestCollisionSpheres(const std::vector<std::string>& link_names) const 01399 { 01400 planning_models::KinematicState state(monitor_->getKinematicModel()); 01401 ProximityInfo prox; 01402 getEnvironmentProximity(current_link_names, prox); 01403 const BodyDecomposition* bsd = body_decomposition_map_.find(prox.link_name)->second; 01404 if(!prox.attached_object_name.empty()) { 01405 bsd = attached_object_map_.find(prox.attached_object_name)->second->getBodyDecomposition(prox.att_index); 01406 } 01407 if(prox.sphere_index < bsd->getCollisionSpheres().size()) { 01408 visualization_msgs::Marker mark; 01409 mark.header.frame_id = monitor_->getRobotFrameId(); 01410 mark.header.stamp = ros::Time::now(); 01411 mark.type = mark.SPHERE; 01412 mark.ns = "closest_sphere"; 01413 mark.id = 0; 01414 mark.scale.x = bsd->getCollisionSpheres()[prox.sphere_index].radius_*2.0; 01415 mark.scale.y = mark.scale.x; 01416 mark.scale.z = mark.scale.x; 01417 mark.color.b = 1.0; 01418 mark.color.a = .5; 01419 mark.pose.position.x = bsd->getCollisionSpheres()[prox.sphere_index].center_.x(); 01420 mark.pose.position.y = bsd->getCollisionSpheres()[prox.sphere_index].center_.y(); 01421 mark.pose.position.z = bsd->getCollisionSpheres()[prox.sphere_index].center_.z(); 01422 mark.pose.orientation.w = 1.0; 01423 vis_marker_publisher_.publish(mark); 01424 } 01425 01426 std::vector<CollisionSphere> all_collision_spheres; 01427 for(unsigned int i = 0; i < link_names.size(); i++) { 01428 const planning_models::KinematicState::LinkState* ls = state.getLinkState(link_names[i]); 01429 if(body_decomposition_map_.find(ls->getName()) == body_decomposition_map_.end()) { 01430 //ROS_WARN_STREAM("Can't find body decomposition for link state " << ls->getName()); 01431 continue; 01432 } 01433 const std::vector<CollisionSphere>& body_spheres = body_decomposition_map_.find(ls->getName())->second->getCollisionSpheres(); 01434 all_collision_spheres.insert(all_collision_spheres.end(), body_spheres.begin(), body_spheres.end()); 01435 for(unsigned int j = 0; j < ls->getAttachedBodyStateVector().size(); j++) { 01436 const planning_models::KinematicState::AttachedBodyState* att_state = ls->getAttachedBodyStateVector()[j]; 01437 std::string id = makeAttachedObjectId(ls->getName(), att_state->getName()); 01438 if(attached_object_map_.find(id) == attached_object_map_.end()) { 01439 continue; 01440 } 01441 const std::vector<CollisionSphere>& body_spheres2 = attached_object_map_.find(id)->second->getCollisionSpheres(); 01442 all_collision_spheres.insert(all_collision_spheres.end(), body_spheres2.begin(), body_spheres2.end()); 01443 } 01444 } 01445 std::vector<btVector3> all_collision_gradients; 01446 std::vector<double> distances; 01447 double closest_distance; 01448 getCollisionSphereGradients(all_collision_spheres, closest_distance, distances, all_collision_gradients); 01449 visualization_msgs::MarkerArray arr; 01450 for(unsigned int i = 0; i < all_collision_gradients.size(); i++) { 01451 visualization_msgs::Marker arrow_mark; 01452 arrow_mark.header.frame_id = monitor_->getRobotFrameId(); 01453 arrow_mark.header.stamp = ros::Time::now(); 01454 arrow_mark.ns = "closest_sphere"; 01455 arrow_mark.id = i+1; 01456 double xscale = 0.0; 01457 double yscale = 0.0; 01458 double zscale = 0.0; 01459 if(all_collision_gradients[i].length() > 0.0) { 01460 xscale = all_collision_gradients[i].x()/all_collision_gradients[i].length(); 01461 yscale = all_collision_gradients[i].y()/all_collision_gradients[i].length(); 01462 zscale = all_collision_gradients[i].z()/all_collision_gradients[i].length(); 01463 } 01464 arrow_mark.points.resize(2); 01465 arrow_mark.points[0].x = all_collision_spheres[i].center_.x(); 01466 arrow_mark.points[0].y = all_collision_spheres[i].center_.y(); 01467 arrow_mark.points[0].z = all_collision_spheres[i].center_.z(); 01468 arrow_mark.points[1] = arrow_mark.points[0]; 01469 arrow_mark.points[1].x -= xscale*distances[i]; 01470 arrow_mark.points[1].y -= yscale*distances[i]; 01471 arrow_mark.points[1].z -= zscale*distances[i]; 01472 arrow_mark.scale.x = 0.01; 01473 arrow_mark.scale.y = 0.03; 01474 arrow_mark.color.r = 1.0; 01475 arrow_mark.color.g = 0.2; 01476 arrow_mark.color.b = .5; 01477 arrow_mark.color.a = 1.0; 01478 arr.markers.push_back(arrow_mark); 01479 } 01480 01481 vis_marker_array_publisher_.publish(arr); 01482 } 01483 */ 01484 void CollisionProximitySpace::visualizeCollisions(const std::vector<std::string>& link_names, 01485 const std::vector<std::string>& attached_body_names, 01486 const std::vector<CollisionType> collisions) const 01487 { 01488 visualization_msgs::MarkerArray arr; 01489 01490 for(unsigned int i = 0; i < link_names.size(); i++) { 01491 std::string name = link_names[i]; 01492 //if(collisions[i] != NONE) { 01493 boost::shared_ptr<const urdf::Link> urdf_link = collision_models_interface_->getParsedDescription()->getLink(name); 01494 if(urdf_link == NULL) { 01495 ROS_INFO_STREAM("No entry in urdf for link " << name); 01496 continue; 01497 } 01498 if(!urdf_link->collision) { 01499 continue; 01500 } 01501 const urdf::Geometry *geom = urdf_link->collision->geometry.get(); 01502 if(!geom) { 01503 ROS_DEBUG_STREAM("No collision geometry for link " << name); 01504 continue; 01505 } 01506 const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom); 01507 if(mesh) { 01508 if (!mesh->filename.empty()) { 01509 visualization_msgs::Marker mark; 01510 mark.header.frame_id = collision_models_interface_->getRobotFrameId(); 01511 mark.header.stamp = ros::Time::now(); 01512 mark.ns = "proximity_collisions"; 01513 mark.id = i; 01514 mark.type = mark.MESH_RESOURCE; 01515 mark.scale.x = 1.05; 01516 mark.scale.y = 1.05; 01517 mark.scale.z = 1.05; 01518 mark.color.a = .5; 01519 if(collisions[i].intra) { 01520 ROS_DEBUG_STREAM(name << " in intra_group"); 01521 mark.color.r = 1.0; 01522 } else if(collisions[i].environment) { 01523 ROS_DEBUG_STREAM(name << " in environment"); 01524 mark.color.r = 1.0; 01525 mark.color.g = 1.0; 01526 } else if(collisions[i].self) { 01527 ROS_DEBUG_STREAM(name << " in self"); 01528 mark.color.b = 1.0; 01529 } else { 01530 mark.color.g = 1.0; 01531 } 01532 //mark.lifetime = ros::Duration(1.0); 01533 mark.frame_locked = true; 01534 const btTransform& trans = body_decomposition_map_.find(name)->second->getBody()->getPose(); 01535 tf::poseTFToMsg(trans, mark.pose); 01536 mark.mesh_resource = mesh->filename; 01537 arr.markers.push_back(mark); 01538 } 01539 } 01540 //} 01541 } 01542 for(unsigned int i = 0; i < attached_body_names.size(); i++) { 01543 std::string name = attached_body_names[i]; 01544 const BodyDecompositionVector* bdv = attached_object_map_.find(attached_body_names[i])->second; 01545 for(unsigned int j = 0; j < bdv->getSize(); j++) { 01546 const BodyDecomposition* bd = bdv->getBodyDecomposition(j); 01547 visualization_msgs::Marker mark; 01548 mark.header.frame_id = collision_models_interface_->getRobotFrameId(); 01549 mark.header.stamp = ros::Time::now(); 01550 mark.ns = "proximity_collisions"; 01551 mark.id = i+link_names.size(); 01552 mark.type = mark.CYLINDER; 01553 bodies::BoundingCylinder cyl; 01554 bd->getBody()->computeBoundingCylinder(cyl); 01555 mark.scale.x = cyl.radius*2.0; 01556 mark.scale.y = cyl.radius*2.0; 01557 mark.scale.z = cyl.length; 01558 mark.color.a = .5; 01559 if(collisions[i+link_names.size()].intra) { 01560 ROS_DEBUG_STREAM(name << " in intra_group"); 01561 mark.color.r = 1.0; 01562 } else if(collisions[i+link_names.size()].environment) { 01563 ROS_DEBUG_STREAM(name << " in environment"); 01564 mark.color.r = 1.0; 01565 mark.color.g = 1.0; 01566 } else if(collisions[i+link_names.size()].self) { 01567 ROS_DEBUG_STREAM(name << " in self"); 01568 mark.color.b = 1.0; 01569 } else { 01570 mark.color.g = 1.0; 01571 } 01572 //mark.lifetime = ros::Duration(1.0); 01573 const btTransform& trans = bd->getBody()->getPose(); 01574 tf::poseTFToMsg(trans, mark.pose); 01575 arr.markers.push_back(mark); 01576 } 01577 } 01578 vis_marker_array_publisher_.publish(arr); 01579 } 01580 01581 void CollisionProximitySpace::visualizeObjectVoxels(const std::vector<std::string>& object_names) const 01582 { 01583 visualization_msgs::Marker cube_list; 01584 cube_list.header.frame_id = collision_models_interface_->getRobotFrameId(); 01585 cube_list.header.stamp = ros::Time::now(); 01586 cube_list.type = cube_list.CUBE_LIST; 01587 cube_list.ns = "body_voxels"; 01588 cube_list.id = 1000; 01589 cube_list.scale.x = resolution_; 01590 cube_list.scale.y = resolution_; 01591 cube_list.scale.z = resolution_; 01592 cube_list.color.b = 1.0; 01593 cube_list.color.a = .5; 01594 for(unsigned int i = 0; i < object_names.size(); i++) { 01595 const std::vector<btVector3>* coll_points; 01596 if(body_decomposition_map_.find(object_names[i]) != body_decomposition_map_.end()) { 01597 coll_points = &(body_decomposition_map_.find(object_names[i])->second)->getCollisionPoints(); 01598 } else if(static_object_map_.find(object_names[i]) != static_object_map_.end()) { 01599 coll_points = &(static_object_map_.find(object_names[i])->second)->getCollisionPoints(); 01600 } else if(attached_object_map_.find(object_names[i]) != attached_object_map_.end()) { 01601 coll_points = &(attached_object_map_.find(object_names[i])->second)->getCollisionPoints(); 01602 } else { 01603 ROS_WARN_STREAM("Don't have object named " << object_names[i]); 01604 continue; 01605 } 01606 for(unsigned int i = 0; i < coll_points->size(); i++) { 01607 geometry_msgs::Point p; 01608 p.x = (*coll_points)[i].x(); 01609 p.y = (*coll_points)[i].y(); 01610 p.z = (*coll_points)[i].z(); 01611 cube_list.points.push_back(p); 01612 } 01613 } 01614 vis_marker_publisher_.publish(cube_list); 01615 } 01616 01617 void CollisionProximitySpace::visualizeObjectSpheres(const std::vector<std::string>& object_names) const 01618 { 01619 visualization_msgs::MarkerArray arr; 01620 unsigned int count = 0; 01621 for(unsigned int i = 0; i < object_names.size(); i++) { 01622 // visualization_msgs::Marker sphere_list; 01623 // sphere_list.header.frame_id = monitor_->getRobotFrameId(); 01624 // sphere_list.header.stamp = ros::Time::now(); 01625 // sphere_list.type = sphere_list.SPHERE_LIST; 01626 // sphere_list.ns = "body_spheres"; 01627 // sphere_list.id = i; 01628 // sphere_list.color.g = 1.0; 01629 // sphere_list.color.a = .5; 01630 const std::vector<CollisionSphere>* coll_spheres; 01631 if(body_decomposition_map_.find(object_names[i]) != body_decomposition_map_.end()) { 01632 coll_spheres = &(body_decomposition_map_.find(object_names[i])->second)->getCollisionSpheres(); 01633 } else if(static_object_map_.find(object_names[i]) != static_object_map_.end()) { 01634 coll_spheres = &(static_object_map_.find(object_names[i])->second)->getCollisionSpheres(); 01635 } else if(attached_object_map_.find(object_names[i]) != attached_object_map_.end()) { 01636 coll_spheres = &(attached_object_map_.find(object_names[i])->second)->getCollisionSpheres(); 01637 } else { 01638 ROS_WARN_STREAM("Don't have object named " << object_names[i]); 01639 continue; 01640 } 01641 // sphere_list.scale.x = (*coll_spheres)[0].radius_*2.0; 01642 // sphere_list.scale.y = sphere_list.scale.x; 01643 // sphere_list.scale.z = sphere_list.scale.x; 01644 for(unsigned int i = 0; i < coll_spheres->size(); i++) { 01645 visualization_msgs::Marker sphere; 01646 sphere.header.frame_id = collision_models_interface_->getRobotFrameId(); 01647 sphere.header.stamp = ros::Time::now(); 01648 sphere.type = sphere.SPHERE; 01649 sphere.ns = "body_spheres"; 01650 sphere.id = count++; 01651 sphere.color.g = 1.0; 01652 sphere.color.a = .5; 01653 sphere.scale.x = (*coll_spheres)[i].radius_*2.0; 01654 sphere.scale.y = sphere.scale.x; 01655 sphere.scale.z = sphere.scale.x; 01656 geometry_msgs::Point p; 01657 sphere.pose.position.x = (*coll_spheres)[i].center_.x(); 01658 sphere.pose.position.y = (*coll_spheres)[i].center_.y(); 01659 sphere.pose.position.z = (*coll_spheres)[i].center_.z(); 01660 sphere.points.push_back(p); 01661 arr.markers.push_back(sphere); 01662 } 01663 } 01664 vis_marker_array_publisher_.publish(arr); 01665 } 01666 01667 void CollisionProximitySpace::visualizeBoundingCylinders(const std::vector<std::string>& object_names) const 01668 { 01669 visualization_msgs::MarkerArray arr; 01670 for(unsigned int i = 0; i < object_names.size(); i++) { 01671 visualization_msgs::Marker mark; 01672 mark.header.frame_id = collision_models_interface_->getRobotFrameId(); 01673 mark.header.stamp = ros::Time::now(); 01674 mark.type = mark.CYLINDER; 01675 mark.ns = "body_cylinders"; 01676 mark.id = i; 01677 mark.color.r = 1.0; 01678 mark.color.a = .5; 01679 const BodyDecomposition* bsd = NULL; 01680 if(body_decomposition_map_.find(object_names[i]) != body_decomposition_map_.end()) { 01681 bsd = body_decomposition_map_.find(object_names[i])->second; 01682 } else if(static_object_map_.find(object_names[i]) != static_object_map_.end()) { 01683 bsd = static_object_map_.find(object_names[i])->second->getBodyDecomposition(0); 01684 } else if(attached_object_map_.find(object_names[i]) != attached_object_map_.end()) { 01685 bsd = attached_object_map_.find(object_names[i])->second->getBodyDecomposition(0); 01686 } else { 01687 ROS_WARN_STREAM("Don't have object named " << object_names[i]); 01688 continue; 01689 } 01690 bodies::BoundingCylinder cyl; 01691 bsd->getBody()->computeBoundingCylinder(cyl); 01692 mark.scale.x = cyl.radius*2.0; 01693 mark.scale.y = cyl.radius*2.0; 01694 mark.scale.z = cyl.length; 01695 mark.pose.position.x = cyl.pose.getOrigin().x(); 01696 mark.pose.position.y = cyl.pose.getOrigin().y(); 01697 mark.pose.position.z = cyl.pose.getOrigin().z(); 01698 mark.pose.orientation.x = cyl.pose.getRotation().x(); 01699 mark.pose.orientation.y = cyl.pose.getRotation().y(); 01700 mark.pose.orientation.z = cyl.pose.getRotation().z(); 01701 mark.pose.orientation.w = cyl.pose.getRotation().w(); 01702 01703 arr.markers.push_back(mark); 01704 } 01705 vis_marker_array_publisher_.publish(arr); 01706 }