From 8cc9df5435425f40788ae65e34d2a10b8f19829e Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 22 Jul 2016 11:35:29 +0200
Subject: [PATCH] [C++] Added radius algorithm (related to geometry).
---
src/algorithm/geometry.hpp | 6 ++--
src/algorithm/geometry.hxx | 56 +++++++++++++++++++++++++++++++++++---
src/multibody/geometry.hpp | 8 ++++++
unittest/geom.cpp | 29 +++++++++++++++++++-
4 files changed, 91 insertions(+), 8 deletions(-)
diff --git a/src/algorithm/geometry.hpp b/src/algorithm/geometry.hpp
index f4747025e..aabea1f48 100644
--- a/src/algorithm/geometry.hpp
+++ b/src/algorithm/geometry.hpp
@@ -15,8 +15,8 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#ifndef __se3_geometry_hpp__
-#define __se3_geometry_hpp__
+#ifndef __se3_algo_geometry_hpp__
+#define __se3_algo_geometry_hpp__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
@@ -87,4 +87,4 @@ namespace se3
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/geometry.hxx"
-#endif // ifndef __se3_geometry_hpp__
+#endif // ifndef __se3_algo_geometry_hpp__
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 5fbaa70dc..abd9c8b9f 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -15,8 +15,10 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-#ifndef __se3_geometry_hxx__
-#define __se3_geometry_hxx__
+#ifndef __se3_algo_geometry_hxx__
+#define __se3_algo_geometry_hxx__
+
+#include <boost/foreach.hpp>
namespace se3
{
@@ -95,7 +97,53 @@ namespace se3
updateGeometryPlacements (model, data, model_geom, data_geom, q);
computeDistances(data_geom);
}
-
+
+ /// Given p1..3 being either "min" or "max", return one of the corners of the
+ /// AABB cub of the FCL object.
+#define SE3_GEOM_AABB(FCL,p1,p2,p3) \
+ SE3::Vector3( \
+ FCL->aabb_local.p1##_ [0], \
+ FCL->aabb_local.p1##_ [1], \
+ FCL->aabb_local.p1##_ [2])
+
+
+ /// For all bodies of the model, compute the point of the geometry model
+ /// that is the further from the center of the joint. This quantity is used
+ /// in some continuous collision test.
+ inline void computeBodyRadius(const Model & model,
+ const GeometryModel & geomModel,
+ GeometryData & geomData)
+ {
+ geomData.radius.resize(model.joints.size());
+ BOOST_FOREACH(const GeometryObject & geom,geomModel.geometryObjects)
+ {
+ std::cout << "New body radius" << geom.name << std::endl;
+ const boost::shared_ptr<const fcl::CollisionGeometry> & fcl
+ = geom.collision_object.collisionGeometry();
+ const SE3 & jMb = geom.placement; // placement in joint.
+
+ double radius = 0.0;
+
+ // The radius is simply the one of the 8 corners of the AABB cube, expressed
+ // in the joint frame, whose norm is the highest.
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,min,min)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,min,max)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,max,min)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,max,max)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,min,min)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,min,max)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,max,min)).squaredNorm(),radius);
+ radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,max,max)).squaredNorm(),radius);
+
+ // Don't forget to sqroot the squared norm before storing it.
+ assert (geom.parent<geomData.radius.size());
+ geomData.radius[geom.parent] = sqrt(radius);
+ }
+ }
+
+#undef SE3_GEOM_AABB
+
+
} // namespace se3
-#endif __se3_geometry_hxx__
+#endif // ifnded __se3_algo_geometry_hxx__
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index f77a1e341..1040900e5 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -380,6 +380,12 @@ struct GeometryObject
///
std::vector <CollisionResult> collision_results;
+ ///
+ /// \brief Radius of the bodies, i.e. distance of the further point of the geometry model
+ /// attached to the body from the joint center.
+ ///
+ std::vector<double> radius;
+
GeometryData(const GeometryModel & model_geom)
: model_geom(model_geom)
, oMg(model_geom.ngeoms)
@@ -388,6 +394,8 @@ struct GeometryObject
, nCollisionPairs(0)
, distance_results()
, collision_results()
+ , radius()
+
{
const std::size_t num_max_collision_pairs = (model_geom.ngeoms * (model_geom.ngeoms-1))/2;
collision_pairs.reserve(num_max_collision_pairs);
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 4e822f7b5..240d2ddfd 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -223,6 +223,34 @@ BOOST_AUTO_TEST_CASE ( loading_model )
BOOST_CHECK(geometry_data.computeCollision(1,10).fcl_collision_result.isCollision() == false);
}
+
+#ifdef WITH_URDFDOM
+#ifdef WITH_HPP_FCL
+BOOST_AUTO_TEST_CASE (radius)
+{
+ typedef se3::Model Model;
+ typedef se3::GeometryModel GeometryModel;
+ typedef se3::Data Data;
+ typedef se3::GeometryData GeometryData;
+ typedef std::vector<double> vector_t;
+
+ // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::vector < std::string > package_dirs;
+ std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ package_dirs.push_back(meshDir);
+
+ se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
+ se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
+ Data data(model);
+ GeometryData geomData(geom);
+
+ se3::computeBodyRadius(model, geom, geomData);
+ BOOST_FOREACH( double radius, geomData.radius) radius;
+}
+#endif // #ifdef WITH_URDFDOM
+#endif // #ifdef WITH_HPP_FCL
+
#ifdef WITH_HPP_MODEL_URDF
BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
{
@@ -331,7 +359,6 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
}
-
BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
{
typedef se3::Model Model;
--
GitLab