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

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

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() );
     

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);