text:
import os, sys
import numpy
from math import cos, sin, sqrt
from copy import deepcopy

from helper import BaseTypeFoot, BaseTypeSupportFoot
from helper import ZMPState, CoMState
from visualization_traj import PlotData

class BaseGeneratorTraj(object):
    """
    Base class of walking pattern generator for humanoids, cf.
    LAAS-UHEI walking report.

    BaseClass provides all matrices and timestepping methods that are
    defined for the pattern generator family. In derived classes different
    problems and their solvers can be realized.
    """
    # define some constants
    g = 9.81

    # define list of members for plotting
    _plot_keys = [
        'time',
        'c_k_x',
        'c_k_y',
        'c_k_q',
        'f_k_x',
        'f_k_y',
        'f_k_q',
        'z_k_x',
        'z_k_y',
        'xi_k_x',
        'xi_k_y',
        'h_com',
        'C_kp1_x',
        'dC_kp1_x',
        'ddC_kp1_x',
        'C_kp1_y',
        'dC_kp1_y',
        'ddC_kp1_y',
        'C_kp1_q',
        'dC_kp1_q',
        'ddC_kp1_q',
        'dddC_k_x',
        'dddC_k_y',
        'dddC_k_q',
        'C_kp1_x_ref',
        'C_kp1_y_ref',
        'C_kp1_q_ref',
        'f_k_qL',
        'f_k_qR',
        'F_k_qL',
        'F_k_qR',
        'dF_k_qL',
        'dF_k_qR',
        'ddF_k_qL',
        'ddF_k_qR',
        'dddF_k_qL',
        'dddF_k_qR',
        'F_kp1_x',
        'F_kp1_y',
        'F_kp1_q',
        'F_k_x',
        'F_k_y',
        'F_k_q',
        'Z_kp1_x',
        'Z_kp1_y',
        'Xi_kp1_x',
        'Xi_kp1_y',
        'fsm_state',
        'fsm_states',
    ]

    # define hull names for plotting
    _hull_keys = [
        'rfposhull',
        'lfposhull',
        'lfoot',
        'rfoot',
        'lfcophull',
        'rfcophull',
        'dscophull',
    ]

    # define values needed for calculations
    _data_keys = [
        'N',
        'nf',
        'T',
        'T_step',
        'footWidth',
        'footHeight',
        'footDistance',
    ]

    def __init__(
        self, N=16, T=0.1, T_step=0.8,
        fsm_state='D', fsm_sl=1
    ):
        """
        Initialize pattern generator, i.e.
        * allocate memory for matrices
        * populate them according to walking reference document

        Parameters
        ----------

        N : int
            Number of time steps of prediction horizon

        T : float
            Time increment between two time steps (default: 0.1 [s])

        nf : int
            Number of foot steps planned in advance (default: 2 [footsteps])

        T_step : float
            Time for the robot to make 1 step

        T_window : float
            Duration of the preview window of the controller
        fsm_state: str
            Initial state of the finite state machine for startin and stopping
            maneuvers.

        fsm_sl: int
            Number of steps in inplace stepping until stop
        """
        self.N = N
        self.T = T
        self.T_window = N*T
        self.T_step = T_step
        self.nf = (int)(self.T_window/T_step)
        self.time = 0.0
        # finite state machine for starting and landing maneuvers
        self._fsm_states = ('D', 'L/R', 'R/L', 'Lbar/Rbar', 'Rbar/Lbar')

        err_str = 'proposed state {} not in FSM states ({})'.format(fsm_state, self._fsm_states)
        assert fsm_state in self._fsm_states, err_str
        self.fsm_state = fsm_state
        self.fsm_states = numpy.array((self.fsm_state,)*self.nf, dtype=str)
        self._fsm_sl = fsm_sl

        # objective weights

        self.a = 1.0   # weight for CoM velocity tracking
        self.b = 0.0   # weight for CoM average velocity tracking
        self.c = 1e-06 # weight for ZMP reference tracking
        self.d = 1e-05 # weight for jerk minimization

        # center of mass initial values
        # NOTE they are not all equal to zero, because of half-sitting initial
        #      position of HRP-2 and the case, that its hip is not equal to its
        #      center of mass. z position is fixed because of LIPM approximation.

        self.c_k_x = numpy.array(
            (-1.98477637e-03, 0.0, 0.0), # 0.00124774 (HRP2) | -1.98477637e-03 (Pyrene)
            dtype=float
        )
        self.c_k_y = numpy.array(
            (7.22356707e-05, 0.0, 0.0), # 0.00157175(HRP2) | 7.22356707e-05 (Pyrene)
            dtype=float
        )
        self.c_k_q = numpy.zeros((3,), dtype=float)
        self.h_com = 8.92675352e-01 # 0.814 (HRP2) | 8.92675352e-01 (Pyrene)

        # center of mass matrices

        self.  C_kp1_x = numpy.zeros((N,), dtype=float)
        self. dC_kp1_x = numpy.zeros((N,), dtype=float)
        self.ddC_kp1_x = numpy.zeros((N,), dtype=float)

        self.  C_kp1_y = numpy.zeros((N,), dtype=float)
        self. dC_kp1_y = numpy.zeros((N,), dtype=float)
        self.ddC_kp1_y = numpy.zeros((N,), dtype=float)

        self.  C_kp1_q = numpy.zeros((N,), dtype=float)
        self. dC_kp1_q = numpy.zeros((N,), dtype=float)
        self.ddC_kp1_q = numpy.zeros((N,), dtype=float)

        # jerk controls for center of mass

        self.dddC_k_x = numpy.zeros((N,), dtype=float)
        self.dddC_k_y = numpy.zeros((N,), dtype=float)
        self.dddC_k_q = numpy.zeros((N,), dtype=float)

        # reference matrices

        # self. dC_kp1_x_ref = numpy.zeros((N,), dtype=float)
        # self. dC_kp1_y_ref = numpy.zeros((N,), dtype=float)
        # self. dC_kp1_q_ref = numpy.zeros((N,), dtype=float)
        # self.local_vel_ref = numpy.zeros((3,), dtype=float)

        self. C_kp1_x_ref = numpy.zeros((N,), dtype=float)
        self. C_kp1_y_ref = numpy.zeros((N,), dtype=float)
        self. C_kp1_q_ref = numpy.zeros((N,), dtype=float)
        self.traj_ref = numpy.zeros((3,N), dtype=float)

        # feet matrices
        # initial support foot positions and orientation
        # NOTE we assume the left foot to be the initial support foot
        self.f_k_x = 0.00949035
        self.f_k_y = 0.095
        self.f_k_q = 0.0

        self.F_k_x = numpy.zeros((self.nf,), dtype=float)
        self.F_k_y = numpy.zeros((self.nf,), dtype=float)
        self.F_k_q = numpy.zeros((self.nf,), dtype=float)

        # states for the foot orientation

        self.   f_k_qL = numpy.zeros((3,), dtype=float)
        self.   f_k_qR = numpy.zeros((3,), dtype=float)

        self.   F_k_qL = numpy.zeros((self.N,), dtype=float)
        self.   F_k_qR = numpy.zeros((self.N,), dtype=float)

        self.  dF_k_qL = numpy.zeros((self.N,), dtype=float)
        self.  dF_k_qR = numpy.zeros((self.N,), dtype=float)

        self. ddF_k_qL = numpy.zeros((self.N,), dtype=float)
        self. ddF_k_qR = numpy.zeros((self.N,), dtype=float)

        self.dddF_k_qL = numpy.zeros((self.N,), dtype=float)
        self.dddF_k_qR = numpy.zeros((self.N,), dtype=float)

        # foot angular velocity selection matrices objective
        # E_F = ( E_FR    0 )
        #       (    0 E_FL )
        # c est faux par rapport a la def en dessous...

        self.E_F  = numpy.zeros((self.N, 2*self.N), dtype=float)
        self.E_FR = self.E_F[:, :self.N]
        self.E_FL = self.E_F[:, self.N:]

        # foot angular velocity selection matrices objective

        self.E_F_bar  = numpy.zeros((self.N, 2*self.N), dtype=float)
        self.E_FR_bar = self.E_F_bar[:, :self.N]
        self.E_FL_bar = self.E_F_bar[:, self.N:]

        # zero moment point matrices

        self.z_k_x = self.c_k_x[0] - self.h_com/self.g * self.c_k_x[2]
        self.z_k_y = self.c_k_y[0] - self.h_com/self.g * self.c_k_y[2]

        self.Z_kp1_x = numpy.zeros((N,), dtype=float)
        self.Z_kp1_y = numpy.zeros((N,), dtype=float)

        # capture point matrices #NEW

        self.omega = sqrt(self.h_com/self.g)
        self.xi_k_x = self.c_k_x[0] + self.omega *self.c_k_x[1]
        self.xi_k_y = self.c_k_y[0] + self.omega * self.c_k_y[1]

        self.Xi_kp1_x = numpy.zeros((N,), dtype=float)
        self.Xi_kp1_y = numpy.zeros((N,), dtype=float)

        # transformation matrices

        self.Pps = numpy.zeros((N,3), dtype=float)
        self.Ppu = numpy.zeros((N,N), dtype=float)

        self.Pvs = numpy.zeros((N,3), dtype=float)
        self.Pvu = numpy.zeros((N,N), dtype=float)

        self.Pas = numpy.zeros((N,3), dtype=float)
        self.Pau = numpy.zeros((N,N), dtype=float)

        self.Pzs = numpy.zeros((N,3), dtype=float)
        self.Pzu = numpy.zeros((N,N), dtype=float)

        self.Pxis = numpy.zeros((N,3), dtype=float)
        self.Pxiu = numpy.zeros((N,N), dtype=float)

        # convex hulls used to bound the free placement of the foot
        self.nFootPosHullEdges = 5
            # support foot : right
        self.rfposhull = numpy.array((
                (-0.28, -0.2),
                (-0.20, -0.3),
                ( 0.00, -0.4),
                ( 0.20, -0.3),
                ( 0.28, -0.2),
        ), dtype=float)

            # support foot : left
        self.lfposhull = numpy.array((
                (-0.28, 0.2),
                (-0.20, 0.3),
                ( 0.00, 0.4),
                ( 0.20, 0.3),
                ( 0.28, 0.2),
        ), dtype=float)

        # set of Cartesian equalities
        self.A0r = numpy.zeros((5,2), dtype=float)
        self.ubB0r = numpy.zeros((5,), dtype=float)
        self.A0l = numpy.zeros((5,2), dtype=float)
        self.ubB0l = numpy.zeros((5,), dtype=float)

        # Linear constraints matrix
        self.nc_fchange_eq = 2
        self.eqAfoot = numpy.zeros((self.nc_fchange_eq,2*(self.N+self.nf)), dtype=float)
        self.eqBfoot = numpy.zeros((self.nc_fchange_eq,), dtype=float)

        # Linear constraints vector
        self.nc_foot_position = self.nf * self.nFootPosHullEdges
        self.Afoot   = numpy.zeros((self.nc_foot_position, 2*(self.N + self.nf)), dtype=float)
        self.lbBfoot =-numpy.ones((self.nc_foot_position), dtype=float)*1e+08
        self.ubBfoot = numpy.zeros((self.nc_foot_position), dtype=float)

        # security margins for CoP constraints
        self.SecurityMarginX = SMx = 0.09
        self.SecurityMarginY = SMy = 0.05

        # Position of the foot in the local foot frame
        self.nFootEdge    = 4
        self.footWidth    = 0.2 # 0.2172 (HRP2) | 0.2 (Pyrene)
        self.footHeight   = 0.12 # 0.1380 (HRP2) | 0.12 (Pyrene)
        self.footDistance = 0.19 # 0.2000 (HRP2) | 0.19 (Pyrene)

        # position of the vertices of the feet in the foot coordinates.
        # left foot
        self.lfoot = numpy.zeros((self.nFootEdge, 2), dtype=float)

        # right foot
        self.rfoot = numpy.zeros((self.nFootEdge, 2), dtype=float)

        # left foot
        self.lfcophull = numpy.zeros((self.nFootEdge, 2), dtype=float)

        # right foot
        self.rfcophull = numpy.zeros((self.nFootEdge, 2), dtype=float)

        # double support
        self.dscophull = numpy.zeros((self.nFootEdge, 2), dtype=float)

        # update hull arrays
        self._update_hulls()

        # Corresponding linear system from polygonal set
            # right foot
        self.A0rf   = numpy.zeros((self.nFootEdge,2), dtype=float)
        self.ubB0rf = numpy.zeros((self.nFootEdge,),  dtype=float)
            # left foot
        self.A0lf   = numpy.zeros((self.nFootEdge,2), dtype=float)
        self.ubB0lf = numpy.zeros((self.nFootEdge,),  dtype=float)
            # double support
        self.A0drf   = numpy.zeros((self.nFootEdge,2), dtype=float)
        self.ubB0drf = numpy.zeros((self.nFootEdge,),  dtype=float)
        self.A0dlf   = numpy.zeros((self.nFootEdge,2), dtype=float)
        self.ubB0dlf = numpy.zeros((self.nFootEdge,),  dtype=float)

        # transformation matrix for the CoP constraints in buildCoPconstraint()
        # constant in variables but varying in time, because of V_kp1
        # PzuV = ( PzuVx )
        #        ( PzuVy )
        #      = ( Pzu | -V_kp1 |   0 |      0 )
        #        (   0 |      0 | Pzu | -V_kp1 )

        self.PzuV  = numpy.zeros((2*self.N, 2*(self.N + self.nf)), dtype=float )
        self.PzuVx = self.PzuV[:self.N,:]
        self.PzuVy = self.PzuV[self.N:,:]

        # TODO tidy this up, because lots of redundant matrices
        # PzsC = ( PzsCx )
        #        ( PzsCy )
        #      = ( Pzs*c_k_x + v_kp1*f_k_x )
        #      = ( Pzs*c_k_y + v_kp1*f_k_y )
        self.PzsC  = numpy.zeros((2*self.N,), dtype=float )
        self.PzsCx = self.PzsC[:self.N]
        self.PzsCy = self.PzsC[self.N:]

        # v_kp1fc = ( v_kp1fc_x ) = ( v_kp1 * f_k_x)
        #           ( v_kp1fc_y )   ( v_kp1 * f_k_y)
        self.v_kp1fc = numpy.zeros((2*self.N,), dtype=float)
        self.v_kp1fc_x = self.v_kp1fc[:self.N]
        self.v_kp1fc_y = self.v_kp1fc[self.N:]

        # transformation matrix for the CP constraints in buildCoPconstraint()
        # constant in variables but varying in time, because of V_kp1
        # PxiuV = ( PxiuVx )
        #        ( PxiuVy )
        #      = ( Pxiu | -V_kp1 |   0 |      0 )
        #        (   0 |      0 | Pxiu | -V_kp1 )

        self.PxiuV  = numpy.zeros((2*self.N, 2*(self.N + self.nf)), dtype=float )
        self.PxiuVx = self.PxiuV[:self.N,:]
        self.PxiuVy = self.PxiuV[self.N:,:]

        # TODO tidy this up, because lots of redundant matrices
        # PxisC = ( PxisCx )
        #        ( PxisCy )
        #      = ( Pxis*c_k_x + v_kp1*f_k_x )
        #      = ( Pxis*c_k_y + v_kp1*f_k_y )
        self.PxisC  = numpy.zeros((2*self.N,), dtype=float )
        self.PxisCx = self.PxisC[:self.N]
        self.PxisCy = self.PxisC[self.N:]

        # D_kp1 = (D_kp1x, Dkp1_y)
        self.D_kp1  = numpy.zeros( (self.nFootEdge*self.N, 2*self.N), dtype=float )
        self.D_kp1x = self.D_kp1[:, :N] # view on big matrix
        self.D_kp1y = self.D_kp1[:,-N:] # view on big matrix
        self.b_kp1 = numpy.zeros( (self.nFootEdge*self.N,), dtype=float )

        # Constraint matrices
        self.nc_cop = self.N*self.nFootEdge
        self.Acop = numpy.zeros(
            (self.nc_cop, 2*(self.N+self.nf)),
             dtype=float
        )
        self.lbBcop = -numpy.ones((self.nc_cop), dtype=float)*1e+08
        self.ubBcop =  numpy.zeros((self.nc_cop), dtype=float)

        # Terminal Constraint : capture point
        self.nc_dcm = self.nFootEdge
        self.Adcm = numpy.zeros(
            (self.nc_dcm, 2*(self.N+self.nf)),
             dtype=float
        )
        self.lbBdcm = -numpy.ones((self.nc_dcm), dtype=float)*1e+08
        self.ubBdcm =  numpy.zeros((self.nc_dcm), dtype=float)

        # foot rotation constraints
        self.nc_fvel_eq = self.N # velocity constraints on support foot
        self.A_fvel_eq  = numpy.zeros((self.nc_fvel_eq, 2*self.N), dtype=float)
        self.B_fvel_eq  = numpy.zeros((self.nc_fvel_eq,), dtype=float)

        self.nc_fpos_ineq  = self.N # maximum orientation change
        self.A_fpos_ineq   = numpy.zeros((self.nc_fpos_ineq, 2*self.N), dtype=float)
        self.ubB_fpos_ineq = numpy.zeros((self.nc_fpos_ineq,), dtype=float)
        self.lbB_fpos_ineq = numpy.zeros((self.nc_fpos_ineq,), dtype=float)

        self.nc_fvel_ineq  = self.N # maximum angular velocity
        self.A_fvel_ineq   = numpy.zeros((self.nc_fvel_ineq, 2*self.N), dtype=float)
        self.ubB_fvel_ineq = numpy.zeros((self.nc_fvel_ineq,), dtype=float)
        self.lbB_fvel_ineq = numpy.zeros((self.nc_fvel_ineq,), dtype=float)

        # Current support state
        self.currentSupport = BaseTypeSupportFoot(x=self.f_k_x, y=self.f_k_y, theta=self.f_k_q, foot="left")
        self.currentSupport.timeLimit = 0
        self.currentSupport.ds = 0
        self.supportDeque = numpy.empty( (N,) , dtype=object )
        for i in range(N):
            self.supportDeque[i] = BaseTypeSupportFoot()
        self.supportDeque[0].ds = 1
        self.supportDeque[8].ds = 1

        """
        NOTE number of foot steps in prediction horizon changes between
        nf and nf+1, because when robot takes first step nf steps are
        planned on the prediction horizon, which makes a total of nf+1 steps.
        """
        self.v_kp1 = numpy.zeros((N,),   dtype=int)
        self.V_kp1 = numpy.zeros((N,self.nf), dtype=int)

        # initialize all elementary problem matrices, e.g.
        # state transformation matrices, constraints, etc.
        self._initialize_constant_matrices()
        self._initialize_cop_matrices()
        self._initialize_cp_matrices()
        self._initialize_selection_matrix()
        self._initialize_convex_hull_systems()

        self.data = PlotData(self)

    def _update_foot_selection_matrices(self):
        """ update the foot selection matrices E_F and E_F_bar """
        self.E_FR    [...] = 0.0
        self.E_FR_bar[...] = 0.0
        self.E_FL    [...] = 0.0
        self.E_FL_bar[...] = 0.0

        i = 0
        for j,supp in enumerate(self.supportDeque):
            if supp.foot == 'left':
                self.E_FR    [i,j] = 1.0
                self.E_FL    [i,j] = 0.0

                self.E_FR_bar[i,j] = 0.0
                self.E_FL_bar[i,j] = 1.0
            else:# supp.foot == 'right:'
                self.E_FR    [i,j] = 0.0
                self.E_FL    [i,j] = 1.0

                self.E_FR_bar[i,j] = 1.0
                self.E_FL_bar[i,j] = 0.0
            i += 1

    def _initialize_constant_matrices(self):
        """
        Initializes the constant transformation matrices, e.g. Pps, Ppu, Pvs,
        Pvu, Pas, Pau.
        """
        # renaming for convenience
        T_step = self.T_step
        T = self.T
        N = self.N
        nf = self.nf

        for i in range(N):
            j = i+1
            self.Pps[i, :] = (1.,   j*T,           (j**2*T**2)/2.)
            self.Pvs[i, :] = (0.,    1.,                      j*T)
            self.Pas[i, :] = (0.,    0.,                       1.)

            for j in range(N):
                if j <= i:
                    self.Ppu[i, j] = (3.*(i-j)**2 + 3.*(i-j) + 1.)*T**3/6.
                    self.Pvu[i, j] = (2.*(i-j) + 1.)*T**2/2.
                    self.Pau[i, j] = T

    def _initialize_cop_matrices(self):
        # """
        # Initialize center of pressure matrices, which are dependent on current
        # height of center of mass (self.h_com).
        # """
        # # renaming for convenience
        # T_step = self.T_step
        # T = self.T
        # N = self.N
        # nf = self.nf
        # h_com = self.h_com
        # g = self.g

        # for i in range(N):
        #     j = i+1
        #     self.Pzs[i, :] = (1.,   j*T, (j**2*T**2)/2. - h_com/g)

        #     for j in range(N):
        #         if j <= i:
        #             self.Pzu[i, j] = (3.*(i-j)**2 + 3.*(i-j) + 1.)*T**3/6. - T*h_com/g

        #NEW
        self.Pzs = self.Pps - self.omega**2 * self.Pas
        self.Pzu = self.Ppu - self.omega**2 * self.Pau

    def _initialize_cp_matrices(self):
        """
        Initialize capture point matrices, which are dependent on current
        height of center of mass (self.h_com).
        """
        self.Pxis = self.Pps + 1./self.omega * self.Pvs
        self.Pxiu = self.Ppu + 1./self.omega * self.Pvu

    def _initialize_selection_matrix(self):
        """ Initialize selection vector and matrix. """
        # renaming for convenience
        T_step = self.T_step
        T = self.T
        N = self.N
        nf = self.nf

        # initialize foot decision vector and matrix
        nstep = int(self.T_step/T) # time span of single support phase
        self.v_kp1[:nstep] = 1 # definitions of initial support leg

        for j in range (nf):
            a = min((j+1)*nstep, N)
            b = min((j+2)*nstep, N)
            self.V_kp1[a:b,j] = 1

        self._calculate_support_order()

    def _update_hulls(self):
        """ update shape polygon of convex hulls """
        # renaming for convenience
        fW = self.footWidth
        fH = self.footHeight
        fD = self.footDistance

        SMx = self.SecurityMarginX
        SMy = self.SecurityMarginY

        #  #<---footWidth---># --- ---
        #  #<>=SMx     SMx=<>#  |   |=SMy
        #  #  *-----------*  #  |  ---
        #  #  |           |  #  |=footHeight
        #  #  *-----------*  #  |  ---
        #  #                 #  |   |=SMy
        #  #-----------------# --- ---

        # left foot
        self.lfoot[0,:] =  0.5*fW,  0.5*fH
        self.lfoot[1,:] =  0.5*fW, -0.5*fH
        self.lfoot[2,:] = -0.5*fW, -0.5*fH
        self.lfoot[3,:] = -0.5*fW,  0.5*fH

        # right foot
        self.rfoot[0,:] =  0.5*fW, -0.5*fH
        self.rfoot[1,:] =  0.5*fW,  0.5*fH
        self.rfoot[2,:] = -0.5*fW,  0.5*fH
        self.rfoot[3,:] = -0.5*fW, -0.5*fH

        # left foot
        self.lfcophull[0,:] =  (0.5*fW - SMx),  (0.5*fH - SMy)
        self.lfcophull[1,:] =  (0.5*fW - SMx), -(0.5*fH - SMy)
        self.lfcophull[2,:] = -(0.5*fW - SMx), -(0.5*fH - SMy)
        self.lfcophull[3,:] = -(0.5*fW - SMx),  (0.5*fH - SMy)

        # right foot
        self.rfcophull[0,:] =  (0.5*fW - SMx), -(0.5*fH - SMy)
        self.rfcophull[1,:] =  (0.5*fW - SMx),  (0.5*fH - SMy)
        self.rfcophull[2,:] = -(0.5*fW - SMx),  (0.5*fH - SMy)
        self.rfcophull[3,:] = -(0.5*fW - SMx), -(0.5*fH - SMy)

        # double support
        # |<----d---->| d = 2*footHeight + footDistance
        # |-----------|
        # | *   *   * |
        # |-^-------^-|
        # left     right
        # foot     foot
        self.dscophull[0,:] =  (0.5*fW - SMx),  (0.5*(fD + fH) - SMy)
        self.dscophull[1,:] =  (0.5*fW - SMx), -(0.5*(fD + fH) - SMy)
        self.dscophull[2,:] = -(0.5*fW - SMx), -(0.5*(fD + fH) - SMy)
        self.dscophull[3,:] = -(0.5*fW - SMx),  (0.5*(fD + fH) - SMy)

    def _update_selection_matrices(self):
        """
        Update selection vector v_kp1 and selection matrix V_kp1.

        Therefore shift foot decision vector and matrix by one row up,
        i.e. the first entry in the selection vector and the first row in the
        selection matrix drops out and selection vector's dropped first value
        becomes the last entry in the decision matrix
        """
        nf = self.nf
        nstep = int(self.T_step/self.T)
        N = self.N

        # save first value for concatenation
        first_entry_v_kp1 = self.v_kp1[0].copy()

        self.v_kp1[:-1]   = self.v_kp1[1:]
        self.V_kp1[:-1,:] = self.V_kp1[1:,:]

        # clear last row
        self.V_kp1[-1,:] = 0

        # concatenate last entry
        self.V_kp1[-1, -1] = first_entry_v_kp1

        # when first column of selection matrix becomes zero,
        # then shift columns by one to the front
        if (self.v_kp1 == 0).all():
            self.v_kp1[:] = self.V_kp1[:,0]
            self.V_kp1[:,:-1] = self.V_kp1[:,1:]
            self.V_kp1[:,-1] = 0

            # update support foot
            self.f_k_x = self.F_k_x[0]
            self.f_k_y = self.F_k_y[0]
            self.f_k_q = self.F_k_q[0]

            # print("---",self.f_k_x,"---")

            self.currentSupport.x = self.f_k_x
            self.currentSupport.y = self.f_k_y
            self.currentSupport.q = self.f_k_q

            if self.currentSupport.foot == 'right':
                self.currentSupport.foot  = 'left'
            else:
                self.currentSupport.foot = 'right'

            # update support order with new foot state
            self._calculate_support_order()

            """
            # also update finite state machine
            self.fsm_state = self.fsm_states[0].copy()
            self.fsm_states[:-1] = self.fsm_states[1:]

            # TODO How to update last entry in FSM?
            # if any reference velocity is given and the CoM is moving then
            # the next step last optimized step should be moving
            if (self.dC_kp1_x_ref != 0.0).any() \
            or (self.dC_kp1_y_ref != 0.0).any() \
            or (self.dC_kp1_q_ref != 0.0).any():
                if (self.c_k_x[1:] != 0.0).any() \
                or (self.c_k_y[1:] != 0.0).any() \
                or (self.c_k_q[1:] != 0.0).any():
                    if self.supportDeque[-1].foot == 'right':
                        self.fsm_states[-1] = 'L/R'
                    else:
                        self.fsm_states[-1] = 'R/L'
            # else stay in double support
            else:
                self.fsm_states[-1] = 'D'
            """

    def _initialize_convex_hull_systems(self):
        # linear system corresponding to the convex hulls
        self.ComputeLinearSystem( self.rfposhull, "right", self.A0r, self.ubB0r)
        self.ComputeLinearSystem( self.lfposhull, "left", self.A0l, self.ubB0l)

        # linear system corresponding to the convex hulls
            # right foot
        self.ComputeLinearSystem( self.rfcophull,  "right", self.A0rf, self.ubB0rf)
            # left foot
        self.ComputeLinearSystem( self.lfcophull,  "left",  self.A0lf, self.ubB0lf)
            # double support
            # NOTE hull has to be shifted by half of feet distance in y direction
        self.ComputeLinearSystem(self.dscophull, "left",  self.A0dlf, self.ubB0dlf)
        self.ComputeLinearSystem(self.dscophull, "right", self.A0drf, self.ubB0drf)

    def ComputeLinearSystem(self, hull, foot, A0, B0 ):
        """
        automatically calculate linear constraints from polygon description
        """
        # get number of edged from the hull specification, e.g.
        # single and double support polygon, foot position hull
        nEdges = hull.shape[0]

        # get sign for hull from given foot
        if foot == "left" :
            sign = 1
        else :
            sign = -1

        # calculate linear constraints from hull
        # walk around polygon and calculate coordinate representation of
        # constraints
        for i in range(nEdges):
            # special case for first and last entry in hull
            if i == nEdges-1 :
                k = 0
            else :
                k = i + 1

            # rename point coordinates for convenience
            x1 = hull[i,0]
            y1 = hull[i,1]
            x2 = hull[k,0]
            y2 = hull[k,1]

            # calculate support vectors
            dx = y1 - y2
            dy = x2 - x1
            dc = dx*x1 + dy*y1

            # symmetrical constraints
            A0[i,0] = sign * dx
            A0[i,1] = sign * dy
            B0[i] =   sign * dc

    def _calculate_support_order(self):
        self.currentSupport.ds = deepcopy(self.supportDeque[0].ds)

        # find correct initial support foot
        if (self.currentSupport.foot == "left" ) :
            pair = "left"
            impair = "right"
        else :
            pair = "right"
            impair = "left"

        # define support feet for whole horizon
        for i in range(self.N):
            if self.v_kp1[i] == 1:
                self.supportDeque[i].foot = self.currentSupport.foot
                self.supportDeque[i].stepNumber = 0
            else:
                for j in range(self.nf):
                    if self.V_kp1[i][j] == 1 :
                        self.supportDeque[i].stepNumber = j+1
                        if (j % 2) == 1:
                            self.supportDeque[i].foot = pair
                        else :
                            self.supportDeque[i].foot = impair

        if numpy.sum(self.v_kp1)==8 :
            self.supportDeque[0].ds = 1
        else :
            self.supportDeque[0].ds = 0
        for i in range (1,self.N):
            self.supportDeque[i].ds = self.supportDeque[i].stepNumber - self.supportDeque[i-1].stepNumber

        timeLimit = self.supportDeque[0].timeLimit
        for i in range(self.N):
            if self.supportDeque[i].ds == 1 :
                timeLimit = timeLimit + self.T_step
            self.supportDeque[i].timeLimit = timeLimit

    def set_security_margin(self, margin_x = 0.04, margin_y=0.04):
        """
        define security margins for constraints CoP constraints

        .. NOTE: Will recreate constraint matrices

        Parameters
        ----------

        margin_x: 0 < float < footWidth
            security margin to narrow center of pressure constraints in x direction

        margin_y: 0 < float < footWidth
            security margin to narrow center of pressure constraints in x direction
        """
        self.SecurityMarginX = margin_x
        self.SecurityMarginY = margin_y

        # update cop hull
        self._update_hulls()

        # rebuild cop constraints
        self._initialize_convex_hull_systems()

        # rebuild constraints
        self.buildConstraints()

    # def set_velocity_reference(self,local_vel_ref):
    #     """
    #     Velocity reference update and computed from a local frame to a global frame using the
    #     current support foot frame

    #     Parameters
    #     ----------

    #     vel_ref: [dx,dy,dq]
    #         reference velocity in x, y and q
    #     """
    #     # get feet orientation states from feet jerks
    #     self.local_vel_ref = local_vel_ref
    #     self.  F_kp1_qL = self.Pps.dot(self.f_k_qL) + self.Ppu.dot(self.dddF_k_qL)
    #     self.  F_kp1_qR = self.Pps.dot(self.f_k_qR) + self.Ppu.dot(self.dddF_k_qR)

    #     flyingFoot = self.E_FR.dot(self.F_kp1_qR) + self.E_FL.dot(self.F_kp1_qL)
    #     supportFoot = self.E_FR_bar.dot(self.F_kp1_qR) + self.E_FL_bar.dot(self.F_kp1_qL)
    #     q = (flyingFoot[0] + supportFoot[0])*0.5
    #     self.dC_kp1_x_ref[...] = deepcopy( local_vel_ref[0] * cos(q) - local_vel_ref[1] * sin(q) )
    #     self.dC_kp1_y_ref[...] = deepcopy( local_vel_ref[0] * sin(q) + local_vel_ref[1] * cos(q) )
    #     self.dC_kp1_q_ref[...] = deepcopy( local_vel_ref[2] )

    #     print self.dC_kp1_x_ref[...]
    #     print self.dC_kp1_y_ref[...]
    #     print self.dC_kp1_q_ref[...]

    def set_trajectory_reference(self,traj_ref):

        self.traj_ref = traj_ref
        self.C_kp1_x_ref[...] = deepcopy(traj_ref[0])
        self.C_kp1_y_ref[...] = deepcopy(traj_ref[1])
        self.C_kp1_q_ref[...] = deepcopy(traj_ref[2])

        # print self.C_kp1_x_ref[...]
        # print self.C_kp1_y_ref[...]
        # print self.C_kp1_q_ref[...]

    def set_initial_values(self,
        com_x, com_y , com_z,
        foot_x, foot_y, foot_q, foot='left',
        com_q=(0,0,0)
    ):
        """
        initial value embedding for pattern generator, i.e. each iteration of
        pattern generator differs in:

        * initial com state, i.e. com_a = [c_a, dc_a, ddc_a], a in {x,y,q}
        * initials support foot setup, i.e.

        .. NOTE: Will recreate constraint matrices, support order and
                 transformation matrices.

        Parameters
        ----------

        com_x: [pos, vec, acc]
            current x position, velocity and acceleration of center of mass

        com_y: [pos, vec, acc]
            current y position, velocity and acceleration of center of mass

        com_z: float
            current z position of center of mass

        foot_x: float
            current x position of support foot

        foot_y: float
            current y position of support foot

        foot_q: float
            current orientation of support foot

        foot: str
            tells actual support foot state, i.e. 'right' or by default 'left'

        com_q: [ang, vec, acc]
            current orientation, angular velocity and acceleration of center of mass

        """
        # update CoM states
        self.c_k_x[...] = com_x
        self.c_k_y[...] = com_y

        if not self.h_com == com_z:
            self.h_com = com_z
            self._initialize_cop_matrices()

        # update support foot if necessary
        newSupport = BaseTypeSupportFoot(x=foot_x, y=foot_y, theta=foot_q, foot=foot)
        if self.currentSupport != newSupport \
        or self.f_k_x != foot_x \
        or self.f_k_y != foot_y \
        or self.f_k_q != foot_q :
            # take newSupport as current support
            self.currentSupport = newSupport

            # update support foot states
            self.f_k_x = foot_x
            self.f_k_y = foot_y
            self.f_k_q = foot_q

        # always recalculate support order
        self._calculate_support_order()

        # update current CoP values
        self.z_k_x = self.c_k_x[0] - self.h_com/self.g * self.c_k_x[2]
        self.z_k_y = self.c_k_y[0] - self.h_com/self.g * self.c_k_y[2]

        # update current CP values
        self.xi_k_x = self.c_k_x[0] + self.omega * self.c_k_x[1]
        self.xi_k_y = self.c_k_y[0] + self.omega * self.c_k_y[1]

        # rebuild all constraints
        self.buildConstraints()

    def update(self):
        """
        Update all interior matrices, vectors.
        Has to be used to prepare the QP after each iteration
        """

        # after solution simulate to get current states on horizon
        self.simulate()

        # update internal data
        self._update_data()

        # update internal time
        self.time += self.T

        # update matrices
        oldSupport = deepcopy(self.currentSupport)
        self._update_selection_matrices()
        if self.currentSupport != oldSupport \
        or self.f_k_x != self.currentSupport.x \
        or self.f_k_y != self.currentSupport.y \
        or self.f_k_q != self.currentSupport.q :
            raise NotImplementedError

        # provide copy of updated states as return value
        f_k_x = deepcopy(self.f_k_x)
        f_k_y = deepcopy(self.f_k_y)
        f_k_q = deepcopy(self.f_k_q)
        foot  = deepcopy(self.currentSupport.foot)

        # get data for initialization of next iteration
        c_k_x = numpy.zeros((3,), dtype=float)
        c_k_x[0] = self.  C_kp1_x[0]
        c_k_x[1] = self. dC_kp1_x[0]
        c_k_x[2] = self.ddC_kp1_x[0]

        c_k_y = numpy.zeros((3,), dtype=float)
        c_k_y[0] = self.  C_kp1_y[0]
        c_k_y[1] = self. dC_kp1_y[0]
        c_k_y[2] = self.ddC_kp1_y[0]

        c_k_q = numpy.zeros((3,), dtype=float)
        c_k_q[0] = self.  C_kp1_q[0]
        c_k_q[1] = self. dC_kp1_q[0]
        c_k_q[2] = self.ddC_kp1_q[0]

        # left foot
        f_k_qR = numpy.zeros((3,), dtype=float)
        f_k_qR[0] = self.  F_kp1_qR[0]
        f_k_qR[1] = self. dF_kp1_qR[0]
        f_k_qR[2] = self.ddF_kp1_qR[0]
        self.f_k_qR[...] = f_k_qR

        f_k_qL = numpy.zeros((3,), dtype=float)
        f_k_qL[0] = self.  F_kp1_qL[0]
        f_k_qL[1] = self. dF_kp1_qL[0]
        f_k_qL[2] = self.ddF_kp1_qL[0]
        self.f_k_qL[...] = f_k_qL

        if self.currentSupport.foot == "left" :
            self.f_k_q = self.f_k_qL[0]
        else :
            self.f_k_q = self.f_k_qR[0]
        self.currentSupport.q = self.f_k_q

        self.set_trajectory_reference(self.traj_ref)

        # # ZMP_k and CP_k
        # zmp_k_x = self.  Z_kp1_x[0]
        # zmp_k_y = self.  Z_kp1_y[0]
        # cp_k_x = self.  Xi_kp1_x[0]
        # cp_k_y = self.  Xi_kp1_y[0]

        # # ZMP_k+N and CP_k+N
        # zmp_N_x = self.  Z_kp1_x[-1]
        # zmp_N_y = self.  Z_kp1_y[-1]
        # cp_N_x = self.  Xi_kp1_x[-1]
        # cp_N_y = self.  Xi_kp1_y[-1]

        # # CoM_k+N
        # c_N_x = numpy.zeros((3,), dtype=float)
        # c_N_x[0] = self.  C_kp1_x[-1]
        # c_N_x[1] = self. dC_kp1_x[-1]
        # c_N_x[2] = self.ddC_kp1_x[-1]

        # c_N_y = numpy.zeros((3,), dtype=float)
        # c_N_y[0] = self.  C_kp1_y[-1]
        # c_N_y[1] = self. dC_kp1_y[-1]
        # c_N_y[2] = self.ddC_kp1_y[-1]

        # Future steps

        if(numpy.sum(self.v_kp1) != 8):
            F_k_x, F_k_y = self.F_k_x[0], self.F_k_y[0]
        else :
            F_k_x, F_k_y = self.F_k_x[1], self.F_k_y[1]
        # print("------")
        # print(self.v_kp1,numpy.sum(self.v_kp1))
        # print("actual",f_k_x,f_k_q,self.f_k_q,foot)
        # print(f_k_qR[0],f_k_qL[0])
        # print("future",self.F_k_x)
        # print(self.F_kp1_qR,self.F_kp1_qL)

        if self.currentSupport.foot == "left" :
            F_k_q = self.F_kp1_qR[numpy.sum(self.v_kp1)-1]
        else:
            F_k_q = self.F_kp1_qL[numpy.sum(self.v_kp1)-1]
        # print(F_k_q)

        return c_k_x, c_k_y, self.h_com, f_k_x, f_k_y, f_k_q, foot,\
            c_k_q, F_k_x, F_k_y, F_k_q

    def _update_data(self):
        self.data.update()

    def simulate(self):
        """
        integrates model for given initial CoM states, jerks and feet positions
        and orientations by applying the linear time stepping scheme
        """
        # get CoM states from jerks
        self.  C_kp1_x = self.Pps.dot(self.c_k_x) + self.Ppu.dot(self.dddC_k_x)
        self. dC_kp1_x = self.Pvs.dot(self.c_k_x) + self.Pvu.dot(self.dddC_k_x)
        self.ddC_kp1_x = self.Pas.dot(self.c_k_x) + self.Pau.dot(self.dddC_k_x)

        self.  C_kp1_y = self.Pps.dot(self.c_k_y) + self.Ppu.dot(self.dddC_k_y)
        self. dC_kp1_y = self.Pvs.dot(self.c_k_y) + self.Pvu.dot(self.dddC_k_y)
        self.ddC_kp1_y = self.Pas.dot(self.c_k_y) + self.Pau.dot(self.dddC_k_y)

        # get feet orientation states from feet jerks
        self.  F_kp1_qL = self.Pps.dot(self.f_k_qL) + self.Ppu.dot(self.dddF_k_qL)
        self. dF_kp1_qL = self.Pvs.dot(self.f_k_qL) + self.Pvu.dot(self.dddF_k_qL)
        self.ddF_kp1_qL = self.Pas.dot(self.f_k_qL) + self.Pau.dot(self.dddF_k_qL)

        self.  F_kp1_qR = self.Pps.dot(self.f_k_qR) + self.Ppu.dot(self.dddF_k_qR)
        self. dF_kp1_qR = self.Pvs.dot(self.f_k_qR) + self.Pvu.dot(self.dddF_k_qR)
        self.ddF_kp1_qR = self.Pas.dot(self.f_k_qR) + self.Pau.dot(self.dddF_k_qR)

        self.  C_kp1_q = 0.5 * ( self.  F_kp1_qL + self.  F_kp1_qR )
        self. dC_kp1_q = 0.5 * ( self. dF_kp1_qL + self. dF_kp1_qR )
        self.ddC_kp1_q = 0.5 * ( self.ddF_kp1_qL + self.ddF_kp1_qR )

        # get support foot orientation
        self.F_kp1_q = self.E_FR_bar.dot(self.F_kp1_qR) \
                     + self.E_FL_bar.dot(self.F_kp1_qL)

        for j in range(self.nf):
            for i in range(self.N):
                if self.V_kp1[i,j] != 0:
                    self.F_k_q[j] = self.F_kp1_q[i]
                    break
            else:
                self.F_k_q[j] = 0.0

        # get ZMP states from jerks
        self.Z_kp1_x = self.Pzs.dot(self.c_k_x) + self.Pzu.dot(self.dddC_k_x)
        self.Z_kp1_y = self.Pzs.dot(self.c_k_y) + self.Pzu.dot(self.dddC_k_y)

        # get CP states from jerks
        self.Xi_kp1_x = self.Pxis.dot(self.c_k_x) + self.Pxiu.dot(self.dddC_k_x)
        self.Xi_kp1_y = self.Pxis.dot(self.c_k_y) + self.Pxiu.dot(self.dddC_k_y)

    def buildConstraints(self):
        """
        builds constraint matrices for solver

        NOTE problems are assembled in the solver implementations
        """
        self.buildCoPconstraint()
        self.buildFootEqConstraint()
        self.buildFootIneqConstraint()
        self.buildFootRotationConstraints()
        self.buildRotIneqConstraint()

    def _update_cop_constraint_transformation(self):
        """ update foot constraint transformation matrices. """
        # every time instant in the pattern generator constraints
        # depend on the support order
        theta_vec = [self.f_k_q,self.F_k_q[0],self.F_k_q[1]]
        for i in range(self.N):
            theta = theta_vec[self.supportDeque[i].stepNumber]
            rotMat = numpy.array([[cos(theta), sin(theta)],[-sin(theta), cos(theta)]])
            if self.supportDeque[i].foot == "left" :
                A0 = self.A0lf.dot(rotMat)
                B0 = self.ubB0lf
                D0 = self.A0dlf.dot(rotMat)
                d0 = self.ubB0dlf
            else :
                A0 = self.A0rf.dot(rotMat)
                B0 = self.ubB0rf
                D0 = self.A0drf.dot(rotMat)
                d0 = self.ubB0drf

            # get support foot and check if it is double support
            for j in range(self.nf):
                if self.V_kp1[i,j] == 1:
                    if self.fsm_states[j] == 'D':
                        A0 = D0
                        B0 = d0
                else:
                    pass

            for k in range(self.nFootEdge):
                # get d_i+1^x(f^theta)
                self.D_kp1x[i*self.nFootEdge+k, i] = A0[k][0]
                # get d_i+1^y(f^theta)
                self.D_kp1y[i*self.nFootEdge+k, i] = A0[k][1]
                # get right hand side of equation
                self.b_kp1 [i*self.nFootEdge+k]    = B0[k]

    def buildCoPconstraint(self):
        """
        build the constraint enforcing the center of pressure to stay inside
        the support polygon given through the convex hull of the foot.
        """
        # change entries according to support order changes in D_kp1
        self._update_cop_constraint_transformation()

        #rename for convenience
        D_kp1 = self.D_kp1
        PzuV  = self.PzuV
        PzuVx = self.PzuVx
        PzuVy = self.PzuVy
        PzsC  = self.PzsC
        PzsCx = self.PzsCx
        PzsCy = self.PzsCy
        v_kp1fc   = self.v_kp1fc
        v_kp1fc_x = self.v_kp1fc_x
        v_kp1fc_y = self.v_kp1fc_y

        # build constraint transformation matrices
        # PzuV = ( PzuVx )
        #        ( PzuVy )

        # PzuVx = ( Pzu | -V_kp1 |   0 |      0 )
        PzuVx[:,      :self.N        ] =  self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
        PzuVx[:,self.N:self.N+self.nf] = -self.V_kp1

        # PzuVy = (   0 |      0 | Pzu | -V_kp1 )
        PzuVy[:,-self.N-self.nf:-self.nf] =  self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
        PzuVy[:,       -self.nf:       ] = -self.V_kp1

        # PzuV = ( PzsCx ) = ( Pzs * c_k_x)
        #        ( PzsCy )   ( Pzs * c_k_y)
        PzsCx[...] = self.Pzs.dot(self.c_k_x) #+ self.v_kp1.dot(self.f_k_x)
        PzsCy[...] = self.Pzs.dot(self.c_k_y) #+ self.v_kp1.dot(self.f_k_y)

        # v_kp1fc = ( v_kp1fc_x ) = ( v_kp1 * f_k_x)
        #           ( v_kp1fc_y )   ( v_kp1 * f_k_y)
        v_kp1fc_x[...] = self.v_kp1.dot(self.f_k_x)
        v_kp1fc_y[...] = self.v_kp1.dot(self.f_k_y)

        # build CoP linear constraints
        # NOTED_kp1x D_kp1 is member and D_kp1 = ( D_kp1x | D_kp1y )
        #      D_kp1x,y contains entries from support polygon

        self.Acop[...]   = D_kp1.dot(PzuV)
        self.ubBcop[...] = self.b_kp1 - D_kp1.dot(PzsC) + D_kp1.dot(v_kp1fc)

        PxiuV  = self.PxiuV
        PxiuVx = self.PxiuVx
        PxiuVy = self.PxiuVy
        PxisC  = self.PxisC
        PxisCx = self.PxisCx
        PxisCy = self.PxisCy

        # build constraint transformation matrices
        # PxiuV = ( PxiuVx )
        #        ( PxiuVy )

        # PxiuVx = ( Pxiu | -V_kp1 |   0 |      0 )
        PxiuVx[:,      :self.N        ] =  self.Pxiu # TODO this is constant in matrix and should go into the build up matrice part
        PxiuVx[:,self.N:self.N+self.nf] = -self.V_kp1

        # PzuVy = (   0 |      0 | Pzu | -V_kp1 )
        PxiuVy[:,-self.N-self.nf:-self.nf] =  self.Pxiu # TODO this is constant in matrix and should go into the build up matrice part
        PxiuVy[:,       -self.nf:       ] = -self.V_kp1

        # PxiuV = ( PxisCx ) = ( Pxis * c_k_x)
        #        ( PxisCy )   ( Pxis * c_k_y)
        PxisCx[...] = self.Pxis.dot(self.c_k_x) #+ self.v_kp1.dot(self.f_k_x)
        PxisCy[...] = self.Pxis.dot(self.c_k_y) #+ self.v_kp1.dot(self.f_k_y)

        # # build CP linear constraints

        self.Adcm[...]   = D_kp1[-self.nFootEdge:,:].dot(PxiuV)
        self.ubBdcm[...] = self.b_kp1[-self.nFootEdge:] - D_kp1[-self.nFootEdge:,:].dot(PxisC) + D_kp1[-self.nFootEdge:,:].dot(v_kp1fc)

        # print("cop",self.Acop[-self.nFootEdge:,:])
        # print("dcm",self.Adcm)

    def buildFootEqConstraint(self):
        """[-self.nFootEdge
        create constraints that freezes foot position optimization when swing
        foot comes close to foot step in preview window. Needed for proper
        interpolation of trajectory.
        """
        # B <= A x <= B
        # Support_Foot(k+1) = Support_Foot(k)
        itBeforeLanding = numpy.sum(self.v_kp1)
        itBeforeLandingThreshold = 3
        if ( itBeforeLanding < itBeforeLandingThreshold ) :
            self.eqAfoot[0,   self.N        ] = 1.
            self.eqAfoot[1, 2*self.N+self.nf] = 1.

            self.eqBfoot[0] = self.F_k_x[0]
            self.eqBfoot[1] = self.F_k_y[0]
        else:
            self.eqAfoot[0,   self.N        ] = 0.0
            self.eqAfoot[1, 2*self.N+self.nf] = 0.0

            self.eqBfoot[0] = 0.0
            self.eqBfoot[1] = 0.0

    def buildFootIneqConstraint(self):
        """
        build linear inequality constraints for the placement of the feet

        NOTE: needs actual self.supportFoot to work properly
        """
        # inequality constraint on both feet A u + B <= 0
        # A0 R(theta) [Fx_k+1 - Fx_k] <= ubB0
        #             [Fy_k+1 - Fy_k]

        matSelec = numpy.array([ [1, 0],[-1, 1] ])
        footSelec = numpy.array([ [self.f_k_x, 0],[self.f_k_y, 0] ])
        theta_vec = [self.f_k_q,self.F_k_q[0]]

        # rotation matrice from F_k+1 to F_k
        rotMat1 = numpy.array([[cos(theta_vec[0]), sin(theta_vec[0])],[-sin(theta_vec[0]), cos(theta_vec[0])]])
        rotMat2 = numpy.array([[cos(theta_vec[1]), sin(theta_vec[1])],[-sin(theta_vec[1]), cos(theta_vec[1])]])
        nf = self.nf
        nEdges = self.A0l.shape[0]
        N = self.N
        ncfoot = nf * nEdges

        if self.currentSupport.foot == "left":
            A_f1 = self.A0r.dot(rotMat1)
            A_f2 = self.A0l.dot(rotMat2)
            B_f1 = self.ubB0r
            B_f2 = self.ubB0l
        else :
            A_f1 = self.A0l.dot(rotMat1)
            A_f2 = self.A0r.dot(rotMat2)
            B_f1 = self.ubB0l
            B_f2 = self.ubB0r

        tmp1 = numpy.array( [A_f1[:,0],numpy.zeros((nEdges,),dtype=float)] )
        tmp2 = numpy.array( [numpy.zeros((nEdges,),dtype=float),A_f2[:,0]] )
        tmp3 = numpy.array( [A_f1[:,1],numpy.zeros((nEdges,),dtype=float)] )
        tmp4 = numpy.array( [numpy.zeros(nEdges,),A_f2[:,1]] )

        X_mat = numpy.concatenate( (tmp1.T,tmp2.T) , 0)
        A0x = X_mat.dot(matSelec)
        Y_mat = numpy.concatenate( (tmp3.T,tmp4.T) , 0)
        A0y = Y_mat.dot(matSelec)

        B0full = numpy.concatenate( (B_f1, B_f2) , 0 )
        B0 = B0full + X_mat.dot(footSelec[0,:]) + Y_mat.dot(footSelec[1,:])

        self.Afoot[...] = numpy.concatenate ((
            numpy.zeros((ncfoot,N),dtype=float), A0x,
            numpy.zeros((ncfoot,N),dtype=float), A0y
            ), 1
        )
        self.ubBfoot[...] = B0

    def buildFootRotationConstraints(self):
        """ constraints that freeze foot orientation for support leg """
        # 0 = E_F_bar * dF_k_q
        # <=>
        # ( 0 ) = ( E_FR_bar        0 ) * ( Pvs * f_k_qR + Pvu * dddF_k_qR )
        # ( 0 )   (        0 E_FL_bar ) * ( Pvs * f_k_qL + Pvu * dddF_k_qL )
        # <=>
        # 0 = E_FR_bar * Pvs * f_k_qR + E_FR_bar * Pvu * dddF_k_qR
        # 0 = E_FL_bar * Pvs * f_k_qL + E_FL_bar * Pvu * dddF_k_qL
        # <=>
        # E_FR_bar * Pvu * dddF_k_qR = - E_FR_bar * Pvs * f_k_qR
        # E_FL_bar * Pvu * dddF_k_qL = - E_FL_bar * Pvs * f_k_qL
        # <=>
        # A_fvel =
        # (E_FR_bar * Pvu              0 ) * dddF_k_qR
        # (             0 E_FL_bar * Pvu ) * dddF_k_qL
        # B_rot_eq =
        # ( - E_FR_bar * Pvs * f_k_qR)
        # ( - E_FL_bar * Pvs * f_k_qL)

        # rename for convenience
        A_fvel_eq_R = self.A_fvel_eq[:, :self.N]
        A_fvel_eq_L = self.A_fvel_eq[:, self.N:]
        B_fvel_eq   = self.B_fvel_eq

        # calculate proper selection matrices
        self._update_foot_selection_matrices()

        # build foot angular velocity constraints
        # A_fvel_eq =
        # (E_FR_bar * Pvu              0 ) * dddF_k_qR
        # (             0 E_FL_bar * Pvu ) * dddF_k_qL
        # B_fvel_eq =
        # ( - E_FR_bar * Pvs * f_k_qR)
        # ( - E_FL_bar * Pvs * f_k_qL)
        A_fvel_eq_R[...] = self.E_FR_bar.dot(self.Pvu)
        A_fvel_eq_L[...] = self.E_FL_bar.dot(self.Pvu)

        B_fvel_eq[...]   = -self.E_FR_bar.dot(self.Pvs).dot(self.f_k_qR) \
                           -self.E_FL_bar.dot(self.Pvs).dot(self.f_k_qL)

    def buildRotIneqConstraint(self):
        """ constraints on relative angular velocity """
        # rename for convenience
        A_fpos_ineq   = self.A_fpos_ineq
        A_fpos_ineq_R = A_fpos_ineq[:, :self.N]
        A_fpos_ineq_L = A_fpos_ineq[:, self.N:]
        ubB_fpos_ineq = self.ubB_fpos_ineq
        lbB_fpos_ineq = self.lbB_fpos_ineq

        A_fvel_ineq   = self.A_fvel_ineq
        A_fvel_ineq_R = A_fvel_ineq[:, :self.N]
        A_fvel_ineq_L = A_fvel_ineq[:, self.N:]
        ubB_fvel_ineq = self.ubB_fvel_ineq
        lbB_fvel_ineq = self.lbB_fvel_ineq

        # calculate proper selection matrices
        self._update_foot_selection_matrices()

        # build foot position constraints
        # || F_kp1_qR - F_kp1_qL ||_2^2 <= 0.09 ~ 5 degrees
        # <=>
        # -0.09 <= F_kp1_qR - F_kp1_qL <= 0.09
        # -0.09 - Pps(f_k_qR - f_k_qL) <= Ppu * ( 1 | -1 ) U_k <= 0.09 - Pps(f_k_qR - f_k_qL)
        A_fpos_ineq_R[...] =  numpy.eye(self.N)
        A_fpos_ineq_L[...] = -numpy.eye(self.N)
        A_fpos_ineq[...]   = self.Ppu.dot(A_fpos_ineq)

        ubB_fpos_ineq[...] =  0.09 - self.Pps.dot(self.f_k_qR - self.f_k_qL)
        lbB_fpos_ineq[...] = -0.09 - self.Pps.dot(self.f_k_qR - self.f_k_qL)

        # build foot velocity constraints
        A_fvel_ineq_R[...] =  numpy.eye(self.N)
        A_fvel_ineq_L[...] = -numpy.eye(self.N)
        A_fvel_ineq[...]   = self.Pvu.dot(A_fvel_ineq)

        ubB_fvel_ineq[...] =  0.22 - self.Pvs.dot(self.f_k_qR - self.f_k_qL)
        lbB_fvel_ineq[...] = -0.22   - self.Pvs.dot(self.f_k_qR - self.f_k_qL)

    def solve(self):
        """
        Solve problem on given prediction horizon with implemented solver.
        """
        err_str = 'Please derive from this class to implement your problem and solver'
        raise NotImplementedError(err_str)


