Drake
Model Class Referenceabstract

Model is an abstract base class of a collision model. More...

#include <multibody/collision/model.h>

Inheritance diagram for Model:
[legend]
Collaboration diagram for Model:
[legend]

Public Member Functions

 Model ()
 
virtual ~Model ()
 
ElementAddElement (std::unique_ptr< Element > element)
 Adds a collision element to this model. More...
 
bool RemoveElement (ElementId id)
 Removes a collision element from this model. More...
 
virtual const ElementFindElement (ElementId id) const
 Gets a read-only pointer to a collision element in this model. More...
 
virtual ElementFindMutableElement (ElementId id)
 Gets a pointer to a mutable collision element in this model. More...
 
void GetTerrainContactPoints (ElementId id0, Eigen::Matrix3Xd *terrain_points)
 
virtual void UpdateModel ()=0
 Updates the collision model. More...
 
virtual bool UpdateElementWorldTransform (ElementId id, const Eigen::Isometry3d &X_WL)
 Updates the stored transformation from a collision element's canonical space to world space (X_WE), where X_WE = X_WL * X_LE. More...
 
virtual bool ClosestPointsAllToAll (const std::vector< ElementId > &ids_to_check, bool use_margins, std::vector< PointPair > *closest_points)=0
 Computes the points of closest approach between all eligible pairs of collision elements drawn from a specified set of elements. More...
 
virtual bool ComputeMaximumDepthCollisionPoints (bool use_margins, std::vector< PointPair > *closest_points)=0
 Computes the point of closest approach between collision elements that are in contact. More...
 
virtual bool ClosestPointsPairwise (const std::vector< ElementIdPair > &id_pairs, bool use_margins, std::vector< PointPair > *closest_points)=0
 Computes the points of closest approach between specified pairs of collision elements. More...
 
virtual void ClearCachedResults (bool use_margins)=0
 Clears possibly cached results so that a fresh computation can be performed. More...
 
virtual void CollisionDetectFromPoints (const Eigen::Matrix3Xd &points, bool use_margins, std::vector< PointPair > *closest_points)=0
 Computes the closest distance from each point to any surface in the collision model utilizing Bullet's collision detection code. More...
 
virtual std::vector< size_t > CollidingPoints (const std::vector< Eigen::Vector3d > &input_points, double collision_threshold)=0
 Given a vector of points in the world coordinate frame, returns the indices of those points that are within the provided collision_threshold distance of any collision geometry in the model. More...
 
virtual bool CollidingPointsCheckOnly (const std::vector< Eigen::Vector3d > &input_points, double collision_threshold)=0
 Given a vector of points in the world coordinate frame, reports if any of those points lie within a specified distance of any collision geometry in the model. More...
 
virtual bool CollisionRaycast (const Eigen::Matrix3Xd &origin, const Eigen::Matrix3Xd &ray_endpoint, bool use_margins, Eigen::VectorXd *distances, Eigen::Matrix3Xd *normals)=0
 Performs a raycast intersection test (like a LIDAR / laser range finder). More...
 
virtual bool TransformCollisionFrame (const drake::multibody::collision::ElementId &eid, const Eigen::Isometry3d &transform_body_to_joint)
 Modifies a collision element's local transform to be relative to a joint's frame rather than a link's frame. More...
 
Does not allow copy, move, or assignment
 Model (const Model &)=delete
 
Modeloperator= (const Model &)=delete
 
 Model (Model &&)=delete
 
Modeloperator= (Model &&)=delete
 

Protected Member Functions

virtual void DoAddElement (const Element &element)
 Allows sub-classes to do additional processing on elements added to the collision model. More...
 
virtual void DoRemoveElement (ElementId id)
 Allows sub-classes to do additional processing when elements are removed from the collision model. More...
 

Protected Attributes

std::unordered_map< ElementId, std::unique_ptr< Element > > elements
 

Friends

std::ostream & operator<< (std::ostream &, const Model &)
 A toString method for this class. More...
 

Detailed Description

Model is an abstract base class of a collision model.

Child classes of Model implement the actual collision detection logic.

Constructor & Destructor Documentation

Model ( const Model )
delete
Model ( Model &&  )
delete
Model ( )
inline
virtual ~Model ( )
inlinevirtual

Here is the call graph for this function:

Member Function Documentation

Element * AddElement ( std::unique_ptr< Element element)

Adds a collision element to this model.

This operation is frequently referred to as "registering" the collision element. This is the process by which a fully-realized Drake collision element is fed to a specific drake::multibody::collision::Model implementation. Prior to this registration, the collision model knows nothing about the collision element. After registration, it owns the collision element.

