collision_robot_fcl.cpp
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 #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   // we keep the same order of objects as what RobotState *::getLinkState() returns
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       // the CollisionGeometryData is already stored in the class member geoms_, so we need not copy it
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         // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
00098         // and would be destroyed when objs goes out of scope.
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   // m->tree_init_level = 2;
00108   manager.manager_.reset(m);
00109   constructFCLObject(state, manager.object_);
00110   manager.object_.registerTo(manager.manager_.get());
00111   // manager.manager_->update();
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:52