surfaces_from_path.py 9.39 KB
Newer Older
1
from numpy import arange, array
2
from pinocchio import Quaternion, SE3, XYZQUATToSe3
3
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
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
65
66
def getRotationMatrixFromConfigs(configs):
    eigenpy.switchToNumpyMatrix()
    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


67
# get contacted surface names at configuration
68
69
70
71
72
73
74
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

75
76

# get intersections with the rom and surface at configuration
77
78
79
80
81
82
83
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

84
85

# merge phases with the next phase
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
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])
    v_angular = np.matrix(log3(quat0.matrix().T * quat1.matrix())).T
    #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))
    #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,
                                   max_surface_area=MAX_SURFACE):
    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[::]
139
    configs.append(q)
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
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
    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)
            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)]
                    if len(intersection) > 3:
                        phase_surfaces.append(intersection)
            else:
                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)
            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