Program Listing for File geometry.hxx
↰ Return to documentation for file (include/pinocchio/multibody/geometry.hxx
)
//
// Copyright (c) 2015-2022 CNRS INRIA
//
#ifndef __pinocchio_multibody_geometry_hxx__
#define __pinocchio_multibody_geometry_hxx__
#include <algorithm>
#include "pinocchio/multibody/model.hpp"
#if BOOST_VERSION / 100 % 1000 >= 60
#include <boost/bind/bind.hpp>
#include <boost/utility.hpp>
#else
#include <boost/bind.hpp>
#endif
#include <boost/foreach.hpp>
#include <sstream>
namespace pinocchio
{
inline CollisionPair::CollisionPair()
: Base((std::numeric_limits<GeomIndex>::max)(), (std::numeric_limits<GeomIndex>::max)())
{
}
inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
: Base(co1, co2)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(co1 != co2, "The index of collision objects must not be equal.");
}
inline bool CollisionPair::operator==(const CollisionPair & rhs) const
{
return (first == rhs.first && second == rhs.second)
|| (first == rhs.second && second == rhs.first);
}
inline bool CollisionPair::operator!=(const CollisionPair & other) const
{
return !(*this == other);
}
inline void CollisionPair::disp(std::ostream & os) const
{
os << "collision pair (" << first << "," << second << ")\n";
}
inline std::ostream & operator<<(std::ostream & os, const CollisionPair & X)
{
X.disp(os);
return os;
}
inline GeometryModel GeometryModel::clone() const
{
GeometryModel res;
res.ngeoms = ngeoms;
res.collisionPairs = collisionPairs;
res.collisionPairMapping = collisionPairMapping;
res.geometryObjects.reserve(geometryObjects.size());
for (const GeometryObject & geometry_object : geometryObjects)
{
res.geometryObjects.push_back(geometry_object.clone());
}
return res;
}
inline GeometryData::GeometryData(const GeometryModel & geom_model)
: oMg(geom_model.ngeoms)
, activeCollisionPairs(geom_model.collisionPairs.size(), true)
#ifdef PINOCCHIO_WITH_HPP_FCL
, distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true))
, distanceResults(geom_model.collisionPairs.size())
, collisionRequests(
geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST, 1))
, collisionResults(geom_model.collisionPairs.size())
, radius()
, collisionPairIndex(0)
#endif // PINOCCHIO_WITH_HPP_FCL
, innerObjects()
, outerObjects()
{
#ifdef PINOCCHIO_WITH_HPP_FCL
BOOST_FOREACH (hpp::fcl::CollisionRequest & creq, collisionRequests)
{
creq.enable_cached_gjk_guess = true;
}
#if HPP_FCL_VERSION_AT_LEAST(1, 4, 5)
BOOST_FOREACH (hpp::fcl::DistanceRequest & dreq, distanceRequests)
{
dreq.enable_cached_gjk_guess = true;
}
#endif
collision_functors.reserve(geom_model.collisionPairs.size());
distance_functors.reserve(geom_model.collisionPairs.size());
for (size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
{
const CollisionPair & cp = geom_model.collisionPairs[cp_index];
const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first];
const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second];
collision_functors.push_back(ComputeCollision(obj_1, obj_2));
distance_functors.push_back(ComputeDistance(obj_1, obj_2));
}
#endif
fillInnerOuterObjectMaps(geom_model);
}
inline GeometryData::GeometryData(const GeometryData & other)
: oMg(other.oMg)
, activeCollisionPairs(other.activeCollisionPairs)
#ifdef PINOCCHIO_WITH_HPP_FCL
, distanceRequests(other.distanceRequests)
, distanceResults(other.distanceResults)
, collisionRequests(other.collisionRequests)
, collisionResults(other.collisionResults)
, radius(other.radius)
, collisionPairIndex(other.collisionPairIndex)
, collision_functors(other.collision_functors)
, distance_functors(other.distance_functors)
#endif // PINOCCHIO_WITH_HPP_FCL
, innerObjects(other.innerObjects)
, outerObjects(other.outerObjects)
{
}
inline GeometryData & GeometryData::operator=(const GeometryData & other)
{
if (this != &other)
{
oMg = other.oMg;
activeCollisionPairs = other.activeCollisionPairs;
#ifdef PINOCCHIO_WITH_HPP_FCL
distanceRequests = other.distanceRequests;
distanceResults = other.distanceResults;
collisionRequests = other.collisionRequests;
collisionResults = other.collisionResults;
radius = other.radius;
collisionPairIndex = other.collisionPairIndex;
collision_functors = other.collision_functors;
distance_functors = other.distance_functors;
#endif // PINOCCHIO_WITH_HPP_FCL
innerObjects = other.innerObjects;
outerObjects = other.outerObjects;
}
return *this;
}
inline GeometryData::~GeometryData()
{
}
template<typename S2, int O2, template<typename, int> class JointCollectionTpl>
GeomIndex GeometryModel::addGeometryObject(
const GeometryObject & object, const ModelTpl<S2, O2, JointCollectionTpl> & model)
{
if (object.parentFrame < (FrameIndex)model.nframes)
PINOCCHIO_CHECK_INPUT_ARGUMENT(
model.frames[object.parentFrame].parentJoint == object.parentJoint,
"The object joint parent and its frame joint parent do not match.");
Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++);
geometryObjects.push_back(object);
geometryObjects.back().parentJoint = model.frames[object.parentFrame].parentJoint;
collisionPairMapping.conservativeResize(idx + 1, idx + 1);
collisionPairMapping.col(idx).fill(-1);
collisionPairMapping.row(idx).fill(-1);
assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms);
return (GeomIndex)idx;
}
inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object)
{
Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++);
geometryObjects.push_back(object);
collisionPairMapping.conservativeResize(idx + 1, idx + 1);
collisionPairMapping.col(idx).fill(-1);
collisionPairMapping.row(idx).fill(-1);
assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms);
return (GeomIndex)idx;
}
inline void GeometryModel::removeGeometryObject(const std::string & name)
{
GeomIndex i = 0;
GeometryObjectVector::iterator it;
for (it = geometryObjects.begin(); it != geometryObjects.end(); ++it, ++i)
{
if (it->name == name)
{
break;
}
}
PINOCCHIO_THROW(
it != geometryObjects.end(), std::invalid_argument,
(std::string("Object ") + name + std::string(" does not belong to model")).c_str());
// Remove all collision pairs that contain i as first or second index,
for (CollisionPairVector::iterator itCol = collisionPairs.begin();
itCol != collisionPairs.end(); ++itCol)
{
if ((itCol->first == i) || (itCol->second == i))
{
itCol = collisionPairs.erase(itCol);
itCol--;
}
else
{
// Indices of objects after the one that is removed should be decreased by one.
if (itCol->first > i)
itCol->first--;
if (itCol->second > i)
itCol->second--;
}
}
geometryObjects.erase(it);
ngeoms--;
}
inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
#if BOOST_VERSION / 100 % 1000 >= 60
using namespace boost::placeholders;
#endif
GeometryObjectVector::const_iterator it = std::find_if(
geometryObjects.begin(), geometryObjects.end(),
boost::bind(&GeometryObject::name, _1) == name);
return GeomIndex(it - geometryObjects.begin());
}
inline bool GeometryModel::existGeometryName(const std::string & name) const
{
#if BOOST_VERSION / 100 % 1000 >= 60
using namespace boost::placeholders;
#endif
return std::find_if(
geometryObjects.begin(), geometryObjects.end(),
boost::bind(&GeometryObject::name, _1) == name)
!= geometryObjects.end();
}
inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel)
{
innerObjects.clear();
outerObjects.clear();
for (GeomIndex gid = 0; gid < geomModel.geometryObjects.size(); gid++)
innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid);
BOOST_FOREACH (const CollisionPair & pair, geomModel.collisionPairs)
{
outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second);
}
}
inline std::ostream & operator<<(std::ostream & os, const GeometryModel & geomModel)
{
os << "Nb geometry objects = " << geomModel.ngeoms << std::endl;
for (GeomIndex i = 0; i < (GeomIndex)(geomModel.ngeoms); ++i)
{
os << geomModel.geometryObjects[i] << std::endl;
}
return os;
}
inline std::ostream & operator<<(std::ostream & os, const GeometryData & geomData)
{
#ifdef PINOCCHIO_WITH_HPP_FCL
os << "Number of collision pairs = " << geomData.activeCollisionPairs.size() << std::endl;
for (PairIndex i = 0; i < (PairIndex)(geomData.activeCollisionPairs.size()); ++i)
{
os << "Pairs " << i << (geomData.activeCollisionPairs[i] ? " active" : " inactive")
<< std::endl;
}
#else
os << "WARNING** Without fcl library, no collision checking or distance computations are "
"possible. Only geometry placements can be computed."
<< std::endl;
#endif
os << "Number of geometry objects = " << geomData.oMg.size() << std::endl;
return os;
}
inline void GeometryModel::addCollisionPair(const CollisionPair & pair)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(
pair.first < ngeoms, "The input pair.first is larger than the number of geometries "
"contained in the GeometryModel");
PINOCCHIO_CHECK_INPUT_ARGUMENT(
pair.second < ngeoms, "The input pair.second is larger than the number of geometries "
"contained in the GeometryModel");
if (!existCollisionPair(pair))
{
collisionPairs.push_back(pair);
collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) =
(int)(collisionPairs.size() - 1);
collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) =
collisionPairMapping(
(Eigen::DenseIndex)pair.second,
(Eigen::DenseIndex)pair.first); // make symmetric
}
}
inline void GeometryModel::setCollisionPairs(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))
addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j));
}
else
{
if (map(j, i))
addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j));
}
}
}
}
inline void GeometryModel::addAllCollisionPairs()
{
removeAllCollisionPairs();
for (GeomIndex i = 0; i < ngeoms; ++i)
{
const JointIndex joint_i = geometryObjects[i].parentJoint;
for (GeomIndex j = i + 1; j < ngeoms; ++j)
{
const JointIndex joint_j = geometryObjects[j].parentJoint;
if (joint_i != joint_j)
addCollisionPair(CollisionPair(i, j));
}
}
}
inline void GeometryModel::removeCollisionPair(const CollisionPair & pair)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(
pair.first < ngeoms, "The input pair.first is larger than the number of geometries "
"contained in the GeometryModel");
PINOCCHIO_CHECK_INPUT_ARGUMENT(
pair.second < ngeoms, "The input pair.second is larger than the number of geometries "
"contained in the GeometryModel");
const long index = (long)findCollisionPair(pair);
if (index != (long)collisionPairs.size())
{
collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) =
collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) = -1;
collisionPairs.erase(collisionPairs.begin() + index);
for (Eigen::DenseIndex i = 0; i < collisionPairMapping.cols(); ++i)
{
for (Eigen::DenseIndex j = i + 1; j < collisionPairMapping.cols(); ++j)
{
if (collisionPairMapping(i, j) > index)
{
collisionPairMapping(i, j)--;
collisionPairMapping(j, i) = collisionPairMapping(i, j);
}
}
}
}
}
inline void GeometryModel::removeAllCollisionPairs()
{
collisionPairs.clear();
collisionPairMapping.fill(-1);
}
inline bool GeometryModel::existCollisionPair(const CollisionPair & pair) const
{
return collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second)
!= -1;
}
inline PairIndex GeometryModel::findCollisionPair(const CollisionPair & pair) const
{
int res = collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second);
if (res == -1)
return collisionPairs.size();
else
return (PairIndex)res;
}
inline void GeometryData::activateCollisionPair(const PairIndex pair_id)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(
pair_id < activeCollisionPairs.size(),
"The input argument pair_id is larger than the number of "
"collision pairs contained in activeCollisionPairs.");
activeCollisionPairs[pair_id] = 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::setGeometryCollisionStatus(
const GeometryModel & geom_model, const GeomIndex geom_id, const bool enable_collision)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(
geom_id < geom_model.ngeoms, "The index of the geometry is not valid");
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];
if (cp.first == geom_id || cp.second == geom_id)
{
activeCollisionPairs[k] = enable_collision;
}
}
}
#ifdef PINOCCHIO_WITH_HPP_FCL
inline void GeometryData::setSecurityMargins(
const GeometryModel & geom_model,
const MatrixXs & security_margin_map,
const bool upper,
const bool sync_distance_upper_bound)
{
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 consistent.");
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);
if (sync_distance_upper_bound)
collisionRequests[k].distance_upper_bound = collisionRequests[k].security_margin;
}
}
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
inline void GeometryData::deactivateCollisionPair(const PairIndex pair_id)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(
pair_id < activeCollisionPairs.size(),
"The input argument pair_id is larger than the number of "
"collision pairs contained in activeCollisionPairs.");
activeCollisionPairs[pair_id] = false;
}
inline void GeometryData::deactivateAllCollisionPairs()
{
std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), false);
}
} // namespace pinocchio
#endif // ifndef __pinocchio_multibody_geometry_hxx__