Classes | Public Member Functions | Protected Member Functions | Protected Attributes | Friends
collision_space::EnvironmentModelODE Class Reference

A class describing an environment for a kinematic robot using ODE. More...

#include <environmentODE.h>

Inheritance diagram for collision_space::EnvironmentModelODE:
Inheritance graph
[legend]

List of all members.

Classes

struct  AttGeom
struct  CollisionData
struct  CollisionNamespace
struct  LinkGeom
struct  ModelInfo
class  ODECollide2
struct  ODEStorage
 Structure for maintaining ODE temporary data. More...

Public Member Functions

virtual void addObject (const std::string &ns, shapes::StaticShape *shape)
 Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.
virtual void addObject (const std::string &ns, shapes::Shape *shape, const tf::Transform &pose)
 Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.
virtual void addObjects (const std::string &ns, const std::vector< shapes::Shape * > &shapes, const std::vector< tf::Transform > &poses)
 Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment.
virtual void clearObjects (void)
 Remove all objects from collision model.
virtual void clearObjects (const std::string &ns)
 Remove objects from a specific namespace in the collision model.
virtual EnvironmentModelclone (void) const
 Clone the environment.
 EnvironmentModelODE (void)
virtual bool getAllCollisionContacts (std::vector< Contact > &contacts, unsigned int num_per_contact=1) const
 This function will get the complete list of contacts between any two potentially colliding bodies. The num per contacts specifies the number of contacts per pair that will be returned.
virtual bool getAllObjectEnvironmentCollisionContacts (const std::string &object_name, std::vector< Contact > &contacts, unsigned int num_contacts_per_pair) const
virtual void getAttachedBodyPoses (std::map< std::string, std::vector< tf::Transform > > &pose_map) const
virtual bool getCollisionContacts (std::vector< Contact > &contacts, unsigned int max_total=1, unsigned int max_per_pair=1) const
 Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that's in collision.
virtual bool hasObject (const std::string &ns) const
 Tells whether or not there is an object with the given name in the collision model.
virtual bool isCollision (void) const
 Check if a model is in collision.
virtual bool isEnvironmentCollision (void) const
 Check if a model is in environment collision.
virtual bool isObjectInEnvironmentCollision (const std::string &object_name) const
 Check if an object is in collision with the other static objects.
virtual bool isObjectObjectCollision (const std::string &object1_name, const std::string &object2_name) const
 Check if two static objects are in collision.
virtual bool isObjectRobotCollision (const std::string &object_name) const
 Check if a single static object is in collision with the robot.
virtual bool isSelfCollision (void) const
 Check if a model is in self collision.
virtual void revertAlteredLinkPadding ()
virtual void setAlteredLinkPadding (const std::map< std::string, double > &link_padding_map)
virtual void setRobotModel (const planning_models::KinematicModel *model, const AllowedCollisionMatrix &allowed_collision_matrix, const std::map< std::string, double > &link_padding_map, double default_padding=0.0, double scale=1.0)
 Add a robot model. Ignore robot links if their name is not specified in the string vector. The scale argument can be used to increase or decrease the size of the robot's bodies (multiplicative factor). The padding can be used to increase or decrease the robot's bodies with by an additive term.
virtual void updateAttachedBodies (void)
 Update the set of bodies that are attached to the robot (re-creates them)
virtual void updateAttachedBodies (const std::map< std::string, double > &link_padding_map)
 Update the set of bodies that are attached to the robot (re-creates them) using the indicated padding or default if non-specified.
virtual void updateRobotModel (const planning_models::KinematicState *state)
 Update the positions of the geometry used in collision detection.
virtual ~EnvironmentModelODE (void)

Protected Member Functions

