all constraints with inequality form written

constants_and_tools.py

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

planner_l1_generic.py

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