void collision_detection::CollisionRobotFCL::getAttachedBodyObjects(const planning_models::KinematicState::AttachedBody *ab, std::vector<FCLGeometryConstPtr> &geoms) const { const std::vector<shapes::ShapeConstPtr> &shapes = ab->getShapes(); for (std::size_t i = 0 ; i < shapes.size() ; ++i) { FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab); if (co) geoms.push_back(co); } }
void collision_detection::CollisionWorldFCL::constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const { for (std::size_t i = 0 ; i < obj->shapes_.size() ; ++i) { FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj); if (g) { fcl::CollisionObject *co = new fcl::CollisionObject(g->collision_geometry_, transform2fcl(obj->shape_poses_[i])); fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(co)); fcl_obj.collision_geometry_.push_back(g); } } }
void collision_detection::CollisionRobotFCL::updatedPaddingOrScaling(const std::vector<std::string> &links) { for (std::size_t i = 0 ; i < links.size() ; ++i) { std::map<std::string, std::size_t>::const_iterator it = index_map_.find(links[i]); const planning_models::KinematicModel::LinkModel *lmodel = kmodel_->getLinkModel(links[i]); if (it != index_map_.end() && lmodel) { FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShape(), getLinkScale(links[i]), getLinkPadding(links[i]), lmodel); geoms_[it->second] = g; } else ROS_ERROR("Updating padding or scaling for unknown link: '%s'", links[i].c_str()); } }
collision_detection::CollisionRobotFCL::CollisionRobotFCL(const planning_models::KinematicModelConstPtr &kmodel, double padding, double scale) : CollisionRobot(kmodel, padding, scale) { links_ = kmodel_->getLinkModels(); // we keep the same order of objects as what KinematicState::getLinkState() returns for (std::size_t i = 0 ; i < links_.size() ; ++i) if (links_[i] && links_[i]->getShape()) { FCLGeometryConstPtr g = createCollisionGeometry(links_[i]->getShape(), getLinkScale(links_[i]->getName()), getLinkPadding(links_[i]->getName()), links_[i]); if (g) index_map_[links_[i]->getName()] = geoms_.size(); else links_[i] = NULL; geoms_.push_back(g); } else { links_[i] = NULL; geoms_.push_back(FCLGeometryConstPtr()); } }