surfaces_from_path.py 8.27 KB
Newer Older
1
2
3
from numpy import arange, array
from pinocchio import Quaternion,SE3,XYZQUATToSe3
from tools.narrow_convex_hull import getSurfaceExtremumPoints, removeDuplicates, normal, area
4
from hpp.corbaserver.rbprm.tools.display_tools import displaySurfaceFromPoints
5
import numpy as np
6
from pinocchio import Quaternion, log3
7
8
9
10
import eigenpy
eigenpy.switchToNumpyMatrix()

ROBOT_NAME = 'talos'
11
MAX_SURFACE = 1. # if a contact surface is greater than this value, the intersection is used instead of the whole surface
12
13
14
LF = 0
RF = 1  

15
16


17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
# change the format into an array  
def listToArray (seqs):
  nseq = []; nseqs= []
  for seq in seqs:
    nseq = []
    for surface in seq:
      nseq.append(array(surface).T)
    nseqs.append(nseq)
  return nseqs

# get configurations along the path  
def getConfigsFromPath (ps, pathId = 0, discretisationStepSize = 1.) :
  configs = []
  pathLength = ps.pathLength(pathId)
  for s in arange (0, pathLength, discretisationStepSize) :
    configs.append(ps.configAtParam(pathId, s))
  return configs

# get all the contact surfaces (pts and normal)
def getAllSurfaces(afftool) :
  l = afftool.getAffordancePoints("Support")
  return [(getSurfaceExtremumPoints(el), normal(el[0])) for el in l]
    
# get surface information
def getAllSurfacesDict (afftool) :
  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
  return surfaces_dict

# get rotation matrices form configs
def getRotationMatrixFromConfigs(configs) :
49
  eigenpy.switchToNumpyMatrix()
50
51
52
  R = []
  for config in configs:
    q = [0,0,0] + config[3:7]
53
    #print "q = ",q
54
    placement = XYZQUATToSe3(q)
55
    #print "placement = ",placement
56
    rot = placement.rotation
57
    #print "rot = ",rot
58
    R.append(np.array(rot))
59
  #print "R in getRotationMatrixFromConfigs : ",R
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
  return R   
    
# get contacted surface names at configuration
def getContactsNames(rbprmBuilder,i,q):
  if i % 2 == LF : # left leg 
    step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_lleg_rom') 
  elif i % 2 == RF : # right leg 
    step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_rleg_rom')
  return step_contacts

# get intersections with the rom and surface at configuration
def getContactsIntersections(rbprmBuilder,i,q):
  if i % 2 == LF : # left leg
    intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_lleg_rom') 
  elif i % 2 == RF : # right leg
    intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_rleg_rom')
  return intersections

# merge phases with the next phase
def getMergedPhases (seqs):
  nseqs = []
  for i, seq in enumerate(seqs):
    nseq = []
    if i == len(seqs)-1: nseq = seqs[i]
    else: nseq = seqs[i]+seqs[i+1]
    nseq = removeDuplicates(nseq)
    nseqs.append(nseq)  
  return nseqs    

Pierre Fernbach's avatar
Pierre Fernbach committed
89

90
91
92
def computeRootYawAngleBetwwenConfigs(q0,q1):
  quat0 = Quaternion(q0[6],q0[3],q0[4],q0[5])
  quat1 = Quaternion(q1[6],q1[3],q1[4],q1[5])
93
  v_angular = np.matrix(log3(quat0.matrix().T*quat1.matrix())).T
94
95
96
97
98
99
100
  #print "q_prev : ",q0
  #print "q      : ",q1
  #print "v_angular = ",v_angular
  return v_angular[2,0]

def isYawVariationsInsideBounds(q0,q1,max_yaw = 0.5):
  yaw = abs(computeRootYawAngleBetwwenConfigs(q0,q1))
101
  #print "yaw = ",yaw
102
103
  return  yaw < max_yaw

104
def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,step = 1.,useIntersection= False,mergeCandidates = False,max_yaw = 0.5,max_surface_area = MAX_SURFACE):
105
  pathLength = ps.pathLength(pId) #length of the path
106
  discretizationStep = 0.01 # step at which we check the colliding surfaces
107
108
109
110
111
  #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
112
  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
113
114
  configs = []
  t = -discretizationStep
115
116
117
  current_phase_end = step
  end = False
  i = 0
118
119
120
  q_prev = ps.configAtParam(pId, 0)
  q = q_prev[::]
  configs.append(q)
121
  while not end: # for all the path
122
    #print "Looking for surfaces for phase "+str(len(seqs))+" for t in ["+str(t+discretizationStep)+" ; "+str(current_phase_end)+" ] "
123
    phase_contacts_names = []
124
125
126
    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
127
      q = ps.configAtParam(pId, t)
128
      if not isYawVariationsInsideBounds(q_prev,q,max_yaw):
129
        #print "yaw variation out of bounds, try to reduce the time step : "
130
131
132
133
        rot_valid = False
        t -= discretizationStep
        q = ps.configAtParam(pId, t)
        while isYawVariationsInsideBounds(q_prev,q,max_yaw):
134
          t += 0.0001
135
          q = ps.configAtParam(pId, t)
136
      #print " t in getSurfacesFromGuideContinuous : ",t
137
138
139
140
141
142
143
144
145
146
147
148
149
150
      step_contacts = getContactsNames(rbprmBuilder,i,q)
      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: 
    if useIntersection : 
      intersections = getContactsIntersections(rbprmBuilder,i,q)
    phase_surfaces = []
    for name in phase_contacts_names:
      surface = surfaces_dict[name][0]
      if useIntersection and area(surface) > MAX_SURFACE : 
        if name in step_contacts : 
          intersection = intersections[step_contacts.index(name)]
151
152
          if len(intersection) > 3 :
            phase_surfaces.append(intersection)
153
      else :
154
155
156
157
158
159
        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."
    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)
160
161
      if viewer:
        displaySurfaceFromPoints(viewer,surface,[0,0,1,1])
162
163
    #print "phase_surfaces_array = ",phase_surfaces_array    
    seqs.append(phase_surfaces_array)
164
    # increase values for next phase
165
166
    q_prev = q[::]
    configs.append(q)
167
    i += 1 
168
    if t >= (pathLength - discretizationStep/2.):
169
      end = True
170
    t -= discretizationStep # because we want the first iteration of the next phase to test the same t as the last iter of this phase
171
    current_phase_end = t + step
172
173
174
175
176
177
178
179
    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


180
def getSurfacesFromGuide(rbprmBuilder, configs, surfaces_dict, viewer = None, useIntersection = False, useMergePhase = False):
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
  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