Commit 1ac429b8 authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

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

parent 2170f8f2
......@@ -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,6 +117,7 @@ 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)]
if len(intersection) > 3 :
phase_surfaces.append(intersection)
if viewer:
displaySurfaceFromPoints(viewer,intersection,[0,0,1,1])
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment