se3::GeometryData Struct Reference

#include <multibody/geometry.hpp>

Collaboration diagram for se3::GeometryData:
[legend]

List of all members.

Public Types

typedef Model::Index Index
typedef Model::GeomIndex GeomIndex
typedef CollisionPair CollisionPair_t
typedef std::vector
< CollisionPair_t
CollisionPairsVector_t

Public Member Functions

 GeometryData (const Data &data, const GeometryModel &model_geom)
 ~GeometryData ()
void addCollisionPair (const GeomIndex co1, const GeomIndex co2)
 Add a collision pair given by the index of the two colliding geometries into the vector of collision_pairs.
void addCollisionPair (const CollisionPair_t &pair)
 Add a collision pair into the vector of collision_pairs.
void addAllCollisionPairs ()
 Add all possible collision pairs.
void removeCollisionPair (const GeomIndex co1, const GeomIndex co2)
 Remove if exists the collision pair given by the index of the two colliding geometries from the vector of collision_pairs.
void removeCollisionPair (const CollisionPair_t &pair)
 Remove if exists the CollisionPair from the vector collision_pairs.
void removeAllCollisionPairs ()
 Remove all collision pairs from collision_pairs.
bool existCollisionPair (const GeomIndex co1, const GeomIndex co2) const
 Check if a collision pair given by the index of the two colliding geometries exists in collision_pairs.
bool existCollisionPair (const CollisionPair_t &pair) const
 Check if a collision pair exists in collision_pairs.
Index findCollisionPair (const GeomIndex co1, const GeomIndex co2) const
 Return the index in collision_pairs of a CollisionPair given by the index of the two colliding geometries.
Index findCollisionPair (const CollisionPair_t &pair) const
 Return the index of a given collision pair in collision_pairs.
void desactivateCollisionPairs ()
void initializeListOfCollisionPairs ()
void addCollisionPairsFromSrdf (const std::string &filename, const bool verbose=false) throw (std::invalid_argument)
 Active all possible collision pairs except those mentioned in the SRDF file.
CollisionResult computeCollision (const GeomIndex co1, const GeomIndex co2) const
 Compute the collision status between two collision objects given by their indexes.
CollisionResult computeCollision (const CollisionPair_t &pair) const
 Compute the collision status between two collision objects of a given collision pair.
void computeAllCollisions ()
 Compute the collision result of all the collision pairs according to the current placements of the geometires stored in GeometryData::oMg.
bool isColliding () const
 Check if at least one of the collision pairs has its two collision objects in collision.
DistanceResult computeDistance (const GeomIndex co1, const GeomIndex co2) const
 Compute the minimal distance between two collision objects given by their indexes.
DistanceResult computeDistance (const CollisionPair_t &pair) const
 Compute the minimal distance between collision objects of a collison pair.
void computeAllDistances ()
 Compute the distance result for all collision pairs according to the current placements of the geometries stored in GeometryData::oMg.
void resetDistances ()
void displayCollisionPairs () const

Public Attributes

const Datadata_ref
 A const reference to the data associated to the robot model.
const GeometryModelmodel_geom
 A const reference to the model storing all the geometries.
std::vector< se3::SE3oMg_collisions
 Vector gathering the SE3 placements of the collision objects relative to the world.
std::vector< se3::SE3oMg_visuals
 Vector gathering the SE3 placements of the visual objects relative to the world.
std::vector< fcl::Transform3f > oMg_fcl_collisions
 Same as oMg but using fcl::Transform3f to store placement.
CollisionPairsVector_t collision_pairs
 Vector of collision pairs.
Index nCollisionPairs
 Number of collision pairs stored in collision_pairs.
std::vector< DistanceResultdistance_results
 Vector gathering the result of the distance computation for all the collision pairs.
std::vector< CollisionResultcollision_results
 Vector gathering the result of the collision computation for all the collision pairs.

Friends

std::ostream & operator<< (std::ostream &os, const GeometryData &data_geom)

Member Typedef Documentation


Constructor & Destructor Documentation

se3::GeometryData::GeometryData ( const Data data,
const GeometryModel model_geom 
) [inline]
se3::GeometryData::~GeometryData ( ) [inline]

Member Function Documentation

void se3::GeometryData::addAllCollisionPairs ( )

Add all possible collision pairs.

void se3::GeometryData::addCollisionPair ( const GeomIndex  co1,
const GeomIndex  co2 
)

Add a collision pair given by the index of the two colliding geometries into the vector of collision_pairs.

The method check before if the given CollisionPair is already included.

Parameters:
[in]co1Index of the first colliding geometry.
[in]co2Index of the second colliding geometry.
void se3::GeometryData::addCollisionPair ( const CollisionPair_t pair)

Add a collision pair into the vector of collision_pairs.

The method check before if the given CollisionPair is already included.

Parameters:
[in]pairThe CollisionPair to add.
void se3::GeometryData::addCollisionPairsFromSrdf ( const std::string &  filename,
const bool  verbose = false 
) throw (std::invalid_argument)

Active all possible collision pairs except those mentioned in the SRDF file.

It throws if the SRDF file is incorrect.

Parameters:
[in]filenameThe complete path to the SRDF file.
[in]verboseVerbosity mode.
void se3::GeometryData::computeAllCollisions ( )

Compute the collision result of all the collision pairs according to the current placements of the geometires stored in GeometryData::oMg.

