Unverified Commit 1377b269 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1010 from jcarpent/topic/collision

Add C++ example for collision
parents f35ef164 cd88459a
Subproject commit a99dc925bde7b976ae1ed286d0ab38906c73dff5
Subproject commit 811bfc472940425a0a96cca7e01331f6cf68fced
#
# Copyright (c) 2015-2019 CNRS INRIA
# Copyright (c) 2015-2020 CNRS INRIA
#
SET(${PROJECT_NAME}_EXAMPLES
......@@ -13,6 +13,12 @@ SET(${PROJECT_NAME}_EXAMPLES
geometry-models
)
IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_EXAMPLES
collisions
)
ENDIF(HPP_FCL_FOUND)
ADD_DEFINITIONS(-DPINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}")
FOREACH(EXAMPLE ${${PROJECT_NAME}_EXAMPLES})
......
// write cpp example here
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include <iostream>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own modeldirectory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int /*argc*/, char ** /*argv*/)
{
using namespace pinocchio;
const std::string robots_model_path = PINOCCHIO_MODEL_DIR + std::string("/others/robots/");
// You should change here to set up your own URDF file
const std::string urdf_filename = robots_model_path + std::string("talos_data/urdf/talos_reduced.urdf");
// You should change here to set up your own SRDF file
const std::string srdf_filename = robots_model_path + std::string("talos_data/srdf/talos.srdf");
// Load the URDF model contained in urdf_filename
Model model;
pinocchio::urdf::buildModel(urdf_filename,model);
// Build the data associated to the model
Data data(model);
// Load the geometries associated to model which are contained in the URDF file
GeometryModel geom_model;
pinocchio::urdf::buildGeom(model, urdf_filename, pinocchio::COLLISION, geom_model, robots_model_path);
// Add all possible collision pairs and remove the ones collected in the SRDF file
geom_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename);
// Build the data associated to the geom_model
GeometryData geom_data(geom_model); // contained the intermediate computations, like the placement of all the geometries with respect to the world frame
// Load the reference configuration of the robots (this one should be collision free)
pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename); // the reference configuratio stored in the SRDF file is called half_sitting
const Model::ConfigVectorType & q = model.referenceConfigurations["half_sitting"];
// And test all the collision pairs
computeCollisions(model,data,geom_model,geom_data,q);
// Print the status of all the collision pairs
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
{
const CollisionPair & cp = geom_model.collisionPairs[k];
const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k];
std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: ";
std::cout << (cr.isCollision() ? "yes" : "no") << std::endl;
}
// If you want to stop as soon as a collision is encounter, just add false for the final default argument stopAtFirstCollision
computeCollisions(model,data,geom_model,geom_data,q,true);
// And if you to check only one collision pair, e.g. the third one, at the neutral element of the Configuration Space of the robot
const PairIndex pair_id = 2;
const Model::ConfigVectorType q_neutral = neutral(model);
updateGeometryPlacements(model, data, geom_model, geom_data, q_neutral); // performs a forward kinematics over the whole kinematics model + update the placement of all the geometries contained inside geom_model
computeCollision(geom_model, geom_data, pair_id);
return 0;
}
......@@ -27,7 +27,7 @@ srdf_filename = "romeo.srdf"
srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename
pin.removeCollisionPairs(model,geom_model,srdf_model_path)
print("num collision pairs - final:",len(geom_model.collisionPairs))
print("num collision pairs - after removing useless collision pairs:",len(geom_model.collisionPairs))
# Load reference configuration
pin.loadReferenceConfigurations(model,srdf_model_path)
......@@ -42,6 +42,12 @@ geom_data = pin.GeometryData(geom_model)
# Compute all the collisions
pin.computeCollisions(model,data,geom_model,geom_data,q,False)
# Print the status of collision for all collision pairs
for k in range(len(geom_model.collisionPairs)):
cr = geom_data.collisionResults[k]
cp = geom_model.collisionPairs[k]
print("collision pair:",cp.first,",",cp.second,"- collision:","Yes" if cr.isCollision() else "No")
# Compute for a single pair of collision
pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)
pin.computeCollision(geom_model,geom_data,0)
......
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_algo_geometry_hpp__
......@@ -23,15 +23,15 @@ namespace pinocchio
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] geomModel The geometry model containing the collision objects.
/// \param[out] geomData The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
/// \param[in] geom_model The geometry model containing the collision objects.
/// \param[out] geom_data The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
/// \param[in] q The joint configuration vector (dim model.nq).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q);
///
......@@ -41,38 +41,38 @@ namespace pinocchio
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] geomModel The geometry model containing the collision objects.
/// \param[out] geomData The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
/// \param[in] geom_model The geometry model containing the collision objects.
/// \param[out] geom_data The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData);
const GeometryModel & geom_model,
GeometryData & geom_data);
///
/// \brief Set a mesh scaling vector to each GeometryObject contained in the the GeometryModel.
///
/// param[in] geomModel The geometry model containing the collision objects.
/// param[in] geom_model The geometry model containing the collision objects.
/// param[in] meshScale The scale to be applied to each GeometryObject
///
template<typename Vector3Like>
inline void setGeometryMeshScales(GeometryModel & geomModel, const Eigen::MatrixBase<Vector3Like> & meshScale)
inline void setGeometryMeshScales(GeometryModel & geom_model, const Eigen::MatrixBase<Vector3Like> & meshScale)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
for(GeomIndex index=0; index<geomModel.ngeoms; index++)
geomModel.geometryObjects[index].meshScale = meshScale;
for(GeomIndex index=0; index<geom_model.ngeoms; index++)
geom_model.geometryObjects[index].meshScale = meshScale;
}
///
/// \brief Set an isotropic mesh scaling to each GeometryObject contained in the the GeometryModel.
///
/// param[in] geomModel The geometry model containing the collision objects.
/// param[in] geom_model The geometry model containing the collision objects.
/// param[in] meshScale The scale, to be applied to each GeometryObject, equally in all directions
///
inline void setGeometryMeshScales(GeometryModel & geomModel, const double meshScale)
inline void setGeometryMeshScales(GeometryModel & geom_model, const double meshScale)
{
setGeometryMeshScales(geomModel, Eigen::Vector3d::Constant(meshScale));
setGeometryMeshScales(geom_model, Eigen::Vector3d::Constant(meshScale));
}
#ifdef PINOCCHIO_WITH_HPP_FCL
......@@ -86,10 +86,10 @@ namespace pinocchio
/// \param[in] pairId The collsion pair index in the GeometryModel.
///
/// \return Return true is the collision objects are colliding.
/// \note The complete collision result is also available in geomData.collisionResults[pairId]
/// \note The complete collision result is also available in geom_data.collisionResults[pairId]
///
bool computeCollision(const GeometryModel & geomModel,
GeometryData & geomData,
bool computeCollision(const GeometryModel & geom_model,
GeometryData & geom_data,
const PairIndex & pairId);
///
......@@ -101,8 +101,8 @@ namespace pinocchio
///
/// \param[in] model robot model (const)
/// \param[out] data corresponding data (nonconst) where FK results are stored
/// \param[in] geomModel geometry model (const)
/// \param[out] geomData corresponding geometry data (nonconst) where distances are computed
/// \param[in] geom_model geometry model (const)
/// \param[out] geom_data corresponding geometry data (nonconst) where distances are computed
/// \param[in] q robot configuration.
/// \param[in] stopAtFirstCollision if true, stop the loop on pairs after the first collision.
/// \return When ComputeShortest is true, the index of the collision pair which has the shortest distance.
......@@ -115,8 +115,8 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline bool computeCollisions(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool stopAtFirstCollision = false);
......@@ -128,11 +128,11 @@ namespace pinocchio
/// \param[in] pairId The index of the collision pair in geom model.
///
/// \return A reference on fcl struct containing the distance result, referring an element
/// of vector geomData::distanceResults.
/// \note The complete distance result is also available in geomData.distanceResults[pairId]
/// of vector geom_data::distanceResults.
/// \note The complete distance result is also available in geom_data.distanceResults[pairId]
///
fcl::DistanceResult & computeDistance(const GeometryModel & geomModel,
GeometryData & geomData,
fcl::DistanceResult & computeDistance(const GeometryModel & geom_model,
GeometryData & geom_data,
const PairIndex & pairId);
///
......@@ -144,8 +144,8 @@ namespace pinocchio
///
/// \param[in] model: robot model (const)
/// \param[in] data: corresponding data (nonconst) where FK results are stored
/// \param[in] geomModel: geometry model (const)
/// \param[out] geomData: corresponding geometry data (nonconst) where distances are computed
/// \param[in] geom_model: geometry model (const)
/// \param[out] geom_data: corresponding geometry data (nonconst) where distances are computed
/// \param[in] q: robot configuration.
///
/// \note A similar function is available without model, data and q, not recomputing the FK.
......@@ -153,8 +153,8 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q);
///
......@@ -165,16 +165,16 @@ namespace pinocchio
///
/// \param[in] model: robot model (const)
/// \param[out] data: corresponding data (const)
/// \param[in] geomModel: geometry model (const)
/// \param[out] geomData: corresponding geometry data (nonconst) where distances are computed
/// \param[in] geom_model: geometry model (const)
/// \param[out] geom_data: corresponding geometry data (nonconst) where distances are computed
///
/// \note A similar function is available without model, data and q, not recomputing the FK.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData);
const GeometryModel & geom_model,
GeometryData & geom_data);
///
/// Compute the radius of the geometry volumes attached to every joints.
......@@ -182,29 +182,29 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void computeBodyRadius(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const GeometryModel & geomModel,
GeometryData & geomData);
const GeometryModel & geom_model,
GeometryData & geom_data);
#endif // PINOCCHIO_WITH_HPP_FCL
///
/// Append geomModel2 to geomModel1
/// Append geom_model2 to geom_model1
///
/// The steps for appending are:
/// \li add GeometryObject of geomModel2 to geomModel1,
/// \li add the collision pairs of geomModel2 into geomModel1 (indexes are updated)
/// \li add all the collision pairs between geometry objects of geomModel1 and geomModel2.
/// \li add GeometryObject of geom_model2 to geom_model1,
/// \li add the collision pairs of geom_model2 into geom_model1 (indexes are updated)
/// \li add all the collision pairs between geometry objects of geom_model1 and geom_model2.
/// It is possible to ommit both data (an additional function signature is available which makes
/// them optionnal), then inner/outer objects are not updated.
///
/// \param[out] geomModel1 geometry model where the data is added
/// \param[in] geomModel2 geometry model from which new geometries are taken
/// \param[out] geom_model1 geometry model where the data is added
/// \param[in] geom_model2 geometry model from which new geometries are taken
///
/// \note Of course, the geomData corresponding to geomModel1 will not be valid anymore,
/// and should be updated (or more simply, re-created from the new setting of geomModel1).
/// \note Of course, the geom_data corresponding to geom_model1 will not be valid anymore,
/// and should be updated (or more simply, re-created from the new setting of geom_model1).
/// \todo This function is not asserted in unittest.
///
inline void appendGeometryModel(GeometryModel & geomModel1,
const GeometryModel & geomModel2);
inline void appendGeometryModel(GeometryModel & geom_model1,
const GeometryModel & geom_model2);
} // namespace pinocchio
......
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_algo_geometry_hxx__
......@@ -15,32 +15,32 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
forwardKinematics(model, data, q);
updateGeometryPlacements(model, data, geomModel, geomData);
updateGeometryPlacements(model, data, geom_model, geom_data);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData)
const GeometryModel & geom_model,
GeometryData & geom_data)
{
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
for (GeomIndex i=0; i < (GeomIndex) geomModel.ngeoms; ++i)
for (GeomIndex i=0; i < (GeomIndex) geom_model.ngeoms; ++i)
{
const Model::JointIndex & joint = geomModel.geometryObjects[i].parentJoint;
if (joint>0) geomData.oMg[i] = (data.oMi[joint] * geomModel.geometryObjects[i].placement);
else geomData.oMg[i] = geomModel.geometryObjects[i].placement;
const Model::JointIndex & joint = geom_model.geometryObjects[i].parentJoint;
if (joint>0) geom_data.oMg[i] = (data.oMi[joint] * geom_model.geometryObjects[i].placement);
else geom_data.oMg[i] = geom_model.geometryObjects[i].placement;
#ifdef PINOCCHIO_WITH_HPP_FCL
geomData.collisionObjects[i].setTransform( toFclTransform3f(geomData.oMg[i]) );
geom_data.collisionObjects[i].setTransform( toFclTransform3f(geom_data.oMg[i]) );
#endif // PINOCCHIO_WITH_HPP_FCL
}
}
......@@ -50,39 +50,39 @@ namespace pinocchio
/* --- COLLISIONS ----------------------------------------------------------------- */
/* --- COLLISIONS ----------------------------------------------------------------- */
inline bool computeCollision(const GeometryModel & geomModel,
GeometryData & geomData,
inline bool computeCollision(const GeometryModel & geom_model,
GeometryData & geom_data,
const PairIndex & pairId)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId];
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geom_model.collisionPairs.size() );
const CollisionPair & pair = geom_model.collisionPairs[pairId];
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomData.collisionResults.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geom_data.collisionResults.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geom_data.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_data.collisionObjects.size() );
fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
fcl::CollisionResult& collisionResult = geom_data.collisionResults[pairId];
collisionResult.clear();
fcl::collide (&geomData.collisionObjects[pair.first],
&geomData.collisionObjects[pair.second],
geomData.collisionRequest,
fcl::collide (&geom_data.collisionObjects[pair.first],
&geom_data.collisionObjects[pair.second],
geom_data.collisionRequest,
collisionResult);
return collisionResult.isCollision();
}
inline bool computeCollisions(const GeometryModel & geomModel,
GeometryData & geomData,
inline bool computeCollisions(const GeometryModel & geom_model,
GeometryData & geom_data,
const bool stopAtFirstCollision = true)
{
bool isColliding = false;
for (std::size_t cpt = 0; cpt < geomModel.collisionPairs.size(); ++cpt)
for (std::size_t cpt = 0; cpt < geom_model.collisionPairs.size(); ++cpt)
{
if(geomData.activeCollisionPairs[cpt])
if(geom_data.activeCollisionPairs[cpt])
{
computeCollision(geomModel,geomData,cpt);
isColliding |= geomData.collisionResults[cpt].isCollision();
computeCollision(geom_model,geom_data,cpt);
isColliding |= geom_data.collisionResults[cpt].isCollision();
if(isColliding && stopAtFirstCollision)
return true;
}
......@@ -95,56 +95,56 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline bool computeCollisions(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool stopAtFirstCollision)
{
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements(model, data, geomModel, geomData, q);
updateGeometryPlacements(model, data, geom_model, geom_data, q);
return computeCollisions(geomModel,geomData, stopAtFirstCollision);
return computeCollisions(geom_model,geom_data, stopAtFirstCollision);
}
/* --- DISTANCES ----------------------------------------------------------------- */
/* --- DISTANCES ----------------------------------------------------------------- */
/* --- DISTANCES ----------------------------------------------------------------- */
inline fcl::DistanceResult & computeDistance(const GeometryModel & geomModel,
GeometryData & geomData,
inline fcl::DistanceResult & computeDistance(const GeometryModel & geom_model,
GeometryData & geom_data,
const PairIndex & pairId)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId];
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geom_model.collisionPairs.size() );
const CollisionPair & pair = geom_model.collisionPairs[pairId];
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomData.distanceResults.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geom_data.distanceResults.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geom_data.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_data.collisionObjects.size() );
geomData.distanceResults[pairId].clear();
fcl::distance ( &geomData.collisionObjects[pair.first],
&geomData.collisionObjects[pair.second],
geomData.distanceRequest,
geomData.distanceResults[pairId]);
geom_data.distanceResults[pairId].clear();
fcl::distance ( &geom_data.collisionObjects[pair.first],
&geom_data.collisionObjects[pair.second],
geom_data.distanceRequest,
geom_data.distanceResults[pairId]);
return geomData.distanceResults[pairId];
return geom_data.distanceResults[pairId];
}
inline std::size_t computeDistances(const GeometryModel & geomModel,
GeometryData & geomData)
inline std::size_t computeDistances(const GeometryModel & geom_model,
GeometryData & geom_data)
{
std::size_t min_index = geomModel.collisionPairs.size();
std::size_t min_index = geom_model.collisionPairs.size();
double min_dist = std::numeric_limits<double>::infinity();
for (std::size_t cpt = 0; cpt < geomModel.collisionPairs.size(); ++cpt)
for (std::size_t cpt = 0; cpt < geom_model.collisionPairs.size(); ++cpt)
{
if(geomData.activeCollisionPairs[cpt])
if(geom_data.activeCollisionPairs[cpt])
{
computeDistance(geomModel,geomData,cpt);
if(geomData.distanceResults[cpt].min_distance < min_dist)
computeDistance(geom_model,geom_data,cpt);
if(geom_data.distanceResults[cpt].min_distance < min_dist)
{
min_index = cpt;
min_dist = geomData.distanceResults[cpt].min_distance;
min_dist = geom_data.distanceResults[cpt].min_distance;
}
}
}
......@@ -154,24 +154,24 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData)
const GeometryModel & geom_model,
GeometryData & geom_data)
{
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements(model,data,geomModel,geomData);
return computeDistances(geomModel,geomData);
updateGeometryPlacements(model,data,geom_model,geom_data);
return computeDistances(geom_model,geom_data);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geomModel,
GeometryData & geomData,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements(model, data, geomModel, geomData, q);
return computeDistances(geomModel,geomData);
updateGeometryPlacements(model, data, geom_model, geom_data, q);
return computeDistances(geom_model,geom_data);
}
/* --- RADIUS -------------------------------------------------------------------- */
......@@ -191,19 +191,19 @@ namespace pinocchio
/// in some continuous collision test.
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void computeBodyRadius(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const GeometryModel & geomModel,
GeometryData & geomData)
const GeometryModel & geom_model,
GeometryData & geom_data)
{
geomData.radius.resize(model.joints.size(),0);
BOOST_FOREACH(const GeometryObject & geom_object,geomModel.geometryObjects)
geom_data.radius.resize(model.joints.size(),0);
BOOST_FOREACH(const GeometryObject & geom_object,geom_model.geometryObjects)
{
const GeometryObject::CollisionGeometryPtr & geometry
= geom_object.geometry;
const GeometryModel::SE3 & jMb = geom_object.placement; // placement in joint.
const Model::JointIndex & i = geom_object.parentJoint;
assert (i<geomData.radius.size());
assert (i<geom_data.radius.size());
double radius = geomData.radius[i] * geomData.radius[i];
double radius = geom_data.radius[i] * geom_data.radius[i];
// The radius is simply the one of the 8 corners of the AABB cube, expressed
// in the joint frame, whose norm is the highest.
......@@ -217,7 +217,7 @@ namespace pinocchio
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,max,max)).squaredNorm(),radius);
// Don't forget to sqroot the squared norm before storing it.
geomData.radius[i] = sqrt(radius);
geom_data.radius[i] = sqrt(radius);
}
}
......@@ -226,35 +226,35 @@ namespace pinocchio
/* --- APPEND GEOMETRY MODEL ----------------------------------------------------------- */
inline void appendGeometryModel(GeometryModel & geomModel1,
const GeometryModel & geomModel2)
inline void appendGeometryModel(GeometryModel & geom_model1,
const GeometryModel & geom_model2)
{
assert (geomModel1.ngeoms == geomModel1.geometryObjects.size());
Index nGeom1 = geomModel1.geometryObjects.size();
Index nColPairs1 = geomModel1.collisionPairs.size();
assert (geomModel2.ngeoms == geomModel2.geometryObjects.size());
Index nGeom2 = geomModel2.geometryObjects.size();
Index nColPairs2 = geomModel2.collisionPairs.size();
assert (geom_model1.ngeoms ==