Cleaning and refactoring

.gitignore

+.vscode/*
+build/*
 *.pyc
 *.aux
 *.glo

.gitlab-ci.yml

+include: https://rainboard.laas.fr/project/sl1m/.gitlab-ci.yml

CMakeLists.txt

@@ -22,11 +22,15 @@ FINDPYTHON()
 SET(${PROJECT_NAME}_SOURCES
   __init__.py
   constants_and_tools.py
+  solver.py
+  generic_solver.py
   fix_sparsity.py
-  planner_l1.py
-  planner.py
+  problem_data.py
+  constraints.py
+  constraints_biped.py
+  planner_generic.py
+  planner_biped.py
   problem_definition.py
-  qp.py
   )
 
 SET(${PROJECT_NAME}_PLANNER_SCENARIOS_SOURCES
@@ -49,13 +53,15 @@ SET(${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES
   __init__.py
 
   complex1.py
-  constraints.py
+  problem_definition_talos.py
   lp_complex1_path.py
   lp_complex1.py
   lp_ramp_path.py
   lp_rubbles_path.py
   lp_slalom_debris_path.py
   lp_slalom_debris.py
+  lp_stairs_path.py
+  stairs.py
   maze.py
   ramp_noGuide.py
   rubbles.py
@@ -65,6 +71,7 @@ SET(${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES
 
 SET(${PROJECT_NAME}_RBPRM_SOURCES
   __init__.py
+  narrow_convex_hull.py
   constants_and_tools.py
   surfaces_from_planning.py
   )
@@ -82,6 +89,7 @@ SET(${PROJECT_NAME}_TOOLS_SOURCES
   obj_to_constraints.py
   plot_plytopes.py
   plot_utils.py
+  plot_tools.py
   polytope_conversion_utils.py
   transformations.py
   )

README.md

+# sl1m
+
+[![Pipeline status](https://gitlab.laas.fr/$ORG/sl1m/badges/master/pipeline.svg)](https://gitlab.laas.fr/loco-3d/sl1m/commits/master)
+[![Coverage report](https://gitlab.laas.fr/loco-3d/sl1m/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/loco-3d/sl1m/master/coverage/)
+
+
 Python dependencies:
 	scipy
 	quadprog

sl1m/constants_and_tools.py

 import numpy as np
-from sl1m.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities, inequalities_to_Inequalities_object
-
-from numpy import array, asmatrix, matrix, zeros, ones
-from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
+from numpy import array, zeros, ones, vstack, hstack, identity, cross, concatenate
 from numpy.linalg import norm
-import numpy as np
-
 from scipy.spatial import ConvexHull
-from .qp import solve_lp
 
-#~ import eigenpy
-#from curves import bezier3
-from random import random as rd
-from random import randint as rdi
-from numpy import squeeze, asarray
+from sl1m.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities, inequalities_to_Inequalities_object
+
+# --------------------------------- CONSTANTS ---------------------------------------------------------------
 
+EPSILON = 0.000001
+EPSILON_EQ = 0.
 
-Id = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
-g = array([0.,0.,-9.81])
-g6 = array([0.,0.,-9.81,0.,0.,0.])
+IDENTITY = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
+GRAVITY = array([0., 0., -9.81])
+GRAVITY_6 = array([0., 0., -9.81, 0., 0., 0.])
 
-mu = 0.45
+X = array([1., 0., 0.])
+Y = array([0., 1., 0.])
+Z = array([0., 0., 1.])
 
-x = array([1.,0.,0.])
-z = array([0.,0.,1.])
-zero3 = zeros(3) 
+# --------------------------------- METHODS ---------------------------------------------------------------
 
 
-eps =0.000001
+def normalize(Ab):
+    A = Ab[0]
+    b = Ab[1]
+    A_normalized = zeros(A.shape)
+    b_normalized = zeros(b.shape)
+    for i in range(A.shape[0]):
+        n = norm(A[i, :])
+        if n <= EPSILON:
+            n = 1.
+        A_normalized[i, :] = A[i, :] / n
+        b_normalized[i] = b[i] / n
+    return A_normalized, b_normalized
 
-#### surface to inequalities ####    
-def convert_surface_to_inequality(s):
-    #TODO does normal orientation matter ?
-    #it will for collisions
-    n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0])
+
+def convert_surface_to_inequality(s, eq_as_ineq):
+    # TODO does normal orientation matter ? It will for collisions
+    n = cross(s[:, 1] - s[:, 0], s[:, 2] - s[:, 0])
     if n[2] <= 0.:
         for i in range(3):
             n[i] = -n[i]
-    n /= norm(n)
-    return surfacePointsToIneq(s, n)
-    
-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_problem(pb):
-    [ replace_surfaces_with_ineq_in_phaseData(phase) for phase in pb ["phaseData"]]
+    norm_n = norm(n)
+    if norm_n > 1e-6:
+        n /= norm(n)
+    else:
+        # FIXME: to something better here
+        n = array([0, 0, 1])
+    return surface_points_to_inequalities(s, n, eq_as_ineq)
+
+
+def normal_from_ineq(s_ineq):
+    n = s_ineq[0][-1]
+    if n[2] < 0:
+        n = -n
+    return n
+
+
+def replace_surfaces_with_ineq_in_phaseData(phase, eq_as_ineq):
+    phase["S"] = [convert_surface_to_inequality(S, eq_as_ineq) for S in phase["S"]]
+
+
+def replace_surfaces_with_ineq_in_problem(pb, eq_as_ineq=False):
+    [replace_surfaces_with_ineq_in_phaseData(phase, eq_as_ineq) for phase in pb["phaseData"]]
+
 
 def ineqQHull(hull):
-    A = hull.equations[:,:-1]
-    b = -hull.equations[:,-1]
-    return A,b
-    
-def vectorProjection (v, n):    
-    v = v / norm(v)
-    n = n / norm(n)
-    proj = v - np.dot(v,n)*n
-    return proj/norm(proj)
-    
-def addHeightConstraint(K,k, val):
-    K1 = vstack([K, -z])
-    k1 = concatenate([k, -ones(1) * val]).reshape((-1,))    
-    return K1, k1
-
-
-def default_transform_from_pos_normal_(transform, pos, normal):
-    #return vstack( [hstack([transform,pos.reshape((-1,1))]), [ 0.        ,  0.        ,  0.        ,  1.        ] ] ) # FIXME : temp stuff, only work on flat floor
-    # FIXME : there is something wrong the the code above
-    #~ print "pos ", pos
-    #~ print "normal ", normal
-    #align the x-axis of the foot to the root heading direction    
-    f = x
-    xp = np.dot(transform, x).tolist()  
-    t = vectorProjection(xp, normal)
-    v = np.cross(f, t)
-    c = np.dot(f, t)
-    
-    if abs(c) > 0.99 :
-        return vstack( [hstack([transform,pos.reshape((-1,1))]), [ 0.        ,  0.        ,  0.        ,  1.        ] ] )     
-    else:
-        u = v/norm(v)
-        h = (1. - c)/(1. - c**2)
+    A = hull.equations[:, :-1]
+    b = -hull.equations[:, -1]
+    return A, b
 
-        vx, vy, vz = v
-        rot1 =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
-              [h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
-              [h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
-    #align the z-axis of the foot to the surface normal
-    f = z
-    t = array(normal)
-    t = t / norm(t)
-    v = np.cross(f, t)
-    c = np.dot(f, t)    
-    if abs(c) > 0.99 :
-        rot2 = identity(3)    
-    else:
-        u = v/norm(v)
-        h = (1. - c)/(1. - c**2)
 
-        vx, vy, vz = v
-        rot2 =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
-              [h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
-              [h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
-              
-    rot = np.dot(rot1,rot2)
-    return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0.        ,  0.        ,  0.        ,  1.        ] ] )
-    
-
-def default_transform_from_pos_normal(pos, normal):
-    f = array([0.,0.,1.])
+def default_transform_from_pos_normal(pos, normal, transform=IDENTITY):
+    """ 
+    transform matrix is a rotation matrix from the root trajectory
+    used to align the foot yaw (z-axis) orientation
+    surface normal is used to align the foot roll and pitch (x- and y- axes) orientation
+    """
+    f = Z
     t = array(normal)
     t = t / norm(t)
     v = np.cross(f, t)
-    c = np.dot(f, t)    
-    if abs(c) > 0.99 :
-        rot = identity(3)    
+    c = np.dot(f, t)
+    if abs(c) > 0.99:
+        rot = identity(3)
     else:
         u = v/norm(v)
         h = (1. - c)/(1. - c**2)
+        s = (1. - c**2)
+        assert s > 0
+        s = np.sqrt(s)
+        ux, uy, uz = u
+        rot = array([[c + h*ux**2, h*ux*uy - uz*s, h*ux*uz + uy*s],
+                     [h*ux*uy + uz*s, c + h*ux**2, h*uy*uz - ux*s],
+                     [h*ux*uz - uy*s, h*uy*uz + ux*s, c + h*uz**2]])
 
-        vx, vy, vz = v
-        rot =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
-              [h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
-              [h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
-    return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0.        ,  0.        ,  0.        ,  1.        ] ] )
+    rot = np.dot(transform, rot)
+    return vstack([hstack([rot, pos.reshape((-1, 1))]), [0.,  0.,  0.,  1.]])
 
-    
-#last is equality
-def surfacePointsToIneq(S, normal):
+
+def surface_points_to_inequalities(S, normal, eq_as_ineq):
     n = array(normal)
-    tr = default_transform_from_pos_normal(array([0.,0.,0.]),n)
-    trinv = tr.copy(); trinv[:3,:3] = tr[:3,:3].T;
-        
-    trpts = [tr[:3,:3].dot(s)[:2] for s in S.T]
+    tr = default_transform_from_pos_normal(array([0., 0., 0.]), n)
+    trinv = tr.copy()
+    trinv[:3, :3] = tr[:3, :3].T
+    trpts = [tr[:3, :3].dot(s)[:2] for s in S.T]
     hull = ConvexHull(array(trpts))
-    A,b = ineqQHull(hull)
-    A = hstack([A,zeros((A.shape[0],1))])
-    ine = inequalities_to_Inequalities_object(A,b)
+    A, b = ineqQHull(hull)
+    A = hstack([A, zeros((A.shape[0], 1))])
+    ine = inequalities_to_Inequalities_object(A, b)
     ine = rotate_inequalities(ine, trinv)
-    #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,))
+    d = array([n.dot(S[:, 0])])
+    if eq_as_ineq:
+        A = vstack([ine.A, n, -n])
+        b = concatenate([ine.b, d + EPSILON_EQ, -d + EPSILON_EQ]).reshape((-1,))
+    else:
+        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,))
+
     return A, b
-    
-############ BENCHMARKING ###############
 
-try:
-    from time import perf_counter as clock
-except ImportError:
-    from time import  clock
 
 def timMs(t1, t2):
+    """ 
+    Expresses the duration between t1 and t2 in milliseconds
+    """
     return (t2-t1) * 1000.

sl1m/constraints.py

+import numpy as np
+
+# Implementation of the class constraints, which implements the constraints used by the generic planner
+#
+# The problem is a LP with these variables for each phase:
+#  [ com_x, com_y, com_z_1, com_z_2, p_i_x, p_i_y, p_i_z, {a_i}                                ]
+#  [ 1    , 1    , 1      , 1      , 3                  , 0 if n_surfaces == 1, else n_surfaces]
+#
+# Under the following constraints :
+# - fixed_foot_com: Ensures the COM is 'above' the fixed feet
+# - foot_relative_distance: Ensures the moving feet is close enough to the other feet
+# - surface: Each foot belongs to one surface
+# - slack_positivity: The slack variables are positive
+# - com_weighted_equality: Fix the horizontal position of the COm at the barycenter of the contact points (fixed feet)
+
+
+class Constraints:
+    def __init__(self, n_effectors):
+        self.n_effectors = n_effectors
+        self.default_n_variables = 7
+
+        self.WEIGHTS = [1./float(n_effectors - 1) for _ in range(n_effectors)]
+
+        self.com_xy = self.__expression_matrix(2, 0)
+        self.com_1 = self.__expression_matrix(3, 0)
+        self.com_2 = self.__expression_matrix(3, 0)
+        self.com_2[2, 2] = 0
+        self.com_2[2, 3] = 1
+        self.com_1_z = self.__expression_matrix(1, 2)
+        self.com_2_z = self.__expression_matrix(1, 3)
+        self.foot = self.__expression_matrix(3, 4)
+        self.foot_xy = self.__expression_matrix(2, 4)
+
+    def __expression_matrix(self, size, j):
+        """
+        Generate a selection matrix for a given variable 
+        @param size number of rows of the variable
+        @param x position of the variable in the phase variables
+        @return a (size, number of default variables (without slacks)) matrix with identity at column j
+        """
+        M = np.zeros((size, self.default_n_variables))
+        M[:, j:j+size] = np.identity(size)
+        return M
+
+    def __fixed_foot_com_2_kinematic(self, pb, phase, G, h, i_start, js, feet_phase):
+        """
+        The COM 2 must belong to a reachable polytope above each end-effector of the phase
+        For each effector id , K_id (c2 - p_id) <= k_id
+        @param pb          The problem specific data
+        @param phase       The phase specific data
+        @param G           The inequality constraint matrix
+        @param h           The inequality constraint vector
+        @param i_start     Initial row to use
+        @param js          List of column corresponding to the start of each phase
+        @param feet_phase List of the feet las moving phase, -1 if they haven't moved
+        @return  i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        j = js[-1]
+        for foot, (K, k) in enumerate(phase["K"]):
+            if foot == phase["Moving"]:
+                l = k.shape[0]
+                G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2 - self.foot)
+                h[i:i + l] = k
+                i += l
+            elif feet_phase[foot] != -1:
+                l = k.shape[0]
+                j_foot = js[feet_phase[foot]]
+                G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
+                G[i:i + l, j_foot:j_foot + self.default_n_variables] = -K.dot(self.foot)
+                h[i:i + l] = k
+                i += l
+            else:
+                l = k.shape[0]
+                G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
+                foot_pose = pb["p0"][foot]
+                h[i:i + l] = k + K.dot(foot_pose)
+                i += l
+        return i
+
+    def __fixed_foot_com_1_kinematic(self, pb, phase,  G, h, i_start, js, feet_phase):
+        """
+        The COM 1 must belong to a reachable polytope above each end-effector of the previous phase
+        For each effector id , K_id (c1- p_id(t-1)) <= k_id
+        This constraint should not be added at the first phase.
+        @param pb          The problem specific data
+        @param phase       The phase specific data
+        @param G           The inequality constraint matrix
+        @param h           The inequality constraint vector
+        @param i_start     Initial row to use
+        @param js          List of column corresponding to the start of each phase
+        @param feet_phase List of the feet las moving phase, -1 if they haven't moved
+        @return  i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        j = js[-1]
+        for foot, (K, k) in enumerate(phase["K"]):
+            if foot == phase["Moving"]:
+                if feet_phase[foot] != -1:
+                    l = k.shape[0]
+                    j_foot = js[feet_phase[foot]]
+                    G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_1)
+                    G[i:i + l, j_foot:j_foot + self.default_n_variables] = -K.dot(self.foot)
+                    h[i:i + l] = k
+                    i += l
+                else:
+                    l = k.shape[0]
+                    G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_1)
+                    foot_pose = pb["p0"][foot]
+                    h[i:i + l] = k + K.dot(foot_pose)
+                    i += l
+            elif feet_phase[foot] != -1:
+                l = k.shape[0]
+                j_foot = js[feet_phase[foot]]
+                G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
+                G[i:i + l, j_foot:j_foot + self.default_n_variables] = -K.dot(self.foot)
+                h[i:i + l] = k
+                i += l
+            else:
+                l = k.shape[0]
+                G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
+                foot_pose = pb["p0"][foot]
+                h[i:i + l] = k + K.dot(foot_pose)
+                i += l
+        return i
+
+    def fixed_foot_com(self, pb, phase,  G, h, i_start, js, phase_id, feet_phase):
+        """
+        The COM must belong to a reachable polytope above each end-effector
+        For each effector id , K_id (c(t) - p_id(t-1)) <= k_id
+        This constraint should not be added at the first phase.
+        @param pb          The problem specific data
+        @param phase       The phase specific data
+        @param G           The inequality constraint matrix
+        @param h           The inequality constraint vector
+        @param i_start     Initial row to use
+        @param js          List of column corresponding to the start of each phase
+        @param phase_id    Phase number
+        @param feet_phase List of the feet las moving phase, -1 if they haven't moved
+        @return  i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        if phase_id != 0:
+            i = self.__fixed_foot_com_1_kinematic(pb, phase,  G, h, i_start, js, feet_phase)
+        return self.__fixed_foot_com_2_kinematic(pb, phase,  G, h, i, js, feet_phase)
+
+    def foot_relative_distance(self, pb, phase, G, h, i_start, js, feet_phase):
+        """
+        The distance between the moving effector and the other ones is limited
+        For i = moving_foot, For j !=i, Ki (pj - pi) <= ki
+        @param pb          The problem specific data
+        @param phase       The phase specific data
+        @param G           The inequality constraint matrix
+        @param h           The inequality constraint vector
+        @param i_start     Initial row to use
+        @param js          List of column corresponding to the start of each phase
+        @param feet_phase List of the feet las moving phase, -1 if they haven't moved
+        @return i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        j = js[-1]
+        constraints = phase["allRelativeK"][phase["Moving"]]
+        for (foot, (K, k)) in constraints:
+            l = k.shape[0]
+            G[i:i + l, j:j + self.default_n_variables] = -K.dot(self.foot)
+            if feet_phase[foot] != -1:
+                j_foot = js[feet_phase[foot]]
+                G[i:i + l, j_foot:j_foot + self.default_n_variables] = K.dot(self.foot)
+                h[i:i + l] = k
+            else:
+                foot_pose = pb["p0"][foot]
+                h[i:i + l] = k - K.dot(foot_pose)
+            i += l
+        return i
+
+    def slack_positivity(self, phase, G, h, i_start, j):
+        """
+        The slack variables (alpha) should be positive
+        Sl for each surface s, -alpha_s <= 0
+        @param phase       The phase specific data
+        @param G           The inequality constraint matrix
+        @param h           The inequality constraint vector
+        @param i_start     Initial row to use
+        @param j           Column corresponding to this phase variables
+        @return i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        n_surfaces = len(phase["S"])
+        if n_surfaces > 1:
+            for s in range(n_surfaces):
+                G[i+s, j + self.default_n_variables + s] = -1
+            i += n_surfaces
+        return i
+
+    def surface_inequality(self, phase, G, h, i_start, j):
+        """
+        The moving foot must belong to one surface
+        For each surface l: Sl pi - alphal <= sl
+        @param phase       The phase specific data
+        @param G           The inequality constraint matrix
+        @param h           The inequality constraint vector
+        @param i_start     Initial row to use
+        @param j           Column corresponding to this phase variables
+        @return i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        n_surfaces = len(phase["S"])
+        j_alpha = self.default_n_variables
+        for (S, s) in phase["S"]:
+            l = S.shape[0]
+            G[i:i + l, j:j + self.default_n_variables] = S.dot(self.foot)
+            h[i:i + l] = s
+            if n_surfaces > 1:
+                G[i:i + l, j + j_alpha] = -np.ones(l)
+                j_alpha += 1
+            i += l
+        return i
+
+    def __com_weighted_equality(self, pb, phase, C, d, i_start, js, feet_phase):
+        """
+        The 2D position of the com should be the baricenter of the fixed feet locations
+        0 =  sum(fixed_foot i) WEIGHT * p_i_x_y - c_x_y
+        @param pb          The problem specific data
+        @param phase       The phase specific data
+        @param C           The equality constraint matrix
+        @param d           The equality constraint vector
+        @param i_start     Initial row to use
+        @param js          List of column corresponding to the start of each phase
+        @param feet_phase List of the feet las moving phase, -1 if they haven't moved
+        @return i_start + the number of rows used by the constraint
+        """
+        i = i_start
+        j = js[-1]
+        for foot in range(self.n_effectors):
+            if foot == phase["Moving"]:
+                continue
+            if feet_phase[foot] != -1:
+                j_foot = js[feet_phase[foot]]
+                C[i:i + 2, j_foot:j_foot + self.default_n_variables] = self.WEIGHTS[foot] * self.foot_xy
+            else:
+                foot_pose = pb["p0"][foot]
+                d[i:i + 2] -= self.WEIGHTS[foot] * foot_pose[:2]
+        C[i:i + 2, j:j + self.default_n_variables] = -self.com_xy
+        i += 2
+        return i
+
+    def __com_equality_init(self, pb, C, d, i_start):
+        """
+        The  initial com position is defined in the problem
+        @param pb          The problem specific data
+        @param C           The equality constraint matrix
+        @param d           The equality constraint vector
+        @param i_start     Initial row to use
+        @return i_start + the number of rows used by the constraint
+        """
+        initial_position = pb["c0"]
+        i = i_start
+        C[i:i + 2, :self.default_n_variables] = self.com_xy
+        d[i:i + 2] = initial_position[:2]
+        i += 2
+        return i
+
+    def com(self, pb, phase, C, d, i_start, js, phase_id, feet_phase):
+        """
+        The com position is defined
+        @param pb          The problem specific data
+        @param phase       The phase specific data
+        @param C           The equality constraint matrix
+        @param d           The equality constraint vector