collision_common.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:43