<s> class NotAvailableError ( Exception ) : <EOL> def __init__ ( self , function_name ) : <EOL> msg = '<STR_LIT>' '<STR_LIT>' % function_name <EOL> Exception . __init__ ( self , msg ) <EOL> try : <EOL> import cvxopt <EOL> except ImportError : <EOL> cvxopt = None <EOL> else : <EOL> import cvxopt <EOL> import cvxopt . solvers <EOL> import numpy <EOL> import sympy <EOL> def mrepl ( m , repl ) : <EOL> return m . applyfunc ( lambda x : x . xreplace ( repl ) ) <EOL> def skew ( v ) : <EOL> return sympy . Matrix ( [ [ <NUM_LIT:0> , - v [ <NUM_LIT:2> ] , v [ <NUM_LIT:1> ] ] , <EOL> [ v [ <NUM_LIT:2> ] , <NUM_LIT:0> , - v [ <NUM_LIT:0> ] ] , <EOL> [ - v [ <NUM_LIT:1> ] , v [ <NUM_LIT:0> ] , <NUM_LIT:0> ] ] ) <EOL> def regr_matrices ( dof , parm_num , q , dq , ddq , tau , regr_func ) : <EOL> sn = q . shape [ <NUM_LIT:0> ] <EOL> H_S = numpy . matrix ( numpy . zeros ( ( dof * sn , parm_num ) ) ) <EOL> tau_S = numpy . matrix ( numpy . zeros ( dof * sn ) ) . T <EOL> for i in range ( sn ) : <EOL> H_S [ i * dof : i * dof + dof , : ] = numpy . array ( regr_func ( q [ i ] , dq [ i ] , ddq [ i ] ) ) . reshape ( dof , parm_num ) <EOL> for i in range ( sn ) : <EOL> tau_S [ i * dof : i * dof + dof ] = numpy . mat ( tau [ i ] ) . T <EOL> return H_S , tau_S <EOL> def get_diag_blocks ( M ) : <EOL> b = [ ] <EOL> n = M . shape [ <NUM_LIT:0> ] <EOL> c = <NUM_LIT:0> <EOL> while c < n - <NUM_LIT:1> : <EOL> for l in range ( n - <NUM_LIT:1> , c , - <NUM_LIT:1> ) : <EOL> if M [ l , c ] != <NUM_LIT:0> or M [ c , l ] != <NUM_LIT:0> : <EOL> break <EOL> elif l == c + <NUM_LIT:1> : <EOL> b . append ( l ) <EOL> c = l <EOL> return b + [ n ] <EOL> def prepare_sdp ( var_symbs , LMI_matrix , split_diag_blocks = True ) : <EOL> v = var_symbs <EOL> vn = len ( v ) <EOL> if isinstance ( LMI_matrix , list ) or isinstance ( LMI_matrix , tuple ) : <EOL> blocks_LMI_matrix = LMI_matrix <EOL> else : <EOL> if split_diag_blocks : <EOL> blocks = get_diag_blocks ( LMI_matrix ) <EOL> blocks_LMI_matrix = [ ] <EOL> for a , b in zip ( [ <NUM_LIT:0> ] + blocks [ : - <NUM_LIT:1> ] , blocks ) : <EOL> blocks_LMI_matrix . append ( LMI_matrix [ a : b , a : b ] ) <EOL> print ( '<STR_LIT>' % ( len ( blocks ) ) ) <EOL> else : <EOL> blocks_LMI_matrix = [ LMI_matrix ] <EOL> blocks_Fi = [ ] <EOL> zero_subs = dict ( zip ( v , [ <NUM_LIT:0> ] * vn ) ) <EOL> for LMI_matrix in blocks_LMI_matrix : <EOL> Fi = [ <NUM_LIT:0> ] * ( <NUM_LIT:1> + vn ) <EOL> Fsym = mrepl ( LMI_matrix , zero_subs ) <EOL> Fi [ <NUM_LIT:0> ] = numpy . matrix ( Fsym ) . astype ( float ) <EOL> for i , s in enumerate ( v ) : <EOL> Fsym = LMI_matrix . applyfunc ( lambda x : <NUM_LIT:0> if not x . coeff ( s ) else x . coeff ( s ) ) <EOL> Fi [ i + <NUM_LIT:1> ] = numpy . matrix ( Fsym ) . astype ( float ) <EOL> blocks_Fi . append ( Fi ) <EOL> return blocks_Fi <EOL> def sdp ( c , Ai_blocks , primalstar = None , dualstart = None , solver = '<STR_LIT>' , verbose = <NUM_LIT:0> , interpret = False , maxiters = <NUM_LIT:1000> , tolerance = <NUM_LIT> ) : <EOL> if cvxopt is None : <EOL> raise NotAvailableError ( sdp . __name__ ) <EOL> c = cvxopt . matrix ( c ) <EOL> h = [ ] <EOL> G = [ ] <EOL> for A in Ai_blocks : <EOL> h . append ( cvxopt . matrix ( A [ <NUM_LIT:0> ] . astype ( float ) . tolist ( ) ) ) <EOL> G . append ( cvxopt . matrix ( [ ( - A [ i ] ) . flatten ( ) . astype ( float ) . tolist ( ) [ <NUM_LIT:0> ] for i in range ( <NUM_LIT:1> , len ( A ) ) ] ) ) <EOL> if solver != '<STR_LIT>' and solver != '<STR_LIT>' : <EOL> raise Exception ( '<STR_LIT>' ) <EOL> if solver == '<STR_LIT>' : solver == '<STR_LIT>' <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = ( <NUM_LIT:1> if verbose > <NUM_LIT:0> else <NUM_LIT:0> ) <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = maxiters <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = tolerance <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = tolerance <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = tolerance <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = ( <NUM_LIT:1> if verbose > <NUM_LIT:0> else <NUM_LIT:0> ) <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = maxiters <EOL> cvxopt . solvers . options [ '<STR_LIT>' ] = tolerance <EOL> sol = cvxopt . solvers . sdp ( c , Gs = G , hs = h , solver = solver ) <EOL> if not interpret : <EOL> if verbose >= <NUM_LIT:0> : print ( sol [ '<STR_LIT:status>' ] ) <EOL> return sol <EOL> if verbose > <NUM_LIT:0> : <EOL> print ( '<STR_LIT>' + sol [ '<STR_LIT:status>' ] + '<STR_LIT>' ) <EOL> if sol [ '<STR_LIT:status>' ] == '<STR_LIT>' : <EOL> if verbose >= <NUM_LIT:0> : print ( '<STR_LIT>' ) <EOL> return numpy . matrix ( sol [ '<STR_LIT:x>' ] ) <EOL> else : <EOL> sp = '<STR_LIT>' <EOL> sd = '<STR_LIT>' <EOL> if solver == '<STR_LIT>' and sol [ '<STR_LIT:status>' ] == '<STR_LIT>' : <EOL> if sol [ sp ] is not None : <EOL> s = '<STR_LIT>' <EOL> elif sol [ sp ] is not None : <EOL> s = '<STR_LIT>' <EOL> else : <EOL> s = '<STR_LIT>' <EOL> if verbose >= <NUM_LIT:0> : print ( s ) <EOL> if verbose >= <NUM_LIT:0> : print ( sp + '<STR_LIT>' + str ( sol [ sp ] ) + '<STR_LIT:\n>' + sd + '<STR_LIT>' + str ( sol [ sd ] ) ) <EOL> return <EOL> else : <EOL> if verbose >= <NUM_LIT:0> : print ( sol [ '<STR_LIT:status>' ] ) <EOL> if verbose >= <NUM_LIT:0> : print ( sp + '<STR_LIT>' + str ( sol [ sp ] ) + '<STR_LIT:\n>' + sd + '<STR_LIT>' + str ( sol [ sd ] ) ) <EOL> return </s>
