Public Types | Public Member Functions | Static Public Member Functions | Protected Member Functions | List of all members
hpp::core::continuousCollisionChecking::BodyPairCollision Class Reference

Computation of collision-free sub-intervals of a path. More...

#include <continuous-collision-checking/body-pair-collision.hh>

Public Types

typedef std::pair< CollisionObjectConstPtr_t, CollisionObjectConstPtr_tCollisionPair_t
 
typedef std::vector< CollisionPair_tCollisionPairs_t
 

Public Member Functions

const std::vector< se3::JointIndex > & joints () const
 
const JointPtr_tjoint_a () const
 Get joint a. More...
 
const JointPtr_tjoint_b () const
 Get joint b. More...
 
const CollisionPairs_tpairs () const
 
bool removeObjectTo_b (const CollisionObjectConstPtr_t &object)
 
void path (const PathPtr_t &path, bool reverse)
 Set path to validate. More...
 
PathConstPtr_t path () const
 Get path. More...
 
bool validateConfiguration (const value_type &t, interval_t &interval, CollisionValidationReportPtr_t &report)
 Validate interval centered on a path parameter. More...
 
value_type tolerance () const
 
value_type maximalVelocity () const
 
std::string name () const
 
std::ostream & print (std::ostream &os) const
 
void addCollisionPair (const CollisionObjectConstPtr_t &left, const CollisionObjectConstPtr_t right)
 

Static Public Member Functions

static BodyPairCollisionPtr_t create (const JointPtr_t &joint_a, const ConstObjectStdVector_t &objects_b, value_type tolerance)
 Create instance and return shared pointer. More...
 
static BodyPairCollisionPtr_t create (const JointPtr_t &joint_a, const JointPtr_t &joint_b, value_type tolerance)
 Create instance and return shared pointer. More...
 

Protected Member Functions

 BodyPairCollision (const JointPtr_t &joint_a, const JointPtr_t &joint_b, value_type tolerance)
 Constructor of inter-body collision checking. More...
 
 BodyPairCollision (const JointPtr_t &joint_a, const ConstObjectStdVector_t &objects_b, value_type tolerance)
 Constructor of collision checking with the environment. More...
 

Detailed Description

Computation of collision-free sub-intervals of a path.

This class aims at validating a path for the absence of collision between two bodies of a robot.

The interval of definition of the path is successively covered by intervals where boths bodies are proved to be collision-free. Each interval is computed by bounding from above the velocity of all points of body 1 in the reference frame of body 2.

Member Typedef Documentation

◆ CollisionPair_t

◆ CollisionPairs_t

Constructor & Destructor Documentation

◆ BodyPairCollision() [1/2]

hpp::core::continuousCollisionChecking::BodyPairCollision::BodyPairCollision ( const JointPtr_t joint_a,
const JointPtr_t joint_b,
value_type  tolerance 
)
inlineprotected

Constructor of inter-body collision checking.

Parameters
body_a,body_bbodies to test for collision
toleranceallowed penetration should be positive
Precondition
body_a and body_b should not be nul pointers.

References assert().

◆ BodyPairCollision() [2/2]

hpp::core::continuousCollisionChecking::BodyPairCollision::BodyPairCollision ( const JointPtr_t joint_a,
const ConstObjectStdVector_t objects_b,
value_type  tolerance 
)
inlineprotected

Constructor of collision checking with the environment.

Parameters
body_abody to test for collision with the environment
toleranceallowed penetration should be positive
Precondition
objects_b should not be attached to a joint

References assert(), and distance().

Member Function Documentation

◆ addCollisionPair()

void hpp::core::continuousCollisionChecking::BodyPairCollision::addCollisionPair ( const CollisionObjectConstPtr_t left,
const CollisionObjectConstPtr_t  right 
)
inline
Note
The left object should belong to joint_a and the right one should belong to joint_b, or vice-versa. This is not checked.

◆ create() [1/2]

static BodyPairCollisionPtr_t hpp::core::continuousCollisionChecking::BodyPairCollision::create ( const JointPtr_t joint_a,
const ConstObjectStdVector_t objects_b,
value_type  tolerance 
)
inlinestatic

Create instance and return shared pointer.

Parameters
body_abody to test for collision with the environment
toleranceallowed penetration should be positive
Precondition
objects_b should not be attached to a joint

◆ create() [2/2]

static BodyPairCollisionPtr_t hpp::core::continuousCollisionChecking::BodyPairCollision::create ( const JointPtr_t joint_a,
const JointPtr_t joint_b,
value_type  tolerance 
)
inlinestatic

Create instance and return shared pointer.

Parameters
body_a,body_bbodies to test for collision
toleranceallowed penetration should be positive
Precondition
body_a and body_b should not be nul pointers.

◆ joint_a()

const JointPtr_t& hpp::core::continuousCollisionChecking::BodyPairCollision::joint_a ( ) const
inline

Get joint a.

◆ joint_b()

const JointPtr_t& hpp::core::continuousCollisionChecking::BodyPairCollision::joint_b ( ) const
inline

Get joint b.

◆ joints()

const std::vector<se3::JointIndex>& hpp::core::continuousCollisionChecking::BodyPairCollision::joints ( ) const
inline

◆ maximalVelocity()

value_type hpp::core::continuousCollisionChecking::BodyPairCollision::maximalVelocity ( ) const
inline

◆ name()

std::string hpp::core::continuousCollisionChecking::BodyPairCollision::name ( ) const
inline

◆ pairs()

const CollisionPairs_t& hpp::core::continuousCollisionChecking::BodyPairCollision::pairs ( ) const
inline

◆ path() [1/2]

void hpp::core::continuousCollisionChecking::BodyPairCollision::path ( const PathPtr_t path,
bool  reverse 
)
inline

Set path to validate.

Parameters
pathpath to validate,
reversewhether path is validated from end to beginning. Compute maximal velocity of point of body a in frame of body b along the path.

References path().

◆ path() [2/2]

PathConstPtr_t hpp::core::continuousCollisionChecking::BodyPairCollision::path ( ) const
inline

Get path.

Referenced by path().

◆ print()

std::ostream& hpp::core::continuousCollisionChecking::BodyPairCollision::print ( std::ostream &  os) const
inline

◆ removeObjectTo_b()

bool hpp::core::continuousCollisionChecking::BodyPairCollision::removeObjectTo_b ( const CollisionObjectConstPtr_t object)
inline

◆ tolerance()

value_type hpp::core::continuousCollisionChecking::BodyPairCollision::tolerance ( ) const
inline

◆ validateConfiguration()

bool hpp::core::continuousCollisionChecking::BodyPairCollision::validateConfiguration ( const value_type t,
interval_t interval,
CollisionValidationReportPtr_t report 
)
inline

Validate interval centered on a path parameter.

Parameters
tparameter value in the path interval of definition
[in,out]intervalas input, interval over which collision checking must be performed. As output, interval over which pair is collision-free, not necessarily included in definition interval.
Returns
true if the body pair is collision free for this parameter value, false if the body pair is in collision.
Note
object should be in the positions defined by the configuration of parameter t on the path.
Todo:
A finer bound could be computed when path is an InterpolatedPath using the maximal velocity on each subinterval

References assert().