Sparsity CSparseCholeskyInterface::linsol_cholesky_sparsity(void* mem, bool tr) const {
auto m = static_cast<CsparseCholMemory*>(mem);
casadi_assert(m->S);
int n = m->A.n;
int nzmax = m->S->cp[n];
std::vector< int > row(n+1);
std::copy(m->S->cp, m->S->cp+n+1, row.begin());
std::vector< int > colind(nzmax);
int *Li = &colind.front();
int *Lp = &row.front();
const cs* C;
C = m->S->pinv ? cs_symperm(&m->A, m->S->pinv, 1) : &m->A;
std::vector< int > temp(2*n);
int *c = &temp.front();
int *s = c+n;
for (int k = 0 ; k < n ; k++) c[k] = m->S->cp[k] ;
for (int k = 0 ; k < n ; k++) { /* compute L(k, :) for L*L' = C */
int top = cs_ereach(C, k, m->S->parent, s, c) ;
for ( ; top < n ; top++) { /* solve L(0:k-1, 0:k-1) * x = C(:, k) */
int i = s[top] ; /* s[top..n-1] is pattern of L(k, :) */
int p = c[i]++ ;
Li[p] = k ; /* store L(k, i) in row i */
}
int p = c[k]++ ;
Li[p] = k ;
}
Lp[n] = m->S->cp[n] ;
Sparsity ret(n, n, row, colind); // BUG?
return tr ? ret.T() : ret;
}
bool IpoptInternal::eval_grad_f(int n, const double* x, bool new_x, double* grad_f)
{
try {
log("eval_grad_f started");
double time1 = clock();
casadi_assert(n == nx_);
// Pass the argument to the function
gradF_.setInput(x,NL_X);
gradF_.setInput(input(NLP_SOLVER_P),NL_P);
// Evaluate, adjoint mode
gradF_.evaluate();
// Get the result
gradF_.output().getArray(grad_f,n,DENSE);
// Printing
if(monitored("eval_grad_f")){
cout << "x = " << gradF_.input(NL_X) << endl;
cout << "grad_f = " << gradF_.output() << endl;
}
if (regularity_check_ && !isRegular(gradF_.output().data())) casadi_error("IpoptInternal::grad_f: NaN or Inf detected.");
double time2 = clock();
t_eval_grad_f_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_grad_f_ += 1;
log("eval_grad_f ok");
return true;
} catch (exception& ex){
cerr << "eval_grad_f failed: " << ex.what() << endl;
return false;
}
}
void Options::check(const Dict& opts) const {
// Make sure all options exist and have the correct type
for (auto&& op : opts) {
const Options::Entry* entry = find(op.first);
// Informative error message if option does not exist
if (entry==nullptr) {
stringstream ss;
ss << "Unknown option: " << op.first << endl;
ss << endl;
ss << "Did you mean one of the following?" << endl;
for (auto&& s : suggestions(op.first)) {
print_one(s, ss);
}
ss << "Use print_options() to get a full list of options." << endl;
casadi_error(ss.str());
}
// Check type
casadi_assert(op.second.can_cast_to(entry->type),
"Illegal type for " + op.first + ": " +
op.second.get_description() +
" cannot be cast to " +
GenericType::get_type_description(entry->type) + ".");
}
}
bool IpoptInternal::get_bounds_info(int n, double* x_l, double* x_u,
int m, double* g_l, double* g_u)
{
try {
casadi_assert(n == nx_);
casadi_assert(m == ng_);
input(NLP_SOLVER_LBX).getArray(x_l,n);
input(NLP_SOLVER_UBX).getArray(x_u,n);
input(NLP_SOLVER_LBG).getArray(g_l,m);
input(NLP_SOLVER_UBG).getArray(g_u,m);
return true;
} catch (exception& ex){
cerr << "get_bounds_info failed: " << ex.what() << endl;
return false;
}
}
void DirectMultipleShootingInternal::setOptimalSolution(const vector<double> &V_opt){
// OCP solution
Matrix<double> &p_opt = output(OCP_P_OPT);
Matrix<double> &x_opt = output(OCP_X_OPT);
Matrix<double> &u_opt = output(OCP_U_OPT);
// Running index
int el=0;
// Pass optimized state
for(int i=0; i<np_; ++i){
p_opt(i) = V_opt[el++];
}
for(int k=0; k<nk_; ++k){
// Pass optimized state
for(int i=0; i<nx_; ++i){
x_opt(i,k) = V_opt[el++];
}
// Pass optimized control
for(int i=0; i<nu_; ++i){
u_opt(i,k) = V_opt[el++];
}
}
// Pass optimized terminal state
for(int i=0; i<nx_; ++i){
x_opt(i,nk_) = V_opt[el++];
}
casadi_assert(el==V_opt.size());
}
CRSSparsity CSparseCholeskyInternal::getFactorizationSparsity() const {
casadi_assert(S_);
int n = AT_.n;
int nzmax = S_->cp[n];
std::vector< int > col(n+1);
std::copy(S_->cp,S_->cp+n+1,col.begin());
std::vector< int > rowind(nzmax);
int *Li = &rowind.front();
int *Lp = &col.front();
const cs* C;
C = S_->pinv ? cs_symperm (&AT_, S_->pinv, 1) : &AT_;
std::vector< int > temp(2*n);
int *c = & temp.front();
int *s = c+n;
for (int k = 0 ; k < n ; k++) c [k] = S_->cp [k] ;
for (int k = 0 ; k < n ; k++) { /* compute L(k,:) for L*L' = C */
int top = cs_ereach (C, k, S_->parent, s, c) ;
for ( ; top < n ; top++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */
{
int i = s [top] ; /* s [top..n-1] is pattern of L(k,:) */
int p = c [i]++ ;
Li [p] = k ; /* store L(k,i) in column i */
}
int p = c [k]++ ;
Li [p] = k ;
}
Lp [n] = S_->cp [n] ;
return trans(CRSSparsity(n, n, rowind, col));
}
bool bad_test4(){
// This will fail
casadi_assert(bad_test3());
// Returns true, but the code won't reach this place
return true;
}
void DirectMultipleShootingInternal::getGuess(vector<double>& V_init) const{
// OCP solution guess
const Matrix<double> &p_init = input(OCP_P_INIT);
const Matrix<double> &x_init = input(OCP_X_INIT);
const Matrix<double> &u_init = input(OCP_U_INIT);
// Running index
int el=0;
// Pass guess for parameters
for(int i=0; i<np_; ++i){
V_init[el++] = p_init.elem(i);
}
for(int k=0; k<nk_; ++k){
// Pass guess for state
for(int i=0; i<nx_; ++i){
V_init[el++] = x_init.elem(i,k);
}
// Pass guess for control
for(int i=0; i<nu_; ++i){
V_init[el++] = u_init.elem(i,k);
}
}
// Pass guess for final state
for(int i=0; i<nx_; ++i){
V_init[el++] = x_init.elem(i,nk_);
}
casadi_assert(el==V_init.size());
}
void FixedStepIntegrator::integrateB(double t_out) {
// Get discrete time sought
int k_out = std::floor((t_out-t0_)/h_);
k_out = std::max(k_out, 0); // make sure that rounding errors does not result in k_out>nk_
casadi_assert(k_out<=nk_);
// Explicit discrete time dynamics
Function& G = getExplicitB();
// Take time steps until end time has been reached
while (k_>k_out) {
// Advance time
k_--;
t_ = t0_ + k_*h_;
// Take step
G.input(RDAE_T).set(t_);
G.input(RDAE_X).setNZ(x_tape_.at(k_));
G.input(RDAE_Z).setNZ(Z_tape_.at(k_));
G.input(RDAE_P).set(input(INTEGRATOR_P));
G.input(RDAE_RX).set(output(INTEGRATOR_RXF));
G.input(RDAE_RZ).set(RZ_);
G.input(RDAE_RP).set(input(INTEGRATOR_RP));
G.evaluate();
G.output(RDAE_ODE).get(output(INTEGRATOR_RXF));
G.output(RDAE_ALG).get(RZ_);
copy(RZ_.end()-nrz_, RZ_.end(), output(INTEGRATOR_RZF).begin());
transform(G.output(RDAE_QUAD).begin(),
G.output(RDAE_QUAD).end(),
output(INTEGRATOR_RQF).begin(),
output(INTEGRATOR_RQF).begin(),
std::plus<double>());
}
}
void DirectSingleShootingInternal::setOptimalSolution(const vector<double> &V_opt){
// OCP solution
Matrix<double> &p_opt = output(OCP_P_OPT);
Matrix<double> &x_opt = output(OCP_X_OPT);
Matrix<double> &u_opt = output(OCP_U_OPT);
// Running index
int el=0;
// Pass optimized parameters
for(int i=0; i<np_; ++i){
p_opt(i) = V_opt[el++];
}
// Pass optimized initial state
for(int i=0; i<nx_; ++i){
x_opt(i,0) = V_opt[el++];
}
// Pass optimized control
for(int k=0; k<nk_; ++k){
for(int i=0; i<nu_; ++i){
u_opt(i,k) = V_opt[el++];
}
}
casadi_assert(el==V_opt.size());
// Evaluate the constraint function to get the rest of the state trajectory
G_.setInput(V_opt);
G_.evaluate();
const vector<double>& g_opt = G_.output().data();
// Loop over the constraints
el = 0;
for(int k=0; k<nk_; ++k){
// Get the state trajectory
for(int i=0; i<nx_; ++i){
x_opt(i,k+1) = g_opt[el++];
}
// Skip the path constraints (for now)
el += nh_;
}
casadi_assert(el==g_opt.size());
}
std::string CodeGenerator::workelement(int n) {
casadi_assert(n>=0);
if (n==0) {
return "*w";
} else {
return "w[+" + numToString(n) + "]";
}
}
void SXFunctionInternal::spEvaluateOpenCL(bool fwd) {
// OpenCL return flag
cl_int ret;
// Select a kernel
cl_kernel kernel = fwd ? sp_fwd_kernel_ : sp_adj_kernel_;
// Set OpenCL Kernel Parameters
int kernel_arg = 0;
// Pass inputs
for (int i=0; i<nIn(); ++i) {
ret = clSetKernelArg(kernel, kernel_arg++,
sizeof(cl_mem), static_cast<void *>(&sp_input_memobj_[i]));
casadi_assert(ret == CL_SUCCESS);
}
// Pass outputs
for (int i=0; i<nOut(); ++i) {
ret = clSetKernelArg(kernel, kernel_arg++,
sizeof(cl_mem), static_cast<void *>(&sp_output_memobj_[i]));
casadi_assert(ret == CL_SUCCESS);
}
// Execute OpenCL Kernel
executeKernel(kernel);
// Get inputs
for (int i=0; i<sp_input_memobj_.size(); ++i) {
ret = clEnqueueReadBuffer(sparsity_propagation_kernel_.command_queue,
sp_input_memobj_[i], CL_TRUE, 0,
inputNoCheck(i).size() * sizeof(cl_ulong),
reinterpret_cast<void*>(inputNoCheck(i).ptr()), 0, NULL, NULL);
casadi_assert(ret == CL_SUCCESS);
}
// Get outputs
for (int i=0; i<sp_output_memobj_.size(); ++i) {
ret = clEnqueueReadBuffer(sparsity_propagation_kernel_.command_queue,
sp_output_memobj_[i], CL_TRUE, 0,
outputNoCheck(i).size() * sizeof(cl_ulong),
reinterpret_cast<void*>(outputNoCheck(i).ptr()), 0, NULL, NULL);
casadi_assert(ret == CL_SUCCESS);
}
}
MX SymbolicMX::join_primitives(std::vector<MX>::const_iterator& it) const {
MX ret = *it++;
if (ret.size()==size()) {
return ret;
} else {
casadi_assert(ret.is_empty(true));
return MX(size());
}
}
IOSchemeCustomInternal::IOSchemeCustomInternal(const std::vector<std::string> &entries,
const std::vector<std::string> &descriptions) :
entries_(entries), descriptions_(descriptions) {
for (int i=0;i<entries.size();++i) {
entrymap_[entries[i]] = i;
}
if (descriptions_.empty()) descriptions_.resize(entries.size());
casadi_assert(descriptions_.size()==entries.size());
}
SwitchInternal::SwitchInternal(const std::vector<Function>& f, const Function& f_def)
: f_(f), f_def_(f_def) {
// Consitency check
casadi_assert(!f_.empty());
// Give a name
setOption("name", "unnamed_switch");
}
std::vector<Sparsity>
LrDpleInternal::getSparsity(const std::map<std::string, std::vector<Sparsity> >& st,
const std::vector< std::vector<int> > &Hs_) {
// Chop-up the arguments
std::vector<Sparsity> As, Vs, Cs, Hs;
if (st.count("a")) As = st.at("a");
if (st.count("v")) Vs = st.at("v");
if (st.count("c")) Cs = st.at("c");
if (st.count("h")) Hs = st.at("h");
bool with_H = !Hs.empty();
int K = As.size();
Sparsity A;
if (K==1) {
A = As[0];
} else {
Sparsity AL = diagcat(vector_slice(As, range(As.size()-1)));
Sparsity AL2 = horzcat(AL, Sparsity(AL.size1(), As[0].size2()));
Sparsity AT = horzcat(Sparsity(As[0].size1(), AL.size2()), As.back());
A = vertcat(AT, AL2);
}
Sparsity V = diagcat(Vs.back(), diagcat(vector_slice(Vs, range(Vs.size()-1))));
Sparsity C;
if (!Cs.empty()) {
C = diagcat(Cs.back(), diagcat(vector_slice(Cs, range(Cs.size()-1))));
}
Sparsity H;
std::vector<int> Hs_agg;
std::vector<int> Hi(1, 0);
if (Hs_.size()>0 && with_H) {
H = diagcat(Hs.back(), diagcat(vector_slice(Hs, range(Hs.size()-1))));
casadi_assert(K==Hs_.size());
for (int k=0;k<K;++k) {
Hs_agg.insert(Hs_agg.end(), Hs_[k].begin(), Hs_[k].end());
int sum=0;
for (int i=0;i<Hs_[k].size();++i) {
sum+= Hs_[k][i];
}
Hi.push_back(Hi.back()+sum);
}
}
Sparsity res = LrDleInternal::getSparsity(make_map("a", A, "v", V, "c", C, "h", H), Hs_agg);
if (with_H) {
return diagsplit(res, Hi);
} else {
return diagsplit(res, As[0].size2());
}
}
Vertcat::Vertcat(const std::vector<MX>& x) : Concat(x) {
// Construct the sparsity
casadi_assert(!x.empty());
Sparsity sp = x.front().sparsity();
for (vector<MX>::const_iterator i=x.begin()+1; i!=x.end(); ++i) {
sp.append(i->sparsity());
}
setSparsity(sp);
}
Diagcat::Diagcat(const std::vector<MX>& x) : Concat(x) {
// Construct the sparsity
casadi_assert(!x.empty());
std::vector<Sparsity> sp;
for (int i=0;i<x.size(); ++i) {
sp.push_back(x[i].sparsity());
}
setSparsity(blkdiag(sp));
}
std::vector<Sparsity> horzsplit(const Sparsity& sp, const std::vector<int>& offset){
// Consistency check
casadi_assert(offset.size()>=1);
casadi_assert(offset.front()==0);
casadi_assert_message(offset.back()==sp.size2(),"horzsplit(Sparsity,std::vector<int>): Last elements of offset (" << offset.back() << ") must equal the number of columns (" << sp.size2() << ")");
casadi_assert(isMonotone(offset));
// Number of outputs
int n = offset.size()-1;
// Get the sparsity of the input
const vector<int>& colind_x = sp.colind();
const vector<int>& row_x = sp.row();
// Allocate result
std::vector<Sparsity> ret;
ret.reserve(n);
// Sparsity pattern as CCS vectors
vector<int> colind, row;
int ncol, nrow = sp.size1();
// Get the sparsity patterns of the outputs
for(int i=0; i<n; ++i){
int first_col = offset[i];
int last_col = offset[i+1];
ncol = last_col - first_col;
// Construct the sparsity pattern
colind.resize(ncol+1);
copy(colind_x.begin()+first_col, colind_x.begin()+last_col+1, colind.begin());
for(vector<int>::iterator it=colind.begin()+1; it!=colind.end(); ++it) *it -= colind[0];
colind[0] = 0;
row.resize(colind.back());
copy(row_x.begin()+colind_x[first_col],row_x.begin()+colind_x[last_col],row.begin());
// Append to the list
ret.push_back(Sparsity(nrow,ncol,colind,row));
}
// Return (RVO)
return ret;
}
HorzRepsum::HorzRepsum(const MX& x, int n) : n_(n) {
casadi_assert(x.size2() % n == 0);
std::vector<Sparsity> sp = horzsplit(x.sparsity(), x.size2()/n);
Sparsity block = sp[0];
for (int i=1;i<sp.size();++i) {
block = block+sp[i];
}
Sparsity goal = repmat(block, 1, n);
setDependencies(project(x, goal));
setSparsity(block);
}
int GslInternal::rhs_wrapper(double t, const double y[], double f[], void *userdata) {
try{
casadi_assert(userdata);
GslInternal *this_ = (GslInternal*)userdata;
this_->rhs(t,y,f);
return 0;
} catch(exception& e){
cerr << "fun failed: " << e.what() << endl;;
return 1;
}
}
int GslInternal::jac_wrapper(double t, const double y[], double *dfdy, double dfdt[], void *userdata) {
try{
casadi_assert(userdata);
GslInternal *this_ = (GslInternal*)userdata;
this_->jac(t,y,dfdy,dfdt);
return 0;
} catch(exception& e){
cerr << "jac failed: " << e.what() << endl;;
return 1;
}
}
std::vector<int> IndexList::getAll(int len) const {
if (type == INT) {
if (i<0) return std::vector<int>(1,i+len);
return std::vector<int>(1,i);
} else if (type == IVECTOR) {
return iv;
} else {
casadi_assert(type == SLICE);
return slice.getAll(len);
}
}
void Concat::generateOperation(std::ostream &stream, const std::vector<std::string>& arg,
const std::vector<std::string>& res, CodeGenerator& gen) const {
int nz_offset = 0;
for (int i=0; i<arg.size(); ++i) {
int nz = dep(i).size();
stream << " for (i=0; i<" << nz << "; ++i) " << res.front() << "[i+" << nz_offset
<< "] = " << arg.at(i) << "[i];" << endl;
nz_offset += nz;
}
casadi_assert(nz_offset == size());
}
void CSparseCholeskyInternal::solve(double* x, int nrhs, bool transpose) {
casadi_assert(prepared_);
casadi_assert(L_!=0);
double *t = &temp_.front();
for (int k=0; k<nrhs; ++k) {
if (transpose) {
cs_pvec(S_->q, x, t, AT_.n) ; // t = P1\b
cs_ltsolve(L_->L, t) ; // t = L\t
cs_lsolve(L_->L, t) ; // t = U\t
cs_pvec(L_->pinv, t, x, AT_.n) ; // x = P2\t
} else {
cs_ipvec(L_->pinv, x, t, AT_.n) ; // t = P1\b
cs_lsolve(L_->L, t) ; // t = L\t
cs_ltsolve(L_->L, t) ; // t = U\t
cs_ipvec(S_->q, t, x, AT_.n) ; // x = P2\t
}
x += ncol();
}
}
void Wrapper<Derived>::checkDimensions() {
Derived* d = static_cast<Derived*>(this);
// Check number of inputs/outputs
casadi_assert(d->nIn()==f_.nIn());
casadi_assert(d->nOut()==f_.nOut());
// Check sparsities of inputs/outputs
for (int i=0;i< d->nIn();++i) {
casadi_assert_message(d->input(i).sparsity()==f_.input(i).sparsity(),
"Sparsity mismatch for input " << i << ":" <<
d->input(i).dimString() << " <-> " << f_.input(i).dimString() << ".");
}
for (int i=0;i< d->nOut();++i) {
casadi_assert_message(d->output(i).sparsity()==f_.output(i).sparsity(),
"Sparsity mismatch for output " << i << ":" <<
d->output(i).dimString() << " <-> " << f_.output(i).dimString() << ".");
}
}
void CollocationIntegratorInternal::calculateInitialConditionsB() {
vector<double>::const_iterator rx0_it = input(INTEGRATOR_RX0).begin();
vector<double>::const_iterator rz_it = input(INTEGRATOR_RZ0).begin();
vector<double>::iterator RZ_it = RZ_.begin();
for (int d=0; d<deg_; ++d) {
copy(rx0_it, rx0_it+nrx_, RZ_it);
RZ_it += nrx_;
copy(rz_it, rz_it+nrz_, RZ_it);
RZ_it += nrz_;
}
casadi_assert(RZ_it==RZ_.end());
}
void CollocationIntegratorInternal::calculateInitialConditions() {
vector<double>::const_iterator x0_it = input(INTEGRATOR_X0).begin();
vector<double>::const_iterator z_it = input(INTEGRATOR_Z0).begin();
vector<double>::iterator Z_it = Z_.begin();
for (int d=0; d<deg_; ++d) {
copy(x0_it, x0_it+nx_, Z_it);
Z_it += nx_;
copy(z_it, z_it+nz_, Z_it);
Z_it += nz_;
}
casadi_assert(Z_it==Z_.end());
}
SparsityPropagationKernel::SparsityPropagationKernel() {
device_id = 0;
context = 0;
command_queue = 0;
platform_id = 0;
cl_int ret;
// Get Platform and Device Info
ret = clGetPlatformIDs(1, &platform_id, &ret_num_platforms);
casadi_assert(ret == CL_SUCCESS);
ret = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, &device_id, &ret_num_devices);
casadi_assert(ret == CL_SUCCESS);
// Create OpenCL context
context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &ret);
casadi_assert(ret == CL_SUCCESS);
// Create Command Queue
command_queue = clCreateCommandQueue(context, device_id, 0, &ret);
casadi_assert(ret == CL_SUCCESS);
}
void readAttribute(const std::string& attribute_name, T& val,
bool assert_existance=true) const {
// find the attribute
std::map<std::string, std::string>::const_iterator it = attributes_.find(attribute_name);
// check if the attribute exists
if (it == attributes_.end()) {
casadi_assert(!assert_existance,
"Error in XmlNode::readAttribute: could not find " + attribute_name);
} else {
readString(it->second, val);
}
}