Parameters
elementThe element to add.
Returns
A pointer to the added element.
Exceptions
Aruntime_error if there was a problem (e.g., duplicate element id, error configuring collision model, etc.)

Here is the call graph for this function:

Here is the caller graph for this function:

virtual void ClearCachedResults ( bool  use_margins)
pure virtual

Clears possibly cached results so that a fresh computation can be performed.

Parameters
[in]use_marginsIf true, the cache of the model with margins is cleared. If false, the cache of the model without margins is cleared.

Depending on the implementation, the collision model may cache results on each dispatch. For instance, Bullet uses cached results to warm-start its LCP solvers.

Clearing cached results allows the collision model to perform a fresh computation without any coupling with previous history.

See also
drake/multibody/collision/test/model_test.cc.

Implemented in BulletModel, FclModel, and UnusableModel.

Here is the caller graph for this function:

virtual bool ClosestPointsAllToAll ( const std::vector< ElementId > &  ids_to_check,
bool  use_margins,
std::vector< PointPair > *  closest_points 
)
pure virtual

Computes the points of closest approach between all eligible pairs of collision elements drawn from a specified set of elements.

Parameters
ids_to_checkThe vector of ElementId for which the all-to-all collision detection should be performed
use_marginsA flag indicating whether or not to use the version of this model with collision margins
[out]closest_pointsA reference to a vector of PointPair objects that contains the closest point information after this method is called
Returns
Whether this method successfully ran.

Implemented in BulletModel, FclModel, and UnusableModel.

Here is the caller graph for this function:

virtual bool ClosestPointsPairwise ( const std::vector< ElementIdPair > &  id_pairs,
bool  use_margins,
std::vector< PointPair > *  closest_points 
)
pure virtual

Computes the points of closest approach between specified pairs of collision elements.

Parameters
id_pairsA vector of ElementIdPair specifying which pairs of elements to consider
use_marginsA flag indicating whether or not to use the version of this model with collision margins
[out]closest_pointsA reference to a vector of PointPair objects that contains the closest point information after this method is called
Returns
Whether this method successfully ran.

Implemented in BulletModel, UnusableModel, and FclModel.

Here is the caller graph for this function:

virtual std::vector<size_t> CollidingPoints ( const std::vector< Eigen::Vector3d > &  input_points,
double  collision_threshold 
)
pure virtual

Given a vector of points in the world coordinate frame, returns the indices of those points that are within the provided collision_threshold distance of any collision geometry in the model.

In other words, the index i is included in the returned vector of indices iff a sphere of radius collision_threshold, located at input_points[i] collides with any collision element in the model.

Parameters
input_pointsThe list of points to check for collisions against the model.
collision_thresholdThe radius of a control sphere around each point used to check for collisions with the model.
Returns
A vector with indexes in input_points of all those points that do collide with the model within the specified threshold.
See also
drake/matlab/systems/plants/test/collidingPointsTest.m for a MATLAB test.

Implemented in BulletModel, UnusableModel, and FclModel.

Here is the caller graph for this function:

virtual bool CollidingPointsCheckOnly ( const std::vector< Eigen::Vector3d > &  input_points,
double  collision_threshold 
)
pure virtual

Given a vector of points in the world coordinate frame, reports if any of those points lie within a specified distance of any collision geometry in the model.

In other words, this method tests if any of the spheres of radius collision_threshold located at input_points[i] collides with any part of the model. This method returns as soon as any of these spheres collides with the model. Points are not checked against one another but only against the existing model.

Parameters
input_pointsThe list of points to check for collisions against the model.
collision_thresholdThe radius of a control sphere around each point used to check for collisions with the model.
Returns
Whether any of the points positively checks for collision.

Implemented in BulletModel, UnusableModel, and FclModel.

Here is the caller graph for this function:

virtual void CollisionDetectFromPoints ( const Eigen::Matrix3Xd &  points,
bool  use_margins,
std::vector< PointPair > *  closest_points 
)
pure virtual

Computes the closest distance from each point to any surface in the collision model utilizing Bullet's collision detection code.

Parameters
pointsAn ordered list of N points represented column-wise by a 3 x N Matrix.
use_marginsA flag indicating whether to use the version of this model with collision margins.
[out]closest_pointsA vector of N PointPair instances such that the i'th instance reports the query result for the i'th input point.

Implemented in BulletModel, FclModel, and UnusableModel.

Here is the caller graph for this function:

virtual bool CollisionRaycast ( const Eigen::Matrix3Xd &  origin,
const Eigen::Matrix3Xd &  ray_endpoint,
bool  use_margins,
Eigen::VectorXd *  distances,
Eigen::Matrix3Xd *  normals 
)
pure virtual

