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 #ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_
00038 #define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_
00039
00040 #include <moveit/collision_detection/world.h>
00041 #include <moveit/collision_detection/collision_world.h>
00042 #include <moveit/macros/class_forward.h>
00043 #include <fcl/broadphase/broadphase.h>
00044 #include <fcl/collision.h>
00045 #include <fcl/distance.h>
00046 #include <set>
00047
00048 namespace collision_detection
00049 {
00050 MOVEIT_CLASS_FORWARD(CollisionGeometryData);
00051
00052 struct CollisionGeometryData
00053 {
00054 CollisionGeometryData(const robot_model::LinkModel* link, int index) : type(BodyTypes::ROBOT_LINK), shape_index(index)
00055 {
00056 ptr.link = link;
00057 }
00058
00059 CollisionGeometryData(const robot_state::AttachedBody* ab, int index)
00060 : type(BodyTypes::ROBOT_ATTACHED), shape_index(index)
00061 {
00062 ptr.ab = ab;
00063 }
00064
00065 CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index)
00066 {
00067 ptr.obj = obj;
00068 }
00069
00070 const std::string& getID() const
00071 {
00072 switch (type)
00073 {
00074 case BodyTypes::ROBOT_LINK:
00075 return ptr.link->getName();
00076 case BodyTypes::ROBOT_ATTACHED:
00077 return ptr.ab->getName();
00078 default:
00079 break;
00080 }
00081 return ptr.obj->id_;
00082 }
00083
00084 std::string getTypeString() const
00085 {
00086 switch (type)
00087 {
00088 case BodyTypes::ROBOT_LINK:
00089 return "Robot link";
00090 case BodyTypes::ROBOT_ATTACHED:
00091 return "Robot attached";
00092 default:
00093 break;
00094 }
00095 return "Object";
00096 }
00097
00099 bool sameObject(const CollisionGeometryData& other) const
00100 {
00101 return type == other.type && ptr.raw == other.ptr.raw;
00102 }
00103
00104 BodyType type;
00105 int shape_index;
00106 union
00107 {
00108 const robot_model::LinkModel* link;
00109 const robot_state::AttachedBody* ab;
00110 const World::Object* obj;
00111 const void* raw;
00112 } ptr;
00113 };
00114
00115 struct CollisionData
00116 {
00117 CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false)
00118 {
00119 }
00120
00121 CollisionData(const CollisionRequest* req, CollisionResult* res, const AllowedCollisionMatrix* acm)
00122 : req_(req), active_components_only_(NULL), res_(res), acm_(acm), done_(false)
00123 {
00124 }
00125
00126 ~CollisionData()
00127 {
00128 }
00129
00131 void enableGroup(const robot_model::RobotModelConstPtr& kmodel);
00132
00134 const CollisionRequest* req_;
00135
00139 const std::set<const robot_model::LinkModel*>* active_components_only_;
00140
00142 CollisionResult* res_;
00143
00145 const AllowedCollisionMatrix* acm_;
00146
00148 bool done_;
00149 };
00150
00151 MOVEIT_CLASS_FORWARD(FCLGeometry);
00152
00153 struct FCLGeometry
00154 {
00155 FCLGeometry()
00156 {
00157 }
00158
00159 FCLGeometry(fcl::CollisionGeometry* collision_geometry, const robot_model::LinkModel* link, int shape_index)
00160 : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index))
00161 {
00162 collision_geometry_->setUserData(collision_geometry_data_.get());
00163 }
00164
00165 FCLGeometry(fcl::CollisionGeometry* collision_geometry, const robot_state::AttachedBody* ab, int shape_index)
00166 : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index))
00167 {
00168 collision_geometry_->setUserData(collision_geometry_data_.get());
00169 }
00170
00171 FCLGeometry(fcl::CollisionGeometry* collision_geometry, const World::Object* obj, int shape_index)
00172 : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index))
00173 {
00174 collision_geometry_->setUserData(collision_geometry_data_.get());
00175 }
00176
00177 template <typename T>
00178 void updateCollisionGeometryData(const T* data, int shape_index, bool newType)
00179 {
00180 if (!newType && collision_geometry_data_)
00181 if (collision_geometry_data_->ptr.raw == reinterpret_cast<const void*>(data))
00182 return;
00183 collision_geometry_data_.reset(new CollisionGeometryData(data, shape_index));
00184 collision_geometry_->setUserData(collision_geometry_data_.get());
00185 }
00186
00187 boost::shared_ptr<fcl::CollisionGeometry> collision_geometry_;
00188 boost::shared_ptr<CollisionGeometryData> collision_geometry_data_;
00189 };
00190
00191 typedef boost::shared_ptr<fcl::CollisionObject> FCLCollisionObjectPtr;
00192 typedef boost::shared_ptr<const fcl::CollisionObject> FCLCollisionObjectConstPtr;
00193
00194 struct FCLObject
00195 {
00196 void registerTo(fcl::BroadPhaseCollisionManager* manager);
00197 void unregisterFrom(fcl::BroadPhaseCollisionManager* manager);
00198 void clear();
00199
00200 std::vector<FCLCollisionObjectPtr> collision_objects_;
00201 std::vector<FCLGeometryConstPtr> collision_geometry_;
00202 };
00203
00204 struct FCLManager
00205 {
00206 FCLObject object_;
00207 boost::shared_ptr<fcl::BroadPhaseCollisionManager> manager_;
00208 };
00209
00210 bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data);
00211
00212 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist);
00213
00214 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
00215 int shape_index);
00216 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
00217 int shape_index);
00218 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj);
00219
00220 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00221 const robot_model::LinkModel* link, int shape_index);
00222 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00223 const robot_state::AttachedBody* ab, int shape_index);
00224 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00225 const World::Object* obj);
00226 void cleanCollisionGeometryCache();
00227
00228 inline void transform2fcl(const Eigen::Affine3d& b, fcl::Transform3f& f)
00229 {
00230 Eigen::Quaterniond q(b.rotation());
00231 f.setTranslation(fcl::Vec3f(b.translation().x(), b.translation().y(), b.translation().z()));
00232 f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
00233 }
00234
00235 inline fcl::Transform3f transform2fcl(const Eigen::Affine3d& b)
00236 {
00237 fcl::Transform3f t;
00238 transform2fcl(b, t);
00239 return t;
00240 }
00241
00242 inline void fcl2contact(const fcl::Contact& fc, Contact& c)
00243 {
00244 c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]);
00245 c.normal = Eigen::Vector3d(fc.normal[0], fc.normal[1], fc.normal[2]);
00246 c.depth = fc.penetration_depth;
00247 const CollisionGeometryData* cgd1 = static_cast<const CollisionGeometryData*>(fc.o1->getUserData());
00248 c.body_name_1 = cgd1->getID();
00249 c.body_type_1 = cgd1->type;
00250 const CollisionGeometryData* cgd2 = static_cast<const CollisionGeometryData*>(fc.o2->getUserData());
00251 c.body_name_2 = cgd2->getID();
00252 c.body_type_2 = cgd2->type;
00253 }
00254
00255 inline void fcl2costsource(const fcl::CostSource& fcs, CostSource& cs)
00256 {
00257 cs.aabb_min[0] = fcs.aabb_min[0];
00258 cs.aabb_min[1] = fcs.aabb_min[1];
00259 cs.aabb_min[2] = fcs.aabb_min[2];
00260
00261 cs.aabb_max[0] = fcs.aabb_max[0];
00262 cs.aabb_max[1] = fcs.aabb_max[1];
00263 cs.aabb_max[2] = fcs.aabb_max[2];
00264
00265 cs.cost = fcs.cost_density;
00266 }
00267 }
00268
00269 #endif