From c925cf02bb069a2af46162bee34376fc7c6ec1b7 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 22 Jul 2016 13:13:18 +0200
Subject: [PATCH] Corrected two typos in the code, as notived by @fvalenza.
---
benchmark/timings-geometry.cpp | 2 +-
src/algorithm/geometry.hpp | 5 +++--
src/algorithm/geometry.hxx | 9 ++++-----
3 files changed, 8 insertions(+), 8 deletions(-)
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index db0584db5..91537a9d3 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -26,7 +26,7 @@
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/algorithm/collisions.hpp"
+#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"
diff --git a/src/algorithm/geometry.hpp b/src/algorithm/geometry.hpp
index aabea1f48..33b6b8a3a 100644
--- a/src/algorithm/geometry.hpp
+++ b/src/algorithm/geometry.hpp
@@ -79,8 +79,9 @@ namespace se3
const Eigen::VectorXd & q
);
- inline void computeBodyRadius(const GeometryModel & geomModel,
- GeometryData & geomData);
+ inline void computeBodyRadius(const Model & model,
+ const GeometryModel & geomModel,
+ GeometryData & geomData);
} // namespace se3
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index abd9c8b9f..c10a7081b 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -103,9 +103,8 @@ namespace se3
#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])
-
+ FCL->aabb_local.p2##_ [1], \
+ FCL->aabb_local.p3##_ [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
@@ -114,7 +113,7 @@ namespace se3
const GeometryModel & geomModel,
GeometryData & geomData)
{
- geomData.radius.resize(model.joints.size());
+ geomData.radius.resize(model.joints.size(),0);
BOOST_FOREACH(const GeometryObject & geom,geomModel.geometryObjects)
{
std::cout << "New body radius" << geom.name << std::endl;
@@ -122,7 +121,7 @@ namespace se3
= geom.collision_object.collisionGeometry();
const SE3 & jMb = geom.placement; // placement in joint.
- double radius = 0.0;
+ double radius = geomData.radius[geom.parent];
// The radius is simply the one of the 8 corners of the AABB cube, expressed
// in the joint frame, whose norm is the highest.
--
GitLab