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,
00040                                                           double scale)
00041   : CollisionRobot(model, padding, scale)
00042 {
00043   const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
00044   std::size_t index;
00045   geoms_.resize(robot_model_->getLinkGeometryCount());
00046   fcl_objs_.resize(robot_model_->getLinkGeometryCount());
00047   // we keep the same order of objects as what RobotState *::getLinkState() returns
00048   for (std::size_t i = 0; i < links.size(); ++i)
00049     for (std::size_t j = 0; j < links[i]->getShapes().size(); ++j)
00050     {
00051       FCLGeometryConstPtr g = createCollisionGeometry(links[i]->getShapes()[j], getLinkScale(links[i]->getName()),
00052                                                       getLinkPadding(links[i]->getName()), links[i], j);
00053       if (g)
00054       {
00055         index = links[i]->getFirstCollisionBodyTransformIndex() + j;
00056         geoms_[index] = g;
00057 
00058         // Need to store the FCL object so the AABB does not get recreated every time.
00059         // Every time this object is created, g->computeLocalAABB() is called  which is
00060         // very expensive and should only be calculated once. To update the AABB, use the
00061         // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
00062         fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObject(g->collision_geometry_));
00063       }
00064       else
00065         logError("Unable to construct collision geometry for link '%s'", links[i]->getName().c_str());
00066     }
00067 }
00068 
00069 collision_detection::CollisionRobotFCL::CollisionRobotFCL(const CollisionRobotFCL& other) : CollisionRobot(other)
00070 {
00071   geoms_ = other.geoms_;
00072   fcl_objs_ = other.fcl_objs_;
00073 }
00074 
00075 void collision_detection::CollisionRobotFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab,
00076                                                                     std::vector<FCLGeometryConstPtr>& geoms) const
00077 {
00078   const std::vector<shapes::ShapeConstPtr>& shapes = ab->getShapes();
00079   for (std::size_t i = 0; i < shapes.size(); ++i)
00080   {
00081     FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i);
00082     if (co)
00083       geoms.push_back(co);
00084   }
00085 }
00086 
00087 void collision_detection::CollisionRobotFCL::constructFCLObject(const robot_state::RobotState& state,
00088                                                                 FCLObject& fcl_obj) const
00089 {
00090   fcl_obj.collision_objects_.reserve(geoms_.size());
00091   fcl::Transform3f fcl_tf;
00092 
00093   for (std::size_t i = 0; i < geoms_.size(); ++i)
00094     if (geoms_[i] && geoms_[i]->collision_geometry_)
00095     {
00096       transform2fcl(state.getCollisionBodyTransform(geoms_[i]->collision_geometry_data_->ptr.link,
00097                                                     geoms_[i]->collision_geometry_data_->shape_index),
00098                     fcl_tf);
00099       fcl::CollisionObject* collObj = new fcl::CollisionObject(*fcl_objs_[i]);
00100       collObj->setTransform(fcl_tf);
00101       collObj->computeAABB();
00102       fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(collObj));
00103     }
00104 
00105   // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's
00106   std::vector<const robot_state::AttachedBody*> ab;
00107   state.getAttachedBodies(ab);
00108   for (std::size_t j = 0; j < ab.size(); ++j)
00109   {
00110     std::vector<FCLGeometryConstPtr> objs;
00111     getAttachedBodyObjects(ab[j], objs);
00112     const EigenSTL::vector_Affine3d& ab_t = ab[j]->getGlobalCollisionBodyTransforms();
00113     for (std::size_t k = 0; k < objs.size(); ++k)
00114       if (objs[k]->collision_geometry_)
00115       {
00116         transform2fcl(ab_t[k], fcl_tf);
00117         fcl_obj.collision_objects_.push_back(
00118             FCLCollisionObjectPtr(new fcl::CollisionObject(objs[k]->collision_geometry_, fcl_tf)));
00119         // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
00120         // and would be destroyed when objs goes out of scope.
00121         fcl_obj.collision_geometry_.push_back(objs[k]);
00122       }
00123   }
00124 }
00125 
00126 void collision_detection::CollisionRobotFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state,
00127                                                                           FCLManager& manager) const
00128 {
00129   fcl::DynamicAABBTreeCollisionManager* m = new fcl::DynamicAABBTreeCollisionManager();
00130   // m->tree_init_level = 2;
00131   manager.manager_.reset(m);
00132   constructFCLObject(state, manager.object_);
00133   manager.object_.registerTo(manager.manager_.get());
00134   // manager.manager_->update();
00135 }
00136 
00137 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00138                                                                 const robot_state::RobotState& state) const
00139 {
00140   checkSelfCollisionHelper(req, res, state, NULL);
00141 }
00142 
00143 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00144                                                                 const robot_state::RobotState& state,
00145                                                                 const AllowedCollisionMatrix& acm) const
00146 {
00147   checkSelfCollisionHelper(req, res, state, &acm);
00148 }
00149 
00150 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00151                                                                 const robot_state::RobotState& state1,
00152                                                                 const robot_state::RobotState& state2) const
00153 {
00154   logError("FCL continuous collision checking not yet implemented");
00155 }
00156 
00157 void collision_detection::CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00158                                                                 const robot_state::RobotState& state1,
00159                                                                 const robot_state::RobotState& state2,
00160                                                                 const AllowedCollisionMatrix& acm) const
00161 {
00162   logError("FCL continuous collision checking not yet implemented");
00163 }
00164 
00165 void collision_detection::CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
00166                                                                       const robot_state::RobotState& state,
00167                                                                       const AllowedCollisionMatrix* acm) const
00168 {
00169   FCLManager manager;
00170   allocSelfCollisionBroadPhase(state, manager);
00171   CollisionData cd(&req, &res, acm);
00172   cd.enableGroup(getRobotModel());
00173   manager.manager_->collide(&cd, &collisionCallback);
00174   if (req.distance)
00175     res.distance = distanceSelfHelper(state, acm);
00176 }
00177 
00178 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00179                                                                  const robot_state::RobotState& state,
00180                                                                  const CollisionRobot& other_robot,
00181                                                                  const robot_state::RobotState& other_state) const
00182 {
00183   checkOtherCollisionHelper(req, res, state, other_robot, other_state, NULL);
00184 }
00185 
00186 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00187                                                                  const robot_state::RobotState& state,
00188                                                                  const CollisionRobot& other_robot,
00189                                                                  const robot_state::RobotState& other_state,
00190                                                                  const AllowedCollisionMatrix& acm) const
00191 {
00192   checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm);
00193 }
00194 
00195 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00196                                                                  const robot_state::RobotState& state1,
00197                                                                  const robot_state::RobotState& state2,
00198                                                                  const CollisionRobot& other_robot,
00199                                                                  const robot_state::RobotState& other_state1,
00200                                                                  const robot_state::RobotState& other_state2) const
00201 {
00202   logError("FCL continuous collision checking not yet implemented");
00203 }
00204 
00205 void collision_detection::CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00206                                                                  const robot_state::RobotState& state1,
00207                                                                  const robot_state::RobotState& state2,
00208                                                                  const CollisionRobot& other_robot,
00209                                                                  const robot_state::RobotState& other_state1,
00210                                                                  const robot_state::RobotState& other_state2,
00211                                                                  const AllowedCollisionMatrix& acm) const
00212 {
00213   logError("FCL continuous collision checking not yet implemented");
00214 }
00215 
00216 void collision_detection::CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest& req,
00217                                                                        CollisionResult& res,
00218                                                                        const robot_state::RobotState& state,
00219                                                                        const CollisionRobot& other_robot,
00220                                                                        const robot_state::RobotState& other_state,
00221                                                                        const AllowedCollisionMatrix* acm) const
00222 {
00223   FCLManager manager;
00224   allocSelfCollisionBroadPhase(state, manager);
00225 
00226   const CollisionRobotFCL& fcl_rob = dynamic_cast<const CollisionRobotFCL&>(other_robot);
00227   FCLObject other_fcl_obj;
00228   fcl_rob.constructFCLObject(other_state, other_fcl_obj);
00229 
00230   CollisionData cd(&req, &res, acm);
00231   cd.enableGroup(getRobotModel());
00232   for (std::size_t i = 0; !cd.done_ && i < other_fcl_obj.collision_objects_.size(); ++i)
00233     manager.manager_->collide(other_fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
00234   if (req.distance)
00235     res.distance = distanceOtherHelper(state, other_robot, other_state, acm);
00236 }
00237 
00238 void collision_detection::CollisionRobotFCL::updatedPaddingOrScaling(const std::vector<std::string>& links)
00239 {
00240   std::size_t index;
00241   for (std::size_t i = 0; i < links.size(); ++i)
00242   {
00243     const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(links[i]);
00244     if (lmodel)
00245     {
00246       for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j)
00247       {
00248         FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()),
00249                                                         getLinkPadding(lmodel->getName()), lmodel, j);
00250         if (g)
00251         {
00252           index = lmodel->getFirstCollisionBodyTransformIndex() + j;
00253           geoms_[index] = g;
00254           fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObject(g->collision_geometry_));
00255         }
00256       }
00257     }
00258     else
00259       logError("Updating padding or scaling for unknown link: '%s'", links[i].c_str());
00260   }
00261 }
00262 
00263 double collision_detection::CollisionRobotFCL::distanceSelf(const robot_state::RobotState& state) const
00264 {
00265   return distanceSelfHelper(state, NULL);
00266 }
00267 
00268 double collision_detection::CollisionRobotFCL::distanceSelf(const robot_state::RobotState& state,
00269                                                             const AllowedCollisionMatrix& acm) const
00270 {
00271   return distanceSelfHelper(state, &acm);
00272 }
00273 
00274 double collision_detection::CollisionRobotFCL::distanceSelfHelper(const robot_state::RobotState& state,
00275                                                                   const AllowedCollisionMatrix* acm) const
00276 {
00277   FCLManager manager;
00278   allocSelfCollisionBroadPhase(state, manager);
00279 
00280   CollisionRequest req;
00281   CollisionResult res;
00282   CollisionData cd(&req, &res, acm);
00283   cd.enableGroup(getRobotModel());
00284 
00285   manager.manager_->distance(&cd, &distanceCallback);
00286 
00287   return res.distance;
00288 }
00289 
00290 double collision_detection::CollisionRobotFCL::distanceOther(const robot_state::RobotState& state,
00291                                                              const CollisionRobot& other_robot,
00292                                                              const robot_state::RobotState& other_state) const
00293 {
00294   return distanceOtherHelper(state, other_robot, other_state, NULL);
00295 }
00296 
00297 double collision_detection::CollisionRobotFCL::distanceOther(const robot_state::RobotState& state,
00298                                                              const CollisionRobot& other_robot,
00299                                                              const robot_state::RobotState& other_state,
00300                                                              const AllowedCollisionMatrix& acm) const
00301 {
00302   return distanceOtherHelper(state, other_robot, other_state, &acm);
00303 }
00304 
00305 double collision_detection::CollisionRobotFCL::distanceOtherHelper(const robot_state::RobotState& state,
00306                                                                    const CollisionRobot& other_robot,
00307                                                                    const robot_state::RobotState& other_state,
00308                                                                    const AllowedCollisionMatrix* acm) const
00309 {
00310   FCLManager manager;
00311   allocSelfCollisionBroadPhase(state, manager);
00312 
00313   const CollisionRobotFCL& fcl_rob = dynamic_cast<const CollisionRobotFCL&>(other_robot);
00314   FCLObject other_fcl_obj;
00315   fcl_rob.constructFCLObject(other_state, other_fcl_obj);
00316 
00317   CollisionRequest req;
00318   CollisionResult res;
00319   CollisionData cd(&req, &res, acm);
00320   cd.enableGroup(getRobotModel());
00321   for (std::size_t i = 0; !cd.done_ && i < other_fcl_obj.collision_objects_.size(); ++i)
00322     manager.manager_->distance(other_fcl_obj.collision_objects_[i].get(), &cd, &distanceCallback);
00323 
00324   return res.distance;
00325 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49