text:
r"""
Hecke Character Basis

The basis of symmetric functions given by characters of the
Hecke algebra (of type `A`).

AUTHORS:

- Travis Scrimshaw (2017-08): Initial version
"""
#*****************************************************************************
#       Copyright (C) 2017 Travis Scrimshaw <tcscrims at gmail.com>
#
#  Distributed under the terms of the GNU General Public License (GPL)
#
#    This code is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
#    General Public License for more details.
#
#  The full text of the GPL is available at:
#
#                  http://www.gnu.org/licenses/
#*****************************************************************************

from __future__ import absolute_import

from sage.combinat.partition import _Partitions, Partitions
from sage.combinat.sf.multiplicative import SymmetricFunctionAlgebra_multiplicative
from sage.matrix.all import matrix
from sage.categories.morphism import SetMorphism
from sage.categories.homset import Hom
from sage.categories.modules_with_basis import ModulesWithBasis
from sage.rings.integer_ring import ZZ

class HeckeCharacter(SymmetricFunctionAlgebra_multiplicative):
    r"""
    Basis of the symmetric functions that gives the characters of the
    Hecke algebra in analogy to the Frobenius formula for the
    symmetric group.

    Consider the Hecke algebra `H_n(q)` with quadratic relations

    .. MATH::

        T_i^2 = (q - 1) T_i + q.

    Let `\mu` be a partition of `n` with length `\ell`. The character
    `\chi` of a `H_n(q)`-representation is completely determined by
    the elements `T_{\gamma_{\mu}}`, where

    .. MATH::

        \gamma_{\mu} = (\mu_1, \ldots, 1) (\mu_2 + \mu_1, \ldots, 1 + \mu_1)
        \cdots (n, \ldots, 1 + \sum_{i < \ell} \mu_i),

    (written in cycle notation). We define a basis of the symmetric
    functions by

    .. MATH::

        \bar{q}_{\mu} = \sum_{\lambda \vdash n}
        \chi^{\lambda}(T_{\gamma_{\mu}}) s_{\lambda}.

    INPUT:

    - ``sym`` -- the ring of symmetric functions
    - ``q`` -- (default: ``'q'``) the parameter `q`

    EXAMPLES::

        sage: q = ZZ['q'].fraction_field().gen()
        sage: Sym = SymmetricFunctions(q.parent())
        sage: qbar = Sym.hecke_character(q)
        sage: qbar[2] * qbar[3] * qbar[3,1]
        qbar[3, 3, 2, 1]

        sage: s = Sym.s()
        sage: s(qbar([2]))
        -s[1, 1] + q*s[2]
        sage: s(qbar([4]))
        -s[1, 1, 1, 1] + q*s[2, 1, 1] - q^2*s[3, 1] + q^3*s[4]
        sage: qbar(s[2])
        (1/(q+1))*qbar[1, 1] + (1/(q+1))*qbar[2]
        sage: qbar(s[1,1])
        (q/(q+1))*qbar[1, 1] - (1/(q+1))*qbar[2]

        sage: s(qbar[2,1])
        -s[1, 1, 1] + (q-1)*s[2, 1] + q*s[3]
        sage: qbar(s[2,1])
        (q/(q^2+q+1))*qbar[1, 1, 1] + ((q-1)/(q^2+q+1))*qbar[2, 1]
         - (1/(q^2+q+1))*qbar[3]

    We compute character tables for Hecke algebras, which correspond
    to the transition matrix from the `\bar{q}` basis to the Schur
    basis::

        sage: qbar.transition_matrix(s, 1)
        [1]
        sage: qbar.transition_matrix(s, 2)
        [ q -1]
        [ 1  1]
        sage: qbar.transition_matrix(s, 3)
        [  q^2    -q     1]
        [    q q - 1    -1]
        [    1     2     1]
        sage: qbar.transition_matrix(s, 4)
        [      q^3      -q^2         0         q        -1]
        [      q^2   q^2 - q        -q    -q + 1         1]
        [      q^2 q^2 - 2*q   q^2 + 1  -2*q + 1         1]
        [        q   2*q - 1     q - 1     q - 2        -1]
        [        1         3         2         3         1]

    We can do computations with a specialized `q` to a generic element
    of the base ring. We compute some examples with `q = 2`::

        sage: qbar = Sym.qbar(q=2)
        sage: s = Sym.schur()
        sage: qbar(s[2,1])
        2/7*qbar[1, 1, 1] + 1/7*qbar[2, 1] - 1/7*qbar[3]
        sage: s(qbar[2,1])
        -s[1, 1, 1] + s[2, 1] + 2*s[3]

    REFERENCES:

    - [Ram1991]_
    - [RR1997]_
    """
    def __init__(self, sym, q='q'):
        r"""
        Initialize ``self``.

        TESTS::

            sage: Sym = SymmetricFunctions(FractionField(ZZ['q']))
            sage: qbar = Sym.qbar()
            sage: TestSuite(qbar).run()

            sage: Sym = SymmetricFunctions(QQ)
            sage: qbar = Sym.qbar(q=2)
            sage: TestSuite(qbar).run()

        Check that the conversion `q \to p \to s` agrees with
        the definition of `q \to s` from [Ram1991]_::

            sage: Sym = SymmetricFunctions(FractionField(ZZ['q']))
            sage: qbar = Sym.qbar()
            sage: s = Sym.s()
            sage: q = qbar.q()
            sage: def to_schur(mu):
            ....:     if not mu:
            ....:        return s.one()
            ....:     mone = -qbar.base_ring().one()
            ....:     return s.prod(sum(mone**(r-m) * q**(m-1)
            ....:                       * s[Partition([m] + [1]*(r-m))]
            ....:                       for m in range(1, r+1))
            ....:                   for r in mu)
            sage: phi = qbar.module_morphism(to_schur, codomain=s)
            sage: all(phi(qbar[mu]) == s(qbar[mu]) for n in range(6)
            ....:     for mu in Partitions(n))
            True
        """
        self.q = sym.base_ring()(q)
        SymmetricFunctionAlgebra_multiplicative.__init__(self, sym,
            basis_name="Hecke character with q={}".format(self.q),
            prefix="qbar")
        self._p = sym.power()

        # temporary until Hom(GradedHopfAlgebrasWithBasis work better)
        category = ModulesWithBasis(self._sym.base_ring())
        self   .register_coercion(self._p._module_morphism(self._p_to_qbar_on_basis,
                                                           codomain=self))
        self._p.register_coercion(self._module_morphism(self._qbar_to_p_on_basis,
                                                        codomain=self._p))

    def _p_to_qbar_on_generator(self, n):
        r"""
        Convert `p_n` to ``self``

        INPUT:

        - ``n`` -- a non-negative integer

        EXAMPLES::

            sage: qbar = SymmetricFunctions(QQ['q'].fraction_field()).qbar('q')
            sage: qbar._p_to_qbar_on_generator(3)
            ((q^2-2*q+1)/(q^2+q+1))*qbar[1, 1, 1]
             + ((-3*q+3)/(q^2+q+1))*qbar[2, 1]
             + (3/(q^2+q+1))*qbar[3]
            sage: qbar = SymmetricFunctions(QQ).qbar(-1)
            sage: qbar._p_to_qbar_on_generator(3)
            2*qbar[2, 1] + 3*qbar[3]
        """
        if n == 1:
            return self([1])
        q = self.q
        if q**n == self.base_ring().one():
            raise ValueError("the parameter q=%s must not be a %s root of unity"%(q,n))
        out = n * self([n]) - sum((q**i-1) * self._p_to_qbar_on_generator(i)
                                  * self([n-i]) for i in range(1,n) if q**i != 1)
        return out*(q-1) / (q**n-1)

    def _p_to_qbar_on_basis(self, mu):
        r"""
        Convert the power sum basis element indexed by ``mu`` to ``self``.

        INPUT:

        - ``mu`` -- a partition or a list of non-negative integers

        EXAMPLES::

            sage: qbar = SymmetricFunctions(QQ['q'].fraction_field()).qbar('q')
            sage: qbar._p_to_qbar_on_basis([3,1])
            ((q^2-2*q+1)/(q^2+q+1))*qbar[1, 1, 1, 1]
             + ((-3*q+3)/(q^2+q+1))*qbar[2, 1, 1]
             + (3/(q^2+q+1))*qbar[3, 1]
            sage: qbar = SymmetricFunctions(QQ).qbar(2)
            sage: qbar._p_to_qbar_on_basis([3,1])
            1/7*qbar[1, 1, 1, 1] - 3/7*qbar[2, 1, 1] + 3/7*qbar[3, 1]
        """
        return self.prod(self._p_to_qbar_on_generator(p) for p in mu)

    def _qbar_to_p_on_generator(self, n):
        r"""
        Convert a generator of the basis indexed by ``n`` to the
        power sum basis.

        INPUT:

        - ``n`` -- a non-negative integer

        EXAMPLES::

            sage: qbar = SymmetricFunctions(QQ['q'].fraction_field()).qbar('q')
            sage: qbar._qbar_to_p_on_generator(3)
            (1/6*q^2-1/3*q+1/6)*p[1, 1, 1]
             + (1/2*q^2-1/2)*p[2, 1]
             + (1/3*q^2+1/3*q+1/3)*p[3]
            sage: qbar = SymmetricFunctions(QQ).qbar(-1)
            sage: qbar._qbar_to_p_on_generator(3)
            2/3*p[1, 1, 1] + 1/3*p[3]
        """
        if n == 1:
            return self._p([1])
        q = self.q
        BR = self.base_ring()
        return q**(n-1) * self._p.sum(sum(q**(-i) for i in range(mu[0]))
                                      * BR.prod(1 - q**(-p) for p in mu[1:])
                                      * self._p(mu) / mu.centralizer_size()
                                      for mu in Partitions(n)
                                      if not any(q**p == 1 for p in mu[1:]))

    def _qbar_to_p_on_basis(self, mu):
        r"""
        Convert a basis element indexed by the partition ``mu``
        to the power basis.

        INPUT:

        - ``mu`` -- a partition or a list of non-negative integers

        EXAMPLES::

            sage: qbar = SymmetricFunctions(QQ['q'].fraction_field()).qbar('q')
            sage: qbar._qbar_to_p_on_basis([3,1])
            (1/6*q^2-1/3*q+1/6)*p[1, 1, 1, 1]
             + (1/2*q^2-1/2)*p[2, 1, 1]
             + (1/3*q^2+1/3*q+1/3)*p[3, 1]
            sage: qbar = SymmetricFunctions(QQ).qbar(-1)
            sage: qbar._qbar_to_p_on_basis([3,1])
            2/3*p[1, 1, 1, 1] + 1/3*p[3, 1]
        """
        return self._p.prod(self._qbar_to_p_on_generator(p) for p in mu)

    def coproduct_on_generators(self, r):
        r"""
        Return the coproduct on the generator `\bar{q}_r` of ``self``.

        Define the coproduct on `\bar{q}_r` by

        .. MATH::

            \Delta(\bar{q}_r) = \bar{q}_0 \otimes \bar{q}_r
            + (q - 1) \sum_{j=1}^{r-1} \bar{q}_j \otimes \bar{q}_{r-j}
            + \bar{q}_r \otimes \bar{q}_0.

        EXAMPLES::

            sage: q = ZZ['q'].fraction_field().gen()
            sage: Sym = SymmetricFunctions(q.parent())
            sage: qbar = Sym.hecke_character()
            sage: s = Sym.s()
            sage: qbar[2].coproduct()
            qbar[] # qbar[2] + (q-1)*qbar[1] # qbar[1] + qbar[2] # qbar[]
        """
        def P(i): return _Partitions([i]) if i else _Partitions([])
        T = self.tensor_square()
        one = self.base_ring().one()
        q = self.q
        return T.sum_of_terms(((P(j), P(r-j)), one if j in [0,r] else q-one)
                              for j in range(r+1))

