1 /* 2 * This file is part of CasADi. 3 * 4 * CasADi -- A symbolic framework for dynamic optimization. 5 * Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, 6 * K.U. Leuven. All rights reserved. 7 * Copyright (C) 2011-2014 Greg Horn 8 * 9 * CasADi is free software; you can redistribute it and/or 10 * modify it under the terms of the GNU Lesser General Public 11 * License as published by the Free Software Foundation; either 12 * version 3 of the License, or (at your option) any later version. 13 * 14 * CasADi is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 * Lesser General Public License for more details. 18 * 19 * You should have received a copy of the GNU Lesser General Public 20 * License along with CasADi; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 */ 24 25 26 #include "sqic_interface.hpp" 27 #include "casadi/core/conic.hpp" 28 #include "casadi/core/mx_function.hpp" 29 #include "casadi/core/casadi_misc.hpp" 30 31 #include "wsqic.hpp" 32 #include "casadi/interfaces/sqic/resource_sqic.hpp" 33 34 using namespace std; 35 namespace casadi { 36 37 extern "C" 38 int CASADI_CONIC_SQIC_EXPORT casadi_register_conic_sqic(Conic::Plugin * plugin)39 casadi_register_conic_sqic(Conic::Plugin* plugin) { 40 plugin->creator = SqicInterface::creator; 41 plugin->name = "sqic"; 42 plugin->doc = SqicInterface::meta_doc.c_str(); 43 plugin->version = CASADI_VERSION; 44 plugin->options = &SqicInterface::options_; 45 return 0; 46 } 47 48 extern "C" casadi_load_conic_sqic()49 void CASADI_CONIC_SQIC_EXPORT casadi_load_conic_sqic() { 50 Conic::registerPlugin(casadi_register_conic_sqic); 51 } 52 SqicInterface(const std::map<std::string,Sparsity> & st)53 SqicInterface::SqicInterface(const std::map<std::string, Sparsity>& st) : Conic(st) { 54 is_init_ = false; 55 } 56 ~SqicInterface()57 SqicInterface::~SqicInterface() { 58 sqicDestroy(); 59 } 60 evaluate()61 void SqicInterface::evaluate() { 62 if (inputs_check_) { 63 check_inputs(input(CONIC_LBX).ptr(), input(CONIC_UBX).ptr(), 64 input(CONIC_LBA).ptr(), input(CONIC_UBA).ptr()); 65 } 66 67 std::copy(input(CONIC_X0)->begin(), input(CONIC_X0)->end(), x_.begin()); 68 std::fill(x_.begin()+n_, x_.end(), 0); 69 70 std::transform(input(CONIC_LAM_X0)->begin(), input(CONIC_LAM_X0)->end(), 71 rc_.begin(), negate<double>()); 72 std::fill(rc_.begin()+n_, rc_.end(), 0); 73 74 std::copy(input(CONIC_LBX)->begin(), input(CONIC_LBX)->end(), bl_.begin()); 75 std::copy(input(CONIC_UBX)->begin(), input(CONIC_UBX)->end(), bu_.begin()); 76 77 std::copy(input(CONIC_LBA)->begin(), input(CONIC_LBA)->end(), bl_.begin()+n_); 78 std::copy(input(CONIC_UBA)->begin(), input(CONIC_UBA)->end(), bu_.begin()+n_); 79 80 for (casadi_int i=0;i<n_+nc_+1;++i) { 81 if (bl_[i]==-std::numeric_limits<double>::infinity()) bl_[i]=-inf_; 82 if (bu_[i]==std::numeric_limits<double>::infinity()) bu_[i]=inf_; 83 } 84 85 formatA_.setInput(input(CONIC_A), 0); 86 formatA_.setInput(input(CONIC_G), 1); 87 formatA_.evaluate(); 88 89 sqicSolve(&output(CONIC_COST).nonzeros()[0]); 90 91 std::copy(x_.begin(), x_.begin()+n_, output(CONIC_X)->begin()); 92 std::transform(rc_.begin(), rc_.begin()+n_, output(CONIC_LAM_X)->begin(), 93 negate<double>()); 94 std::transform(rc_.begin()+n_, rc_.begin()+n_+nc_, output(CONIC_LAM_A)->begin(), 95 negate<double>()); 96 97 output(CONIC_COST)[0]+= x_[n_+nc_]; 98 } 99 init()100 void SqicInterface::init() { 101 // Call the init method of the base class 102 Conic::init(); 103 104 if (is_init_) sqicDestroy(); 105 106 inf_ = 1.0e+20; 107 108 // Allocate data structures for SQIC 109 bl_.resize(n_+nc_+1, 0); 110 bu_.resize(n_+nc_+1, 0); 111 x_.resize(n_+nc_+1, 0); 112 hs_.resize(n_+nc_+1, 0); 113 hEtype_.resize(n_+nc_+1, 0); 114 pi_.resize(nc_+1, 0); 115 rc_.resize(n_+nc_+1, 0); 116 117 locH_ = st_[QP_STRUCT_H].colind(); 118 indH_ = st_[QP_STRUCT_H].row(); 119 120 // Fortran indices are one-based 121 for (casadi_int i=0;i<indH_.size();++i) indH_[i]+=1; 122 for (casadi_int i=0;i<locH_.size();++i) locH_[i]+=1; 123 124 // Sparsity of augmented linear constraint matrix 125 Sparsity A_ = vertcat(st_[QP_STRUCT_A], Sparsity::dense(1, n_)); 126 locA_ = A_.colind(); 127 indA_ = A_.row(); 128 129 // Fortran indices are one-based 130 for (casadi_int i=0;i<indA_.size();++i) indA_[i]+=1; 131 for (casadi_int i=0;i<locA_.size();++i) locA_[i]+=1; 132 133 // helper functions for augmented linear constraint matrix 134 MX a = MX::sym("A", st_[QP_STRUCT_A]); 135 MX g = MX::sym("g", n_); 136 std::vector<MX> ins; 137 ins.push_back(a); 138 ins.push_back(g); 139 formatA_ = Function("formatA", ins, vertcat(a, g.T())); 140 141 // Set objective row of augmented linear constraints 142 bu_[n_+nc_] = inf_; 143 bl_[n_+nc_] = -inf_; 144 145 is_init_ = true; 146 147 casadi_int n = n_; 148 casadi_int m = nc_+1; 149 150 casadi_int nnzA=formatA_.size_out(0); 151 casadi_int nnzH=input(CONIC_H).size(); 152 153 std::fill(hEtype_.begin()+n_, hEtype_.end(), 3); 154 155 sqic(&m , &n, &nnzA, &indA_[0], &locA_[0], &formatA_.output().nonzeros()[0], &bl_[0], &bu_[0], 156 &hEtype_[0], &hs_[0], &x_[0], &pi_[0], &rc_[0], &nnzH, &indH_[0], &locH_[0], 157 &input(CONIC_H).nonzeros()[0]); 158 159 } 160 calc_flagmap()161 std::map<casadi_int, string> SqicInterface::calc_flagmap() { 162 std::map<casadi_int, string> f; 163 164 return f; 165 } 166 167 std::map<casadi_int, string> SqicInterface::flagmap = SqicInterface::calc_flagmap(); 168 sqic_error(const string & module,casadi_int flag)169 void SqicInterface::sqic_error(const string& module, casadi_int flag) { 170 // Find the error 171 std::map<casadi_int, string>::const_iterator it = flagmap.find(flag); 172 173 stringstream ss; 174 if (it == flagmap.end()) { 175 ss << "Unknown error (" << flag << ") from module \"" << module << "\"."; 176 } else { 177 ss << "Module \"" << module << "\" returned flag \"" << it->second << "\"."; 178 } 179 ss << " Consult SQIC documentation."; 180 casadi_error(ss.str()); 181 } 182 generateNativeCode(std::ostream & file) const183 void SqicInterface::generateNativeCode(std::ostream& file) const { 184 // Dump the contents of resource_sqic, but filter out the C bind stuff 185 std::string resource_sqic_input(resource_sqic); 186 std::istringstream stream(resource_sqic_input); 187 std::string line; 188 while (std::getline(stream, line)) { 189 size_t b_i = line.find("bind ( C, "); 190 if (b_i!=std::string::npos) { 191 file << line.substr(0, b_i) << std::endl; 192 } else { 193 file << line << std::endl; 194 } 195 } 196 197 file.precision(std::numeric_limits<double>::digits10+2); 198 file << std::scientific; // This is really only to force a decimal dot, 199 // would be better if it can be avoided 200 201 file << "program exported" << std::endl; 202 file << " use SQICModule" << std::endl; 203 file << " implicit none" << std::endl; 204 file << " integer(ip) :: m, n, n_inf, nnH, nnzH, nnzA, nS" << std::endl; 205 206 207 file << " real(rp) :: Obj" << std::endl; 208 209 file << " real(rp), allocatable:: bl(:), bu(:), x(:), valA(:), valH(:) , pi(:), rc(:)" 210 << std::endl; 211 file << " integer(ip), allocatable:: indA(:), locA(:), indH(:), locH(:), hEtype(:), hs(:)" 212 << std::endl; 213 214 casadi_int n = n_; 215 casadi_int m = nc_+1; 216 casadi_int nnzA=formatA_.size_out(0); 217 casadi_int nnzH=input(CONIC_H).size(); 218 219 file << " n = " << n << std::endl; 220 file << " m = " << m << std::endl; 221 file << " nnzA = " << nnzA << std::endl; 222 file << " nnzH = " << nnzH << std::endl; 223 224 file << " allocate ( bl(n+m), bu(n+m) )" << std::endl; 225 file << " allocate ( hEtype(n+m) )" << std::endl; 226 file << " allocate ( locA(n+1), valA(nnzA), indA(nnzA) )" << std::endl; 227 file << " allocate ( pi(m), rc(n+m), x(n+m) )" << std::endl; 228 file << " allocate ( hs(n+m) )" << std::endl; 229 file << " allocate ( valH(nnzH), locH(n+1), indH(nnzH) )" << std::endl; 230 231 for (casadi_int i=0;i<indA_.size();++i) { 232 file << " indA(" << i +1 << ") = " << indA_[i] << std::endl; 233 } 234 for (casadi_int i=0;i<locA_.size();++i) { 235 file << " locA(" << i +1 << ") = " << locA_[i] << std::endl; 236 } 237 for (casadi_int i=0;i<formatA_.size_out(0);++i) { 238 file << " valA(" << i +1 << ") = " << formatA_.output().at(i) << std::endl; 239 } 240 for (casadi_int i=0;i<bl_.size();++i) { 241 file << " bl(" << i +1 << ") = " << bl_[i] << std::endl; 242 file << " bu(" << i +1 << ") = " << bu_[i] << std::endl; 243 } 244 for (casadi_int i=0;i<hEtype_.size();++i) { 245 file << " hEtype(" << i +1 << ") = " << hEtype_[i] << std::endl; 246 } 247 for (casadi_int i=0;i<hs_.size();++i) { 248 file << " hs(" << i +1 << ") = " << hs_[i] << std::endl; 249 } 250 for (casadi_int i=0;i<indH_.size();++i) { 251 file << " indH(" << i +1 << ") = " << indH_[i] << std::endl; 252 } 253 for (casadi_int i=0;i<locH_.size();++i) { 254 file << " locH(" << i +1 << ") = " << locH_[i] << std::endl; 255 } 256 for (casadi_int i=0;i<input(CONIC_H).size();++i) { 257 file << " valH(" << i +1 << ") = " << input(CONIC_H).at(i) << std::endl; 258 } 259 for (casadi_int i=0;i<input(CONIC_X0).size();++i) { 260 file << " x(" << i +1 << ") = " << input(CONIC_X0).at(i) << std::endl; 261 } 262 for (casadi_int i=0;i<pi_.size();++i) { 263 file << " pi(" << i +1 << ") = " << 0 << std::endl; //pi_[i] << std::endl; 264 } 265 uout() << "lam_x0:::" << input(CONIC_LAM_X0) << std::endl; 266 for (casadi_int i=0;i<rc_.size();++i) { 267 file << " rc(" << i +1 << ") = " 268 << ((i<input(CONIC_LAM_X0).size()) ? -input(CONIC_LAM_X0).at(i) : 0.0) 269 << std::endl; 270 } 271 272 file << " call wsqic (m, n, nnzA, indA, locA, valA, bl, bu, hEtype, " 273 << "hs, x, pi, rc, nnzH, indH, locH, valH)" << std::endl; 274 /**for (casadi_int i=0;i<input(CONIC_X0).size();++i) { 275 file << " x(" << i +1 << ") = " << input(CONIC_X0).at(i) << std::endl; 276 } 277 for (casadi_int i=0;i<pi_.size();++i) { 278 file << " pi(" << i +1 << ") = " << pi_[i] << std::endl; 279 } 280 uout() << "lam_x0:::" << input(CONIC_LAM_X0) << std::endl; 281 for (casadi_int i=0;i<rc_.size();++i) { 282 file << " rc(" << i +1 << ") = " 283 << ((i<input(CONIC_LAM_X0).size()) ? -input(CONIC_LAM_X0).at(i) : 0.0) 284 << std::endl; 285 }*/ 286 /** 287 file << " call sqicSolve(Obj)" << std::endl; 288 for (casadi_int i=0;i<input(CONIC_X0).size();++i) { 289 file << " x(" << i +1 << ") = " << input(CONIC_X0).at(i) << std::endl; 290 } 291 for (casadi_int i=0;i<pi_.size();++i) { 292 file << " pi(" << i +1 << ") = " << pi_[i] << std::endl; 293 } 294 uout() << "lam_x0:::" << input(CONIC_LAM_X0) << std::endl; 295 for (casadi_int i=0;i<rc_.size();++i) { 296 file << " rc(" << i +1 << ") = " 297 << ((i<input(CONIC_LAM_X0).size()) ? -input(CONIC_LAM_X0).at(i) : 0.0) 298 << std::endl; 299 } 300 */ 301 file << " call sqicSolve(Obj)" << std::endl; 302 file << " deallocate ( bl, bu )" << std::endl; 303 file << " deallocate ( hEtype )" << std::endl; 304 file << " deallocate ( locA, valA, indA )" << std::endl; 305 file << " deallocate ( pi, rc, x )" << std::endl; 306 file << " deallocate ( valH, locH, indH )" << std::endl; 307 file << " call sqicDestroy()" << std::endl; 308 file << "end program exported" << std::endl; 309 310 311 } 312 313 } // namespace casadi 314