Verified Commit b0769f37 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test/geometry: test extend API

parent a2facc31
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#include <iostream>
......@@ -155,6 +155,89 @@ BOOST_AUTO_TEST_CASE ( loading_model )
fcl::DistanceResult distance_res = computeDistance(geomModel,geomData,idx);
BOOST_CHECK(distance_res.min_distance > 0.);
}
BOOST_AUTO_TEST_CASE(manage_collision_pairs)
{
typedef pinocchio::Model Model;
typedef pinocchio::GeometryModel GeometryModel;
typedef pinocchio::GeometryData GeometryData;
std::string filename = PINOCCHIO_MODEL_DIR + std::string("/others/robots/romeo_description/urdf/romeo_small.urdf");
std::vector < std::string > package_dirs;
std::string mesh_dir = PINOCCHIO_MODEL_DIR + std::string("/others/robots/");
package_dirs.push_back(mesh_dir);
Model model;
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
GeometryModel geom_model;
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs);
geom_model.addAllCollisionPairs();
GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
{
const CollisionPair & cp = geom_model.collisionPairs[k];
collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = true;
}
GeometryModel::MatrixXb collision_map_lower = collision_map.transpose();
GeometryModel geom_model_copy, geom_model_copy_lower;
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs);
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs);
geom_model_copy.addCollisionPairs(collision_map);
geom_model_copy_lower.addCollisionPairs(collision_map_lower,false);
BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size());
BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size());
for(size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k)
{
BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k]));
BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k]));
}
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
{
BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k]));
BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k]));
}
{
GeometryData geom_data(geom_model);
geom_data.activateAllCollisionPairs();
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
BOOST_CHECK(geom_data.activeCollisionPairs[k]);
}
{
GeometryData geom_data(geom_model);
geom_data.deactivateAllCollisionPairs();
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
}
{
GeometryData geom_data(geom_model), geom_data_copy(geom_model), geom_data_copy_lower(geom_model);
geom_data_copy.deactivateAllCollisionPairs();
geom_data_copy_lower.deactivateAllCollisionPairs();
GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
{
const CollisionPair & cp = geom_model.collisionPairs[k];
collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = geom_data.activeCollisionPairs[k];
}
GeometryData::MatrixXb collision_map_lower = collision_map.transpose();
geom_data_copy.setActiveCollisionPairs(geom_model, collision_map);
BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs);
geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false);
BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs);
}
}
BOOST_AUTO_TEST_CASE ( test_collisions )
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment