$search
#include <collision_proximity_space.h>
Public Types | |
enum | TrajectoryPointType { StartCollision, EndCollision, Middle, None } |
Public Member Functions | |
CollisionProximitySpace (const std::string &robot_description_name, bool register_with_environment_server=true, bool use_signed_environment_field=false, bool use_signed_self_field=false) | |
planning_environment::CollisionModelsInterface * | getCollisionModelsInterface () const |
std::vector< std::string > | getCurrentAttachedBodyNames () const |
const std::string & | getCurrentGroupName () const |
std::vector< std::string > | getCurrentLinkNames () const |
bool | getEnvironmentCollisions (std::vector< bool > &collisions, bool stop_at_first=false) const |
bool | getEnvironmentProximityGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const |
bool | getIntraGroupCollisions (std::vector< bool > &collisions, bool stop_at_first=false) const |
bool | getIntraGroupProximityGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const |
btTransform | getInverseWorldTransform (const planning_models::KinematicState &state) const |
void | getProximityGradientMarkers (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, const std::vector< GradientInfo > &gradients, const std::string &ns, visualization_msgs::MarkerArray &arr) const |
bool | getSelfCollisions (std::vector< bool > &collisions, bool stop_at_first=false) const |
bool | getSelfProximityGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const |
bool | getStateCollisions (bool &in_collision, std::vector< CollisionType > &collisions) const |
bool | getStateGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const |
bool | isEnvironmentCollision () const |
bool | isIntraGroupCollision () const |
bool | isSelfCollision () const |
bool | isStateInCollision () const |
bool | isTrajectorySafe (const trajectory_msgs::JointTrajectory &trajectory, const arm_navigation_msgs::Constraints &goal_constraints, const arm_navigation_msgs::Constraints &path_constraints, const std::string &groupName) |
void | revertAfterGroupQueries () |
void | revertPlanningSceneCallback () |
void | setCollisionTolerance (double tol) |
void | setCurrentGroupState (const planning_models::KinematicState &state) |
bool | setPlanningScene (const arm_navigation_msgs::PlanningScene &planning_scene) |
void | setPlanningSceneCallback (const arm_navigation_msgs::PlanningScene &scene) |
void | setupForGroupQueries (const std::string &group_name, const arm_navigation_msgs::RobotState &rob_state, std::vector< std::string > &link_names, std::vector< std::string > &attached_body_names) |
void | visualizeBoundingCylinders (const std::vector< std::string > &object_names) const |
void | visualizeCollisions (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, const std::vector< CollisionType > collisions) const |
void | visualizeDistanceField (distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > *distance_field) const |
void | visualizeDistanceFieldPlane (distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > *distance_field) const |
void | visualizeObjectSpheres (const std::vector< std::string > &object_names) const |
void | visualizeObjectVoxels (const std::vector< std::string > &object_names) const |
~CollisionProximitySpace () | |
Private Member Functions | |
void | deleteAllAttachedObjectDecompositions () |
void | deleteAllStaticObjectDecompositions () |
bool | getGroupLinkAndAttachedBodyNames (const std::string &group_name, const planning_models::KinematicState &state, std::vector< std::string > &link_names, std::vector< unsigned int > &link_indices, std::vector< std::string > &attached_body_names, std::vector< unsigned int > &attached_body_link_indices) const |
void | loadDefaultCollisionOperations () |
void | loadRobotBodyDecompositions () |
void | prepareEnvironmentDistanceField (const planning_models::KinematicState &state) |
void | prepareSelfDistanceField (const std::vector< std::string > &link_names, const planning_models::KinematicState &state) |
void | setBodyPosesGivenKinematicState (const planning_models::KinematicState &state) |
void | setBodyPosesToCurrent () |
void | setDistanceFieldForGroupQueries (const std::string &group_name, const planning_models::KinematicState &state) |
bool | setupGradientStructures (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, std::vector< GradientInfo > &gradients) const |
void | syncObjectsWithCollisionSpace (const planning_models::KinematicState &state) |
bool | updateSphereLocations (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, std::vector< GradientInfo > &gradients) |
Private Attributes | |
std::map< std::string, std::map< std::string, bool > > | attached_object_collision_links_ |
std::map< std::string, BodyDecompositionVector * > | attached_object_map_ |
std::map< std::string, BodyDecomposition * > | body_decomposition_map_ |
planning_environment::CollisionModelsInterface * | collision_models_interface_ |
std::vector< std::vector < double > > | colors_ |
std::vector < BodyDecompositionVector * > | current_attached_body_decompositions_ |
std::vector< unsigned int > | current_attached_body_indices_ |
std::vector< std::string > | current_attached_body_names_ |
std::vector< GradientInfo > | current_gradients_ |
std::string | current_group_name_ |
std::vector< std::vector< bool > > | current_intra_group_collision_links_ |
std::vector< BodyDecomposition * > | current_link_body_decompositions_ |
std::vector< unsigned int > | current_link_indices_ |
std::vector< std::string > | current_link_names_ |
std::vector< bool > | current_self_excludes_ |
std::map< std::string, std::map< std::string, bool > > | enabled_self_collision_links_ |
distance_field::DistanceField < distance_field::PropDistanceFieldVoxel > * | environment_distance_field_ |
boost::recursive_mutex | group_queries_lock_ |
std::map< std::string, std::map< std::string, bool > > | intra_group_collision_links_ |
double | max_environment_distance_ |
double | max_self_distance_ |
double | origin_x_ |
double | origin_y_ |
double | origin_z_ |
ros::NodeHandle | priv_handle_ |
double | resolution_ |
ros::NodeHandle | root_handle_ |
distance_field::DistanceField < distance_field::PropDistanceFieldVoxel > * | self_distance_field_ |
std::map< std::string, bool > | self_excludes_ |
double | size_x_ |
double | size_y_ |
double | size_z_ |
std::map< std::string, BodyDecompositionVector * > | static_object_map_ |
double | tolerance_ |
double | undefined_distance_ |
ros::Publisher | vis_marker_array_publisher_ |
ros::Publisher | vis_marker_publisher_ |
Definition at line 58 of file collision_proximity_space.h.
Definition at line 63 of file collision_proximity_space.h.
CollisionProximitySpace::CollisionProximitySpace | ( | const std::string & | robot_description_name, | |
bool | register_with_environment_server = true , |
|||
bool | use_signed_environment_field = false , |
|||
bool | use_signed_self_field = false | |||
) |
Definition at line 64 of file collision_proximity_space.cpp.
CollisionProximitySpace::~CollisionProximitySpace | ( | ) |
Definition at line 123 of file collision_proximity_space.cpp.
void CollisionProximitySpace::deleteAllAttachedObjectDecompositions | ( | ) | [private] |
Definition at line 147 of file collision_proximity_space.cpp.
void CollisionProximitySpace::deleteAllStaticObjectDecompositions | ( | ) | [private] |
Definition at line 137 of file collision_proximity_space.cpp.
planning_environment::CollisionModelsInterface* collision_proximity::CollisionProximitySpace::getCollisionModelsInterface | ( | ) | const [inline] |
Definition at line 160 of file collision_proximity_space.h.
std::vector<std::string> collision_proximity::CollisionProximitySpace::getCurrentAttachedBodyNames | ( | ) | const [inline] |
Definition at line 175 of file collision_proximity_space.h.
const std::string& collision_proximity::CollisionProximitySpace::getCurrentGroupName | ( | ) | const [inline] |
Definition at line 166 of file collision_proximity_space.h.
std::vector<std::string> collision_proximity::CollisionProximitySpace::getCurrentLinkNames | ( | ) | const [inline] |
Definition at line 170 of file collision_proximity_space.h.
bool CollisionProximitySpace::getEnvironmentCollisions | ( | std::vector< bool > & | collisions, | |
bool | stop_at_first = false | |||
) | const |
Definition at line 1090 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getEnvironmentProximityGradients | ( | std::vector< GradientInfo > & | gradients, | |
bool | subtract_radii = false | |||
) | const |
Definition at line 1119 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getGroupLinkAndAttachedBodyNames | ( | const std::string & | group_name, | |
const planning_models::KinematicState & | state, | |||
std::vector< std::string > & | link_names, | |||
std::vector< unsigned int > & | link_indices, | |||
std::vector< std::string > & | attached_body_names, | |||
std::vector< unsigned int > & | attached_body_link_indices | |||
) | const [private] |
Definition at line 656 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getIntraGroupCollisions | ( | std::vector< bool > & | collisions, | |
bool | stop_at_first = false | |||
) | const |
Definition at line 924 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getIntraGroupProximityGradients | ( | std::vector< GradientInfo > & | gradients, | |
bool | subtract_radii = false | |||
) | const |
Definition at line 964 of file collision_proximity_space.cpp.
btTransform CollisionProximitySpace::getInverseWorldTransform | ( | const planning_models::KinematicState & | state | ) | const |
Definition at line 436 of file collision_proximity_space.cpp.
void CollisionProximitySpace::getProximityGradientMarkers | ( | const std::vector< std::string > & | link_names, | |
const std::vector< std::string > & | attached_body_names, | |||
const std::vector< GradientInfo > & | gradients, | |||
const std::string & | ns, | |||
visualization_msgs::MarkerArray & | arr | |||
) | const |
Definition at line 1143 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getSelfCollisions | ( | std::vector< bool > & | collisions, | |
bool | stop_at_first = false | |||
) | const |
Definition at line 1029 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getSelfProximityGradients | ( | std::vector< GradientInfo > & | gradients, | |
bool | subtract_radii = false | |||
) | const |
Definition at line 1058 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getStateCollisions | ( | bool & | in_collision, | |
std::vector< CollisionType > & | collisions | |||
) | const |
Definition at line 798 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::getStateGradients | ( | std::vector< GradientInfo > & | gradients, | |
bool | subtract_radii = false | |||
) | const |
Definition at line 822 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::isEnvironmentCollision | ( | void | ) | const |
Definition at line 1084 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::isIntraGroupCollision | ( | ) | const |
Definition at line 918 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::isSelfCollision | ( | void | ) | const |
Definition at line 1023 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::isStateInCollision | ( | ) | const |
Definition at line 791 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::isTrajectorySafe | ( | const trajectory_msgs::JointTrajectory & | trajectory, | |
const arm_navigation_msgs::Constraints & | goal_constraints, | |||
const arm_navigation_msgs::Constraints & | path_constraints, | |||
const std::string & | groupName | |||
) |
Definition at line 1203 of file collision_proximity_space.cpp.
void CollisionProximitySpace::loadDefaultCollisionOperations | ( | ) | [private] |
Definition at line 160 of file collision_proximity_space.cpp.
void CollisionProximitySpace::loadRobotBodyDecompositions | ( | ) | [private] |
Definition at line 248 of file collision_proximity_space.cpp.
void CollisionProximitySpace::prepareEnvironmentDistanceField | ( | const planning_models::KinematicState & | state | ) | [private] |
Definition at line 502 of file collision_proximity_space.cpp.
void CollisionProximitySpace::prepareSelfDistanceField | ( | const std::vector< std::string > & | link_names, | |
const planning_models::KinematicState & | state | |||
) | [private] |
Definition at line 522 of file collision_proximity_space.cpp.
void collision_proximity::CollisionProximitySpace::revertAfterGroupQueries | ( | ) |
void CollisionProximitySpace::revertPlanningSceneCallback | ( | ) |
Definition at line 371 of file collision_proximity_space.cpp.
void CollisionProximitySpace::setBodyPosesGivenKinematicState | ( | const planning_models::KinematicState & | state | ) | [private] |
Definition at line 406 of file collision_proximity_space.cpp.
void collision_proximity::CollisionProximitySpace::setBodyPosesToCurrent | ( | ) | [private] |
void collision_proximity::CollisionProximitySpace::setCollisionTolerance | ( | double | tol | ) | [inline] |
Definition at line 180 of file collision_proximity_space.h.
void CollisionProximitySpace::setCurrentGroupState | ( | const planning_models::KinematicState & | state | ) |
Definition at line 381 of file collision_proximity_space.cpp.
void CollisionProximitySpace::setDistanceFieldForGroupQueries | ( | const std::string & | group_name, | |
const planning_models::KinematicState & | state | |||
) | [private] |
Definition at line 484 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::setPlanningScene | ( | const arm_navigation_msgs::PlanningScene & | planning_scene | ) |
Definition at line 269 of file collision_proximity_space.cpp.
void CollisionProximitySpace::setPlanningSceneCallback | ( | const arm_navigation_msgs::PlanningScene & | scene | ) |
Definition at line 273 of file collision_proximity_space.cpp.
void CollisionProximitySpace::setupForGroupQueries | ( | const std::string & | group_name, | |
const arm_navigation_msgs::RobotState & | rob_state, | |||
std::vector< std::string > & | link_names, | |||
std::vector< std::string > & | attached_body_names | |||
) |
Definition at line 286 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::setupGradientStructures | ( | const std::vector< std::string > & | link_names, | |
const std::vector< std::string > & | attached_body_names, | |||
std::vector< GradientInfo > & | gradients | |||
) | const [private] |
Definition at line 745 of file collision_proximity_space.cpp.
void CollisionProximitySpace::syncObjectsWithCollisionSpace | ( | const planning_models::KinematicState & | state | ) | [private] |
Definition at line 446 of file collision_proximity_space.cpp.
bool CollisionProximitySpace::updateSphereLocations | ( | const std::vector< std::string > & | link_names, | |
const std::vector< std::string > & | attached_body_names, | |||
std::vector< GradientInfo > & | gradients | |||
) | [private] |
Definition at line 710 of file collision_proximity_space.cpp.
void CollisionProximitySpace::visualizeBoundingCylinders | ( | const std::vector< std::string > & | object_names | ) | const |
Definition at line 1667 of file collision_proximity_space.cpp.
void CollisionProximitySpace::visualizeCollisions | ( | const std::vector< std::string > & | link_names, | |
const std::vector< std::string > & | attached_body_names, | |||
const std::vector< CollisionType > | collisions | |||
) | const |
Definition at line 1484 of file collision_proximity_space.cpp.
void CollisionProximitySpace::visualizeDistanceField | ( | distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > * | distance_field | ) | const |
Definition at line 1371 of file collision_proximity_space.cpp.
void CollisionProximitySpace::visualizeDistanceFieldPlane | ( | distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > * | distance_field | ) | const |
Definition at line 1378 of file collision_proximity_space.cpp.
void CollisionProximitySpace::visualizeObjectSpheres | ( | const std::vector< std::string > & | object_names | ) | const |
Definition at line 1617 of file collision_proximity_space.cpp.
void CollisionProximitySpace::visualizeObjectVoxels | ( | const std::vector< std::string > & | object_names | ) | const |
Definition at line 1581 of file collision_proximity_space.cpp.
std::map<std::string, std::map<std::string, bool> > collision_proximity::CollisionProximitySpace::attached_object_collision_links_ [private] |
Definition at line 255 of file collision_proximity_space.h.
std::map<std::string, BodyDecompositionVector*> collision_proximity::CollisionProximitySpace::attached_object_map_ [private] |
Definition at line 251 of file collision_proximity_space.h.
std::map<std::string, BodyDecomposition*> collision_proximity::CollisionProximitySpace::body_decomposition_map_ [private] |
Definition at line 249 of file collision_proximity_space.h.
planning_environment::CollisionModelsInterface* collision_proximity::CollisionProximitySpace::collision_models_interface_ [private] |
Definition at line 240 of file collision_proximity_space.h.
std::vector<std::vector<double> > collision_proximity::CollisionProximitySpace::colors_ [mutable, private] |
Definition at line 235 of file collision_proximity_space.h.
std::vector<BodyDecompositionVector*> collision_proximity::CollisionProximitySpace::current_attached_body_decompositions_ [private] |
Definition at line 265 of file collision_proximity_space.h.
std::vector<unsigned int> collision_proximity::CollisionProximitySpace::current_attached_body_indices_ [private] |
Definition at line 263 of file collision_proximity_space.h.
std::vector<std::string> collision_proximity::CollisionProximitySpace::current_attached_body_names_ [private] |
Definition at line 261 of file collision_proximity_space.h.
std::vector<GradientInfo> collision_proximity::CollisionProximitySpace::current_gradients_ [private] |
Definition at line 270 of file collision_proximity_space.h.
std::string collision_proximity::CollisionProximitySpace::current_group_name_ [private] |
Definition at line 259 of file collision_proximity_space.h.
std::vector<std::vector<bool> > collision_proximity::CollisionProximitySpace::current_intra_group_collision_links_ [private] |
Definition at line 266 of file collision_proximity_space.h.
std::vector<BodyDecomposition*> collision_proximity::CollisionProximitySpace::current_link_body_decompositions_ [private] |
Definition at line 264 of file collision_proximity_space.h.
std::vector<unsigned int> collision_proximity::CollisionProximitySpace::current_link_indices_ [private] |
Definition at line 262 of file collision_proximity_space.h.
std::vector<std::string> collision_proximity::CollisionProximitySpace::current_link_names_ [private] |
Definition at line 260 of file collision_proximity_space.h.
std::vector<bool> collision_proximity::CollisionProximitySpace::current_self_excludes_ [private] |
Definition at line 267 of file collision_proximity_space.h.
std::map<std::string, std::map<std::string, bool> > collision_proximity::CollisionProximitySpace::enabled_self_collision_links_ [private] |
Definition at line 253 of file collision_proximity_space.h.
distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* collision_proximity::CollisionProximitySpace::environment_distance_field_ [private] |
Definition at line 237 of file collision_proximity_space.h.
boost::recursive_mutex collision_proximity::CollisionProximitySpace::group_queries_lock_ [mutable, private] |
Definition at line 247 of file collision_proximity_space.h.
std::map<std::string, std::map<std::string, bool> > collision_proximity::CollisionProximitySpace::intra_group_collision_links_ [private] |
Definition at line 254 of file collision_proximity_space.h.
Definition at line 277 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::max_self_distance_ [private] |
Definition at line 278 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::origin_x_ [private] |
Definition at line 274 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::origin_y_ [private] |
Definition at line 274 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::origin_z_ [private] |
Definition at line 274 of file collision_proximity_space.h.
Definition at line 242 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::resolution_ [private] |
Definition at line 275 of file collision_proximity_space.h.
Definition at line 242 of file collision_proximity_space.h.
distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* collision_proximity::CollisionProximitySpace::self_distance_field_ [private] |
Definition at line 238 of file collision_proximity_space.h.
std::map<std::string, bool> collision_proximity::CollisionProximitySpace::self_excludes_ [private] |
Definition at line 256 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::size_x_ [private] |
Definition at line 273 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::size_y_ [private] |
Definition at line 273 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::size_z_ [private] |
Definition at line 273 of file collision_proximity_space.h.
std::map<std::string, BodyDecompositionVector*> collision_proximity::CollisionProximitySpace::static_object_map_ [private] |
Definition at line 250 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::tolerance_ [private] |
Definition at line 275 of file collision_proximity_space.h.
double collision_proximity::CollisionProximitySpace::undefined_distance_ [private] |
Definition at line 279 of file collision_proximity_space.h.
Definition at line 245 of file collision_proximity_space.h.
Definition at line 244 of file collision_proximity_space.h.