Class CollisionAvoidance

Inheritance Relationships

Base Type

Class Documentation

class CollisionAvoidance : public OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>

The SelfCollisionAvoidance class implements a constraint of full-body Self-Collision Avoidance for Walkman This constraint is implemented by inequality: Aineq * x <= bUpperBound where the dimension of Aineq is n * m, n is the number of Link pairs to be constrained, and m is total DOFs of the robot to be controlled; the x is infinitesimal increament of the joint variable vector which is the optimization variable, and its dimension is m * 1; the bUpperBound is the minimum distance vector of all the Link pairs, the dimension of which is n * 1.

the element in bUpperBound is the minimum distance between the corresponding Link pair with taking the Link pair threshold into account.

Public Types

typedef std::shared_ptr<CollisionAvoidance> Ptr
typedef std::pair<std::string, std::string> LinksPair
typedef XBot::Collision::CollisionModel::WitnessPointVector WitnessPointVector
typedef XBot::Collision::CollisionModel::LinkPairVector LinkPairVector

Public Functions

CollisionAvoidance(const XBot::ModelInterface &robot, int max_pairs = -1, urdf::ModelConstSharedPtr collision_urdf = nullptr, srdf::ModelConstSharedPtr collision_srdf = nullptr)

SelfCollisionAvoidance.

Parameters

  • x – status

  • robot – model

  • max_pairs – maximum number of link pairs checked for SCA (-1 for all)

  • collision_urdf – urdf used for collision model

  • collision_srdf – srdf used for collision model NOTE: if urdf and srdf are not specified, the one inside the robot model will be used

double getLinkPairThreshold()

getLinkPairThreshold distance offset between two link pairs

Returns

_LinkPair_threshold distance offset between two link pairs

void getError(Eigen::VectorXd &e)

getError

Parameters

e –

double getDetectionThreshold()

getDetectionThreshold

Returns

_Detection_threshold the maximum distance which we use to look for link pairs

void setLinkPairThreshold(const double linkPair_threshold)

setLinkPairThreshold set _LinkPair_threshold distance offset between two link pairs

Parameters

linkPair_threshold – (always positive) distance offset between two link pairs

void setDetectionThreshold(const double detection_threshold)

setDetectionThreshold set _Detection_threshold

Parameters

detection_threshold – (always positive), the maximum distance which we use to look for link pairs

virtual void update()

update recomputes Aineq and bUpperBound if x is different than the previously stored value

Parameters

x – the state vector.

void setMaxPairs(const unsigned int max_pairs)
void setCollisionList(std::set<std::pair<std::string, std::string>> collisionList)
void collisionModelUpdated()

collisionModelUpdated must be called after a collision has been added or removed from the model

bool addCollisionShape(const std::string &name, const std::string &link, const XBot::Collision::Shape::Variant &shape, const Eigen::Affine3d &link_T_shape, const std::vector<std::string> &disabled_collisions = {})
bool moveCollisionShape(const std::string &id, const Eigen::Affine3d &new_pose)

change the transform w.r.t.

the world for the given world collision

void setBoundScaling(const double boundScaling)

setBoundScaling sets bound scaling for the capsule constraint

Parameters

boundScaling – is a number which should be lower than 1.0 (e.g. 1./2. means we are looking two steps ahead and will avoid collision with the capsule by slowing down)

void setLinksVsEnvironment(const std::set<std::string> &links)

setLinksVsEnvironment

Parameters

links –

XBot::Collision::CollisionModel &getCollisionModel()

getter for the internal collision model

const XBot::Collision::CollisionModel &getCollisionModel() const

getter for the internal collision model

void getOrderedWitnessPointVector(WitnessPointVector &wp) const

get the vector of witness points used by this constraint during the last call to update(), in ascending distance order

Parameters

wp –

void getOrderedLinkPairVector(LinkPairVector &lp) const

getOrderedLinkPairVector

Parameters

lp –

void getOrderedDistanceVector(std::vector<double> &d) const

getOrderedDistanceVector

Parameters

d –

const Eigen::MatrixXd &getCollisionJacobian() const
~CollisionAvoidance()

Protected Attributes

bool _include_env

_include_env

double _bound_scaling

_bound_scaling

double _distance_threshold

_distance_threshold is the allowable minimum distance between every Link pair

double _detection_threshold

all the link pairs whose minimum distance are smaller than this “_Detection_threshold” would be dealt with further

int _max_pairs

_max_pairs

const XBot::ModelInterface &_robot

_robot

XBot::ModelInterface::Ptr _collision_model
std::unique_ptr<XBot::Collision::CollisionModel> _dist_calc

_dist_calc

Eigen::VectorXd _distances
Eigen::MatrixXd _distance_J
int _num_active_pairs
mutable WitnessPointVector _wpv
mutable LinkPairVector _lpv
Eigen::MatrixXd _Jtmp

_Jtmp