moveit_experimental: collision_world

Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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: E. Gil Jones */
00036 
00037 #ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_HYBRID_H_
00038 #define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_HYBRID_H_
00039 
00040 #include <moveit/collision_detection_fcl/collision_world_fcl.h>
00041 #include <moveit/collision_distance_field/collision_distance_field_types.h>
00042 #include <moveit/collision_distance_field/collision_world_distance_field.h>
00043 
00044 namespace collision_detection
00045 {
00046 class CollisionRobotHybrid;
00047 
00048 class CollisionWorldHybrid : public CollisionWorldFCL
00049 {
00050 public:
00051   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00052   CollisionWorldHybrid(Eigen::Vector3d size = Eigen::Vector3d(3, 3, 4),
00053                        Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field = false,
00054                        double resolution = .02, double collision_tolerance = 0.0,
00055                        double max_propogation_distance = .25);
00056 
00057   explicit CollisionWorldHybrid(const WorldPtr& world, Eigen::Vector3d size = Eigen::Vector3d(3, 3, 4),
00058                                 Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
00059                                 bool use_signed_distance_field = false, double resolution = .02,
00060                                 double collision_tolerance = 0.0, double max_propogation_distance = .25);
00061 
00062   CollisionWorldHybrid(const CollisionWorldHybrid& other, const WorldPtr& world);
00063 
00064   virtual ~CollisionWorldHybrid()
00065   {
00066   }
00067 
00068   void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00069                                    const robot_state::RobotState& state) const;
00070 
00071   void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00072                                    const robot_state::RobotState& state,
00073                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00074 
00075   void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00076                                    const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
00077 
00078   void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00079                                    const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
00080                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00081 
00082   void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00083                                         const robot_state::RobotState& state) const;
00084 
00085   void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00086                                         const robot_state::RobotState& state,
00087                                         boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00088 
00089   void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00090                                         const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
00091 
00092   void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00093                                         const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
00094                                         boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00095 
00096   virtual void setWorld(const WorldPtr& world);
00097 
00098   void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00099                              const robot_state::RobotState& state, const AllowedCollisionMatrix* acm,
00100                              boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00101 
00102   void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00103                         const robot_state::RobotState& state, const AllowedCollisionMatrix* acm,
00104                         boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00105 
00106   const boost::shared_ptr<const collision_detection::CollisionWorldDistanceField> getCollisionWorldDistanceField() const
00107   {
00108     return cworld_distance_;
00109   }
00110 
00111 protected:
00112   boost::shared_ptr<collision_detection::CollisionWorldDistanceField> cworld_distance_;
00113 };
00114 }
00115 
00116 #endif