Public Types | Public Member Functions | Private Member Functions | Private Attributes
collision_proximity::CollisionProximitySpace Class Reference

#include <collision_proximity_space.h>

List of all members.

Public Types

enum  TrajectoryPointType { StartCollision, EndCollision, Middle, None }
enum  TrajectorySafety {
  ErrorUnsafe, StartUnsafe, MiddleUnsafe, EndUnsafe,
  MeshToMeshSafe, InCollisionSafe
}

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::CollisionModelsInterfacegetCollisionModelsInterface () 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
tf::Transform 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
TrajectorySafety 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::CollisionModelsInterfacecollision_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< GradientInfocurrent_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_distance_field_marker_publisher_
ros::Publisher vis_marker_array_publisher_
ros::Publisher vis_marker_publisher_

Detailed Description

Definition at line 58 of file collision_proximity_space.h.


Member Enumeration Documentation

Enumerator:
StartCollision 
EndCollision 
Middle 
None 

Definition at line 63 of file collision_proximity_space.h.

Enumerator:
ErrorUnsafe 
StartUnsafe 
MiddleUnsafe 
EndUnsafe 
MeshToMeshSafe 
InCollisionSafe 

Definition at line 68 of file collision_proximity_space.h.


Constructor & Destructor Documentation

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.

Definition at line 124 of file collision_proximity_space.cpp.


Member Function Documentation

Definition at line 148 of file collision_proximity_space.cpp.

Definition at line 138 of file collision_proximity_space.cpp.

Definition at line 169 of file collision_proximity_space.h.

Definition at line 184 of file collision_proximity_space.h.

Definition at line 175 of file collision_proximity_space.h.

std::vector<std::string> collision_proximity::CollisionProximitySpace::getCurrentLinkNames ( ) const [inline]

Definition at line 179 of file collision_proximity_space.h.

bool CollisionProximitySpace::getEnvironmentCollisions ( std::vector< bool > &  collisions,
bool  stop_at_first = false 
) const

Definition at line 1094 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getEnvironmentProximityGradients ( std::vector< GradientInfo > &  gradients,
bool  subtract_radii = false 
) const

Definition at line 1123 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 660 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getIntraGroupCollisions ( std::vector< bool > &  collisions,
bool  stop_at_first = false 
) const

Definition at line 928 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getIntraGroupProximityGradients ( std::vector< GradientInfo > &  gradients,
bool  subtract_radii = false 
) const

Definition at line 968 of file collision_proximity_space.cpp.

Definition at line 439 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 1147 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getSelfCollisions ( std::vector< bool > &  collisions,
bool  stop_at_first = false 
) const

Definition at line 1033 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getSelfProximityGradients ( std::vector< GradientInfo > &  gradients,
bool  subtract_radii = false 
) const

Definition at line 1062 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getStateCollisions ( bool &  in_collision,
std::vector< CollisionType > &  collisions 
) const

Definition at line 802 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getStateGradients ( std::vector< GradientInfo > &  gradients,
bool  subtract_radii = false 
) const

Definition at line 826 of file collision_proximity_space.cpp.

Definition at line 1088 of file collision_proximity_space.cpp.

Definition at line 922 of file collision_proximity_space.cpp.

Definition at line 1027 of file collision_proximity_space.cpp.

Definition at line 795 of file collision_proximity_space.cpp.

CollisionProximitySpace::TrajectorySafety 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 1207 of file collision_proximity_space.cpp.

Definition at line 161 of file collision_proximity_space.cpp.

Definition at line 249 of file collision_proximity_space.cpp.

Definition at line 505 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 526 of file collision_proximity_space.cpp.

Definition at line 374 of file collision_proximity_space.cpp.

Definition at line 409 of file collision_proximity_space.cpp.

Definition at line 189 of file collision_proximity_space.h.

Definition at line 384 of file collision_proximity_space.cpp.

void CollisionProximitySpace::setDistanceFieldForGroupQueries ( const std::string &  group_name,
const planning_models::KinematicState state 
) [private]

Definition at line 487 of file collision_proximity_space.cpp.

Definition at line 270 of file collision_proximity_space.cpp.

Definition at line 274 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 287 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 749 of file collision_proximity_space.cpp.

Definition at line 449 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 714 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeBoundingCylinders ( const std::vector< std::string > &  object_names) const

Definition at line 1698 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 1515 of file collision_proximity_space.cpp.

Definition at line 1398 of file collision_proximity_space.cpp.

Definition at line 1407 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeObjectSpheres ( const std::vector< std::string > &  object_names) const

Definition at line 1648 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeObjectVoxels ( const std::vector< std::string > &  object_names) const

Definition at line 1612 of file collision_proximity_space.cpp.


Member Data Documentation

std::map<std::string, std::map<std::string, bool> > collision_proximity::CollisionProximitySpace::attached_object_collision_links_ [private]

Definition at line 265 of file collision_proximity_space.h.

Definition at line 261 of file collision_proximity_space.h.

Definition at line 259 of file collision_proximity_space.h.

Definition at line 249 of file collision_proximity_space.h.

std::vector<std::vector<double> > collision_proximity::CollisionProximitySpace::colors_ [mutable, private]

Definition at line 244 of file collision_proximity_space.h.

Definition at line 275 of file collision_proximity_space.h.

Definition at line 273 of file collision_proximity_space.h.

Definition at line 271 of file collision_proximity_space.h.

Definition at line 280 of file collision_proximity_space.h.

Definition at line 269 of file collision_proximity_space.h.

Definition at line 276 of file collision_proximity_space.h.

Definition at line 274 of file collision_proximity_space.h.

Definition at line 272 of file collision_proximity_space.h.

Definition at line 270 of file collision_proximity_space.h.

Definition at line 277 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 263 of file collision_proximity_space.h.

Definition at line 246 of file collision_proximity_space.h.

boost::recursive_mutex collision_proximity::CollisionProximitySpace::group_queries_lock_ [mutable, private]

Definition at line 257 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 264 of file collision_proximity_space.h.

Definition at line 287 of file collision_proximity_space.h.

Definition at line 288 of file collision_proximity_space.h.

Definition at line 284 of file collision_proximity_space.h.

Definition at line 284 of file collision_proximity_space.h.

Definition at line 284 of file collision_proximity_space.h.

Definition at line 251 of file collision_proximity_space.h.

Definition at line 285 of file collision_proximity_space.h.

Definition at line 251 of file collision_proximity_space.h.

Definition at line 247 of file collision_proximity_space.h.

std::map<std::string, bool> collision_proximity::CollisionProximitySpace::self_excludes_ [private]

Definition at line 266 of file collision_proximity_space.h.

Definition at line 283 of file collision_proximity_space.h.

Definition at line 283 of file collision_proximity_space.h.

Definition at line 283 of file collision_proximity_space.h.

Definition at line 260 of file collision_proximity_space.h.

Definition at line 285 of file collision_proximity_space.h.

Definition at line 289 of file collision_proximity_space.h.

Definition at line 253 of file collision_proximity_space.h.

Definition at line 255 of file collision_proximity_space.h.

Definition at line 254 of file collision_proximity_space.h.


The documentation for this class was generated from the following files:


collision_proximity
Author(s): Gil Jones
autogenerated on Fri Dec 6 2013 21:11:30