ext: py

max_stars_repo_name: bopopescu/sage

max_issues_repo_name: bopopescu/sage

max_forks_repo_name: bopopescu/sage

reason: from sage


text:
#!/usr/bin/env python
# by TR

from numpy.random import random, seed
from sito import util
import sito.util.fftw3_be
import numpy as np
import os
import unittest
from scipy import fftpack
from sito.stream import read
from scipy.fftpack import fft, ifft
from obspy.signal.util import nextpow2
from sito.util.seispy import EarthModel1D, Iasp91

class TestCase(unittest.TestCase):
    """
    Test suite for tspy.util.
    """

    def setUp(self):
        # set specific seed value such that random numbers are reproducible
        seed(42)
        self.Z = random(412) - 0.5
        self.N = random(412) - 0.5
        self.E = random(412) - 0.5
        self.stream = read(os.path.join(os.path.dirname(__file__), 'data', 'PKD_1997_246.mseed'))

### test_main
    def test_util_window(self):
        N = 101
#        from pylab import plot, legend, show
#        plot(util.main.getWindow('tukey', N, 0.8), label='0.8')
#        plot(util.main.getWindow('tukey', N, 0.1), label='0.1')
#        plot(util.cosTaper(N, 0.1), label='cosTaper0.1')
#        plot(util.cosTaper(N, 0.8), label='cosTaper0.8')
#        plot(util.cosTaper(N, 1), label='cosTaper1.0')
#        plot(util.main.getWindow('tukey', N, 1), label='1')
#        plot(util.main.getWindow('hanning', N), label='hanning')
#        legend()
#        show()
        np.testing.assert_array_almost_equal(util.main.getWindow('tukey', N, 0.), util.get_window('boxcar', N))
        np.testing.assert_array_almost_equal(util.main.getWindow('tukey', N, 1.), util.get_window('hanning', N))
        self.assertTrue(np.sum((util.main.getWindow('tukey', N, 0.1) -
                                util.cosTaper(N, 0.1)) ** 2) < 0.2) #cosTaper from Obspy is no normal cosine -> high difference

    def test_util_filterResponse(self):
        sr = self.stream[0].stats.sampling_rate

        # test filterResponse vs filter2
        st = self.stream.copy()
        data = st[0].data.copy()
        N = len(data)
        nfft = nextpow2(len(data))
        values = util.main.filterResp(1, 5, corners=2, sr=20, N=nfft, whole=True)[1]
        st.filter2(1, 5, corners=2)
        data2 = ifft(fft(data, nfft) * values, nfft)
        np.testing.assert_array_almost_equal(st[0].data, data2[:N])

        # test stream interface
        st = self.stream.copy()
        st.fft()
        st.filter2(1, 5, corners=2)
        st.ifft()
        np.testing.assert_array_almost_equal(st[0].data, data2[:N])

        # filtering with filterResponse and zerophase=Trueproduces a peak at
        # the end of data. With nfft=N there is no peak anymore, but still the values are not the same
        st = self.stream.copy()
        freqs, values = util.main.filterResp(1, 5, sr=20, N=nfft, corners=2, whole=True, zerophase=True)
        st.filter2(1, 5, corners=2, zerophase=True)
        data2 = ifft(fft(data, nfft) * values, nfft)
        np.testing.assert_array_almost_equal(st[0].data[:-10 * sr], data2[:N - 10 * sr])

        return

        from numpy.fft.helper import ifftshift, fftfreq
        import matplotlib.pyplot as plt

        real_freqs = (ifftshift(freqs) - np.pi) * 0.5 * sr / np.pi
        freqs2 = fftfreq(nfft, 1 / 20.)
        print real_freqs
        print freqs2

        plt.subplot(411)
        plt.plot(real_freqs, np.abs(values))
        ax = plt.subplot(412)
        plt.plot(data, label='data')
        plt.legend()
        plt.subplot(413, sharex=ax)
        plt.plot(st[0].data, alpha=0.5, label='stream.filter2')
        plt.plot(data2[:N], alpha=0.5, label='filterResponse')
        plt.legend()
        plt.show()

