From d5050de827abcd5486f9105bfb531e6c44251cfc Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 8 Sep 2016 10:08:43 +0200
Subject: [PATCH] [C++][Minor] Corrected bugs in assert that prevented
compilation in debug mode.
---
bindings/python/robot_wrapper.py | 1 +
src/algorithm/geometry.hxx | 11 +++++++----
src/multibody/geometry.hxx | 2 +-
3 files changed, 9 insertions(+), 5 deletions(-)
diff --git a/bindings/python/robot_wrapper.py b/bindings/python/robot_wrapper.py
index 8e8b8156b..ebb0d943c 100644
--- a/bindings/python/robot_wrapper.py
+++ b/bindings/python/robot_wrapper.py
@@ -32,6 +32,7 @@ class RobotWrapper(object):
self.data = self.model.createData()
self.model_filename = filename
+ print dir(se3)
if "buildGeomFromUrdf" not in dir(se3):
self.collision_model = None
self.visual_model = None
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index dc0f62608..08fec67c8 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -62,14 +62,17 @@ namespace se3
GeometryData & geomData,
const PairIndex& pairId)
{
+ assert( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId];
fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
- const PairIndex & co1 = pair.first; assert(co1<collisionObjects.size());
- const PairIndex & co2 = pair.second; assert(co2<collisionObjects.size());
+ assert( pairId < geomData.distanceResults.size() );
+ assert( pair.first < geomData.collisionObjects.size() );
+ assert( pair.second < geomData.collisionObjects.size() );
collisionResult.clear();
- fcl::collide (&geomData.collisionObjects[co1],&geomData.collisionObjects[co2],
+ fcl::collide (&geomData.collisionObjects[pair.first],
+ &geomData.collisionObjects[pair.second],
geomData.collisionRequest,
collisionResult);
@@ -121,7 +124,7 @@ namespace se3
assert( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId];
- assert( pair < geomData.distanceResults.size() );
+ assert( pairId < geomData.distanceResults.size() );
assert( pair.first < geomData.collisionObjects.size() );
assert( pair.second < geomData.collisionObjects.size() );
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index a3b87b282..3fd502c43 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -66,7 +66,7 @@ namespace se3
assert( (object.parentFrame == -1)
|| (model.frames[object.parentFrame].type == se3::BODY) );
assert( (object.parentFrame == -1)
- || (model.frames[object.parentFrame].parentJoint == object.parentJoint) );
+ || (model.frames[object.parentFrame].parent == object.parentJoint) );
GeomIndex idx = (GeomIndex) (ngeoms ++);
geometryObjects.push_back(object);
--
GitLab