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