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

geometry: add setSecurityMargins helper

parent b0769f37
......@@ -315,6 +315,17 @@ namespace pinocchio
///
void deactivateAllCollisionPairs();
///
/// \brief Set the security margin of all the collision request in a row, according to the values stored in the associative map.
///
/// \param[in] geom_model Geometry model associated to the data.
/// \param[in] security_margin_map Associative map related the security margin of a given input collision pair (i,j).
/// \param[in] upper Wheter the security_margin_map is an upper or lower triangular filled array.
///
void setSecurityMargins(const GeometryModel & geom_model,
const MatrixXs & security_margin_map,
const bool upper = true);
friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
}; // struct GeometryData
......
......@@ -271,8 +271,37 @@ namespace pinocchio
i = (Eigen::DenseIndex)std::max(cp.first,cp.second);
j = (Eigen::DenseIndex)std::min(cp.first,cp.second);
}
activeCollisionPairs[k] = map(i,j);
}
}
inline void GeometryData::setSecurityMargins(const GeometryModel & geom_model,
const MatrixXs & security_margin_map,
const bool upper)
{
const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms;
PINOCCHIO_CHECK_ARGUMENT_SIZE(security_margin_map.rows(),ngeoms,"Input map does not have the correct number of rows.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(security_margin_map.cols(),ngeoms,"Input map does not have the correct number of columns.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(geom_model.collisionPairs.size(),collisionRequests.size(),"Current geometry data and the input geometry model are not conistent.");
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
{
const CollisionPair & cp = geom_model.collisionPairs[k];
Eigen::DenseIndex i,j;
if(upper)
{
j = (Eigen::DenseIndex)std::max(cp.first,cp.second);
i = (Eigen::DenseIndex)std::min(cp.first,cp.second);
}
else
{
i = (Eigen::DenseIndex)std::max(cp.first,cp.second);
j = (Eigen::DenseIndex)std::min(cp.first,cp.second);
}
collisionRequests[k].security_margin = security_margin_map(i,j);
}
}
......
......@@ -235,7 +235,27 @@ BOOST_AUTO_TEST_CASE(manage_collision_pairs)
geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false);
BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs);
}
// Test security margins
{
GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model);
const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
GeometryData::MatrixXs security_margin_map_upper(security_margin_map);
security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.);
geom_data_upper.setSecurityMargins(geom_model, security_margin_map);
for(size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k)
{
BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.);
}
geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false);
for(size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k)
{
BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.);
}
}
}
......
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