Performs a raycast intersection test (like a LIDAR / laser range finder).

Parameters
originA 3 x N matrix where each column specifies the position of a ray's origin in the world coordinate frame. If origin has dimensions of 3 x 1, the same origin is used for all rays.
ray_endpointA 3 x N matrix where each column specifies a second point on the corresponding ray.
use_marginsA flag indicating whether or not to use the version of this model with collision margins.
[out]distanceThe distance to the first collision, or -1 if no collision occurs.
Returns
Whether this method successfully ran.

Implemented in BulletModel, UnusableModel, and FclModel.

Here is the caller graph for this function:

virtual bool ComputeMaximumDepthCollisionPoints ( bool  use_margins,
std::vector< PointPair > *  closest_points 
)
pure virtual

Computes the point of closest approach between collision elements that are in contact.

Parameters
[in]use_marginsIf true the model uses the representation with margins. If false, the representation without margins is used instead.
[out]closest_pointsA reference to a vector of PointPair objects that contains the closest point information after this method is called.
Returns
Whether this method successfully ran.

Implemented in BulletModel, FclModel, and UnusableModel.

Here is the caller graph for this function:

void DoAddElement ( const Element element)
protectedvirtual

Allows sub-classes to do additional processing on elements added to the collision model.

This is called each time Model::AddElement is called.

Parameters
elementThe element that has been added.
Exceptions
std::runtime_errorIf there was a problem processing the element.

Reimplemented in BulletModel, and FclModel.

Here is the caller graph for this function:

void DoRemoveElement ( ElementId  id)
protectedvirtual

Allows sub-classes to do additional processing when elements are removed from the collision model.

This is called by Model::RemoveElement() prior to removing id from elements. The derived class should not do this removal.

Parameters
idThe id of the element that will be removed.

Reimplemented in BulletModel, and FclModel.

Here is the caller graph for this function:

const Element * FindElement ( ElementId  id) const
virtual

Gets a read-only pointer to a collision element in this model.

Parameters
idAn ElementId corresponding to the desired collision element
Returns
A read-only pointer to the collision element corresponding to the given id or nullptr if no such collision element is present in the model.

Here is the caller graph for this function:

Element * FindMutableElement ( ElementId  id)
virtual

Gets a pointer to a mutable collision element in this model.

Parameters
[in]idAn ElementId corresponding to the desired collision element.
Returns
A pointer to a mutable collision element corresponding to the given id or nullptr if no such collision element is present in the model.

Here is the caller graph for this function:

void GetTerrainContactPoints ( ElementId  id0,
Eigen::Matrix3Xd *  terrain_points 
)

Here is the caller graph for this function:

Model& operator= ( Model &&  )
delete
Model& operator= ( const Model )
delete
bool RemoveElement ( ElementId  id)

Removes a collision element from this model.

Parameters
idThe id of the element that will be removed.
Returns
True if the element is successfully removed, false if the model does not contain an element with the given id.

Here is the call graph for this function:

Here is the caller graph for this function:

bool TransformCollisionFrame ( const drake::multibody::collision::ElementId eid,
const Eigen::Isometry3d &  transform_body_to_joint 
)
virtual

Modifies a collision element's local transform to be relative to a joint's frame rather than a link's frame.

This is necessary because Drake requires that link frames be defined by their parent joint frames.

Parameters
eidThe ID of the collision element to update.
transform_body_to_jointThe transform from the collision element's link's frame to the joint's coordinate frame.
Returns
Whether the collision element was successfully updated.

Here is the caller graph for this function:

bool UpdateElementWorldTransform ( ElementId  id,
const Eigen::Isometry3d &  X_WL 
)
virtual

Updates the stored transformation from a collision element's canonical space to world space (X_WE), where X_WE = X_WL * X_LE.

X_LE is the transform that maps the element to its parent body's space, referred to as "local". X_WL maps the body/local space to the world.

Parameters
idThe ID of the element being updated.
X_WLThe new value for the local-to-world transform. It reflects the current world pose of the parent body in a given context.
Returns
Whether the update was successful.

Reimplemented in BulletModel, FclModel, and UnusableModel.

Here is the caller graph for this function:

virtual void UpdateModel ( )
pure virtual

Updates the collision model.

This method is typically called after changes are made to its collision elements.

Implemented in BulletModel, FclModel, and UnusableModel.

Here is the caller graph for this function:

Friends And Related Function Documentation

std::ostream& operator<< ( std::ostream &  os,
const Model model 
)
friend

A toString method for this class.

Member Data Documentation

std::unordered_map<ElementId, std::unique_ptr<Element> > elements
protected

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