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

geometry: extend current API with more helpers

parent baa408de
......@@ -30,6 +30,7 @@ namespace pinocchio
typedef ::pinocchio::GeometryObject GeometryObject;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(GeometryObject) GeometryObjectVector;
typedef std::vector<CollisionPair> CollisionPairVector;
typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
typedef pinocchio::GeomIndex GeomIndex;
......@@ -96,6 +97,17 @@ namespace pinocchio
/// \note Collision pairs between geometries having the same parent joint are not added.
///
void addAllCollisionPairs();
///
/// \brief Add the collision pairs from a given input array.
/// Each entry of the input matrix defines the activation of a given collision pair
/// (map[i,j] > 0 means that the pair (i,j) is active).
///
/// \param[in] collision_map Associative array.
/// \param[in] upper Wheter the collision_map is an upper or lower triangular filled array.
///
void addCollisionPairs(const MatrixXb & collision_map,
const bool upper = true);
///
/// \brief Remove if exists the CollisionPair from the vector collision_pairs.
......@@ -173,6 +185,7 @@ namespace pinocchio
typedef SE3Tpl<Scalar,Options> SE3;
typedef std::vector<GeomIndex> GeomIndexList;
typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
///
/// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
......@@ -266,6 +279,25 @@ namespace pinocchio
/// \sa GeomData
///
void activateCollisionPair(const PairIndex pairId);
///
/// \brief Activate all collision pairs.
///
/// \sa GeomData::deactivateAllCollisionPairs, GeomData::activateCollisionPair, GeomData::deactivateCollisionPair
///
void activateAllCollisionPairs();
///
/// \brief Set the collision pair association from a given input array.
/// Each entry of the input matrix defines the activation of a given collision pair.
///
/// \param[in] geom_model Geometry model associated to the data.
/// \param[in] collision_map Associative array.
/// \param[in] upper Wheter the collision_map is an upper or lower triangular filled array.
///
void setActiveCollisionPairs(const GeometryModel & geom_model,
const MatrixXb & collision_map,
const bool upper = true);
///
/// Deactivate a collision pair.
......@@ -277,6 +309,13 @@ namespace pinocchio
/// \sa GeomData::activateCollisionPair
///
void deactivateCollisionPair(const PairIndex pairId);
///
/// \brief Deactivate all collision pairs.
///
/// \sa GeomData::activateAllCollisionPairs, GeomData::activateCollisionPair, GeomData::deactivateCollisionPair
///
void deactivateAllCollisionPairs();
friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
......
......@@ -5,6 +5,8 @@
#ifndef __pinocchio_multibody_geometry_hxx__
#define __pinocchio_multibody_geometry_hxx__
#include <algorithm>
#include "pinocchio/multibody/model.hpp"
#if BOOST_VERSION / 100 % 1000 >= 60
......@@ -160,6 +162,32 @@ namespace pinocchio
"The input pair.second is larger than the number of geometries contained in the GeometryModel");
if (!existCollisionPair(pair)) { collisionPairs.push_back(pair); }
}
inline void GeometryModel::addCollisionPairs(const MatrixXb & map,
const bool upper)
{
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.rows(),(Eigen::DenseIndex)ngeoms,
"Input map does not have the correct number of rows.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.cols(),(Eigen::DenseIndex)ngeoms,
"Input map does not have the correct number of columns.");
removeAllCollisionPairs();
for(Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)ngeoms; ++i)
{
for(Eigen::DenseIndex j = i+1; j < (Eigen::DenseIndex)ngeoms; ++j)
{
if(upper)
{
if(map(i,j))
collisionPairs.push_back(CollisionPair((std::size_t)i,(std::size_t)j));
}
else
{
if(map(j,i))
collisionPairs.push_back(CollisionPair((std::size_t)i,(std::size_t)j));
}
}
}
}
inline void GeometryModel::addAllCollisionPairs()
{
......@@ -214,6 +242,40 @@ namespace pinocchio
activeCollisionPairs[pairId] = true;
}
inline void GeometryData::activateAllCollisionPairs()
{
std::fill(activeCollisionPairs.begin(),activeCollisionPairs.end(),true);
}
inline void GeometryData::setActiveCollisionPairs(const GeometryModel & geom_model,
const MatrixXb & map,
const bool upper)
{
const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms;
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.rows(),ngeoms,"Input map does not have the correct number of rows.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.cols(),ngeoms,"Input map does not have the correct number of columns.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(geom_model.collisionPairs.size(),activeCollisionPairs.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);
}
activeCollisionPairs[k] = map(i,j);
}
}
inline void GeometryData::deactivateCollisionPair(const PairIndex pairId)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(pairId < activeCollisionPairs.size(),
......@@ -221,6 +283,11 @@ namespace pinocchio
activeCollisionPairs[pairId] = false;
}
inline void GeometryData::deactivateAllCollisionPairs()
{
std::fill(activeCollisionPairs.begin(),activeCollisionPairs.end(),false);
}
} // namespace pinocchio
/// @endcond
......
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