[tools] surfaces_from_path : check if the intersection is not empty before adding it

script/tools/surfaces_from_path.py

@@ -43,11 +43,16 @@ def getAllSurfacesDict (afftool) :
 
 # get rotation matrices form configs
 def getRotationMatrixFromConfigs(configs) :
+  eigenpy.switchToNumpyMatrix()
   R = []
   for config in configs:
     q = [0,0,0] + config[3:7]
-    #print "q = ",q
-    R.append(np.array(XYZQUATToSe3(q).rotation))
+    print "q = ",q
+    placement = XYZQUATToSe3(q)
+    print "placement = ",placement
+    rot = placement.rotation
+    print "rot = ",rot
+    R.append(np.array(rot))
   print "R in getRotationMatrixFromConfigs : ",R
   return R   
     
@@ -112,9 +117,10 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,ste
       if useIntersection and area(surface) > MAX_SURFACE : 
         if name in step_contacts : 
           intersection = intersections[step_contacts.index(name)]
-          phase_surfaces.append(intersection)
-          if viewer:
-            displaySurfaceFromPoints(viewer,intersection,[0,0,1,1])
+          if len(intersection) > 3 :
+            phase_surfaces.append(intersection)
+            if viewer:
+                displaySurfaceFromPoints(viewer,intersection,[0,0,1,1])
       else :
         phase_surfaces.append(surface) # [0] because the last vector contain the normal of the surface
     #print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."

src/hpp/corbaserver/rbprm/tools/display_tools.py

@@ -48,6 +48,8 @@ def addVector(viewer, rbprmBuilder, color, v, name=None):
 
 
 def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None):
+    if len(p_list) <2 :
+      return
     gui = viewer.client.gui
     if name is None:
         i = 0