### util.helper
    def test_util_pop(self):
        self.assertEqual(util.pop('echo echo test_pop'), 0)

    def test_util_vectorize_args(self):
        """ Tests the decorator with example pspier function. """
        @util.vectorize_args((0, 1, 2, 3, 4))
        def pspier(depth, slat, slon, slowness, azi, model=util.iasp91):
            tp, ts, xp, xs = model.trace(slowness) #@UnusedVariable
            xs = xs[model.layer(depth)]
            return tuple([xs]) + util.dist2gps(xs, azi, slat, slon)
        # test with scalars
        np.testing.assert_array_almost_equal(np.array(pspier(100, -10, -100, 6.4, 0)), np.array([25.037491884001234, -9.7748325744502864, -100.0]))

        # test with 1D array
        slat = np.array([-10, 0])
        slon = np.array([-160, 10])
        slowness = np.array([6.4, 3.2])
        azi = np.array([13., 170.])
        depth = np.array([200, 200])
        rpier, plat, plon = pspier(depth, slat, slon, slowness, azi)
        #print pspier(200, -10, -160, 6.4, 13)
        #print pspier(200, 0, 10, 3.2, 170)
        #print rpier, plat, plon
        np.testing.assert_array_almost_equal(rpier, np.array([ 53.24810772, 25.94769729]))
        np.testing.assert_array_almost_equal(plat, np.array([-9.53338447, -0.22980792]))
        np.testing.assert_array_almost_equal(plon, np.array([-159.89077001, 10.04052156]))

        # test with 2D array
        slat = np.array([[-10, 0, 75], [30, 40, 50]])
        slon = np.array([[-100, 0, 150], [300, 40, 50]])
        azi = np.array([[0, 10, 20], [30, 40, 50]])
        rpier3, plat3, plon3 = pspier(100, slat, slon, 6.4, azi)
        #print rpier3
        #print plat3
        #print plon3
        #print util.pspier(100, -10, -100, 6.4, 0)
        #print util.pspier(100, 0, 0, 6.4, 10)
        #print util.pspier(100, 75, 150, 6.4, 20)
        #print util.pspier(100, 30, 300, 6.4, 30)
        #print util.pspier(100, 40, 40, 6.4, 40)
        #print util.pspier(100, 50, 50, 6.4, 50)
        np.testing.assert_array_almost_equal(rpier3, np.array([[25.037491884001234, 25.037491884001234, 25.037491884001234], [25.037491884001234, 25.037491884001234, 25.037491884001234]]))
        np.testing.assert_array_almost_equal(plat3, np.array([[-9.7748325744502864, 0.22174660919583036, 75.21139224817631], [30.194936597642542, 40.172334289001896, 50.144424254999045]]))
        np.testing.assert_array_almost_equal(plon3, np.array([[-100.0, 0.039100108343866131, 150.30170770482789], [-59.869743130561837, 40.189416675883059, 50.26915378427686]]))

    def test_util_add_doc(self):
        def bla2():
            """2"""
            pass
        @util.add_doc(bla2)
        def bla12():
            """1"""
            pass
        @util.add_doc('4', bla12)
        def bla34():
            """3"""
            pass
        self.assertEqual(bla12.__doc__, '12')
        self.assertEqual(bla34.__doc__, '3412')

### util.seispy
    def test_util_region(self):
        reg = util.feregion(10, 10)
        self.assertEqual(reg, 'Nigeria')

    def  test_util_ttt(self):
        ttt = util.ttt(50, 50, surface=True)
        self.assertEqual(abs(ttt[0].time - 529.101318359375) < 1e-5, True)

    def  test_util_correct_ttt(self):
        self.assertEqual(util.correct_ttt(), 0.)
        husen99 = EarthModel1D(os.path.join(os.path.dirname(__file__), 'data', 'husen99.dat'))
        cor = util.correct_ttt(newmodel=husen99, oldmodel=util.iasp91)
        cor2 = util.correct_ttt('S', 3.2, newmodel=husen99, oldmodel=util.iasp91)
        self.assertAlmostEqual(cor, -1.36098024643)
        self.assertAlmostEqual(cor2, -3.1553046897)

    def test_util_gps2dist(self):
        """ Tests gps2dist against seispy. """
        try:
            import seis.geo
            from obspy.core.util import gps2DistAzimuth
        except ImportError:
            pass
        else:
            dist_deg = sito.util.gps2DistDegree(10, 20, 30, 40)
            dist_km, az1, az2 = gps2DistAzimuth(10, 20, 30, 40) #@UnusedVariable
            dist_deg_seis, az1_seis, az2_seis = seis.geo.delazi(10, 20, 30, 40)
            #print util.gps2dist(10, 20, 30, 40)
            #print seis.geo.delazi(10, 20, 30, 40)
            self.assertEqual(abs(dist_deg - dist_deg_seis) < 1e-5, True)
            self.assertEqual(abs(az1 - az1_seis) < 0.2, True) # one of both routines is not working exactly
            self.assertEqual(abs(az2 - az2_seis) < 0.2, True)
            #obspy.signal.rotate.gps2DistAzimuth | util.gps2dist vs seis.geo.delazi

    def test_util_pspier(self):
        """ Tests pspier and pspier2. """
        slat = np.array([-10, 0])
        slon = np.array([-160, 10])
        slowness = np.array([6.4, 3.2])
        azi = np.array([13., 170.])
        depth = np.array([200, 200])
        rpier, plat, plon = util.pspier(depth, slat, slon, slowness, azi)
        rpier2, plat2, plon2 = util.pspier2(200, slat, slon, slowness, azi)

        np.testing.assert_array_almost_equal(rpier2, np.array([ 53.25370502, 25.95027721]))
        np.testing.assert_array_almost_equal(plat2, np.array([-9.53337395, -0.22982045]))
        np.testing.assert_array_almost_equal(plon2, np.array([-159.890609, 10.04052357]))
        np.testing.assert_array_almost_equal(rpier, rpier2, decimal=2)
        np.testing.assert_array_almost_equal(plat, plat2, decimal=2)
        np.testing.assert_array_almost_equal(plon, plon2, decimal=2)

    def test_util_trace3(self):
        from obspy.taup.taup import getTravelTimes
        iasp91 = Iasp91(5., 5000)
        slat = np.array([-10, 0])
        slon = np.array([-160, 10])
        slowness = np.array([6.4, 3.2])
        azi = np.array([13., 170.])
        depth = np.array([200, 200])
        rpier, plat, plon = iasp91.pspier(depth, slat, slon, slowness, azi, phase='S')
        rpier2, plat2, plon2 = util.pspier(depth, slat, slon, slowness, azi)
        np.testing.assert_array_almost_equal(rpier, rpier2, 0)
        np.testing.assert_array_almost_equal(plat, plat2, 2)
        np.testing.assert_array_almost_equal(plon, plon2, 2)

        tp, rp, phip = iasp91.trace3(6.4, phase='P', till_turn=False)
        taup = getTravelTimes(phip[-1] * 180 / np.pi, 0)
        self.assertTrue(abs(tp[-1] - taup[0]['time']) < 2)

    def test_util_time2depth(self):
        test = np.array((util.time2depth(20, 'P'), util.time2depth(20, 'Ps'), util.time2depth(20, 'Pppp')))
        np.testing.assert_array_almost_equal(test, np.array((168.7292053, 183.00481858, 76.94140119)))
        np.testing.assert_array_almost_equal(util.time2depth([20, 10], 'P'), np.array((168.7292053, 76.94140119)))
        #print 'depth:'
        #print test
        #print util.time2depth(20, 'P*')
        #print util.time2depth(20, 'S*')

    def test_util_depth2time(self):
        test = np.array((util.depth2time(100, 'S'), util.depth2time(100, 'Sppp'), util.depth2time(100, 'Ssss')))
        np.testing.assert_array_almost_equal(test, np.array((23.75012441, 13.84042822, 47.50024882)))
        #print 'time:'
        #print test
        #print util.depth2time(183, 'P*')
        #print util.depth2time(183, 'S*')

    def test_util_interpolate(self):
        t = np.array([1., 2., 3., 5.])
        z = np.array([0, 1., 3., 8.])
        zneu = util.interpolate(2.25, t, z)
        self.assertEqual(zneu, 1.5)
        zneu = util.interpolate(np.array([-0.2, 2., 4.5, 5, 12.]), t, z)
        np.testing.assert_array_almost_equal(zneu, [0, 1, 6.75, 8, 8.])

    def test_util_model(self):
        model = Iasp91()
        self.assertEqual(model.vp[0] / model.vs[0], 5.8 / 3.36)
        #print(model)

    def test_util_mocorr(self):
        model = Iasp91()
        phc = util.phase_dict['Ps']
        #self.Z = np.sin(np.linspace(0, 10*np.pi, 500))
        #import psmout
        #y=psmout.psmout([self.Z], [8.5], 0., 5.0, 0.01, 1) # much slower becasue of io of modell
        data = util.mocorr(self.Z, model.z, model.vp, model.vs, 8.5, 6.4, 100, phc)
        #print(sum(self.Z), sum(model.z), phc, sum(model.vp), sum(model.vs))
        #print(sum(data[:len(data)*4//5]), len(data))
        self.assertEqual(abs(sum(data[:len(data) * 4 // 5]) + 2.33691887) < 1e-5, True) # very, very strange anomalies

### util.ffwt3
    def test_util_ffwt3(self):
        data = np.arange(10)
        nfft = 16
        fft1 = fftpack.fft(data, nfft)
        fft2 = sito.util.fftw3_be.fft(data, nfft).copy()
        ifft1 = fftpack.ifft(fft1)
        ifft2 = sito.util.fftw3_be.ifft(fft2)
#        from pylab import plot, show, subplot, legend
#        subplot(211)
#        plot(data, label='data')
#        plot(ifft1, label='scipy.fftpack')
#        plot(ifft2, label='fftw3_bg')
#        legend()
#        subplot(212)
#        plot(fft1, label='scipy.fftpack')
#        plot(fft2, label='fftw3_bg')
#        legend()
#        show()
        np.testing.assert_array_almost_equal(fft1, fft2)
        np.testing.assert_array_almost_equal(ifft1, ifft2)

# untested util.main: getWindow
# untested util.helper: parameters, myhash, setRootLogger, runBash
# untested util.imaging: getTimeIntervall, getDataWindow
# tests could be enhanced: nearly all

def suite():
    return unittest.makeSuite(TestCase, 'test')

if __name__ == '__main__':
    unittest.main(defaultTest='suite')

max_stars_repo_name: trichter/sito

max_issues_repo_name: trichter/sito

max_forks_repo_name: trichter/sito

reason: import numpy,from numpy,from scipy


text:
import os
import numpy as np
import csv
from sklearn.neural_network import MLPClassifier
import pyxhook.pyxhook as pyxhook
import time
import csv
import sys
from sklearn import svm
from termios import tcflush, TCIOFLUSH

def load_trainingData():
	"""
	This reads file `datasetPath` and returns the training data in
	an ndarray of shape noOfTrainingVectors*noOfFeatures and target ndarray
	of shape (noOfTrainingVectors*noOfTotalClasses)*1.
	"""
	global datasetPath
	dataset = np.empty([0,noOfFeatures])
	target = np.empty(0)
	file = open(datasetPath)
	reader = csv.reader(file)
	reader.next()
	for i in range(noOfTotalClasses):
		for j in range(noOfTrainingVectors):
			tempData = reader.next()					# Read one vector
			currentSubject = tempData[0]			# Save subject's name
			for k in range(3):								# Discard first 3 values
				del tempData[0]
			tempData = map(float, tempData)
			tempData = np.array(tempData, ndmin=2)
			dataset = np.append(dataset, tempData, axis=0)
			target  = np.append(target, [currentSubject], axis=0)
		for j in range(noOfTestingVectors):	# Discard testing vectors for now
			tempData = reader.next()					# Discard one vector
		# Discard the rest of the unused vectors now
		for j in range(noOfTotalVectors - noOfTrainingVectors - noOfTestingVectors):
			tempData = reader.next()						# Discard one vector
	return dataset,target

# Total number of classes.
noOfTotalClasses = 5
# Total number of vectors available for one class.
noOfTotalVectors = 250
# For training purposes for one class use first `noOfTrainingVectors` vectors.
noOfTrainingVectors = 150
# For testing purposes for one class use first `noOfTestingVectors` vectors.
# noOfTestingVectors = noOfTotalVectors - noOfTrainingVectors
noOfTestingVectors = 0
# Each vector contains `noOfFeatures` features.
noOfFeatures = 31
# This contains the path for the dataset.
fileName = "IIITBh-Small.csv"
datasetPath = os.path.normpath(os.getcwd() + os.sep + os.pardir)
datasetPath = datasetPath + os.sep + fileName

noOfInputNodes = noOfFeatures
# The number of hidden nodes.
noOfHiddenNodes = 25
# The number of output nodes is equal to the number of classes
noOfOutputNodes = noOfTotalClasses

# X: We take all the features. Or we can take only some features here by slicing.
# y: This contains the actual classes for each training vector i.e the target.
X,y = load_trainingData()

# Neural network classifiers
adam_clf = MLPClassifier(hidden_layer_sizes = (noOfInputNodes, noOfHiddenNodes, noOfOutputNodes),
				activation = "tanh", solver = "adam", max_iter = 1000, random_state=0, alpha=0.001)
sgd_clf = MLPClassifier(hidden_layer_sizes = (noOfInputNodes, noOfHiddenNodes, noOfOutputNodes),
				activation = "tanh", solver = "sgd", max_iter = 1800, learning_rate = "adaptive",
				learning_rate_init=0.01, random_state=0, alpha=0.01)
# SVM Classifiers
# C is SVM regularization parameter
rbf_svc_clf = svm.SVC(kernel='rbf', gamma=0.05, C=401)
lin_svc_clf = svm.SVC(kernel='linear', C=801, gamma=0.01)

# Training of neural networks classifiers
sgd_clf.fit(X,y)
adam_clf.fit(X,y)
# Training of SVM classifiers
lin_svc = lin_svc_clf.fit(X, y)
rbf_svc = rbf_svc_clf.fit(X, y)

print "Model Trained"
print "\nThe password to type is: .xat17padn"
print "\nEnter the password and we will predict the rest:"

############################################################

class KeyStroke:
	def __init__(self, key, mname, time):
		self.key = key
		self.mname = mname
		self.time = time

class StrokesLine:
	def __init__(self, UpArray, DownArray):
		self.UpArray = UpArray
		self.DownArray = DownArray
		self.line = ""

	def returnVector(self):
		line = []
		for i in range(len(self.UpArray) - 1):
			holdTime = float(self.UpArray[i].time) - float(self.DownArray[i].time)
			downDownTime = float(self.DownArray[i+1].time) - float(self.DownArray[i].time)
			upDownTime = float(self.DownArray[i+1].time) - float(self.UpArray[i].time)
			line.append(str(holdTime))
			line.append(str(downDownTime))
			line.append(str(upDownTime))
		line.append(str(self.UpArray[i+1].time - self.DownArray[i+1].time))

		return line

def checkPass(array):
	"""
	This function is called at each iteration to check if the password entered is correct
	"""
	global authPass
	typedPass = ""
	for i in range(len(array)):
		typedPass += array[i].key
	# print("Typed Password: "+typedPass+"\n      Password: "+authPass)
	if authPass == typedPass:
		return True
	else:
		return False

def kbevent(event):
	"""
	This function is called every time a key is presssed
	"""
	global running
	global UpArray, DownArray
	# print key info
	# tm = repr(time.time())
	tm = time.time()

	keyEvent = KeyStroke(event.Key, event.MessageName, tm)
	if keyEvent.mname == "key down":
		DownArray.append(keyEvent)
	else:
		UpArray.append(keyEvent)

	# If the ascii value matches carriage return, terminate the while loop
	if event.Ascii == 13:
		if keyEvent.mname == "key up":
			running = False

for i in range(10):
	UpArray = []
	DownArray = []
	# Create hookmanager
	hookman = pyxhook.HookManager()
	# Define our callback to fire when a key is pressed down
	hookman.KeyDown = kbevent
	hookman.KeyUp = kbevent
	# Hook the keyboard
	hookman.HookKeyboard()
	# Start our listener

	hookman.start()
	# Create a loop to keep the application running
	running = True
	while running:
			time.sleep(0.00001)

	# Close the listener when we are done
	hookman.cancel()
	tcflush(sys.stdin, TCIOFLUSH)

	writer = StrokesLine(UpArray, DownArray)
	testing_vector = writer.returnVector()
	testing_vector = map(float, testing_vector)
	testing_vector = [[testing_vector[x] for x in range(len(testing_vector))]]

	print "\nOkay, testing against our classifiers..."

	classifiers = ['NN(SGD)', 'NN(Adam)', 'SVC(rbf)', 'SVC(linear)']
	for i, clf in enumerate((sgd_clf, adam_clf, rbf_svc, lin_svc)):
		# Pass testing data to the classifier
		Z = clf.predict(testing_vector)
		print "\n", classifiers[i],"predicts:", Z[0]

max_stars_repo_name: aroonav/furry-strokes

max_issues_repo_name: aroonav/furry-strokes

max_forks_repo_name: aroonav/furry-strokes

reason: import numpy


text:
"""Generating random numbers."""
import numpy as np

def test_run():
    # Generate an array full of random numbers, uniformly sampled from [0.0, 1.0)
    print(np.random.random((5, 4)))

    # Use function arguments instead of tuple to generate array of random numbers
    print(np.random.rand(5, 4))

    # Sample numbers from a normal distribution: mean = 0, SD = 1
    print(np.random.normal(size=(2, 3)))

    # Sample numbers from a normal distribution: mean = 50, SD = 10
    print(np.random.normal(50, 10, size=(2, 3)))

    # Random integers
    print(np.random.randint(10))  # a single integer in [0, 10)
    print(np.random.randint(0, 10))  # same as above, specifying [low, high) explicit
    print(np.random.randint(0, 10, size=5))  # 5 random integers as a 1D array
    print(np.random.randint(0, 10, size=(2, 3)))  # 2x3 array of random integers

if __name__ == "__main__":
    test_run()

max_stars_repo_name: srishag/Machine-Learning-For-Trading

max_issues_repo_name: srishag/Machine-Learning-For-Trading

max_forks_repo_name: srishag/Machine-Learning-For-Trading


text:
import numpy as np
import matplotlib.pyplot as plt

# this data uses "ATT distance" defined here:
# http://debian.cse.msu.edu/CSE/pub/GA/programs/Genocop-doc/TSP/READ_ME
# (this explains the division by 10 inside the square root)
def distance(tour, xy):
  # index with list, and repeat the first city
  tour = np.append(tour, tour[0])
  d = np.diff(xy[tour], axis=0)
  return np.sqrt((d**2).sum(axis=1)/10).sum()

num_seeds = 11
max_NFE = 100000
ft = np.zeros((num_seeds, max_NFE))
T0 = 1000 # initial temperature
alpha = 0.95 # cooling parameter

# load the data for the USA 48 capital cities
# optimal value = 10628 (https://people.sc.fsu.edu/~jburkardt/datasets/tsp/tsp.html)
xy = np.loadtxt('data/tsp-48.txt')
num_cities = len(xy)

# simulated annealing
for seed in range(num_seeds):
  np.random.seed(seed)
  T = T0

  # random initial tour
  tour = np.arange(num_cities)
  tour = np.random.permutation(tour)
  bestf = distance(tour, xy)

  for i in range(max_NFE):

    # mutate the tour using two random cities
    trial_tour = np.copy(tour) # do not operate on original list

    # 2018: FIX to not allow same a,b
    a = np.random.randint(num_cities)
    b = np.random.randint(num_cities)

    # option 1: just swap the pair (in-class version)
    trial_tour[a],trial_tour[b] = trial_tour[b],trial_tour[a]

    trial_f = distance(trial_tour, xy)

    r = np.random.rand()
    if T > 10**-3: # protect division by zero
      P = np.min([1.0, np.exp((bestf - trial_f)/T)])
    else:
      P = 0.0

    if trial_f < bestf or r < P:
      tour = trial_tour
      bestf = trial_f

    T = T0*alpha**i
    ft[seed,i] = bestf

  print(tour)
  print(bestf)

plt.subplot(1,2,1)
tour = np.append(tour, tour[0]) # for plotting
plt.plot(xy[tour][:,0], xy[tour][:,1], marker='o')

plt.subplot(1,2,2)
plt.semilogx(ft.T, color='steelblue', linewidth=1)
plt.xlabel('Iterations')
plt.ylabel('Length of Tour')
plt.show()

max_stars_repo_name: jdherman/evolutionary-algorithms-course

max_issues_repo_name: jdherman/evolutionary-algorithms-course

max_forks_repo_name: jdherman/evolutionary-algorithms-course


text:
import os
import random
import numpy as np
import torch
from skimage.io import imread
from torch.utils.data import Dataset

class DataSetfromNumpy(Dataset):
    """
    Custom dataset object thats loads the entire training images and masks into memory
    """
    def __init__(self,
                 image_npy_path='data/train_img.npy',
                 mask_npy_path='data/train_mask.npy',
                 transform=None):
        self.images = np.load(image_npy_path)
        self.masks = np.load(mask_npy_path)
        self.n = len(self.images)
        self.transform = transform

    def __len__(self):
        return self.n

    def __getitem__(self, item):
        sample = {'image':self.images[item], 'mask':self.masks[item]}

        if self.transform is not None:
            return self.transform(sample)
        else:
            return sample

"""
dataset = DataSetfromNumpy()
img, mask = dataset[1]
import matplotlib.pyplot as plt
plt.imshow(img)
plt.show()
plt.imshow(np.squeeze(mask))
plt.show()
"""


text:
import argparse
import numpy as np
import h5py
from sklearn.linear_model import LinearRegression, Ridge
import gym
import load_policy
import tensorflow as tf
import tf_util

training_file = 'data.h5'

class EstimatorPolicy(object):
    """Wraps a fitted scikit-learn estimator to call it directly."""

    def __init__(self, actions, observations, expert_policy, use_past_obs=False,
                 use_past_act=False):
        X_train = observations[1:]

        self.use_past_obs = use_past_obs
        if use_past_obs:
            X_train = np.hstack([X_train, observations[:-1]])

        self.use_past_act = use_past_act
        if use_past_act:
            X_train = np.hstack([X_train, actions[:-1]])

        self.estimator = Ridge(alpha=0.5)
        self.estimator.fit(X_train, actions[1:])

        self.nact, self.nfeat = self.estimator.coef_.shape
        self.clear()

        self.expert_policy = expert_policy

    def __call__(self, obs):
        x = obs
        if self.use_past_obs:
            x = np.hstack([x, self.past_obs])
        if self.past_act is None:
            self.past_act = self.expert_policy(self.past_obs[None, :]).squeeze()
        if self.use_past_act:
            x = np.hstack([x, self.past_act])
        x = np.atleast_2d(x)
        act = self.estimator.predict(x)
        self.past_act = act.squeeze()
        self.past_obs = obs.squeeze()
        return act

    def clear(self):
        self.past_act = None
        n = self.nfeat
        if self.use_past_act:
            n -= self.nact
        if self.use_past_obs:
            n = n // 2
        self.past_obs = np.zeros(n)

def train(envname):
    with h5py.File(training_file, 'r') as f:
        actions = f.get('/{}/actions'.format(envname))[:]
        observations = f.get('/{}/observations'.format(envname))[:]

    actions = actions.squeeze()

    exp_pol = load_policy.load_policy('experts/{}.pkl'.format(envname))
    return EstimatorPolicy(actions, observations, exp_pol, use_past_obs=True,
                           use_past_act=True)

def run_episode(env, policy, render=False):
    obs = env.reset()
    observations = []
    rewards = []
    rewards = []
    done = False
    totalr = 0.0
    steps = 0
    while not done:
        action = policy(obs)
        obs, reward, done, _ = env.step(action)
        totalr += reward
        steps += 1

        if render:
            env.render()

    return totalr

def parse_args():
    parser = argparse.ArgumentParser()
    parser.add_argument('envname',
        help='Name of the OpenAI Gym environment')
    parser.add_argument('-r', '--render', action='store_true', default=False,
        help='Render environment in a window.')
    parser.add_argument('-n', '--num_rollouts', type=int, default=10,
        help='Number of times to run the policy')
    return parser.parse_args()

if __name__ == '__main__':
    args = parse_args()

    with tf.Session():
        tf_util.initialize()

        env = gym.make(args.envname)
        policy = train(args.envname)
        for i in range(10):
            policy.clear()
            r = run_episode(env, policy, render=args.render)
            print("reward: {}".format(r))