Commit 35e73c49 authored by Steve T's avatar Steve T
Browse files

bug fix. Contact created when w = 1

parent 707b106d
......@@ -279,11 +279,23 @@ def SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow):
for footId in range(N_EFFECTORS):
# -Mwi <= b_i x y <= M wi
# -Mwi - b_i xy} <= 0 ; b_ixy- M wi <= 0
A[idRow:idRow+2, startCol + W_START + footId ] = ones(2)*-M;
# ~ A[idRow:idRow+2, startCol + W_START + footId ] = ones(2)*-M;
# ~ A[idRow:idRow+2, (startCol + BETA_START + footId*2):(startCol + BETA_START + footId*2) + 2 ] = -identity(2);
# ~ idRow += 2
# ~ A[idRow:idRow+2, startCol + W_START + footId ] = ones(2)*-M;
# ~ A[idRow:idRow+2, (startCol + BETA_START + footId*2):(startCol + BETA_START + footId*2) + 2 ] = identity(2);
# ~ idRow += 2
# -M (1 - wi) <= b_i x y <= M (1 - wi)
# -M + Mwi <= b_i x y <= M - Mwi
# Mwi - b <= M; <= Mwi + b <= M
A[idRow:idRow+2, startCol + W_START + footId ] = ones(2)*M;
A[idRow:idRow+2, (startCol + BETA_START + footId*2):(startCol + BETA_START + footId*2) + 2 ] = -identity(2);
b[idRow:idRow+2] = ones(2)*M;
idRow += 2
A[idRow:idRow+2, startCol + W_START + footId ] = ones(2)*-M;
A[idRow:idRow+2, startCol + W_START + footId ] = ones(2)*M;
A[idRow:idRow+2, (startCol + BETA_START + footId*2):(startCol + BETA_START + footId*2) + 2 ] = identity(2);
b[idRow:idRow+2] = ones(2)*M;
idRow += 2
# -Mwi <= g_i x + g_i y + g_i_z <= M wi
# -Mwi - g_i x - g_i y - g_i_z <= 0 ; g_ix + g_iy + g_iz - M wi <= 0
......@@ -319,6 +331,7 @@ def CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRow):
idRow = startRow
for flyingFootId in range(N_EFFECTORS):
# 0 = sum(j != i) o_j'i p_j't + [b_ix't, b_iy't]^T - c_x_y
# one beta per equality dum dum TODO
EqMat = -COM_XY_ExpressionMatrix[:]
for otherFootId in range(N_EFFECTORS):
if flyingFootId != otherFootId:
......@@ -369,7 +382,7 @@ def convertProblemToLp(pb, convertSurfaces = True):
startRow = SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow)
#equalities
# ~ startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
startRowEq = FootContinuityEqualityConstraint(pb, phaseDataT, E, e, previousStartCol, startCol, endCol, startRowEq, phaseId)
previousStartCol = startCol
startCol = endCol
......@@ -535,6 +548,47 @@ 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)
from sl1m.tools.plot_plytopes import plot_polytope_H_rep
from collections import namedtuple
Ineq = namedtuple("Inequality", "A b")
colors = ["r","b","g","y"]
Af = None
bf = None
weighted = []
for footId, (K, k) in enumerate(pb["phaseData"][0]["K"][0]):
Afoot = K
bfoot = (k + K.dot(footpos[footId][0])).reshape([-1,1])
if Af is None:
Af = K; bf = bfoot
else:
Af = np.vstack([Af, Afoot])
bf = np.vstack([bf, bfoot])
# ~ print ("footposid", footpos[footId][0])
# ~ bfoot = k.reshape([-1,1])
# ~ print ("range", (K.dot([-1,0.,0.]) - k).max())
pond = zeros(3);
for otherFootId, positions in enumerate(footpos):
if otherFootId != footId:
pond += 0.33 * positions[0]
pond[-1] = 0.4
weighted += [pond]
plot_polytope_H_rep(Af,bf, color = "r", just_pts = False, ax = ax)
# ~ plot(Ineq(Afoot,k), ax = ax, show = False, color = colors[footId])
# ~ [print("wtf", (Af[:,:2].dot(el[:2])-bf).max() ) for el in weighted]
Afreez = Af[:,2:3]
for el in weighted:
bFreez = bf.reshape((-1,)) - Af[:,:2].dot(el[:2])
# ~ print ('AAAAAAAAAAAAAAAAAAA',bf.shape)
from qp import solve_lp
z = solve_lp(zeros(1),Afreez, bFreez)
el[-1] = z.x
plotPoints(ax, [el], color = "b")
# ~ plotPoints(ax, coms[1:], color = "b")
footColors = ['r', 'g', 'y', 'b']
......@@ -911,8 +965,8 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
if __name__ == '__main__':
# ~ from sl1m.stand_alone_scenarios.escaliers import gen_stair_pb, draw_scene, surfaces
from sl1m.stand_alone_scenarios.complex import gen_stair_pb, draw_scene, surfaces
from sl1m.stand_alone_scenarios.escaliers import gen_stair_pb, draw_scene, surfaces
# ~ from sl1m.stand_alone_scenarios.complex import gen_stair_pb, draw_scene, surfaces
pb = gen_stair_pb()
t1 = clock()
......
......@@ -18,8 +18,8 @@ floor = [ [-10, -10., 0.], [10, -10., 0.], [10, 10., 0.], [-10, 10., 0.] ]
afloor = array(floor).T
all_surfaces = [floor]
# ~ surfaces = [[afloor] for _ in range (10)]
surfaces = [[afloor] for _ in range (10)]
# ~ surfaces = [[afloor] for _ in range (1)]
def gen_flat_pb():
kinematicConstraints = genCOMConstraints()
......@@ -84,17 +84,22 @@ if __name__ == '__main__':
ax = draw_scene(None)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = True)
# ~ for i in range(5):
# ~ coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
# ~ pb = gen_flat_pb()
# ~ initPos = None
# ~ endPos = None
# ~ initCom = coms[-1]
# ~ endCom = coms[-1] + array([0.7, 0.0, 0.0])
# ~ initPos = allfeetpos[-1]
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False)
pb = gen_flat_pb()
A, b, E, e = convertProblemToLp(pb)
bcom = b - A[:,4:].dot(res[4:]) - A[:,2].dot(res[2])
Acom = np.hstack([A[:,:2], A[:,3:4]])
for i in range(5):
print ("WTTTTTTTTTTFFFFFFFFFFFFFFf!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i)
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
pb = gen_flat_pb()
initPos = None
endPos = None
initCom = coms[-1]
endCom = coms[-1] + array([0.6, 0.0, 0.0])
initPos = allfeetpos[-1]
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False)
# ~ plt.show(block = False)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = None)
......
......@@ -43,7 +43,7 @@ a_all_surfaces = [array(el).T for el in all_surfaces]
# ~ surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3, astep1], [astep3], [astep4],[astep4]]
# ~ surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3], [astep3], [astep4],[astep4]]
# ~ surfaces = [a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces,[astep4]]
surfaces = [[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],a_all_surfaces,a_all_surfaces,a_all_surfaces,a_all_surfaces,a_all_surfaces,[astep4]]
surfaces = [[afloor],a_all_surfaces,a_all_surfaces,a_all_surfaces,a_all_surfaces,a_all_surfaces,[astep4]]
# ~ surfaces = [a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces]
# ~ surfaces = [[afloor], [afloor], a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces,[astep4]]
# ~ surfaces = [[astep4]]
......
......@@ -38,9 +38,10 @@ astep4 = array(step4).T
a_all_surfaces = [array(el).T for el in all_surfaces]
# ~ surfaces = [[afloor], [afloor], [astep1,astep2,astep3],[astep2,astep3,astep1], [astep3,astep2,astep1,astep4], [astep3,astep4], [astep4],[astep4]]
surfaces = [[afloor], [afloor], [astep1,astep2,astep3],[astep2,astep3,astep1], [astep3,astep2,astep1,astep4], [astep3,astep4], [astep4],[astep4]]
# ~ surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3, astep1], [astep3], [astep4],[astep4]]
surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3], [astep3], [astep4],[astep4]]
# ~ surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3], [astep3], [astep4],[astep4]]
# ~ surfaces = [[afloor], [afloor], [astep1],]
# ~ surfaces = [a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces,[astep4]]
# ~ surfaces = [a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces]
# ~ surfaces = [[afloor], [afloor], a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces,[astep4]]
......
......@@ -7,10 +7,10 @@ Inequalities = namedtuple("Inequality", "A b N V")
__EPS = 0.0
def toFloat(stringArray):
res= np.zeros(len(stringArray))
for i in range(0,len(stringArray)):
res[i] = float(stringArray[i])
return res
res= np.zeros(len(stringArray))
for i in range(0,len(stringArray)):
res[i] = float(stringArray[i])
return res
def load_obj(filename) :
V = [] #vertex
......@@ -72,65 +72,65 @@ def inequalities_to_Inequalities_object(A,b):
def inequality(v, n):
#the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
#inequality is then ax + by +cz -d <= 0
# last var is v because we need it
return [n[0], n[1], n[2], np.array(v).dot(np.array(n)) + __EPS]
#the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
#inequality is then ax + by +cz -d <= 0
# last var is v because we need it
return [n[0], n[1], n[2], np.array(v).dot(np.array(n)) + __EPS]
def as_inequalities(obj):
#for each face, find first three points and deduce plane
#inequality is given by normal
A= np.empty([len(obj.F), 3])
b = np.empty(len(obj.F))
V = np.ones([len(obj.F), 4])
N = np.empty([len(obj.F), 3])
for f in range(0, len(obj.F)):
face = obj.F[f]
v = obj.V[face[0][0]]
# assume normals are in obj
n = obj.N[face[0][2]]
ineq = inequality(v,n)
A[f,:] = ineq[0:3]
b[f] = ineq[3]
V[f,0:3] = v
N[f,:] = n
return Inequalities(A,b, N, V)
#for each face, find first three points and deduce plane
#inequality is given by normal
A= np.empty([len(obj.F), 3])
b = np.empty(len(obj.F))
V = np.ones([len(obj.F), 4])
N = np.empty([len(obj.F), 3])
for f in range(0, len(obj.F)):
face = obj.F[f]
v = obj.V[face[0][0]]
# assume normals are in obj
n = obj.N[face[0][2]]
ineq = inequality(v,n)
A[f,:] = ineq[0:3]
b[f] = ineq[3]
V[f,0:3] = v
N[f,:] = n
res = Inequalities(A,b, N, V)
return res
def is_inside(inequalities, pt):
return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
#~ def rotate_inequalities_q():
# TODO this is naive, should be a way to simply update d
def rotate_inequalities(ineq, transform):
#for each face, find first three points and deduce plane
#inequality is given by normal
A = np.empty([len(ineq.A), 3])
b = np.empty(len(ineq.b))
V = np.ones([len(ineq.V), 4])
N = np.ones([len(ineq.N), 3])
for i in range(0, len(b)):
v = transform.dot(ineq.V[i,:])
n = transform[0:3,0:3].dot(ineq.N[i,0:3])
ine = inequality(v[0:3],n[0:3])
A[i,:] = ine[0:3]
b[i] = ine[3]
V[i,:] = v
N[i,:] = n
return Inequalities(A,b, N, V)
#for each face, find first three points and deduce plane
#inequality is given by normal
A = np.empty([len(ineq.A), 3])
b = np.empty(len(ineq.b))
V = np.ones([len(ineq.V), 4])
N = np.ones([len(ineq.N), 3])
for i in range(0, len(b)):
v = transform.dot(ineq.V[i,:])
n = transform[0:3,0:3].dot(ineq.N[i,0:3])
ine = inequality(v[0:3],n[0:3])
A[i,:] = ine[0:3]
b[i] = ine[3]
V[i,:] = v
N[i,:] = n
return Inequalities(A,b, N, V)
from pickle import dump
def ineq_to_file(ineq, filename):
f1=open(filename, 'w+')
res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V}
dump(res, f1)
f1.close()
f1=open(filename, 'w+')
res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V}
dump(res, f1)
f1.close()
from pickle import load
def ineq_from_file(filename):
f1=open(filename, 'r')
res = load(f1)
return Inequalities(res['A'], res['b'],res['N'],res['V'])
f1=open(filename, 'r')
res = load(f1)
return Inequalities(res['A'], res['b'],res['N'],res['V'])
# ~ def plot(Inequalities)
......@@ -5,7 +5,7 @@ from numpy import array, dot, vstack, hstack, asmatrix, identity, cross
from numpy.linalg import norm
from scipy.spatial import ConvexHull
from hpp_bezier_com_traj import *
# ~ from hpp_bezier_com_traj import *
#~ from qp import solve_lp
import eigenpy
......
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