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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:46