Skip to content
Snippets Groups Projects
Commit d5050de8 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[C++][Minor] Corrected bugs in assert that prevented compilation in debug mode.

parent 6d6b547e
No related branches found
No related tags found
No related merge requests found
...@@ -32,6 +32,7 @@ class RobotWrapper(object): ...@@ -32,6 +32,7 @@ class RobotWrapper(object):
self.data = self.model.createData() self.data = self.model.createData()
self.model_filename = filename self.model_filename = filename
print dir(se3)
if "buildGeomFromUrdf" not in dir(se3): if "buildGeomFromUrdf" not in dir(se3):
self.collision_model = None self.collision_model = None
self.visual_model = None self.visual_model = None
......
...@@ -62,14 +62,17 @@ namespace se3 ...@@ -62,14 +62,17 @@ namespace se3
GeometryData & geomData, GeometryData & geomData,
const PairIndex& pairId) const PairIndex& pairId)
{ {
assert( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId]; const CollisionPair & pair = geomModel.collisionPairs[pairId];
fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId]; fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
const PairIndex & co1 = pair.first; assert(co1<collisionObjects.size()); assert( pairId < geomData.distanceResults.size() );
const PairIndex & co2 = pair.second; assert(co2<collisionObjects.size()); assert( pair.first < geomData.collisionObjects.size() );
assert( pair.second < geomData.collisionObjects.size() );
collisionResult.clear(); collisionResult.clear();
fcl::collide (&geomData.collisionObjects[co1],&geomData.collisionObjects[co2], fcl::collide (&geomData.collisionObjects[pair.first],
&geomData.collisionObjects[pair.second],
geomData.collisionRequest, geomData.collisionRequest,
collisionResult); collisionResult);
...@@ -121,7 +124,7 @@ namespace se3 ...@@ -121,7 +124,7 @@ namespace se3
assert( pairId < geomModel.collisionPairs.size() ); assert( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId]; const CollisionPair & pair = geomModel.collisionPairs[pairId];
assert( pair < geomData.distanceResults.size() ); assert( pairId < geomData.distanceResults.size() );
assert( pair.first < geomData.collisionObjects.size() ); assert( pair.first < geomData.collisionObjects.size() );
assert( pair.second < geomData.collisionObjects.size() ); assert( pair.second < geomData.collisionObjects.size() );
......
...@@ -66,7 +66,7 @@ namespace se3 ...@@ -66,7 +66,7 @@ namespace se3
assert( (object.parentFrame == -1) assert( (object.parentFrame == -1)
|| (model.frames[object.parentFrame].type == se3::BODY) ); || (model.frames[object.parentFrame].type == se3::BODY) );
assert( (object.parentFrame == -1) assert( (object.parentFrame == -1)
|| (model.frames[object.parentFrame].parentJoint == object.parentJoint) ); || (model.frames[object.parentFrame].parent == object.parentJoint) );
GeomIndex idx = (GeomIndex) (ngeoms ++); GeomIndex idx = (GeomIndex) (ngeoms ++);
geometryObjects.push_back(object); geometryObjects.push_back(object);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment