C++ casadi_assert_message函数代码示例

  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;
  }