def getSurfacesFromGuide(rbprmBuilder,
configs,
surfaces_dict,
viewer=None,
useIntersection=False,
useMergePhase=False):
seqs = []
# get sequence of surface candidates at each discretization step
for i, q in enumerate(configs):
seq = []
intersections = getContactsIntersections(
rbprmBuilder, i, q) # get intersections at config
phase_contacts_names = getContactsNames(
rbprmBuilder, i,
q) # get the list of names of the surface in contact at config
for j, intersection in enumerate(intersections):
if useIntersection and area(
intersection) > MAX_SURFACE: # append the intersection
seq.append(intersection)
else:
if len(intersections) == len(
phase_contacts_names
): # in case getCollidingObstacleAtConfig does not work (because of the rom?)
seq.append(surfaces_dict[phase_contacts_names[j]]
[0]) # append the whole surface
else:
seq.append(intersection) # append the intersection
if viewer:
displaySurfaceFromPoints(viewer, intersection, [0, 0, 1, 1])
seqs.append(seq)
# merge candidates with the previous and the next phase
if useMergePhase: seqs = getMergedPhases(seqs)
seqs = listToArray(seqs)
R = getRotationMatrixFromConfigs(configs)
return R, seqs
def getSurfacesFromGuideContinuous(rbprmBuilder,
ps,
afftool,
pId,
viewer=None,
step=1.,
useIntersection=False,
mergeCandidates=False,
max_yaw=0.5,
max_surface_area=MAX_SURFACE,
use_all_limbs=False):
pathLength = ps.pathLength(pId) #length of the path
discretizationStep = 0.01 # step at which we check the colliding surfaces
#print "path length = ",pathLength
# get surface information
all_surfaces = getAllSurfaces(afftool)
all_names = afftool.getAffRefObstacles(
"Support") # id in names and surfaces match
surfaces_dict = dict(zip(
all_names, all_surfaces)) # map surface names to surface points
seqs = [
] # list of list of surfaces : for each phase contain a list of surfaces. One phase is defined by moving of 'step' along the path
configs = []
t = -discretizationStep
current_phase_end = step
end = False
i = 0
q_prev = ps.configAtParam(pId, 0)
q = q_prev[::]
configs.append(q)
while not end: # for all the path
#print "Looking for surfaces for phase "+str(len(seqs))+" for t in ["+str(t+discretizationStep)+" ; "+str(current_phase_end)+" ] "
phase_contacts_names = []
rot_valid = True
while t < current_phase_end and rot_valid: # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end
t += discretizationStep
q = ps.configAtParam(pId, t)
if not isYawVariationsInsideBounds(q_prev, q, max_yaw):
#print "yaw variation out of bounds, try to reduce the time step : "
rot_valid = False
t -= discretizationStep
q = ps.configAtParam(pId, t)
while isYawVariationsInsideBounds(q_prev, q, max_yaw):
t += 0.0001
q = ps.configAtParam(pId, t)
#print " t in getSurfacesFromGuideContinuous : ",t
step_contacts = getContactsNames(rbprmBuilder, i, q, use_all_limbs)
for contact_name in step_contacts:
if not contact_name in phase_contacts_names:
phase_contacts_names.append(contact_name)
# end current phase
# get all the surfaces from the names and add it to seqs:
phase_surfaces = []
if useIntersection:
intersections = getContactsIntersections(rbprmBuilder, i, q,
use_all_limbs)
# First add intersection for the surfaces in contact at the last discretization step
for k, name in enumerate(step_contacts):
surface = surfaces_dict[name][
0] # [0] because the last vector contain the normal of the surface
if area(surface) > max_surface_area:
intersection = intersections[k]
if len(intersection) > 3:
phase_surfaces.append(intersection)
else:
phase_surfaces.append(surfaces_dict[name][0])
# then add possible small surfaces encountered during the discretization of the path
for name in phase_contacts_names:
if name not in step_contacts:
phase_surfaces.append(surfaces_dict[name][0])
else:
# add all the surfaces encountered during the path
for name in phase_contacts_names:
phase_surfaces.append(surfaces_dict[name][0])
for name in phase_contacts_names:
surface = surfaces_dict[name][
0] # [0] because the last vector contain the normal of the surface
if useIntersection and area(surface) > max_surface_area:
if name in step_contacts:
intersection = intersections[step_contacts.index(name)]
if len(intersection) > 3:
phase_surfaces.append(intersection)
else:
phase_surfaces.append(surface)
#print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."
phase_surfaces = sorted(
phase_surfaces
) # why is this step required ? without out the lp can fail
phase_surfaces_array = [
] # convert from list to array, we cannot do this before because sorted() require list
for surface in phase_surfaces:
phase_surfaces_array.append(array(surface).T)
if viewer:
displaySurfaceFromPoints(viewer, surface, [0, 0, 1, 1])
#print "phase_surfaces_array = ",phase_surfaces_array
seqs.append(phase_surfaces_array)
# increase values for next phase
q_prev = q[::]
configs.append(q)
i += 1
if t >= (pathLength - discretizationStep / 2.):
end = True
t -= discretizationStep # because we want the first iteration of the next phase to test the same t as the last iter of this phase
current_phase_end = t + step
if current_phase_end >= pathLength:
current_phase_end = pathLength
# end for all the guide path
#get rotation matrix of the root at each discretization step
R = getRotationMatrixFromConfigs(configs)
return R, seqs