00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_
00038 #define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_
00039
00040 #include <moveit/collision_detection_fcl/collision_common.h>
00041
00042 namespace collision_detection
00043 {
00044 class CollisionRobotFCL : public CollisionRobot
00045 {
00046 friend class CollisionWorldFCL;
00047
00048 public:
00049 CollisionRobotFCL(const robot_model::RobotModelConstPtr& kmodel, double padding = 0.0, double scale = 1.0);
00050
00051 CollisionRobotFCL(const CollisionRobotFCL& other);
00052
00053 virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00054 const robot_state::RobotState& state) const;
00055 virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00056 const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
00057 virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00058 const robot_state::RobotState& state1, const robot_state::RobotState& state2) const;
00059 virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
00060 const robot_state::RobotState& state1, const robot_state::RobotState& state2,
00061 const AllowedCollisionMatrix& acm) const;
00062
00063 virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00064 const robot_state::RobotState& state, const CollisionRobot& other_robot,
00065 const robot_state::RobotState& other_state) const;
00066 virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00067 const robot_state::RobotState& state, const CollisionRobot& other_robot,
00068 const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const;
00069 virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00070 const robot_state::RobotState& state1, const robot_state::RobotState& state2,
00071 const CollisionRobot& other_robot, const robot_state::RobotState& other_state1,
00072 const robot_state::RobotState& other_state2) const;
00073 virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
00074 const robot_state::RobotState& state1, const robot_state::RobotState& state2,
00075 const CollisionRobot& other_robot, const robot_state::RobotState& other_state1,
00076 const robot_state::RobotState& other_state2,
00077 const AllowedCollisionMatrix& acm) const;
00078
00079 virtual double distanceSelf(const robot_state::RobotState& state) const;
00080 virtual double distanceSelf(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
00081 virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot,
00082 const robot_state::RobotState& other_state) const;
00083 virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot,
00084 const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const;
00085
00086 protected:
00087 virtual void updatedPaddingOrScaling(const std::vector<std::string>& links);
00088 void constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const;
00089 void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const;
00090 void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector<FCLGeometryConstPtr>& geoms) const;
00091
00092 void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
00093 const AllowedCollisionMatrix* acm) const;
00094 void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res,
00095 const robot_state::RobotState& state, const CollisionRobot& other_robot,
00096 const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const;
00097 double distanceSelfHelper(const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const;
00098 double distanceOtherHelper(const robot_state::RobotState& state, const CollisionRobot& other_robot,
00099 const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const;
00100
00101 std::vector<FCLGeometryConstPtr> geoms_;
00102 std::vector<FCLCollisionObjectConstPtr> fcl_objs_;
00103 };
00104 }
00105
00106 #endif