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 <moveit/collision_detection_fcl/collision_robot_fcl.h>
00038
00039 collision_detection::CollisionRobotFCL::CollisionRobotFCL(const robot_model::RobotModelConstPtr &kmodel, double padding, double scale) : CollisionRobot(kmodel, padding, scale)
00040 {
00041 links_ = kmodel_->getLinkModels();
00042
00043
00044 for (std::size_t i = 0 ; i < links_.size() ; ++i)
00045 if (links_[i] && links_[i]->getShape())
00046 {
00047 FCLGeometryConstPtr g = createCollisionGeometry(links_[i]->getShape(), getLinkScale(links_[i]->getName()), getLinkPadding(links_[i]->getName()), links_[i]);
00048 if (g)
00049 index_map_[links_[i]->getName()] = geoms_.size();
00050 else
00051 links_[i] = NULL;
00052 geoms_.push_back(g);
00053 }
00054 else
00055 {
00056 links_[i] = NULL;
00057 geoms_.push_back(FCLGeometryConstPtr());
00058 }
00059 }
00060
00061 collision_detection::CollisionRobotFCL::CollisionRobotFCL(const CollisionRobotFCL &other) : CollisionRobot(other)
00062 {
00063 links_ = other.links_;
00064 geoms_ = other.geoms_;
00065 index_map_ = other.index_map_;
00066 }
00067
00068 void collision_detection::CollisionRobotFCL::getAttachedBodyObjects(const robot_state::AttachedBody *ab,
00069 std::vector<FCLGeometryConstPtr> &geoms) const
00070 {
00071 const std::vector<shapes::ShapeConstPtr> &shapes = ab->getShapes();
00072 for (std::size_t i = 0 ; i < shapes.size() ; ++i)
00073 {
00074 FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab);
00075 if (co)
00076 geoms.push_back(co);
00077 }
00078 }
00079
00080 void collision_detection::CollisionRobotFCL::constructFCLObject(const robot_state::RobotState &state, FCLObject &fcl_obj) const
00081 {
00082 const std::vector<robot_state::LinkState*> &link_states = state.getLinkStateVector();
00083 fcl_obj.collision_objects_.reserve(geoms_.size());
00084
00085 for (std::size_t i = 0 ; i < geoms_.size() ; ++i)
00086 {
00087 if (geoms_[i] && geoms_[i]->collision_geometry_)
00088 {
00089 fcl::CollisionObject *collObj = new fcl::CollisionObject(geoms_[i]->collision_geometry_, transform2fcl(link_states[i]->getGlobalCollisionBodyTransform()));
00090 fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
00091
00092 }
00093 std::vector<const robot_state::AttachedBody*> ab;
00094 link_states[i]->getAttachedBodies(ab);
00095 for (std::size_t j = 0 ; j < ab.size() ; ++j)
00096 {
00097 std::vector<FCLGeometryConstPtr> objs;
00098 getAttachedBodyObjects(ab[j], objs);
00099 const EigenSTL::vector_Affine3d &ab_t = ab[j]->getGlobalCollisionBodyTransforms();
00100 for (std::size_t k = 0 ; k < objs.size() ; ++k)
00101 if (objs[k]->collision_geometry_)
00102 {
00103 fcl::CollisionObject *collObj = new fcl::CollisionObject(objs[k]->collision_geometry_, transform2fcl(ab_t[k]));
00104 fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
00105
00106
00107 fcl_obj.collision_geometry_.push_back(objs[k]);
00108 }
00109 }
00110 }
00111 }
00112
00113 void collision_detection::CollisionRobotFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState &state, FCLManager &manager) const
00114 {
00115 fcl::DynamicAABBTreeCollisionManager* m = new fcl::DynamicAABBTreeCollisionManager();
00116
00117 manager.manager_.reset(m);
00118 constructFCLObject(state, manager.object_);
00119 manager.object_.registerTo(manager.manager_.get());
00120
00121 }
00122
00123 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state) const
00124 {
00125 checkSelfCollisionHelper(req, res, state, NULL);
00126 }
00127
00128 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
00129 const AllowedCollisionMatrix &acm) const
00130 {
00131 checkSelfCollisionHelper(req, res, state, &acm);
00132 }
00133
00134 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
00135 {
00136 logError("FCL continuous collision checking not yet implemented");
00137 }
00138
00139 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
00140 {
00141 logError("FCL continuous collision checking not yet implemented");
00142 }
00143
00144 void collision_detection::CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
00145 const AllowedCollisionMatrix *acm) const
00146 {
00147 FCLManager manager;
00148 allocSelfCollisionBroadPhase(state, manager);
00149 CollisionData cd(&req, &res, acm);
00150 cd.enableGroup(getRobotModel());
00151 manager.manager_->collide(&cd, &collisionCallback);
00152 if (req.distance)
00153 res.distance = distanceSelfHelper(state, acm);
00154 }
00155
00156 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
00157 const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const
00158 {
00159 checkOtherCollisionHelper(req, res, state, other_robot, other_state, NULL);
00160 }
00161
00162 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
00163 const CollisionRobot &other_robot, const robot_state::RobotState &other_state,
00164 const AllowedCollisionMatrix &acm) const
00165 {
00166 checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm);
00167 }
00168
00169 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,
00170 const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2) const
00171 {
00172 logError("FCL continuous collision checking not yet implemented");
00173 }
00174
00175 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,
00176 const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2,
00177 const AllowedCollisionMatrix &acm) const
00178 {
00179 logError("FCL continuous collision checking not yet implemented");
00180 }
00181
00182 void collision_detection::CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state,
00183 const CollisionRobot &other_robot, const robot_state::RobotState &other_state,
00184 const AllowedCollisionMatrix *acm) const
00185 {
00186 FCLManager manager;
00187 allocSelfCollisionBroadPhase(state, manager);
00188
00189 const CollisionRobotFCL &fcl_rob = dynamic_cast<const CollisionRobotFCL&>(other_robot);
00190 FCLObject other_fcl_obj;
00191 fcl_rob.constructFCLObject(other_state, other_fcl_obj);
00192
00193 CollisionData cd(&req, &res, acm);
00194 cd.enableGroup(getRobotModel());
00195 for (std::size_t i = 0 ; !cd.done_ && i < other_fcl_obj.collision_objects_.size() ; ++i)
00196 manager.manager_->collide(other_fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
00197 if (req.distance)
00198 res.distance = distanceOtherHelper(state, other_robot, other_state, acm);
00199 }
00200
00201 void collision_detection::CollisionRobotFCL::updatedPaddingOrScaling(const std::vector<std::string> &links)
00202 {
00203 for (std::size_t i = 0 ; i < links.size() ; ++i)
00204 {
00205 std::map<std::string, std::size_t>::const_iterator it = index_map_.find(links[i]);
00206 const robot_model::LinkModel *lmodel = kmodel_->getLinkModel(links[i]);
00207 if (it != index_map_.end() && lmodel)
00208 {
00209 FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShape(), getLinkScale(links[i]), getLinkPadding(links[i]), lmodel);
00210 geoms_[it->second] = g;
00211 }
00212 else
00213 logError("Updating padding or scaling for unknown link: '%s'", links[i].c_str());
00214 }
00215 }
00216
00217 double collision_detection::CollisionRobotFCL::distanceSelf(const robot_state::RobotState &state) const
00218 {
00219 return distanceSelfHelper(state, NULL);
00220 }
00221
00222 double collision_detection::CollisionRobotFCL::distanceSelf(const robot_state::RobotState &state,
00223 const AllowedCollisionMatrix &acm) const
00224 {
00225 return distanceSelfHelper(state, &acm);
00226 }
00227
00228 double collision_detection::CollisionRobotFCL::distanceSelfHelper(const robot_state::RobotState &state,
00229 const AllowedCollisionMatrix *acm) const
00230 {
00231 FCLManager manager;
00232 allocSelfCollisionBroadPhase(state, manager);
00233
00234 CollisionRequest req;
00235 CollisionResult res;
00236 CollisionData cd(&req, &res, acm);
00237 cd.enableGroup(getRobotModel());
00238
00239 manager.manager_->distance(&cd, &distanceCallback);
00240
00241 return res.distance;
00242 }
00243
00244 double collision_detection::CollisionRobotFCL::distanceOther(const robot_state::RobotState &state,
00245 const CollisionRobot &other_robot,
00246 const robot_state::RobotState &other_state) const
00247 {
00248 return distanceOtherHelper(state, other_robot, other_state, NULL);
00249 }
00250
00251 double collision_detection::CollisionRobotFCL::distanceOther(const robot_state::RobotState &state,
00252 const CollisionRobot &other_robot,
00253 const robot_state::RobotState &other_state,
00254 const AllowedCollisionMatrix &acm) const
00255 {
00256 return distanceOtherHelper(state, other_robot, other_state, &acm);
00257 }
00258
00259 double collision_detection::CollisionRobotFCL::distanceOtherHelper(const robot_state::RobotState &state,
00260 const CollisionRobot &other_robot,
00261 const robot_state::RobotState &other_state,
00262 const AllowedCollisionMatrix *acm) const
00263 {
00264 FCLManager manager;
00265 allocSelfCollisionBroadPhase(state, manager);
00266
00267 const CollisionRobotFCL& fcl_rob = dynamic_cast<const CollisionRobotFCL&>(other_robot);
00268 FCLObject other_fcl_obj;
00269 fcl_rob.constructFCLObject(other_state, other_fcl_obj);
00270
00271 CollisionRequest req;
00272 CollisionResult res;
00273 CollisionData cd(&req, &res, acm);
00274 cd.enableGroup(getRobotModel());
00275 for(std::size_t i = 0; !cd.done_ && i < other_fcl_obj.collision_objects_.size(); ++i)
00276 manager.manager_->distance(other_fcl_obj.collision_objects_[i].get(), &cd, &distanceCallback);
00277
00278 return res.distance;
00279 }