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