Commit 80af5a85 by Steve T

### 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