Commit 80af5a85 authored by Steve T's avatar Steve T
Browse files

all constraints with inequality form written

parent ffb1ea4b
......@@ -30,7 +30,7 @@ zero3 = zeros(3)
eps =0.000001
#### surface to inequalities ####
def convert_surface_to_inequality(s):
def convert_surface_to_inequality(s, eqAsIneq):
#TODO does normal orientation matter ?
#it will for collisions
n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0])
......@@ -38,13 +38,13 @@ def convert_surface_to_inequality(s):
for i in range(3):
n[i] = -n[i]
n /= norm(n)
return surfacePointsToIneq(s, n)
return surfacePointsToIneq(s, n, eqAsIneq)
def replace_surfaces_with_ineq_in_phaseData(phase):
phase["S"] = [convert_surface_to_inequality(S) for S in phase["S"]]
def replace_surfaces_with_ineq_in_phaseData(phase, eqAsIneq):
phase["S"] = [convert_surface_to_inequality(S, eqAsIneq) for S in phase["S"]]
def replace_surfaces_with_ineq_in_problem(pb):
[ replace_surfaces_with_ineq_in_phaseData(phase) for phase in pb ["phaseData"]]
def replace_surfaces_with_ineq_in_problem(pb, eqAsIneq = False):
[ replace_surfaces_with_ineq_in_phaseData(phase, eqAsIneq) for phase in pb ["phaseData"]]
def ineqQHull(hull):
A = hull.equations[:,:-1]
......@@ -125,8 +125,9 @@ def default_transform_from_pos_normal(pos, normal):
return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] )
EPSILON_EQ = 0.005
#last is equality
def surfacePointsToIneq(S, normal):
def surfacePointsToIneq(S, normal, eqAsIneq):
n = array(normal)
tr = default_transform_from_pos_normal(array([0.,0.,0.]),n)
trinv = tr.copy(); trinv[:3,:3] = tr[:3,:3].T;
......@@ -140,10 +141,13 @@ def surfacePointsToIneq(S, normal):
#adding plane constraint
#plane equation given by first point and normal for instance
d = array([n.dot(S[:,0])])
A = vstack([ine.A, n , -n ])
b = concatenate([ine.b, d, -d]).reshape((-1,))
A = vstack([ine.A, n])
b = concatenate([ine.b, d]).reshape((-1,))
if eqAsIneq:
A = vstack([ine.A, n , -n ])
b = concatenate([ine.b, d + EPSILON_EQ, -d + EPSILON_EQ]).reshape((-1,))
else:
A = vstack([ine.A, n])
b = concatenate([ine.b, d]).reshape((-1,))
return A, b
############ BENCHMARKING ###############
......
......@@ -37,7 +37,6 @@ GAMMA_START = None
ALPHA_START = None
#TODO : one surface: all inequalities apply, but without alpha
# breakdown of variables for each phase
......@@ -87,8 +86,8 @@ def initGlobals(nEffectors):
global DEFAULT_NUM_EQUALITY_CONSTRAINTS
N_EFFECTORS = nEffectors
# for one phase, for all i:
# [ c_x't, c_y't, c_z_1't, c_z_2't, p_i't, b_i't, g_i't, a_i_l't , d_i_l't ]
# [ 1 , 1 , 1 , 1 , 3*n_e, 2*n_e, 3*n_e, [0, n_s], [0, n_s]]
# [ c_x't, c_y't, c_z_1't, c_z_2't, p_i't, b_i't, g_i't, a_i_l't , d_i_l't ]
# [ 1 , 1 , 1 , 1 , 2*n_e, 2*n_e, 3*n_e, [0, n_s], [n_s * n_e]]
DEFAULT_NUM_VARS = N_EFFECTORS * 8 + 4
BETA_START = 4 + 3* N_EFFECTORS
GAMMA_START = BETA_START + 2 * N_EFFECTORS
......@@ -135,10 +134,11 @@ def numIneqForPhase(phase, phaseId):
# all inequalities relative to each contact surface
ret += sum([S[1].shape[0]-1 for S in phase["S"]])
numSurfaces =len(phase["S"])
if numSurfaces > 1:
# ~ if numSurfaces > 1:
#adding all slack inequality comparison variables
# TODO: replace by two inequalities ???
ret += numSurfaces * N_EFFECTORS * NUM_INEQ_SLACK_PER_SURFACE
# TODO: replace by two inequalities ???
#TODO : one surface: all inequalities apply, but without alpha
ret += numSurfaces * N_EFFECTORS * NUM_INEQ_SLACK_PER_SURFACE
return ret
def getTotalNumVariablesAndIneqConstraints(pb):
......@@ -222,17 +222,28 @@ def SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow):
def SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow):
numSurfaces =len(phaseDataT["S"])
idRow = startRow
if numSurfaces > 1:
colOmegaOffset = startCol + DEFAULT_NUM_VARS
colDeltaOffset = colOmegaOffset + numSurfaces
# ~ if numSurfaces > 1:
#TODO : one surface: all inequalities apply, but without alpha
colOmegaOffset = startCol + DEFAULT_NUM_VARS
if numSurfaces > 1:
colDeltaOffset = colOmegaOffset + numSurfaces
for i in range(numSurfaces):
# - a0 <= a1_i <= a0
# TODO: beta entre 0 et 1
# M2 (-beta_x't) + M2 (-beta_y't) - M a0 <= a1_i <= M a0 + M2 (1 - beta_x't) + M2 (beta_y't)
for footId in range(N_EFFECTORS):
A[idRow , colOmegaOffset + i ] = [-1]; # - a0 + a1_i <= 0
A[idRow , colDeltaOffset + i * footId + footId] = [ 1]; # - a0 + a1_i <= 0
A[idRow+1 , colOmegaOffset + i ] = [-1]; # - a0 - a1_i <= 0
A[idRow+1 , colDeltaOffset + i * footId + footId] = [-1]; # - a0 - a1_i <= 0
idRow += NUM_INEQ_SLACK_PER_SURFACE
else:
colDeltaOffset = colOmegaOffset
for footId in range(N_EFFECTORS):
A[idRow , colOmegaOffset + i ] = [-1]; # - a0 + a1_i <= 0
A[idRow , colDeltaOffset + i * footId + footId] = [ 1]; # - a0 + a1_i <= 0
A[idRow+1 , colOmegaOffset + i ] = [-1]; # - a0 - a1_i <= 0
A[idRow+1 , colDeltaOffset + i * footId + footId] = [-1]; # - a0 - a1_i <= 0
idRow += NUM_INEQ_SLACK_PER_SURFACE
return idRow
......
This diff is collapsed.
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