void addAttachedBody (LinkGeom *lg, const planning_models::KinematicModel::AttachedBodyModel *attm, double padd)
void checkThreadInit (void) const
dGeomID copyGeom (dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const
dGeomID createODEGeom (dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding)
dGeomID createODEGeom (dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape)
void createODERobotModel ()
void freeMemory (void)
void revertAttachedBodiesLinkPadding ()
void setAttachedBodiesLinkPadding ()
void testCollision (CollisionData *data) const
 Internal function for collision detection.
void testEnvironmentCollision (CollisionData *data) const
 Internal function for collision detection.
void testObjectCollision (CollisionNamespace *cn, CollisionData *data) const
 Internal function for collision detection.
void testObjectEnvironmentCollision (CollisionData *cdata, const std::string &object_name) const
 Test collision between an object and all other static objects.
void testObjectObjectCollision (CollisionData *cdata, const std::string &object1_name, const std::string &object2_name) const
 Test collision between two static objects.
void testSelfCollision (CollisionData *data) const
 Internal function for collision detection.
void updateGeom (dGeomID geom, const tf::Transform &pose) const

Protected Attributes

std::map< std::string, bool > attached_bodies_in_collision_matrix_
std::map< std::string,
CollisionNamespace * > 
coll_namespaces_
std::map< std::string, dSpaceID > dspace_lookup_map_
std::map< dGeomID, std::pair
< std::string, BodyType > > 
geom_lookup_map_
ModelInfo model_geom_
bool previous_set_robot_model_

Friends

void nearCallbackFn (void *data, dGeomID o1, dGeomID o2)

Detailed Description

A class describing an environment for a kinematic robot using ODE.

Definition at line 48 of file environmentODE.h.


Constructor & Destructor Documentation

Definition at line 60 of file environmentODE.cpp.

Definition at line 81 of file environmentODE.cpp.


Member Function Documentation

Definition at line 387 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::addObject ( const std::string &  ns,
shapes::StaticShape shape 
) [virtual]

Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.

Implements collision_space::EnvironmentModel.

Definition at line 1302 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::addObject ( const std::string &  ns,
shapes::Shape shape,
const tf::Transform pose 
) [virtual]

Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.

Implements collision_space::EnvironmentModel.

Definition at line 1279 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::addObjects ( const std::string &  ns,
const std::vector< shapes::Shape * > &  shapes,
const std::vector< tf::Transform > &  poses 
) [virtual]

Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment.

Implements collision_space::EnvironmentModel.

Definition at line 1247 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::checkThreadInit ( void  ) const [protected]

Definition at line 115 of file environmentODE.cpp.

Remove all objects from collision model.

Implements collision_space::EnvironmentModel.

Definition at line 1323 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::clearObjects ( const std::string &  ns) [virtual]

Remove objects from a specific namespace in the collision model.

Implements collision_space::EnvironmentModel.

Definition at line 1334 of file environmentODE.cpp.

Clone the environment.

Implements collision_space::EnvironmentModel.

Definition at line 1421 of file environmentODE.cpp.

dGeomID collision_space::EnvironmentModelODE::copyGeom ( dSpaceID  space,
ODEStorage storage,
dGeomID  geom,
ODEStorage sourceStorage 
) const [protected]

Definition at line 1346 of file environmentODE.cpp.

dGeomID collision_space::EnvironmentModelODE::createODEGeom ( dSpaceID  space,
ODEStorage storage,
const shapes::Shape shape,
double  scale,
double  padding 
) [protected]

Definition at line 239 of file environmentODE.cpp.

dGeomID collision_space::EnvironmentModelODE::createODEGeom ( dSpaceID  space,
ODEStorage storage,
const shapes::StaticShape shape 
) [protected]

Definition at line 222 of file environmentODE.cpp.

Definition at line 177 of file environmentODE.cpp.

Definition at line 99 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::getAllCollisionContacts ( std::vector< Contact > &  contacts,
unsigned int  num_per_contact = 1 
) const [virtual]

This function will get the complete list of contacts between any two potentially colliding bodies. The num per contacts specifies the number of contacts per pair that will be returned.

Implements collision_space::EnvironmentModel.

Definition at line 957 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::getAllObjectEnvironmentCollisionContacts ( const std::string &  object_name,
std::vector< Contact > &  contacts,
unsigned int  num_contacts_per_pair 
) const [virtual]

Implements collision_space::EnvironmentModel.

Definition at line 1063 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::getAttachedBodyPoses ( std::map< std::string, std::vector< tf::Transform > > &  pose_map) const [virtual]

Implements collision_space::EnvironmentModel.

Definition at line 151 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::getCollisionContacts ( std::vector< Contact > &  contacts,
unsigned int  max_total = 1,
unsigned int  max_per_pair = 1 
) const [virtual]

Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that's in collision.

Implements collision_space::EnvironmentModel.

Definition at line 939 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::hasObject ( const std::string &  ns) const [virtual]

Tells whether or not there is an object with the given name in the collision model.

Implements collision_space::EnvironmentModel.

Definition at line 1239 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::isCollision ( void  ) const [virtual]

Check if a model is in collision.

Implements collision_space::EnvironmentModel.

Definition at line 975 of file environmentODE.cpp.

Check if a model is in environment collision.

Implements collision_space::EnvironmentModel.

Definition at line 1005 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::isObjectInEnvironmentCollision ( const std::string &  object_name) const [virtual]

Check if an object is in collision with the other static objects.

Implements collision_space::EnvironmentModel.

Definition at line 1050 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::isObjectObjectCollision ( const std::string &  object1_name,
const std::string &  object2_name 
) const [virtual]

Check if two static objects are in collision.

Implements collision_space::EnvironmentModel.

Definition at line 1036 of file environmentODE.cpp.

bool collision_space::EnvironmentModelODE::isObjectRobotCollision ( const std::string &  object_name) const [virtual]

Check if a single static object is in collision with the robot.

Implements collision_space::EnvironmentModel.

Definition at line 1018 of file environmentODE.cpp.

Check if a model is in self collision.

Implements collision_space::EnvironmentModel.

Definition at line 992 of file environmentODE.cpp.

Reverts link padding to that set at robot initialization

Reimplemented from collision_space::EnvironmentModel.

Definition at line 537 of file environmentODE.cpp.

Definition at line 452 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::setAlteredLinkPadding ( const std::map< std::string, double > &  link_padding_map) [virtual]

Sets a temporary robot padding on the indicated links

Reimplemented from collision_space::EnvironmentModel.

Definition at line 501 of file environmentODE.cpp.

Definition at line 425 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::setRobotModel ( const planning_models::KinematicModel model,
const AllowedCollisionMatrix allowed_collision_matrix,
const std::map< std::string, double > &  link_padding_map,
double  default_padding = 0.0,
double  scale = 1.0 
) [virtual]

Add a robot model. Ignore robot links if their name is not specified in the string vector. The scale argument can be used to increase or decrease the size of the robot's bodies (multiplicative factor). The padding can be used to increase or decrease the robot's bodies with by an additive term.

Reimplemented from collision_space::EnvironmentModel.

Definition at line 129 of file environmentODE.cpp.

Internal function for collision detection.

Definition at line 1220 of file environmentODE.cpp.

Internal function for collision detection.

Definition at line 1231 of file environmentODE.cpp.

Internal function for collision detection.

Definition at line 1130 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::testObjectEnvironmentCollision ( CollisionData cdata,
const std::string &  object_name 
) const [protected]

Test collision between an object and all other static objects.

Definition at line 1081 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::testObjectObjectCollision ( CollisionData cdata,
const std::string &  object1_name,
const std::string &  object2_name 
) const [protected]

Test collision between two static objects.

Definition at line 1091 of file environmentODE.cpp.

Internal function for collision detection.

Definition at line 1226 of file environmentODE.cpp.

Update the set of bodies that are attached to the robot (re-creates them)

Implements collision_space::EnvironmentModel.

Definition at line 342 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::updateAttachedBodies ( const std::map< std::string, double > &  link_padding_map) [virtual]

Update the set of bodies that are attached to the robot (re-creates them) using the indicated padding or default if non-specified.

Definition at line 347 of file environmentODE.cpp.

void collision_space::EnvironmentModelODE::updateGeom ( dGeomID  geom,
const tf::Transform pose 
) const [protected]

Definition at line 332 of file environmentODE.cpp.

Update the positions of the geometry used in collision detection.

Implements collision_space::EnvironmentModel.

Definition at line 479 of file environmentODE.cpp.


Friends And Related Function Documentation

void nearCallbackFn ( void *  data,
dGeomID  o1,
dGeomID  o2 
) [friend]

Definition at line 755 of file environmentODE.cpp.


Member Data Documentation

Definition at line 455 of file environmentODE.h.

Definition at line 463 of file environmentODE.h.

std::map<std::string, dSpaceID> collision_space::EnvironmentModelODE::dspace_lookup_map_ [protected]

Definition at line 466 of file environmentODE.h.

std::map<dGeomID, std::pair<std::string, BodyType> > collision_space::EnvironmentModelODE::geom_lookup_map_ [protected]

Definition at line 465 of file environmentODE.h.

Definition at line 462 of file environmentODE.h.

Definition at line 468 of file environmentODE.h.


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


collision_space
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Mon Dec 2 2013 12:34:21