5 #ifndef __pinocchio_multibody_geometry_hpp__ 6 #define __pinocchio_multibody_geometry_hpp__ 8 #include "pinocchio/multibody/fcl.hpp" 9 #include "pinocchio/multibody/fwd.hpp" 10 #include "pinocchio/container/aligned-vector.hpp" 12 #include "pinocchio/serialization/serializable.hpp" 24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef double Scalar;
32 typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
33 typedef std::vector<CollisionPair> CollisionPairVector;
34 typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
36 typedef pinocchio::GeomIndex GeomIndex;
55 template<
typename S2,
int O2,
template<
typename,
int>
class _JointCollectionTpl>
111 const bool upper =
true);
161 return !(*
this == other);
164 friend std::ostream& operator<<(std::ostream & os,
181 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
183 typedef double Scalar;
184 enum { Options = 0 };
187 typedef std::vector<GeomIndex> GeomIndexList;
188 typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
189 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
191 #ifdef PINOCCHIO_WITH_HPP_FCL 203 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg;
210 #ifdef PINOCCHIO_WITH_HPP_FCL 247 PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors;
250 PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors;
252 #endif // PINOCCHIO_WITH_HPP_FCL 295 void fillInnerOuterObjectMaps(
const GeometryModel & geomModel);
310 void activateCollisionPair(
const PairIndex pair_id);
317 void activateAllCollisionPairs();
327 void setActiveCollisionPairs(
const GeometryModel & geom_model,
328 const MatrixXb & collision_map,
329 const bool upper =
true);
338 void setGeometryCollisionStatus(
const GeometryModel & geom_model,
339 const GeomIndex geom_id,
340 bool enable_collision);
351 void deactivateCollisionPair(
const PairIndex pair_id);
358 void deactivateAllCollisionPairs();
360 #ifdef PINOCCHIO_WITH_HPP_FCL 369 const MatrixXs & security_margin_map,
370 const bool upper =
true);
371 #endif // ifdef PINOCCHIO_WITH_HPP_FCL 373 friend std::ostream & operator<<(std::ostream & os,
const GeometryData & geomData);
383 #ifdef PINOCCHIO_WITH_HPP_FCL 401 return !(*
this == other);
411 #include "pinocchio/multibody/geometry.hxx" 413 #endif // ifndef __pinocchio_multibody_geometry_hpp__
GeomIndex getGeometryId(const std::string &name) const
Return the index of a GeometryObject given by its name.
bool operator==(const GeometryModel &other) const
Returns true if *this and other are equal.
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
CollisionPairVector collisionPairs
Vector of collision pairs.
bool operator!=(const GeometryData &other) const
Returns true if *this and other are not equal.
std::vector< fcl::DistanceResult > distanceResults
Vector gathering the result of the distance computation for all the collision pairs.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
std::vector< fcl::CollisionResult > collisionResults
Vector gathering the result of the collision computation for all the collision pairs.
bool existCollisionPair(const CollisionPair &pair) const
Check if a collision pair exists in collisionPairs. See also findCollisitionPair(const CollisionPair ...
void removeAllCollisionPairs()
Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
void removeCollisionPair(const CollisionPair &pair)
Remove if exists the CollisionPair from the vector collision_pairs.
std::vector< fcl::DistanceRequest > distanceRequests
Defines what information should be computed by distance computation. There is one request per pair of...
std::vector< Scalar > radius
Radius of the bodies, i.e. distance of the further point of the geometry model attached to the body f...
void setCollisionPairs(const MatrixXb &collision_map, const bool upper=true)
Set the collision pairs from a given input array. Each entry of the input matrix defines the activati...
std::map< JointIndex, GeomIndexList > outerObjects
A list of associated collision GeometryObjects to a given joint Id.
Index ngeoms
The number of GeometryObjects.
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
std::map< JointIndex, GeomIndexList > innerObjects
Map over vector GeomModel::geometryObjects, indexed by joints.
GeometryData()
Empty constructor.
void addAllCollisionPairs()
Add all possible collision pairs.
Main pinocchio namespace.
bool operator!=(const GeometryModel &other) const
Returns true if *this and other are not equal.
PairIndex collisionPairIndex
Index of the collision pair.
bool existGeometryName(const std::string &name) const
Check if a GeometryObject given by its name exists.
void addCollisionPair(const CollisionPair &pair)
Add a collision pair into the vector of collision_pairs. The method check before if the given Collisi...
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
bool operator==(const GeometryData &other) const
Returns true if *this and other are equal.
std::vector< fcl::CollisionRequest > collisionRequests
Defines what information should be computed by collision test. There is one request per pair of geome...
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.