surfaces_from_path.py 11.1 KB
Newer Older
1
from numpy import arange, array
2
from pinocchio import Quaternion, SE3, XYZQUATToSe3
3
from hpp.corbaserver.rbprm.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
import eigenpy
8
eigenpy.switchToNumpyArray()
9
10

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
LF = 0
13
RF = 1
14

15

16
17
# change the format into an array
def listToArray(seqs):
18
    nseq = []
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
    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

36
37

# get all the contact surfaces (pts and normal)
38
39
40
41
42
def getAllSurfaces(afftool):
    l = afftool.getAffordancePoints("Support")
    return [(getSurfaceExtremumPoints(el), normal(el[0])) for el in l]


43
# get surface information
44
45
46
47
48
49
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

50
51

# get rotation matrices form configs
52
53
54
55
56
57
58
59
60
61
62
63
64
def getRotationMatrixFromConfigs(configs):
    R = []
    for config in configs:
        q = [0, 0, 0] + config[3:7]
        #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

65
66
67
68
69
def getALlContactsNames(rbprmBuilder, q):
    step_contacts = []
    for limb in rbprmBuilder.urdfNameRom:
        step_contacts += rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, limb)
    return step_contacts
70

71
# get contacted surface names at configuration
72
73
74
def getContactsNames(rbprmBuilder, i, q, use_all_limbs=False):
    if use_all_limbs:
        return getALlContactsNames(rbprmBuilder, q)
75
    if i % 2 == LF:  # left leg
76
        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.lLegId)
77
    elif i % 2 == RF:  # right leg
78
        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
79
80
    return step_contacts

81
82
83
84
85
86
def getALlContactsIntersections(rbprmBuilder, q):
    intersections = []
    for limb in rbprmBuilder.urdfNameRom:
        intersections += rbprmBuilder.getContactSurfacesAtConfig(q, limb)
    return intersections

87
88

# get intersections with the rom and surface at configuration
89
90
91
def getContactsIntersections(rbprmBuilder, i, q, use_all_limbs=False):
    if use_all_limbs:
        return getALlContactsIntersections(rbprmBuilder, q)
92
    if i % 2 == LF:  # left leg
93
        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.lLegId)
94
    elif i % 2 == RF:  # right leg
95
        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.rLegId)
96
97
    return intersections

98
99

# merge phases with the next phase
100
101
102
103
104
105
106
107
108
109
110
111
112
113
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


def computeRootYawAngleBetwwenConfigs(q0, q1):
    quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
    quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
114
    v_angular = np.array(log3(quat0.matrix().T.dot(quat1.matrix())))
115
116
117
118
    #print ("q_prev : ",q0)
    #print ("q      : ",q1)
    #print ("v_angular = ",v_angular)
    return v_angular[2]
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135


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


def getSurfacesFromGuideContinuous(rbprmBuilder,
                                   ps,
                                   afftool,
                                   pId,
                                   viewer=None,
                                   step=1.,
                                   useIntersection=False,
                                   mergeCandidates=False,
                                   max_yaw=0.5,
136
137
                                   max_surface_area=MAX_SURFACE,
                                   use_all_limbs=False):
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
    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[::]
154
    configs.append(q)
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
    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
171
            step_contacts = getContactsNames(rbprmBuilder, i, q, use_all_limbs)
172
173
174
175
176
177
            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 = []
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
        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])

198
        for name in phase_contacts_names:
199
            surface = surfaces_dict[name][0] # [0] because the last vector contain the normal of the surface
200
            if useIntersection and area(surface) > max_surface_area:
201
202
203
204
205
                if name in step_contacts:
                    intersection = intersections[step_contacts.index(name)]
                    if len(intersection) > 3:
                        phase_surfaces.append(intersection)
            else:
206
                phase_surfaces.append(surface)
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
        #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


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