Commit afda6e3f authored by Steve T's avatar Steve T
Browse files

bug fix case 3

parents 24cae4e5 785f14d5
......@@ -27,14 +27,19 @@ np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)})
### This solver is called when the sparsity is fixed. It assumes the first contact surface for each phase
### is the one used for contact creation.
def solve(pb,surfaces, draw_scene = None, plot = True ):
def solve(pb,surfaces, draw_scene = None, plot = True):
t1 = clock()
A, b, E, e = pl.convertProblemToLp(pb)
C = identity(A.shape[1])
c = zeros(A.shape[1])
t2 = clock()
res = qp.quadprog_solve_qp(C, c,A,b,E,e)
try:
res = qp.quadprog_solve_qp(C, c,A,b,E,e) ######
# res = qp.solve_lp(c,A,b,E,e)
except:
print ("CASE3: turned out to be infeasible")
return 3, 3, 3
t3 = clock()
print("time to set up problem" , timMs(t1,t2))
......@@ -48,7 +53,8 @@ def solve(pb,surfaces, draw_scene = None, plot = True ):
ax = draw_scene(surfaces)
pl.plotQPRes(pb, res, ax=ax)
return pb, coms, footpos, allfeetpos, res
return pb, res, timMs(t1,t3)
# return pb, coms, footpos, allfeetpos, res
### Calls the sl1m solver. Brute-forcedly tries to solve non fixed sparsity by handling the combinatorial.
......@@ -58,16 +64,25 @@ def solveL1(pb, surfaces, draw_scene = None, plot = True):
C = identity(A.shape[1]) * 0.00001
c = pl1.slackSelectionMatrix(pb)
res = qp.quadprog_solve_qp(C, c,A,b,E,e)
try:
res = qp.quadprog_solve_qp(C, c,A,b,E,e)
except:
return 4, 4, 4
ok = pl1.isSparsityFixed(pb, res)
solutionIndices = None
solutionComb = None
pbs = None
if not ok:
pbs = pl1.generateAllFixedScenariosWithFixedSparsity(pb, res)
t3 = clock()
if pbs == 1:
print ("CASE1: too big combinatorial")
return 1, 1, 1
for (pbComb, comb, indices) in pbs:
A, b, E, e = pl1.convertProblemToLp(pbComb, convertSurfaces = False)
C = identity(A.shape[1]) * 0.00001
......@@ -100,7 +115,12 @@ def solveL1(pb, surfaces, draw_scene = None, plot = True):
for i, idx in enumerate(solutionIndices):
pb["phaseData"][idx]["S"] = [surfaces[idx][solutionComb[i]]]
return solve(pb,surfaces, draw_scene = draw_scene, plot = True )
return solve (pb, surfaces, draw_scene, plot)
print ("CASE2: combinatorials all sparsity not fixed")
return 2, 2, 2
# return solve(pb,surfaces, draw_scene = draw_scene, plot = True )
############### MIXED-INTEGER SOLVER ###############
......@@ -162,5 +182,6 @@ def solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True):
ax = draw_scene(surfaces)
pl1.plotQPRes(pb, res, ax=ax)
return timMs(t1,t2)
# return timMs(t1,t2)
return pb, res, timMs(t1,t2)
......@@ -160,7 +160,7 @@ def plotPoints(ax, wps, color = "b", D3 = True, linewidth=2):
y = array(wps)[:,1]
if(D3):
z = array(wps)[:,2]
ax.scatter(x, y, z, c=color, marker='o', linewidth = 5)
ax.scatter(x, y, z, color=color, marker='o', linewidth = 5)
else:
ax.scatter(x,y,color=color, linewidth = linewidth)
......
......@@ -283,30 +283,32 @@ def slackSelectionMatrix(pb):
cIdx += phaseVars
assert cIdx == nvars
return c
def num_non_zeros(pb, res):
nvars = getTotalNumVariablesAndIneqConstraints(pb)[1]
# nvars = getTotalNumVariablesAndIneqConstraints(pb)[1]
indices = []
cIdx = 0
wrongsurfaces = []
wrongsurfaces_indices = []
for i, phase in enumerate(pb["phaseData"]):
numSurfaces = len(phase["S"])
phaseVars = numVariablesForPhase(phase)
if numSurfaces > 1:
startIdx = cIdx + DEFAULT_NUM_VARS
betas = [res[startIdx+j] for j in range(0,numSurfaces*2,2) ]
if array(betas).min() > 0.01:
#~ print "wrong ", i, array(betas).min()
if array(betas).min() > 0.01: ####
# print "wrong ", i, array(betas).min()
indices += [i]
sorted_surfaces = np.argsort(betas)
wrongsurfaces_indices += [sorted_surfaces]
#~ print "sorted_surfaces ",sorted_surfaces
wrongsurfaces += [[[phase["S"][idx]] for idx in sorted_surfaces] ]
#~ print "lens ", len([[phase["S"][idx]] for idx in sorted_surfaces] )
cIdx += phaseVars
return indices, wrongsurfaces
return indices, wrongsurfaces, wrongsurfaces_indices
def isSparsityFixed(pb, res):
indices, wrongsurfaces = num_non_zeros(pb, res)
indices, wrongsurfaces, wrongsurfaces_indices = num_non_zeros(pb, res)
return len(indices) == 0
def genOneComb(pb,indices, surfaces, res):
......@@ -318,29 +320,31 @@ def genOneComb(pb,indices, surfaces, res):
import itertools
import copy
def genCombinatorialRec(pb, indices, wrongsurfaces, res):
def genCombinatorialRec(pb, indices, wrongsurfaces, wrongsurfaces_indices, res):
lenss = [len(surfs) for surfs in wrongsurfaces]
all_indexes = [[el for el in range(lens)] for lens in [len(surfs) for surfs in wrongsurfaces]]
wrong_combs = [el for el in itertools.product(*wrongsurfaces_indices)]
combs = [el for el in itertools.product(*all_indexes)]
for comb in combs:
for j, comb in enumerate(combs):
pb1 = copy.deepcopy(pb)
for i, idx in enumerate(indices):
pb1["phaseData"][idx]["S"] = wrongsurfaces[i][comb[i]]
res += [[pb1, comb, indices]]
res += [[pb1, wrong_combs[j], indices]]
def generateAllFixedScenariosWithFixedSparsity(pb, res):
indices, wrongsurfaces = num_non_zeros(pb, res)
indices, wrongsurfaces, wrongsurfaces_indices = num_non_zeros(pb, res)
all_len = [len(surfs) for surfs in wrongsurfaces]
comb = 1
for el in all_len:
comb *= el
res = []
if comb >1000:
print("problem probably too big ", comb)
print ("problem probably too big ", comb)
return 1
else:
genCombinatorialRec(pb, indices, wrongsurfaces, res)
genCombinatorialRec(pb, indices, wrongsurfaces, wrongsurfaces_indices, res)
return res
......@@ -409,7 +413,7 @@ def plotPoints(ax, wps, color = "b", D3 = True, linewidth=2):
y = array(wps)[:,1]
if(D3):
z = array(wps)[:,2]
ax.scatter(x, y, z, c=color, marker='o', linewidth = 5)
ax.scatter(x, y, z, color=color, marker='o', linewidth = 5)
else:
ax.scatter(x,y,color=color, linewidth = linewidth)
......@@ -459,14 +463,14 @@ def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show =
ax.set_autoscale_on(False)
ax.view_init(elev=8.776933438381377, azim=-99.32358055821186)
#~ plotPoints(ax, coms, color = "b")
plotPoints(ax, coms, color = "b")
plotPoints(ax, footpos[RF], color = "r")
plotPoints(ax, footpos[LF], color = "g")
cx = [c[0] for c in coms]
cy = [c[1] for c in coms]
cz = [c[2] for c in coms]
#~ ax.plot(cx, cy, cz)
ax.plot(cx, cy, cz)
px = [c[0] for c in allfeetpos]
py = [c[1] for c in allfeetpos]
pz = [c[2] for c in allfeetpos]
......
......@@ -53,7 +53,12 @@ def solve_least_square(A,b,G=None, h=None, C=None, d=None):
#subject to C x = d
def solve_lp(q, G=None, h=None, C=None, d=None):
res = linprog(q, A_ub=G, b_ub=h, A_eq=C, b_eq=d, bounds=[(-100000.,10000.) for _ in range(q.shape[0])], method='interior-point', callback=None, options={'presolve': True})
return res
# print "success", res['success']
# print "status", res['status']
if res['success']:
return res['x']
else:
return res['status']
if GLPK_OK:
__eps = 0.00001
......
import numpy as np
from numpy import arange, array, append, cross, dot, zeros
from numpy.linalg import norm
from scipy.spatial import ConvexHull
def normal(points):
p1 = array(points[0])
p2 = array(points[1])
p3 = array(points[2])
normal = cross((p2 - p1),(p3 - p1))
normal /= norm(normal)
return normal.tolist()
def area(s):
area = 0
for i in range(1,len(s)-1):
area += abs(s[i][0]*(s[i+1][1] - s[i-1][1]))
i = len(s) -1
area += abs(s[i][0]*(s[0][1] - s[i-1][1]))
return area * 0.5
def cutList2D(l):
return [el[:2] for el in l]
def roundPoints (points, precision):
return [[round(x,precision) for x in p] for p in points]
def removeDuplicates (points):
pList = []
for p in points:
if p not in pList:
pList.append(p)
return pList
def removeNone (l):
return [i for i in l if len(i) is not 0]
def computeAxisAngleRotation(u, c):
ux = u[0] ; uy = u[1] ; uz = u[2]
s = np.sqrt(1- c*c)
return [[c+ux*ux*(1-c), ux*uy*(1-c)-uz*s, ux*uz*(1-c)+uy*s],
[uy*ux*(1-c)+uz*s, c+uy*uy*(1-c), uy*uz*(1-c)-ux*s],
[uz*ux*(1-c)-uy*s, uz*uy*(1-c)+ux*s, c+uz*uz*(1-c)]]
def getSurfaceRotation (surface):
n = surface[1]
cosx = np.dot(surface[1],[0,0,1])
axisx = np.cross(surface[1],[0,0,1]) ;
n_axisx = norm(axisx)
if n_axisx > 0:
axisx /= n_axisx
return computeAxisAngleRotation(axisx, cosx)
def getPtsRotation (points):
return getSurfaceRotation((points,normal(points)))
def getSurfaceTranslation (surface):
return [sum(x)/len(x) for x in zip(*surface[0])]
def getPtsTranslation (points):
return getSurfaceTranslation((points,normal(points)))
def allignSurface (surface):
R = getSurfaceRotation(surface)
t = getSurfaceTranslation(surface)
translatedPts = [(array(p)-array(t)).tolist() for p in surface[0]]
rotatedPts = [np.dot(R,p).tolist() for p in translatedPts]
return [(array(p)+array(t)).tolist() for p in rotatedPts]
def allignPoints (points):
return allignSurface((points, normal(points)))
def pointsTransform (points,R,t):
translatedPts = [(array(pt)-array(t)).tolist() for pt in points]
rotatedPts = [np.dot(R,pt).tolist() for pt in translatedPts]
return [(array(pt)+array(t)).tolist() for pt in rotatedPts]
def getSurfaceExtremumPoints(el):
pts = removeDuplicates(el[0]+el[1]) # This might only works in rectangular shape? with triangular mesh*2
apts = allignPoints(pts)
hull = ConvexHull(cutList2D(apts))
return [pts[idx] for idx in hull.vertices]
from numpy import arange, array
from narrow_convex_hull import getSurfaceExtremumPoints, removeDuplicates, normal, area
from tools.display_tools import displaySurfaceFromPoints
from pinocchio import XYZQUATToSe3
import numpy as np
ROBOT_NAME = 'talos'
MAX_SURFACE = 0.3 # if a contact surface is greater than this value, the intersection is used instead of the whole surface
LF = 0
RF = 1
def removeNull (l):
ll = []
for el in l:
if el != []:
ll.append(el)
return ll
# change the format into an array
def listToArray (seqs):
nseq = []; nseqs= []
for seq in seqs:
nseq = []
for surface in seq:
if surface != []:
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) :
R = []
for config in configs:
q_rot = config[3:7]
R.append(array(XYZQUATToSe3([0,0,0] + q_rot).rotation))
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
return removeNull(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
def getSurfacesFromPathContinuous(rbprmBuilder, ps, surfaces_dict, pId, viewer = None, phaseStepSize = 1., useIntersection = False):
pathLength = ps.pathLength(pId) # length of the path
discretizationStepSize = 0.4 # step at which we check the colliding surfaces
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
t = 0.
current_phase_end = phaseStepSize
end = False
i = 0
while not end: # for all the path
phase_contacts_names = []
while t < current_phase_end: # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end
q = ps.configAtParam(pId, 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)
t += discretizationStepSize
# 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] # [0] because the last vector contain the normal of the surface
if useIntersection and area(surface) > MAX_SURFACE :
if len(step_contacts) == len(intersections): # in case of the error with contact detection
if name in step_contacts :
intersection = intersections[step_contacts.index(name)]
phase_surfaces.append(intersection)
else:
phase_surfaces.append(surface)
else :
phase_surfaces.append(surface)
phase_surfaces = sorted(phase_surfaces) # why is this step required ? without out the lp can fail
seqs.append(phase_surfaces)
# increase values for next phase
t = current_phase_end
i += 1
if current_phase_end == pathLength:
end = True
current_phase_end += phaseStepSize
if current_phase_end >= pathLength:
current_phase_end = pathLength
# end for all the guide path
seqs = listToArray(seqs) # convert from list to array, we cannot do this before because sorted() require list
# get rotation matrix of the root at each discretization step
configs = []
for t in arange (0, pathLength, phaseStepSize) :
configs.append(ps.configAtParam(pId, t))
R = getRotationMatrixFromConfigs(configs)
return R,seqs
"""
LARGE_STEP_SIZE = 1.3
SMALL_STEP_SIZE = 1.0
def getSurfacesFromPathContinuous(rbprmBuilder, ps, surfaces_dict, pId, viewer = None, phaseStepSize = 1., useIntersection = False):
pathLength = ps.pathLength(pId) # length of the path
discretizationStepSize = 0.5 # step at which we check the colliding surfaces
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
t = 0.
current_phase_end = phaseStepSize
end = False
i = 0
phase_contacts_names = []
configs = []
while not end: # for all the path
phase_contacts_prev = phase_contacts_names
phase_contacts_names = []
configs.append(ps.configAtParam(pId, t))
while t < current_phase_end: # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end
q = ps.configAtParam(pId, 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)
t += discretizationStepSize
# end current phase
# phase_contacts_names = sorted(phase_contacts_names) # is this step needed?
# # if the previous phase and the current phase has different surface candidates, decrease the phase step
# if phase_contacts_prev != phase_contacts_names:
# # print "different surface candidates"
# phaseStepSize = SMALL_STEP_SIZE
# else:
# # print "same surface candidates"
# phaseStepSize = LARGE_STEP_SIZE
# get all the surfaces from the names and add it to seqs:
if useIntersection :
intersections = getContactsIntersections(rbprmBuilder,i,q)
if viewer:
for intersection in intersections:
displaySurfaceFromPoints(viewer,intersection,[0,0,1,1])
phase_surfaces = []
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 :
if name in step_contacts :
intersection = intersections[step_contacts.index(name)]
phase_surfaces.append(intersection)
else :
phase_surfaces.append(surface)
phase_surfaces = sorted(phase_surfaces) # why is this step required ? without out the lp can fail
seqs.append(phase_surfaces)
# increase values for next phase
t = current_phase_end
i += 1
if current_phase_end == pathLength:
end = True
current_phase_end += phaseStepSize
if current_phase_end >= pathLength:
current_phase_end = pathLength
# end for all the guide path
seqs = listToArray(seqs) # convert from list to array, we cannot do this before because sorted() require list
# get rotation matrix of the root at each discretization step
# configs = []
# for t in arange (0, pathLength, phaseStepSize) :
# configs.append(ps.configAtParam(pId, t))
R = getRotationMatrixFromConfigs(configs)
return R,seqs
"""
def getSurfacesFromPath(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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment