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