std::pair<int, int> SundialsInterface::getBandwidthB() const {
std::pair<int, int> bw;
// Get upper bandwidth
if (upper_bandwidthB_>=0) {
bw.first = upper_bandwidthB_;
} else {
casadi_assert_message(!jacB_.is_null(),
"\"upper_bandwidthB\" has not been set and cannot be "
"detected since exact Jacobian for backward problem "
"is not available.");
bw.first = jacB_.sparsity_out(0).bw_upper();
}
// Get lower bandwidth
if (lower_bandwidthB_>=0) {
bw.second = lower_bandwidthB_;
} else {
casadi_assert_message(!jacB_.is_null(),
"\"lower_bandwidthB\" has not been set and cannot be "
"detected since exact Jacobian for backward problem "
"is not available.");
bw.second = jacB_.sparsity_out(0).bw_lower();
}
return bw;
}
vector<SXMatrix> FX::evalSX(const vector<SXMatrix>& arg){
casadi_assert_message(isInit(),"Function has not been initialized");
// Copy the arguments into a new vector with the right sparsity
casadi_assert_message(arg.size()<=getNumInputs(), "FX::evalSX: number of passed-in dependencies (" << arg.size() << ") should not exceed the number of inputs of the function (" << getNumInputs() << ").");
vector<SXMatrix> arg2 = arg;
arg2.resize(getNumInputs());
for(int iind=0; iind<arg.size(); ++iind){
// If sparsities do not match, we need to map the nonzeros onto the new pattern
if(!(arg2[iind].sparsity()==input(iind).sparsity())){
arg2[iind] = project(arg2[iind],input(iind).sparsity());
}
}
// Create result vector with correct sparsity for the result
vector<SXMatrix> res(getNumOutputs());
for(int i=0; i<res.size(); ++i){
res[i] = SXMatrix(output(i).sparsity());
}
// No sensitivities
vector<vector<SXMatrix> > dummy;
// Evaluate the algorithm
(*this)->evalSX(arg2,res,dummy,dummy,dummy,dummy,false);
// Return the result
return res;
}
void Newton::init(const Dict& opts) {
// Call the base class initializer
Rootfinder::init(opts);
// Default options
max_iter_ = 1000;
abstol_ = 1e-12;
abstolStep_ = 1e-12;
print_iteration_ = false;
// Read options
for (auto&& op : opts) {
if (op.first=="max_iter") {
max_iter_ = op.second;
} else if (op.first=="abstol") {
abstol_ = op.second;
} else if (op.first=="abstolStep") {
abstolStep_ = op.second;
} else if (op.first=="print_iteration") {
print_iteration_ = op.second;
}
}
casadi_assert_message(oracle_.n_in()>0,
"Newton: the supplied f must have at least one input.");
casadi_assert_message(!linsol_.is_null(),
"Newton::init: linear_solver must be supplied");
// Allocate memory
alloc_w(n_, true); // x
alloc_w(n_, true); // F
alloc_w(sp_jac_.nnz(), true); // J
}
Function simpleRK(Function f, int N, int order) {
// Consistency check
casadi_assert_message(N>=1, "Parameter N (number of steps) must be at least 1, but got "
<< N << ".");
casadi_assert_message(order==4, "Only RK order 4 is supported now.");
casadi_assert_message(f.n_in()==2, "Function must have two inputs: x and p");
casadi_assert_message(f.n_out()==1, "Function must have one outputs: dot(x)");
MX x0 = MX::sym("x0", f.sparsity_in(0));
MX p = MX::sym("p", f.sparsity_in(1));
MX h = MX::sym("h");
std::vector<double> b(order);
b[0]=1.0/6;
b[1]=1.0/3;
b[2]=1.0/3;
b[3]=1.0/6;
std::vector<double> c(order);
c[0]=0;
c[1]=1.0/2;
c[2]=1.0/2;
c[3]=1;
std::vector< std::vector<double> > A(order-1);
A[0].resize(1);
A[0][0]=1.0/2;
A[1].resize(2);
A[1][0]=0;A[1][1]=1.0/2;
A[2].resize(3);
A[2][0]=0;
A[2][1]=0;A[2][2]=1;
// Time step
MX dt = h/N;
std::vector<MX> k(order);
vector<MX> f_arg(2);
// Integrate
MX xf = x0;
for (int i=0; i<N; ++i) {
for (int j=0; j<order; ++j) {
MX xL = 0;
for (int jj=0; jj<j; ++jj) {
xL += k.at(jj)*A.at(j-1).at(jj);
}
f_arg[0] = xf+xL;
f_arg[1] = p;
k[j] = dt*f(f_arg).at(0);
}
for (int j=0; j<order; ++j) {
xf += b.at(j)*k.at(j);
}
}
// Form discrete-time dynamics
return Function("F", {x0, p, h}, {xf}, {"x0", "p", "h"}, {"xf"});
}
T cross(const GenericMatrix<T> &a, const GenericMatrix<T> &b, int dim) {
casadi_assert_message(a.size1()==b.size1() && a.size2()==b.size2(),"cross(a,b): Inconsistent dimensions. Dimension of a (" << a.dimString() << " ) must equal that of b (" << b.dimString() << ").");
casadi_assert_message(a.size1()==3 || a.size2()==3,"cross(a,b): One of the dimensions of a should have length 3, but got " << a.dimString() << ".");
casadi_assert_message(dim==-1 || dim==1 || dim==2,"cross(a,b,dim): Dim must be 1, 2 or -1 (automatic).");
std::vector<T> ret(3);
bool t = a.size1()==3;
if (dim==1) t = true;
if (dim==2) t = false;
T a1 = t ? a(0,ALL) : a(ALL,0);
T a2 = t ? a(1,ALL) : a(ALL,1);
T a3 = t ? a(2,ALL) : a(ALL,2);
T b1 = t ? b(0,ALL) : b(ALL,0);
T b2 = t ? b(1,ALL) : b(ALL,1);
T b3 = t ? b(2,ALL) : b(ALL,2);
ret[0] = a2*b3-a3*b2;
ret[1] = a3*b1-a1*b3;
ret[2] = a1*b2-a2*b1;
return t ? vertcat(ret) : horzcat(ret);
}
void SocpSolverInternal::init() {
// Call the init method of the base class
FunctionInternal::init();
ni_ = getOption("ni");
print_problem_ = getOption("print_problem");
m_ = ni_.size();
const Sparsity& A = st_[SOCP_STRUCT_A];
const Sparsity& G = st_[SOCP_STRUCT_G];
const Sparsity& E = st_[SOCP_STRUCT_E];
N_ = std::accumulate(ni_.begin(), ni_.end(), 0);
casadi_assert_message(N_==G.size2(),
"SocpSolverInternal: Supplied G sparsity: number of cols ("
<< G.size2()
<< ") must match sum of vector provided with option 'ni' ("
<< N_ << ").");
casadi_assert_message(m_==E.size2(),
"SocpSolverInternal: Supplied E sparsity: number of cols ("
<< E.size2()
<< ") must match number of cone (2-norm) constraints ("
<< m_ << ").");
nc_ = A.size1();
n_ = A.size2();
casadi_assert_message(n_==G.size1(),
"SocpSolverInternal: Supplied G sparsity: number of rows ("
<< G.size1()
<< ") must match number of decision variables (cols of A): " << n_ << ".");
casadi_assert_message(n_==E.size1(),
"SocpSolverInternal: Supplied E sparsity: number of rows ("
<< E.size1()
<< ") must match number of decision variables (cols of A): " << n_ << ".");
// Input arguments
setNumInputs(SOCP_SOLVER_NUM_IN);
input(SOCP_SOLVER_G) = DMatrix::zeros(G);
input(SOCP_SOLVER_H) = DMatrix::zeros(N_, 1);
input(SOCP_SOLVER_E) = DMatrix::zeros(E);
input(SOCP_SOLVER_F) = DMatrix::zeros(m_, 1);
input(SOCP_SOLVER_A) = DMatrix::zeros(A);
input(SOCP_SOLVER_C) = DMatrix::zeros(n_);
input(SOCP_SOLVER_LBX) = -DMatrix::inf(n_);
input(SOCP_SOLVER_UBX) = DMatrix::inf(n_);
input(SOCP_SOLVER_LBA) = -DMatrix::inf(nc_);
input(SOCP_SOLVER_UBA) = DMatrix::inf(nc_);
// Output arguments
setNumOutputs(SOCP_SOLVER_NUM_OUT);
output(SOCP_SOLVER_X) = DMatrix::zeros(n_, 1);
output(SOCP_SOLVER_COST) = 0.0;
output(SOCP_SOLVER_DUAL_COST) = 0.0;
output(SOCP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SOCP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
output(SOCP_SOLVER_LAM_CONE) = DMatrix::zeros(m_, 1);
}
Multiplication<TrX,TrY>::Multiplication(const MX& z, const MX& x, const MX& y){
casadi_assert_message(TrX || !TrY, "Illegal combination");
casadi_assert_message(TrX, "Not implemented");
casadi_assert_message(!TrY,"Not implemented");
casadi_assert_message(x.size1() == y.size1() && x.size2() == z.size1() && y.size2() == z.size2(),"Multiplication::Multiplication: dimension mismatch. Attempting to multiply trans(" << x.dimString() << ") with " << y.dimString() << " and add the result to " << z.dimString());
setDependencies(z,x,y);
setSparsity(z.sparsity());
}
void Callback::transfer_ownership() {
casadi_assert_message(!is_null(), "Null pointer.");
casadi_assert_message(!(*this)->own_, "Ownership has already been transferred.");
casadi_assert_message(getCount()>1, "There are no owning references");
// Decrease the reference counter to offset the effect of the owning reference
count_down();
// Mark internal class as owning
(*this)->own_ = true;
}
void OCPSolverInternal::init(){
// Initialize the functions
ffcn_.init();
mfcn_.init();
if(!cfcn_.isNull()) cfcn_.init();
if(!mfcn_.isNull()) mfcn_.init();
// Get the number of grid points
nk_ = getOption("number_of_grid_points");
// Read final time
tf_ = getOption("final_time");
// Get the number of differential states
nx_ = ffcn_.input(DAE_X).size();
// Get the number of parameters
np_ = getOption("number_of_parameters");
// Get the number of controls
nu_ = ffcn_.input(DAE_P).size() - np_;
// Number of point constraints
nh_ = cfcn_.isNull() ? 0 : cfcn_.output().size();
// Number of point coupling constraints
ng_ = 0;
casadi_assert_message(mfcn_.getNumInputs()<=2, "Mayer term must map endstate [ (nx x 1) , (np x 1) ] to cost (1 x 1). So it needs to accept 2 matrix-valued inputs. You supplied " << mfcn_.getNumInputs());
if (mfcn_.getNumInputs()==2) {
casadi_assert_message(mfcn_.input(1).size()==np_, "Mayer term must map endstate [ (nx x 1) , (np x 1) ] to cost (1 x 1). Shape of the second input " << mfcn_.input(1).dimString() << " must match the number of parameters np " << np_);
}
// Specify the inputs
setNumInputs(OCP_NUM_IN);
input(OCP_LBX) = input(OCP_UBX) = input(OCP_X_INIT) = Matrix<double>(nx_,nk_+1,0);
input(OCP_LBU) = input(OCP_UBU) = input(OCP_U_INIT) = Matrix<double>(nu_,nk_,0);
input(OCP_LBP) = input(OCP_UBP) = input(OCP_P_INIT) = Matrix<double>(np_,1,0);
input(OCP_LBH) = input(OCP_UBH) = Matrix<double>(nh_,nk_+1,0);
input(OCP_LBG) = input(OCP_UBG) = Matrix<double>(ng_,1,0);
// Specify the outputs
setNumOutputs(OCP_NUM_OUT);
output(OCP_X_OPT) = input(OCP_X_INIT);
output(OCP_U_OPT) = input(OCP_U_INIT);
output(OCP_P_OPT) = input(OCP_P_INIT);
output(OCP_COST) = 0.;
// Call the init function of the base class
FXInternal::init();
}
void CondensingIndefDpleInternal::init() {
// Initialize the base classes
DpleInternal::init();
casadi_assert_message(!pos_def_,
"pos_def option set to True: Solver only handles the indefinite case.");
casadi_assert_message(const_dim_,
"const_dim option set to False: Solver only handles the True case.");
n_ = A_[0].size1();
MX As = MX::sym("A", horzcat(A_));
MX Vs = MX::sym("V", horzcat(V_));
std::vector< MX > Vss = horzsplit(Vs, n_);
std::vector< MX > Ass = horzsplit(As, n_);
for (int k=0;k<K_;++k) {
Vss[k] = (Vss[k]+Vss[k].T())/2;
}
MX R = MX::zeros(n_, n_);
for (int k=0;k<K_;++k) {
R = mul(mul(Ass[k], R), Ass[k].T()) + Vss[k];
}
std::vector< MX > Assr(K_);
std::reverse_copy(Ass.begin(), Ass.end(), Assr.begin());
MX Ap = mul(Assr);
// Create an dlesolver instance
solver_ = DleSolver(getOption(solvername()), dleStruct("a", Ap.sparsity(), "v", R.sparsity()));
solver_.setOption(getOption(optionsname()));
// Initialize the NLP solver
solver_.init();
std::vector<MX> Pr = solver_.call(dpleIn("a", Ap, "v", R));
std::vector<MX> Ps(K_);
Ps[0] = Pr[0];
for (int k=0;k<K_-1;++k) {
Ps[k+1] = mul(mul(Ass[k], Ps[k]), Ass[k].T()) + Vss[k];
}
f_ = MXFunction(dpleIn("a", As, "v", Vs), dpleOut("p", horzcat(Ps)));
f_.init();
Wrapper::checkDimensions();
}
void ImplicitFunctionInternal::init(){
// Initialize the residual function
if(!f_.isInit()) f_.init();
// Allocate inputs
setNumInputs(f_.getNumInputs()-1);
for(int i=0; i<getNumInputs(); ++i){
input(i) = f_.input(i+1);
}
// Allocate outputs
setNumOutputs(f_.getNumOutputs());
output(0) = f_.input(0);
for(int i=1; i<getNumOutputs(); ++i){
output(i) = f_.output(i);
}
// Call the base class initializer
FXInternal::init();
// Number of equations
N_ = output().size();
// Generate Jacobian if not provided
if(J_.isNull()) J_ = f_.jacobian(0,0);
J_.init();
casadi_assert_message(J_.output().size1()==J_.output().size2(),"ImplicitFunctionInternal::init: the jacobian must be square but got " << J_.output().dimString());
casadi_assert_message(!isSingular(J_.output().sparsity()),"ImplicitFunctionInternal::init: singularity - the jacobian is structurally rank-deficient. sprank(J)=" << sprank(J_.output()) << " (in stead of "<< J_.output().size1() << ")");
// Get the linear solver creator function
if(linsol_.isNull() && hasSetOption("linear_solver")){
linearSolverCreator linear_solver_creator = getOption("linear_solver");
// Allocate an NLP solver
linsol_ = linear_solver_creator(CRSSparsity());
// Pass options
if(hasSetOption("linear_solver_options")){
const Dictionary& linear_solver_options = getOption("linear_solver_options");
linsol_.setOption(linear_solver_options);
}
}
// Initialize the linear solver, if provided
if(!linsol_.isNull()){
linsol_.setSparsity(J_.output().sparsity());
linsol_.init();
}
// Allocate memory for directional derivatives
ImplicitFunctionInternal::updateNumSens(false);
}
std::vector<double> collocationPoints(int order, const std::string& scheme) {
if (scheme=="radau") {
casadi_assert_message(order>0 && order<10,"Error in collocationPoints(order, scheme): only order up to 9 supported for scheme 'radau', but got " << order << ".");
return std::vector<double>(radau_points[order],radau_points[order]+order+1);
} else if (scheme=="legendre") {
casadi_assert_message(order>0 && order<10,"Error in collocationPoints(order, scheme): only order up to 9 supported for scheme 'legendre', but got " << order << ".");
return std::vector<double>(legendre_points[order],legendre_points[order]+order+1);
} else {
casadi_error("Error in collocationPoints(order, scheme): unknown scheme '" << scheme << "'. Select one of 'radau', 'legendre'.");
}
}
// Constructor
SdpSolverInternal::SdpSolverInternal(const std::vector<Sparsity> &st) : st_(st) {
addOption("calc_p", OT_BOOLEAN, true,
"Indicate if the P-part of primal solution should be allocated and calculated. "
"You may want to avoid calculating this variable for problems with n large, "
"as is always dense (m x m).");
addOption("calc_dual", OT_BOOLEAN, true, "Indicate if dual should be allocated and calculated. "
"You may want to avoid calculating this variable for problems with n large, "
"as is always dense (m x m).");
addOption("print_problem", OT_BOOLEAN, false, "Print out problem statement for debugging.");
casadi_assert_message(st_.size()==SDP_STRUCT_NUM, "Problem structure mismatch");
const Sparsity& A = st_[SDP_STRUCT_A];
const Sparsity& G = st_[SDP_STRUCT_G];
const Sparsity& F = st_[SDP_STRUCT_F];
casadi_assert_message(G==G.transpose(), "SdpSolverInternal: Supplied G sparsity must "
"symmetric but got " << G.dimString());
m_ = G.size1();
nc_ = A.size1();
n_ = A.size2();
casadi_assert_message(F.size1()==m_, "SdpSolverInternal: Supplied F sparsity: number of rows ("
<< F.size1() << ") must match m (" << m_ << ")");
casadi_assert_message(F.size2()%n_==0, "SdpSolverInternal: Supplied F sparsity: "
"number of columns (" << F.size2()
<< ") must be an integer multiple of n (" << n_
<< "), but got remainder " << F.size2()%n_);
// Input arguments
setNumInputs(SDP_SOLVER_NUM_IN);
input(SDP_SOLVER_G) = DMatrix(G, 0);
input(SDP_SOLVER_F) = DMatrix(F, 0);
input(SDP_SOLVER_A) = DMatrix(A, 0);
input(SDP_SOLVER_C) = DMatrix::zeros(n_);
input(SDP_SOLVER_LBX) = -DMatrix::inf(n_);
input(SDP_SOLVER_UBX) = DMatrix::inf(n_);
input(SDP_SOLVER_LBA) = -DMatrix::inf(nc_);
input(SDP_SOLVER_UBA) = DMatrix::inf(nc_);
for (int i=0;i<n_;i++) {
Sparsity s = input(SDP_SOLVER_F)(ALL, Slice(i*m_, (i+1)*m_)).sparsity();
casadi_assert_message(s==s.transpose(),
"SdpSolverInternal: Each supplied Fi must be symmetric. "
"But got " << s.dimString() << " for i = " << i << ".");
}
input_.scheme = SCHEME_SDPInput;
output_.scheme = SCHEME_SDPOutput;
}
int GenericType::toInt() const{
if(isDouble()){
double v = toDouble();
casadi_assert_message(v == std::floor(v),"The value is not an integer");
return int(v);
} else if (isBool()) {
return int(toBool());
} else {
casadi_assert_message(isInt(),"type mismatch");
return static_cast<const IntType*>(get())->d_;
}
}
void CSparseCholeskyInternal::prepare() {
prepared_ = false;
// Get a reference to the nonzeros of the linear system
const vector<double>& linsys_nz = input().data();
// Make sure that all entries of the linear system are valid
for (int k=0; k<linsys_nz.size(); ++k) {
casadi_assert_message(!isnan(linsys_nz[k]), "Nonzero " << k << " is not-a-number");
casadi_assert_message(!isinf(linsys_nz[k]), "Nonzero " << k << " is infinite");
}
if (verbose()) {
userOut() << "CSparseCholeskyInternal::prepare: numeric factorization" << endl;
userOut() << "linear system to be factorized = " << endl;
input(0).printSparse();
}
if (L_) cs_nfree(L_);
L_ = cs_chol(&AT_, S_) ; // numeric Cholesky factorization
if (L_==0) {
DMatrix temp = input();
temp.makeSparse();
if (temp.sparsity().issingular()) {
stringstream ss;
ss << "CSparseCholeskyInternal::prepare: factorization failed due "
"to matrix being singular. Matrix contains numerical zeros which are"
" structurally non-zero. Promoting these zeros to be structural "
"zeros, the matrix was found to be structurally rank deficient. "
"sprank: " << sprank(temp.sparsity()) << " <-> " << temp.size2() << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
} else {
stringstream ss;
ss << "CSparseCholeskyInternal::prepare: factorization failed, "
"check if Jacobian is singular" << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
}
}
casadi_assert(L_!=0);
prepared_ = true;
}
Function explicitRK(Function& f, const MX& tf, int order, int ne) {
casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), but got "
<< ne << ".");
casadi_assert_message(order==4, "Only RK order 4 is supported now.");
casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT,
"Supplied function must adhere to dae scheme.");
casadi_assert_message(f.output(DAE_ALG).isEmpty() && f.input(DAE_Z).isEmpty(),
"Supplied function cannot have algebraic states.");
casadi_assert_message(f.output(DAE_QUAD).isEmpty(),
"Supplied function cannot have quadrature states.");
MX X = MX::sym("X", f.input(DAE_X).sparsity());
MX P = MX::sym("P", f.input(DAE_P).sparsity());
MX X0 = X;
MX t = 0;
MX dt = tf/ne;
std::vector<double> b(order);
b[0]=1.0/6;b[1]=1.0/3;b[2]=1.0/3;b[3]=1.0/6;
std::vector<double> c(order);
c[0]=0;c[1]=1.0/2;c[2]=1.0/2;c[3]=1;
std::vector< std::vector<double> > A(order-1);
A[0].resize(1);A[0][0]=1.0/2;
A[1].resize(2);A[1][0]=0;A[1][1]=1.0/2;
A[2].resize(3);A[2][0]=0;A[2][1]=0;A[2][2]=1;
std::vector<MX> k(order);
for (int i=0;i<ne;++i) {
for (int j=0;j<order;++j) {
MX XL = 0;
for (int jj=0;jj<j;++jj) {
XL+=k.at(jj)*A.at(j-1).at(jj);
}
//std::cout << "help: " << A.at(j-1) << ", " << c.at(j) << std::endl;
k[j] = dt*f.call(daeIn("x", X+XL, "p", P, "t", t+dt*c.at(j)))[DAE_ODE];
}
for (int j=0;j<order;++j) {
X += b.at(j)*k.at(j);
}
t+= dt;
}
MXFunction ret(integratorIn("x0", X0, "p", P), integratorOut("xf", X));
return ret;
}
void LpSolverInternal::checkInputs() const {
for (int i=0;i<input(LP_SOLVER_LBX).nnz();++i) {
casadi_assert_message(input(LP_SOLVER_LBX).at(i)<=input(LP_SOLVER_UBX).at(i),
"LBX[i] <= UBX[i] was violated for i=" << i
<< ". Got LBX[i] " << input(LP_SOLVER_LBX).at(i)
<< " and UBX[i] " << input(LP_SOLVER_UBX).at(i));
}
for (int i=0;i<input(LP_SOLVER_LBA).nnz();++i) {
casadi_assert_message(input(LP_SOLVER_LBA).at(i)<=input(LP_SOLVER_UBA).at(i),
"LBA[i] <= UBA[i] was violated for i=" << i
<< ". Got LBA[i] " << input(LP_SOLVER_LBA).at(i)
<< " and UBA[i] " << input(LP_SOLVER_UBA).at(i));
}
}
// Constructor
QpSolverInternal::QpSolverInternal(const std::vector<Sparsity> &st) : st_(st) {
casadi_assert_message(st_.size()==QP_STRUCT_NUM, "Problem structure mismatch");
const Sparsity& A = st_[QP_STRUCT_A];
const Sparsity& H = st_[QP_STRUCT_H];
n_ = H.size2();
nc_ = A.isNull() ? 0 : A.size1();
if (!A.isNull()) {
casadi_assert_message(A.size2()==n_,
"Got incompatible dimensions. min x'Hx + G'x s.t. LBA <= Ax <= UBA :"
<< std::endl <<
"H: " << H.dimString() << " - A: " << A.dimString() << std::endl <<
"We need: H.size2()==A.size2()" << std::endl);
}
casadi_assert_message(H.isSymmetric(),
"Got incompatible dimensions. min x'Hx + G'x" << std::endl <<
"H: " << H.dimString() <<
"We need H square & symmetric" << std::endl);
// Sparsity
Sparsity x_sparsity = Sparsity::dense(n_, 1);
Sparsity bounds_sparsity = Sparsity::dense(nc_, 1);
// Input arguments
setNumInputs(QP_SOLVER_NUM_IN);
input(QP_SOLVER_X0) = DMatrix::zeros(x_sparsity);
input(QP_SOLVER_H) = DMatrix::zeros(H);
input(QP_SOLVER_G) = DMatrix::zeros(x_sparsity);
input(QP_SOLVER_A) = DMatrix::zeros(A);
input(QP_SOLVER_LBA) = -DMatrix::inf(bounds_sparsity);
input(QP_SOLVER_UBA) = DMatrix::inf(bounds_sparsity);
input(QP_SOLVER_LBX) = -DMatrix::inf(x_sparsity);
input(QP_SOLVER_UBX) = DMatrix::inf(x_sparsity);
input(QP_SOLVER_LAM_X0) = DMatrix::zeros(x_sparsity);
//input(QP_SOLVER_LAM_A0) = DMatrix::zeros(x_sparsity);
// Output arguments
setNumOutputs(QP_SOLVER_NUM_OUT);
output(QP_SOLVER_X) = DMatrix::zeros(x_sparsity);
output(QP_SOLVER_COST) = 0.0;
output(QP_SOLVER_LAM_X) = DMatrix::zeros(x_sparsity);
output(QP_SOLVER_LAM_A) = DMatrix::zeros(bounds_sparsity);
input_.scheme = SCHEME_QpSolverInput;
output_.scheme = SCHEME_QpSolverOutput;
}
std::vector<T> vector_slice(const std::vector<T> &v, const std::vector<int> &i) {
std::vector<T> ret;
ret.reserve(i.size());
for (int k=0;k<i.size();++k) {
int j = i[k];
casadi_assert_message(j>=0,
"vector_slice: Indices should be larger than zero."
<< "You have " << j << " at location " << k << ".");
casadi_assert_message(j<v.size(),
"vector_slice: Indices should be larger than zero."
<< "You have " << j << " at location " << k << ".");
ret.push_back(v[j]);
}
return ret;
}
void SimpleIndefDpleInternal::init() {
DpleInternal::init();
casadi_assert_message(!pos_def_,
"pos_def option set to True: Solver only handles the indefinite case.");
casadi_assert_message(const_dim_,
"const_dim option set to False: Solver only handles the True case.");
n_ = A_[0].size1();
MX As = MX::sym("A", n_, K_*n_);
MX Vs = MX::sym("V", n_, K_*n_);
std::vector< MX > Vss = horzsplit(Vs, n_);
std::vector< MX > Ass = horzsplit(As, n_);
for (int k=0;k<K_;++k) {
Vss[k]=(Vss[k]+Vss[k].T())/2;
}
std::vector< MX > AA_list(K_);
for (int k=0;k<K_;++k) {
AA_list[k] = kron(Ass[k], Ass[k]);
}
MX AA = blkdiag(AA_list);
MX A_total = DMatrix::eye(n_*n_*K_) -
vertcat(AA(range(K_*n_*n_-n_*n_, K_*n_*n_), range(K_*n_*n_)),
AA(range(K_*n_*n_-n_*n_), range(K_*n_*n_)));
std::vector<MX> Vss_shift;
Vss_shift.push_back(Vss.back());
Vss_shift.insert(Vss_shift.end(), Vss.begin(), Vss.begin()+K_-1);
MX Pf = solve(A_total, vec(horzcat(Vss_shift)), getOption("linear_solver"));
MX P = reshape(Pf, n_, K_*n_);
std::vector<MX> v_in;
v_in.push_back(As);
v_in.push_back(Vs);
f_ = MXFunction(v_in, P);
f_.setInputScheme(SCHEME_DPLEInput);
f_.setOutputScheme(SCHEME_DPLEOutput);
f_.init();
}
void GurobiInterface::init_memory(void* mem) const {
auto m = static_cast<GurobiMemory*>(mem);
// Load environment
int flag = GRBloadenv(&m->env, 0); // no log file
casadi_assert_message(!flag && m->env, "Failed to create GUROBI environment");
}
FX FX::operator[](int k) const {
// Argument checking
if (k<0) k+=getNumOutputs();
casadi_assert_message(k<getNumOutputs(),"FX[int k]:: Attempt to select the k'th output with k=" << k << ", but should be smaller or equal to number of outputs (" << getNumOutputs() << ").");
// Get the inputs in MX form
std::vector< MX > in = symbolicInput();
// Clone such that we can use const.
FX clone = *this;
// Get the outputs
std::vector< MX > result = clone.call(in);
// Construct an MXFunction with only the k'th output
MXFunction ret(in,result[k]);
ret.setInputScheme(getInputScheme());
// Initialize it
ret.init();
// And return it, will automatically cast to FX
return ret;
}
// Constructor
LpSolverInternal::LpSolverInternal(const std::vector<Sparsity> &st) : st_(st) {
casadi_assert_message(st_.size()==LP_STRUCT_NUM, "Problem structure mismatch");
const Sparsity& A = st_[LP_STRUCT_A];
n_ = A.size2();
nc_ = A.size1();
// Input arguments
setNumInputs(LP_SOLVER_NUM_IN);
input(LP_SOLVER_A) = DMatrix(A);
input(LP_SOLVER_C) = DMatrix::zeros(n_);
input(LP_SOLVER_LBA) = -DMatrix::inf(nc_);
input(LP_SOLVER_UBA) = DMatrix::inf(nc_);
input(LP_SOLVER_LBX) = -DMatrix::inf(n_);
input(LP_SOLVER_UBX) = DMatrix::inf(n_);
// Output arguments
setNumOutputs(LP_SOLVER_NUM_OUT);
output(LP_SOLVER_X) = DMatrix::zeros(n_);
output(LP_SOLVER_COST) = 0.0;
output(LP_SOLVER_LAM_X) = DMatrix::zeros(n_);
output(LP_SOLVER_LAM_A) = DMatrix::zeros(nc_);
input_.scheme = SCHEME_LpSolverInput;
output_.scheme = SCHEME_LpSolverOutput;
}
SparseStorage<DataType>::SparseStorage(const std::vector< std::vector<DataType> >& d) {
// Get dimensions
int nrow=d.size();
int ncol=d.empty() ? 1 : d.front().size();
// Assert consistency
for (int rr=0; rr<nrow; ++rr) {
casadi_assert_message(ncol==d[rr].size(),
"SparseStorage<DataType>::SparseStorage(const std::vector< std::vector<DataType> >& d): "
"shape mismatch" << std::endl <<
"Attempting to construct a matrix from a nested list." << std::endl <<
"I got convinced that the desired size is ("<< nrow << " x " << ncol << " ), "
"but now I encounter a vector of size (" <<
d[rr].size() << " )" << std::endl);
}
// Form matrix
sparsity_ = Sparsity::dense(nrow, ncol);
data().resize(nrow*ncol);
typename std::vector<DataType>::iterator it=begin();
for (int cc=0; cc<ncol; ++cc) {
for (int rr=0; rr<nrow; ++rr) {
*it++ = d[rr][cc];
}
}
}
SparseStorage<DataType>::SparseStorage(const std::vector<DataType>& x, int nrow, int ncol)
: sparsity_(Sparsity::dense(nrow, ncol)), data_(x) {
casadi_assert_message(x.size() == nrow*ncol,
"Dimension mismatch." << std::endl
<< "You supplied a vector of length " << x.size()
<< ", but " << nrow << " x " << ncol << " = " << nrow*ncol);
}
CRSSparsity reshape(const CRSSparsity& a, int n, int m){
casadi_assert_message(a.numel() == n*m,
"reshape: number of elements must remain the same." << endl <<
"Input argument has shape " << a.size1() << " x " << a.size2() << " = " << a.numel() << ", while you request a reshape to " <<
n << " x " << m << " = " << n*m
);
// our strategy is: (col,rowind) -> (col,row) -> modulus calculus -> (col_new, row_new) -> sp_NZ
std::vector<int> row = a.getRow();
const std::vector<int> &col = a.col();
std::vector<int> row_new(a.size());
std::vector<int> col_new(a.size());
// int i=0;int j=0; int z =0;
for(int k=0; k<a.size(); ++k){
int i = row[k];
int j = col[k];
int z = j+i*a.size2();
row_new[k] = z/m;
col_new[k] = z%m;
}
return sp_triplet(n,m,row_new,col_new);
}
SparseStorage<DataType>::SparseStorage(const Sparsity& sparsity, const std::vector<DataType>& d)
: sparsity_(sparsity), data_(d) {
casadi_assert_message(sparsity.size()==d.size(),
"Size mismatch." << std::endl << "You supplied a sparsity of "
<< sparsity_.dimString()
<< ", but the supplied vector is of length " << d.size());
}
void SXFunctionInternal::callReverse(const std::vector<SX>& arg, const std::vector<SX>& res,
const std::vector<std::vector<SX> >& aseed,
std::vector<std::vector<SX> >& asens,
bool always_inline, bool never_inline) {
casadi_assert_message(!never_inline, "SX expressions do not have call nodes");
XFunctionInternal<SXFunction, SXFunctionInternal, SX, SXNode>
::callReverse(arg, res, aseed, asens, true, false);
}
double GenericType::toDouble() const{
if(isInt()){
return double(toInt());
} else {
casadi_assert_message(isDouble(),"type mismatch");
return static_cast<const DoubleType*>(get())->d_;
}
}
Assertion::Assertion(const MX& x, const MX& y, const std::string & fail_message)
: fail_message_(fail_message) {
casadi_assert_message(y.is_scalar(),
"Assertion:: assertion expression y must be scalar, but got "
<< y.dim());
setDependencies(x, y);
setSparsity(x.sparsity());
}
