surfaces_from_path.py 9.36 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
65
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


66
# get contacted surface names at configuration
67
68
def getContactsNames(rbprmBuilder, i, q):
    if i % 2 == LF:  # left leg
69
        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.lLegId)
70
    elif i % 2 == RF:  # right leg
71
        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
72
73
    return step_contacts

74
75

# get intersections with the rom and surface at configuration
76
77
def getContactsIntersections(rbprmBuilder, i, q):
    if i % 2 == LF:  # left leg
78
        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.lLegId)
79
    elif i % 2 == RF:  # right leg
80
        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.rLegId)
81
82
    return intersections

83
84

# merge phases with the next phase
85
86
87
88
89
90
91
92
93
94
95
96
97
98
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])
Florent Lamiraux's avatar
Florent Lamiraux committed
99
    v_angular = np.array(log3(quat0.matrix().dot(quat1.matrix())))
100
101
102
103
    #print ("q_prev : ",q0)
    #print ("q      : ",q1)
    #print ("v_angular = ",v_angular)
    return v_angular[2]
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


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[::]
138
    configs.append(q)
139
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
    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]
166
            if useIntersection and area(surface) > max_surface_area:
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
                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