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 #include <moveit/collision_detection/collision_world.h>
00038 #include <geometric_shapes/shape_operations.h>
00039
00040 collision_detection::CollisionWorld::CollisionWorld() :
00041 world_(new World()),
00042 world_const_(world_)
00043 {
00044 }
00045
00046 collision_detection::CollisionWorld::CollisionWorld(const WorldPtr& world) :
00047 world_(world),
00048 world_const_(world)
00049 {
00050 }
00051
00052 collision_detection::CollisionWorld::CollisionWorld(const CollisionWorld &other, const WorldPtr& world) :
00053 world_(world),
00054 world_const_(world)
00055 {
00056 }
00057
00058 void collision_detection::CollisionWorld::checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
00059 {
00060 robot.checkSelfCollision(req, res, state);
00061 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00062 checkRobotCollision(req, res, robot, state);
00063 }
00064
00065 void collision_detection::CollisionWorld::checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const
00066 {
00067 robot.checkSelfCollision(req, res, state, acm);
00068 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00069 checkRobotCollision(req, res, robot, state, acm);
00070 }
00071
00072 void collision_detection::CollisionWorld::checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot,
00073 const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
00074 {
00075 robot.checkSelfCollision(req, res, state1, state2);
00076 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00077 checkRobotCollision(req, res, robot, state1, state2);
00078 }
00079
00080 void collision_detection::CollisionWorld::checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot,
00081 const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
00082 {
00083 robot.checkSelfCollision(req, res, state1, state2, acm);
00084 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00085 checkRobotCollision(req, res, robot, state1, state2, acm);
00086 }
00087
00088 void collision_detection::CollisionWorld::setWorld(const WorldPtr& world)
00089 {
00090 world_ = world;
00091 if (!world_)
00092 world_.reset(new World());
00093
00094 world_const_ = world;
00095 }