Go to the documentation of this file.
19 #include <boost/test/unit_test.hpp>
23 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
34 BOOST_CHECK(broadphase_manager.
check());
44 sphere_ptr->computeLocalAABB();
46 box_ptr->computeLocalAABB();
61 BOOST_CHECK(broadphase_manager.
check());
62 BOOST_CHECK(sphere_ptr.get() == go.
geometry.get());
65 sphere_new_ptr->computeLocalAABB();
67 BOOST_CHECK(!broadphase_manager.
check());
68 BOOST_CHECK(sphere_ptr.get() != go.
geometry.get());
80 BOOST_CHECK(sphere_new_ptr.get() == go.
geometry.get());
82 broadphase_manager.
update(
false);
92 BOOST_CHECK(broadphase_manager.
check());
99 + std::string(
"/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
100 std::vector<std::string> packageDirs;
102 packageDirs.push_back(meshDir);
105 + std::string(
"/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
121 BOOST_CHECK(manager.check());
129 + std::string(
"/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
130 std::vector<std::string> packageDirs;
132 packageDirs.push_back(meshDir);
135 + std::string(
"/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
148 Eigen::VectorXd
q =
model.referenceConfigurations[
"half_sitting"];
158 std::cout <<
"map:\n" <<
geom_model.collisionPairMapping << std::endl;
164 const int num_configs = 1000;
165 for (
int i = 0;
i < num_configs; ++
i)
168 q_rand.head<7>() =
q.head<7>();
176 BOOST_AUTO_TEST_SUITE_END()
bool computeCollisions(BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, CollisionCallBackBase *callback)
Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeome...
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
BOOST_AUTO_TEST_CASE(test_broadphase_with_empty_models)
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
bool check() const
Check whether the base broad phase manager is aligned with the current collision_objects.
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
CollisionGeometryPtr geometry
The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
void update(bool compute_local_aabb=false)
Update the manager from the current geometry positions and update the underlying FCL broad phase mana...
void loadReferenceConfigurations(ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const bool verbose=false)
Get the reference configurations of a given model associated to a SRDF file. It throws if the SRDF fi...
const CollisionObjectVector & getCollisionObjects() const
Returns the vector of collision objects associated to the manager.
JointCollectionTpl & model
Main pinocchio namespace.
#define PINOCCHIO_MODEL_DIR
void removeCollisionPairs(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, GeometryModel &geom_model, const std::string &filename, const bool verbose=false)
Deactive all possible collision pairs mentioned in the SRDF file. It throws if the SRDF file is incor...
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06