Skip to content

GitLab

Unverified Commit 6593485f authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1131 from jcarpent/devel 

Support Collision pairs even when hpp-fcl is not present
parents 59f55346 e11ed414
Hide whitespace changes
Inline Side-by-side
bindings/python/multibody/geometry-data.hpp
View file @ 6593485f
......@@ -46,7 +46,7 @@ namespace pinocchio
}; // struct CollisionPairPythonVisitor
struct GeometryDataPythonVisitor
: public boost::python::def_visitor< GeometryDataPythonVisitor >
: public boost::python::def_visitor< GeometryDataPythonVisitor >
{
/* --- Exposing C++ API to python through the handler ----------------- */
......@@ -61,11 +61,11 @@ namespace pinocchio
&GeometryData::oMg,
"Vector of collision objects placement relative to the world frame.\n"
"note: These quantities have to be updated by calling updateGeometryPlacements.")
#ifdef PINOCCHIO_WITH_HPP_FCL
.def_readonly("activeCollisionPairs",
&GeometryData::activeCollisionPairs,
"Vector of active CollisionPairs")
#ifdef PINOCCHIO_WITH_HPP_FCL
.def_readonly("distanceRequest",
&GeometryData::distanceRequest,
"Defines which information should be computed by FCL for distance computations")
......@@ -86,6 +86,7 @@ namespace pinocchio
&GeometryData::radius,
"Vector of radius of bodies, i.e. the distance between the further point of the geometry object from the joint center.\n"
"note: This radius information might be usuful in continuous collision checking")
#endif // PINOCCHIO_WITH_HPP_FCL
.def("fillInnerOuterObjectMaps", &GeometryData::fillInnerOuterObjectMaps,
bp::args("self","GeometryModel"),
......@@ -98,9 +99,6 @@ namespace pinocchio
.def("deactivateCollisionPair",&GeometryData::deactivateCollisionPair,
bp::args("self","pair_id"),
"Deactivate the collsion pair pair_id in geomModel.collisionPairs if it exists.")
#endif // PINOCCHIO_WITH_HPP_FCL
;
}
......
bindings/python/multibody/geometry-model.hpp
View file @ 6593485f
......@@ -29,14 +29,14 @@ namespace pinocchio
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Default constructor"))
.def(bp::init<>(bp::arg("self"),"Default constructor"))
.add_property("ngeoms", &GeometryModel::ngeoms, "Number of geometries contained in the Geometry Model.")
.add_property("geometryObjects",
&GeometryModel::geometryObjects,"Vector of geometries objects.")
.def("addGeometryObject",
static_cast <GeometryModel::GeomIndex (GeometryModel::*)(const GeometryObject &)>(&GeometryModel::addGeometryObject),
bp::arg("geometry_object"),
bp::args("self","geometry_object"),
"Add a GeometryObject to a GeometryModel.\n"
"Parameters\n"
"\tgeometry_object : a GeometryObject\n")
......@@ -61,7 +61,6 @@ namespace pinocchio
&GeometryModelPythonVisitor::createData,
bp::arg("self"),
"Create a GeometryData associated to the current model.")
#ifdef PINOCCHIO_WITH_HPP_FCL
.add_property("collisionPairs",
&GeometryModel::collisionPairs,
"Vector of collision pairs.")
......@@ -82,8 +81,7 @@ namespace pinocchio
.def("findCollisionPair", &GeometryModel::findCollisionPair,
bp::args("collision_pair"),
"Return the index of a collision pair.")
#endif // PINOCCHIO_WITH_HPP_FCL
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
......
bindings/python/parsers/srdf.cpp
View file @ 6593485f
......@@ -14,7 +14,6 @@ namespace pinocchio
namespace bp = boost::python;
#ifdef PINOCCHIO_WITH_HPP_FCL
BOOST_PYTHON_FUNCTION_OVERLOADS(removeCollisionPairs_overload,
srdf::removeCollisionPairs,
3,4)
......@@ -22,8 +21,7 @@ namespace pinocchio
BOOST_PYTHON_FUNCTION_OVERLOADS(removeCollisionPairsFromXML_overload,
srdf::removeCollisionPairsFromXML,
3,4)
#endif
BOOST_PYTHON_FUNCTION_OVERLOADS(loadReferenceConfigurations_overload,
srdf::loadReferenceConfigurations,
2,3)
......@@ -47,7 +45,6 @@ namespace pinocchio
void exposeSRDFParser()
{
#ifdef PINOCCHIO_WITH_HPP_FCL
bp::def("removeCollisionPairs",
static_cast<void (*)(const Model &, GeometryModel &, const std::string &, const bool)>(&srdf::removeCollisionPairs),
removeCollisionPairs_overload(bp::args("model", "geom_model","srdf_filename", "verbose"),
......@@ -69,8 +66,7 @@ namespace pinocchio
"\tsrdf_xml_stream: XML stream containing the SRDF information with the collision pairs to remove\n"
"\tverbose: [optional] display to the current terminal some internal information"
));
#endif
bp::def("loadReferenceConfigurations",
static_cast<void (*)(Model &, const std::string &, const bool)>(&srdf::loadReferenceConfigurations),
loadReferenceConfigurations_overload(bp::args("model","srdf_filename","verbose"),
......
cmake @ a5c65a0e
Compare d1340a9e...a5c65a0e
Subproject commit d1340a9e0340c443e5e8841c7ab4a0997f989e59
Subproject commit a5c65a0ee51191be3f2e2667b95830706c211bb2
src/algorithm/geometry.hxx
View file @ 6593485f
......@@ -80,12 +80,16 @@ namespace pinocchio
for (std::size_t cpt = 0; cpt < geom_model.collisionPairs.size(); ++cpt)
{
if(geom_data.activeCollisionPairs[cpt])
{
computeCollision(geom_model,geom_data,cpt);
if(!isColliding && geom_data.collisionResults[cpt].isCollision())
{
computeCollision(geom_model,geom_data,cpt);
isColliding |= geom_data.collisionResults[cpt].isCollision();
if(isColliding && stopAtFirstCollision)
isColliding = true;
geom_data.collisionPairIndex = cpt; // first pair to be in collision
if(stopAtFirstCollision)
return true;
}
}
}
return isColliding;
......
src/multibody/geometry.hpp
View file @ 6593485f
......@@ -6,11 +6,9 @@
#define __pinocchio_multibody_geometry_hpp__
#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include <iostream>
#include <boost/foreach.hpp>
#include <map>
#include <list>
......@@ -84,7 +82,6 @@ namespace pinocchio
*/
bool existGeometryName(const std::string & name) const;
#ifdef PINOCCHIO_WITH_HPP_FCL
///
/// \brief Add a collision pair into the vector of collision_pairs.
/// The method check before if the given CollisionPair is already included.
......@@ -131,8 +128,7 @@ namespace pinocchio
///
PairIndex findCollisionPair(const CollisionPair & pair) const;
#endif // PINOCCHIO_WITH_HPP_FCL
///
/// \brief Returns true if *this and other are equal.
///
......@@ -176,6 +172,7 @@ namespace pinocchio
enum { Options = 0 };
typedef SE3Tpl<Scalar,Options> SE3;
typedef std::vector<GeomIndex> GeomIndexList;
///
/// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
......@@ -186,6 +183,11 @@ namespace pinocchio
///
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMg;
///
/// \brief Vector of collision pairs.
///
std::vector<bool> activeCollisionPairs;
#ifdef PINOCCHIO_WITH_HPP_FCL
///
/// \brief Collision objects (ie a fcl placed geometry).
......@@ -195,11 +197,6 @@ namespace pinocchio
///
std::vector<fcl::CollisionObject> collisionObjects;
///
/// \brief Vector of collision pairs.
///
std::vector<bool> activeCollisionPairs;
///
/// \brief Defines what information should be computed by distance computation.
///
......@@ -227,33 +224,29 @@ namespace pinocchio
std::vector<double> radius;
///
/// \brief index of the collision pair
/// \brief Index of the collision pair
///
/// It is used by some method to return additional information. For instance,
/// the algo computeCollisions() sets it to the first colliding pair.
///
PairIndex collisionPairIndex;
typedef std::vector<GeomIndex> GeomIndexList;
#endif // PINOCCHIO_WITH_HPP_FCL
/// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
///
///
/// The map lists the collision GeometryObjects associated to a given joint Id.
/// Inner objects can be seen as geometry objects that directly move when the associated joint moves
std::map < JointIndex, GeomIndexList > innerObjects;
std::map<JointIndex,GeomIndexList> innerObjects;
/// \brief A list of associated collision GeometryObjects to a given joint Id
///
/// Outer objects can be seen as geometry objects that may often be
/// obstacles to the Inner objects of given joint
std::map < JointIndex, GeomIndexList > outerObjects;
#endif // PINOCCHIO_WITH_HPP_FCL
std::map<JointIndex,GeomIndexList> outerObjects;
GeometryData(const GeometryModel & geomModel);
~GeometryData() {};
#ifdef PINOCCHIO_WITH_HPP_FCL
/// Fill both innerObjects and outerObjects maps, from vectors collisionObjects and
/// collisionPairs.
///
......@@ -291,7 +284,6 @@ namespace pinocchio
///
void deactivateCollisionPair(const PairIndex pairId);
#endif //PINOCCHIO_WITH_HPP_FCL
friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
}; // struct GeometryData
......
src/multibody/geometry.hxx
View file @ 6593485f
......@@ -5,10 +5,7 @@
#ifndef __pinocchio_multibody_geometry_hxx__
#define __pinocchio_multibody_geometry_hxx__
#include <iostream>
#include <map>
#include <list>
#include <utility>
#include "pinocchio/multibody/model.hpp"
/// @cond DEV
......@@ -16,29 +13,28 @@ namespace pinocchio
{
inline GeometryData::GeometryData(const GeometryModel & geom_model)
: oMg(geom_model.ngeoms)
#ifdef PINOCCHIO_WITH_HPP_FCL
, activeCollisionPairs(geom_model.collisionPairs.size(), true)
#ifdef PINOCCHIO_WITH_HPP_FCL
, distanceRequest(true)
, distanceResults(geom_model.collisionPairs.size())
, 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
collisionObjects.reserve(geom_model.geometryObjects.size());
BOOST_FOREACH(const GeometryObject & geom_object, geom_model.geometryObjects)
{
collisionObjects.push_back(fcl::CollisionObject(geom_object.geometry));
}
#endif
fillInnerOuterObjectMaps(geom_model);
}
#else
{}
#endif // PINOCCHIO_WITH_HPP_FCL
template<typename S2, int O2, template<typename,int> class JointCollectionTpl>
GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object,
const ModelTpl<S2,O2,JointCollectionTpl> & model)
......@@ -71,8 +67,6 @@ namespace pinocchio
return GeomIndex(it - geometryObjects.begin());
}
inline bool GeometryModel::existGeometryName(const std::string & name) const
{
return std::find_if(geometryObjects.begin(),
......@@ -80,7 +74,6 @@ namespace pinocchio
boost::bind(&GeometryObject::name, _1) == name) != geometryObjects.end();
}
#ifdef PINOCCHIO_WITH_HPP_FCL
inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel)
{
innerObjects.clear();
......@@ -94,7 +87,6 @@ namespace pinocchio
outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second);
}
}
#endif
inline std::ostream & operator<< (std::ostream & os, const GeometryModel & geomModel)
{
......@@ -125,8 +117,6 @@ namespace pinocchio
return os;
}
#ifdef PINOCCHIO_WITH_HPP_FCL
inline void GeometryModel::addCollisionPair(const CollisionPair & pair)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < ngeoms,
......@@ -196,7 +186,6 @@ namespace pinocchio
activeCollisionPairs[pairId] = false;
}
#endif //PINOCCHIO_WITH_HPP_FCL
} // namespace pinocchio
/// @endcond
......
src/parsers/srdf.hpp
View file @ 6593485f
......@@ -13,8 +13,6 @@ namespace pinocchio
namespace srdf
{
#ifdef PINOCCHIO_WITH_HPP_FCL
///
/// \brief Deactive all possible collision pairs mentioned in the SRDF file.
/// It throws if the SRDF file is incorrect.
......@@ -44,8 +42,6 @@ namespace pinocchio
const std::string & xmlString,
const bool verbose = false);
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
///
/// \brief Get the reference configurations of a given model associated to a SRDF file.
/// It throws if the SRDF file is incorrect. The reference configurations are
......@@ -76,7 +72,6 @@ namespace pinocchio
std::istream & xmlStream,
const bool verbose = false);
///
/// \brief Load the rotor params of a given model associated to a SRDF file.
/// It throws if the SRDF file is incorrect.
......
src/parsers/srdf.hxx
View file @ 6593485f
......@@ -23,8 +23,6 @@ namespace pinocchio
{
namespace srdf
{
#ifdef PINOCCHIO_WITH_HPP_FCL
namespace details
{
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
......@@ -33,6 +31,8 @@ namespace pinocchio
std::istream & stream,
const bool verbose = false)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
// Read xml stream
using boost::property_tree::ptree;
ptree pt;
......@@ -129,8 +129,6 @@ namespace pinocchio
details::removeCollisionPairs(model, geom_model, srdf_stream, verbose);
}
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
bool loadRotorParameters(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const std::string & filename,
......@@ -210,8 +208,8 @@ namespace pinocchio
typedef typename Model::ConfigVectorType ConfigVectorType;
typedef boost::fusion::vector<const std::string&,
const ConfigVectorType&,
ConfigVectorType&> ArgsType;
const ConfigVectorType&,
ConfigVectorType&> ArgsType;
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & joint,
......@@ -225,9 +223,9 @@ namespace pinocchio
private:
template<int axis>
static void _algo (const JointModelRevoluteUnboundedTpl<Scalar,Options,axis> & joint,
const std::string& joint_name,
const ConfigVectorType& fromXML,
ConfigVectorType& config)
const std::string& joint_name,
const ConfigVectorType& fromXML,
ConfigVectorType& config)
{
typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelRUB;
PINOCCHIO_STATIC_ASSERT(JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS);
......@@ -235,16 +233,16 @@ namespace pinocchio
std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl;
else {
SINCOS(fromXML[0],
&config[joint.idx_q()+1],
&config[joint.idx_q()+0]);
&config[joint.idx_q()+1],
&config[joint.idx_q()+0]);
}
}
template<typename JointModel>
static void _algo (const JointModel & joint,
const std::string& joint_name,
const ConfigVectorType& fromXML,
ConfigVectorType& config)
const std::string& joint_name,
const ConfigVectorType& fromXML,
ConfigVectorType& config)
{
if (joint.nq() != fromXML.size())
std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl;
......
src/parsers/urdf/geometry.cpp
View file @ 6593485f
//
// Copyright (c) 2015-2020 CNRS INRIA
//
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/types.hpp"
#include "pinocchio/parsers/utils.hpp"
......
Markdown is supported
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