The results are stored in the vector GeometryData::collision_results.

void se3::GeometryData::computeAllDistances ( )

Compute the distance result for all collision pairs according to the current placements of the geometries stored in GeometryData::oMg.

The results are stored in the vector GeometryData::distance_results.

CollisionResult se3::GeometryData::computeCollision ( const GeomIndex  co1,
const GeomIndex  co2 
) const

Compute the collision status between two collision objects given by their indexes.

Parameters:
[in]co1Index of the first collision object.
[in]co2Index of the second collision object.
Returns:
Return true is the collision objects are colliding.

Referenced by se3::computeCollisions().

CollisionResult se3::GeometryData::computeCollision ( const CollisionPair_t pair) const

Compute the collision status between two collision objects of a given collision pair.

Parameters:
[in]pairThe collsion pair.
Returns:
Return true is the collision objects are colliding.
DistanceResult se3::GeometryData::computeDistance ( const GeomIndex  co1,
const GeomIndex  co2 
) const

Compute the minimal distance between two collision objects given by their indexes.

Parameters:
[in]co1Index of the first collision object.
[in]co2Index of the second collision object.
Returns:
An fcl struct containing the distance result.

Referenced by se3::computeDistances().

DistanceResult se3::GeometryData::computeDistance ( const CollisionPair_t pair) const

Compute the minimal distance between collision objects of a collison pair.

Parameters:
[in]pairThe collsion pair.
Returns:
An fcl struct containing the distance result.
void se3::GeometryData::desactivateCollisionPairs ( )
void se3::GeometryData::displayCollisionPairs ( ) const [inline]

References collision_pairs.

bool se3::GeometryData::existCollisionPair ( const GeomIndex  co1,
const GeomIndex  co2 
) const

Check if a collision pair given by the index of the two colliding geometries exists in collision_pairs.

See also findCollisitionPair(const GeomIndex co1, const GeomIndex co2).

Parameters:
[in]co1Index of the first colliding geometry.
[in]co2Index of the second colliding geometry.
Returns:
True if the CollisionPair exists, false otherwise.
bool se3::GeometryData::existCollisionPair ( const CollisionPair_t pair) const

Check if a collision pair exists in collision_pairs.

See also findCollisitionPair(const CollisionPair_t & pair).

Parameters:
[in]pairThe CollisionPair.
Returns:
True if the CollisionPair exists, false otherwise.
Index se3::GeometryData::findCollisionPair ( const GeomIndex  co1,
const GeomIndex  co2 
) const

Return the index in collision_pairs of a CollisionPair given by the index of the two colliding geometries.

Parameters:
[in]co1Index of the first colliding geometry.
[in]co2Index of the second colliding geometry.
Returns:
The index of the collision pair in collision_pairs.
Index se3::GeometryData::findCollisionPair ( const CollisionPair_t pair) const

Return the index of a given collision pair in collision_pairs.

Parameters:
[in]pairThe CollisionPair.
Returns:
The index of the CollisionPair in collision_pairs.
void se3::GeometryData::initializeListOfCollisionPairs ( )
bool se3::GeometryData::isColliding ( ) const

Check if at least one of the collision pairs has its two collision objects in collision.

void se3::GeometryData::removeAllCollisionPairs ( )

Remove all collision pairs from collision_pairs.

Same as collision_pairs.clear().

void se3::GeometryData::removeCollisionPair ( const GeomIndex  co1,
const GeomIndex  co2 
)

Remove if exists the collision pair given by the index of the two colliding geometries from the vector of collision_pairs.

Parameters:
[in]co1Index of the first colliding geometry.
[in]co2Index of the second colliding geometry.
void se3::GeometryData::removeCollisionPair ( const CollisionPair_t pair)

Remove if exists the CollisionPair from the vector collision_pairs.

Parameters:
[in]pairThe CollisionPair to remove.
void se3::GeometryData::resetDistances ( )

Friends And Related Function Documentation

std::ostream& operator<< ( std::ostream &  os,
const GeometryData data_geom 
) [friend]

Member Data Documentation

Vector of collision pairs.

See addCollisionPair, removeCollisionPair to fill or remove elements in the vector.

Referenced by se3::computeCollisions(), se3::computeDistances(), displayCollisionPairs(), and GeometryData().

Vector gathering the result of the collision computation for all the collision pairs.

Referenced by se3::computeCollisions(), and GeometryData().

A const reference to the data associated to the robot model.

See class Data.

Vector gathering the result of the distance computation for all the collision pairs.

Referenced by se3::computeDistances(), and GeometryData().

A const reference to the model storing all the geometries.

See class GeometryModel.

Referenced by se3::updateGeometryPlacements().

Number of collision pairs stored in collision_pairs.

Vector gathering the SE3 placements of the collision objects relative to the world.

See updateGeometryPlacements to update the placements.

Referenced by se3::updateGeometryPlacements().

std::vector<fcl::Transform3f> se3::GeometryData::oMg_fcl_collisions

Same as oMg but using fcl::Transform3f to store placement.

This pre-allocation avoids dynamic allocation during collision checking or distance computations.

Referenced by se3::updateGeometryPlacements().

Vector gathering the SE3 placements of the visual objects relative to the world.

See updateGeometryPlacements to update the placements.

Referenced by se3::updateGeometryPlacements().