Tesseract 0.28.4
Loading...
Searching...
No Matches
tesseract::collision::ContactResultMap Class Reference

This structure hold contact results for link pairs. More...

#include <types.h>

Public Types

using KeyType = tesseract::common::LinkIdPair
using MappedType = ContactResultVector
using ContainerType
using ConstReferenceType = typename ContainerType::const_reference
using ConstIteratorType = typename ContainerType::const_iterator
using PairType = typename std::pair<const KeyType, MappedType>
using FilterFn = std::function<void(PairType&)>

Public Member Functions

ContactResult & addContactResult (const KeyType &key, ContactResult result)
 Add contact results for the provided key.
ContactResult & addContactResult (const KeyType &key, const MappedType &results)
 Add contact results for the provided key.
ContactResult & setContactResult (const KeyType &key, ContactResult result)
 Set contact results for the provided key.
ContactResult & setContactResult (const KeyType &key, const MappedType &results)
 Set contact results for the provided key.
void addInterpolatedCollisionResults (ContactResultMap &sub_segment_results, long sub_segment_index, long sub_segment_last_index, const std::unordered_set< tesseract::common::LinkId > &active_link_ids, double segment_dt, bool point_in_time, const ContactResultMap::FilterFn &filter=nullptr)
 This processes interpolated contact results by updating the cc_time and cc_type and then adds the result.
void flattenMoveResults (ContactResultVector &v)
void flattenCopyResults (ContactResultVector &v) const
void flattenWrapperResults (std::vector< std::reference_wrapper< ContactResult > > &v)
void flattenWrapperResults (std::vector< std::reference_wrapper< const ContactResult > > &v) const
void filter (const FilterFn &filter)
 Filter out results using the provided function.
long count () const
 Get the total number of contact results storted.
std::size_t size () const
 Get the size of the map.
bool empty () const
 Check if results are present.
void clear ()
 This is a consurvative clear.
void shrinkToFit ()
 Remove map entries with no contact results.
void release ()
 Fully clear all internal data.
const ContainerType & getContainer () const
 Get the underlying container.
ConstIteratorType begin () const
 returns an iterator to the beginning
ConstIteratorType end () const
 returns an iterator to the end
ConstIteratorType cbegin () const
 returns an iterator to the beginning
ConstIteratorType cend () const
 returns an iterator to the end
const ContactResultVector & at (const KeyType &key) const
 access specified element with bounds checking
ConstIteratorType find (const KeyType &key) const
bool operator== (const ContactResultMap &rhs) const
bool operator!= (const ContactResultMap &rhs) const
std::string getSummary () const
 Get a brief summary of the most frequently colliding link pair.

Detailed Description

This structure hold contact results for link pairs.

A custom class was implemented to avoid a large number of heap allocations during motion which avoids full clearing the map. This class provides methods const container methods for access the internal map and has two distinct different when it comes to the clear, size and release methods.

The clear method does not call clear on the map but instead it loops over all entries and calls clear on the vector being stored. This allows the memory to remain with the map and not get release for each of the vectors stored in the map.

The size method loops over the map and counts those that have vectors which are not empty.

The release method actually calls clear on the internal map relasing all memory.

Todo
This should be updated to leverage a object pool for ContactResultVector where in the set and add methods it would check it the pair exists and if not it would pull from the object pool.

Member Typedef Documentation

◆ ContainerType

using tesseract::collision::ContactResultMap::ContainerType
Initial value:
std::unordered_map<KeyType,
MappedType,
std::hash<KeyType>,
std::equal_to<>,
Eigen::aligned_allocator<std::pair<const KeyType, MappedType>>>

Member Function Documentation

◆ addContactResult() [1/2]

ContactResult & tesseract::collision::ContactResultMap::addContactResult ( const KeyType & key,
ContactResult result )

Add contact results for the provided key.

Parameters
keyThe key to append the results to
resultThe results to add

◆ addContactResult() [2/2]

ContactResult & tesseract::collision::ContactResultMap::addContactResult ( const KeyType & key,
const MappedType & results )

Add contact results for the provided key.

Parameters
keyThe key to append the results to
resultThe results to add

◆ setContactResult() [1/2]

ContactResult & tesseract::collision::ContactResultMap::setContactResult ( const KeyType & key,
ContactResult result )

Set contact results for the provided key.

Parameters
keyThe key to assign the provided results to
resultThe results to assign

◆ setContactResult() [2/2]

ContactResult & tesseract::collision::ContactResultMap::setContactResult ( const KeyType & key,
const MappedType & results )

Set contact results for the provided key.

Parameters
keyThe key to assign the provided results to
resultThe results to assign

◆ addInterpolatedCollisionResults()

void tesseract::collision::ContactResultMap::addInterpolatedCollisionResults ( ContactResultMap & sub_segment_results,
long sub_segment_index,
long sub_segment_last_index,
const std::unordered_set< tesseract::common::LinkId > & active_link_ids,
double segment_dt,
bool point_in_time,
const ContactResultMap::FilterFn & filter = nullptr )

This processes interpolated contact results by updating the cc_time and cc_type and then adds the result.

This is copied from the trajopt utility processInterpolatedCollisionResults

Parameters
sub_segment_resultsThe interpolated results to process
sub_segment_indexThe current sub segment index
sub_segment_last_indexThe value of sub_segment_index that marks the end of the segment, where a contact is typed CCType_Time1. It depends on what the caller walks: a walk over n sub-states ends at n - 1, while a walk over the n - 1 casts between them ends at n - 2.
active_link_idsThe set of active link IDs
segment_dtThe segment dt
point_in_timeThe results are a point in time rather than a swept interval, so the endpoint type is forced from the index and any cc_time the checker wrote is discarded. True for a discrete checker, and for a continuous checker asked to test a single state as a zero length cast.
filterAn option filter to exclude results

◆ filter()

void tesseract::collision::ContactResultMap::filter ( const FilterFn & filter)

Filter out results using the provided function.

Parameters
fnThe filter function

◆ count()

long tesseract::collision::ContactResultMap::count ( ) const

Get the total number of contact results storted.

Returns
The number of contact results

◆ size()

std::size_t tesseract::collision::ContactResultMap::size ( ) const

Get the size of the map.

This loops over the internal map and counts entries with contacts

Returns
The number of entries with contacts

◆ empty()

bool tesseract::collision::ContactResultMap::empty ( ) const

Check if results are present.

Returns

◆ clear()

void tesseract::collision::ContactResultMap::clear ( )

This is a consurvative clear.

This does not call clear on the internal map but instead loops over each link pair entry and calls clear on the underlying vector. This way the vector capacity remains the same to avoid uneccessary heap allocation for subsequent contact requests.

Note
Use release to fully clear the internal data structure

◆ getContainer()

const ContactResultMap::ContainerType & tesseract::collision::ContactResultMap::getContainer ( ) const

Get the underlying container.

Warning
Do not use this for anything other than debugging or serialization

◆ getSummary()

std::string tesseract::collision::ContactResultMap::getSummary ( ) const

Get a brief summary of the most frequently colliding link pair.

Returns
A string containing the collision summary