1################################################################################# 2## 3## R package Rsolnp by Alexios Ghalanos and Stefan Theussl Copyright (C) 2009-2013 4## This file is part of the R package Rsolnp. 5## 6## The R package Rsolnp is free software: you can redistribute it and/or modify 7## it under the terms of the GNU General Public License as published by 8## the Free Software Foundation, either version 3 of the License, or 9## (at your option) any later version. 10## 11## The R package Rsolnp is distributed in the hope that it will be useful, 12## but WITHOUT ANY WARRANTY; without even the implied warranty of 13## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14## GNU General Public License for more details. 15## 16################################################################################# 17 18# Based on the original subnp Yinyu Ye 19# http://www.stanford.edu/~yyye/Col.html 20.subnp = function(pars, yy, ob, hessv, lambda, vscale, ctrl, .env, ...) 21{ 22 .solnp_fun = get(".solnp_fun", envir = .env) 23 .solnp_eqfun = get(".solnp_eqfun", envir = .env) 24 .solnp_ineqfun = get(".solnp_ineqfun", envir = .env) 25 ineqLB = get(".ineqLB", envir = .env) 26 ineqUB = get(".ineqUB", envir = .env) 27 LB = get(".LB", envir = .env) 28 UB = get(".UB", envir = .env) 29 #.solnp_gradfun = get(".solnp_gradfun", envir = .env) 30 #.solnp_eqjac = get(".solnp_eqjac", envir = .env) 31 #.solnp_ineqjac = get(".solnp_ineqjac", envir = .env) 32 ind = get("ind", envir = .env) 33 34 # pars [nineq + np] 35 rho = ctrl[ 1 ] 36 maxit = ctrl[ 2 ] 37 delta = ctrl[ 3 ] 38 tol = ctrl[ 4 ] 39 trace = ctrl[ 5 ] 40 # [1] length of pars 41 # [2] has function gradient? 42 # [3] has hessian? 43 # [4] has ineq? 44 # [5] ineq length 45 # [6] has jacobian (inequality) 46 # [7] has eq? 47 # [8] eq length 48 # [9] has jacobian (equality) 49 # [10] has upper / lower bounds 50 # [11] has either lower/upper bounds or ineq 51 52 53 neq = ind[ 8 ] 54 nineq = ind[ 5 ] 55 np = ind[ 1 ] 56 ch = 1 57 alp = c(0,0,0) 58 nc = neq + nineq 59 npic = np + nineq 60 p0 = pars 61 62 # pb [ 2 x (nineq + np) ] 63 pb = rbind( cbind(ineqLB, ineqUB), cbind(LB,UB) ) 64 sob = numeric() 65 ptt = matrix() 66 sc = numeric() 67 68 # scale the cost, the equality constraints, the inequality constraints, 69 # the parameters (inequality parameters AND actual parameters), 70 # and the parameter bounds if there are any 71 # Also make sure the parameters are no larger than (1-tol) times their bounds 72 # ob [ 1 neq nineq] 73 74 ob = ob / vscale[ 1:(nc + 1) ] 75 # p0 [np] 76 p0 = p0 / vscale[ (neq + 2):(nc + np + 1) ] 77 if( ind[ 11 ] ) { 78 79 if( !ind[ 10 ] ) { 80 mm = nineq 81 } else { 82 mm = npic 83 } 84 85 pb = pb / cbind(vscale[ (neq + 2):(neq + mm + 1) ], vscale[ (neq + 2):(neq + mm + 1) ]) 86 } 87 88 # scale the lagrange multipliers and the Hessian 89 90 if( nc > 0) { 91 # yy [total constraints = nineq + neq] 92 # scale here is [tc] and dot multiplied by yy 93 yy = vscale[ 2:(nc + 1) ] * yy / vscale[ 1 ] 94 } 95 # yy = [zeros 3x1] 96 97 # h is [ (np+nineq) x (np+nineq) ] 98 #columnvector %*% row vector (size h) then dotproduct h then dotdivide scale[1] 99 hessv = hessv * (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1)]) ) / vscale[ 1 ] 100 # h[ 8x8 eye] 101 j = ob[ 1 ] 102 103 if( ind[4] ) { 104 105 if( !ind[7] ) { 106 # [nineq x (nineq+np) ] 107 a = cbind( -diag(nineq), matrix(0, ncol = np, nrow = nineq) ) 108 } else { 109 # [ (neq+nineq) x (nineq+np)] 110 a = rbind( cbind( 0 * .ones(neq, nineq), matrix(0, ncol = np, nrow = neq) ), 111 cbind( -diag(nineq), matrix(0, ncol = np, nrow = nineq) ) ) 112 } 113 114 } 115 if( ind[7] && !ind[4] ) { 116 a = .zeros(neq, np) 117 } 118 119 if( !ind[7] && !ind[4] ) { 120 a = .zeros(1, np) 121 } 122 123 # gradient 124 g= 0 * .ones(npic, 1) 125 p = p0 [ 1:npic ] 126 if( nc > 0 ) { 127 # [ nc ] 128 constraint = ob[ 2:(nc + 1) ] 129 # constraint [5 0 11 3x1] 130 # gradient routine 131 for( i in 1:np ) { 132 # scale the parameters (non ineq) 133 p0[ nineq + i ] = p0[ nineq + i ] + delta 134 tmpv = p0[ (nineq + 1):npic ] * vscale[ (nc + 2):(nc + np + 1) ] 135 funv = .safefunx(tmpv, .solnp_fun, .env, ...) 136 eqv = .solnp_eqfun(tmpv, ...) 137 ineqv = .solnp_ineqfun(tmpv, ...) 138 ctmp = get(".solnp_nfn", envir = .env) 139 assign(".solnp_nfn", ctmp + 1, envir = .env) 140 141 ob = c(funv, eqv, ineqv) / vscale[ 1:(nc + 1) ] 142 g[ nineq + i ] = (ob[ 1 ] - j) / delta 143 a[ , nineq + i ] = (ob[ 2:(nc + 1) ] - constraint) / delta 144 p0[ nineq + i ] = p0[ nineq + i ] - delta 145 } 146 147 if( ind[4] ) { 148 constraint[ (neq + 1):(neq + nineq) ] = constraint[ (neq + 1):(neq + nineq) ] - p0[ 1:nineq ] 149 } 150 151 # solver messages 152 if( .solvecond(a) > 1 / .eps ) { 153 if( trace ) .subnpmsg( "m1" ) 154 } 155 156 # a(matrix) x columnvector - columnvector 157 # b [nc,1] 158 b = a %*% p0 - constraint 159 ch = -1 160 alp[ 1 ] = tol - max( abs(constraint) ) 161 162 if( alp[ 1 ] <= 0 ) { 163 ch = 1 164 165 if( !ind[11] ) { 166 # a %*% t(a) gives [nc x nc] 167 # t(a) %*% above gives [(np+nc) x 1] 168 p0 = p0 - t(a) %*% solve(a %*% t(a), constraint) 169 alp[ 1 ] = 1 170 } 171 172 } 173 174 if( alp[ 1 ] <= 0 ) { 175 # this expands p0 to [nc+np+1] 176 p0[ npic + 1 ] = 1 177 a = cbind(a, -constraint) 178 # cx is rowvector 179 cx = cbind(.zeros(1,npic), 1) 180 dx = .ones(npic + 1, 1) 181 go = 1 182 minit = 0 183 184 while( go >= tol ) { 185 minit = minit + 1 186 # gap [(nc + np) x 2] 187 gap = cbind(p0[ 1:mm ] - pb[ , 1 ], pb[ , 2 ] - p0[ 1:mm ] ) 188 # this sorts every row 189 gap = t( apply( gap, 1, FUN=function( x ) sort(x) ) ) 190 dx[ 1:mm ] = gap[ , 1 ] 191 # expand dx by 1 192 dx[ npic + 1 ] = p0[ npic + 1 ] 193 194 if( !ind[10] ) { 195 dx[ (mm + 1):npic ] = max( c(dx[ 1:mm ], 100) ) * .ones(npic - mm, 1) 196 } 197 # t( a %*% diag( as.numeric(dx) ) ) gives [(np+nc + 1 (or more) x nc] 198 # dx * t(cx) dot product of columnvectors 199 # qr.solve returns [nc x 1] 200 201 # TODO: Catch errors here 202 y = try( qr.solve( t( a %*% diag( as.numeric(dx) , length(dx), length(dx) ) ), dx * t(cx) ), silent = TRUE) 203 if(inherits(y, "try-error")){ 204 p = p0 * vscale[ (neq + 2):(nc + np + 1) ] # unscale the parameter vector 205 if( nc > 0 ) { 206 y = 0 # unscale the lagrange multipliers 207 } 208 hessv = vscale[ 1 ] * hessv / (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1) ]) ) 209 ans = list(p = p, y = y, hessv = hessv, lambda = lambda) 210 assign(".solnp_errors", 1, envir = .env) 211 return(ans) 212 } 213 v = dx * ( dx *(t(cx) - t(a) %*% y) ) 214 215 if( v[ npic + 1 ] > 0 ) { 216 z = p0[ npic + 1 ] / v[ npic + 1 ] 217 218 for( i in 1:mm ) { 219 220 if( v[ i ] < 0 ) { 221 z = min(z, -(pb[ i, 2 ] - p0[ i ]) / v[ i ]) 222 } else if( v[ i ] > 0 ) { 223 z = min( z, (p0[ i ] - pb[ i , 1 ]) / v[ i ]) 224 } 225 } 226 227 if( z >= p0[ npic + 1 ] / v[ npic + 1 ] ) { 228 p0 = p0 - z * v 229 } else { 230 p0 = p0 - 0.9 * z * v 231 } 232 go = p0[ npic + 1 ] 233 234 if( minit >= 10 ) { 235 go = 0 236 } 237 238 } else { 239 go = 0 240 minit = 10 241 } 242 243 } 244 245 if( minit >= 10 ) { 246 if( trace ) .subnpmsg( "m2" ) 247 } 248 249 a = matrix(a[ , 1:npic ], ncol = npic) 250 b = a %*% p0[ 1:npic ] 251 } 252 253 } 254 255 p = p0 [ 1:npic ] 256 y = 0 257 258 if( ch > 0 ) { 259 260 tmpv = p[ (nineq + 1):npic ] * vscale[ (nc + 2):(nc + np + 1) ] 261 funv = .safefunx(tmpv, .solnp_fun, .env,...) 262 eqv = .solnp_eqfun(tmpv, ...) 263 ineqv = .solnp_ineqfun(tmpv, ...) 264 ctmp = get(".solnp_nfn", envir = .env) 265 assign(".solnp_nfn", ctmp + 1, envir = .env) 266 ob = c(funv, eqv, ineqv) / vscale[ 1:(nc + 1) ] 267 } 268 269 j = ob[ 1 ] 270 271 if( ind[4] ) { 272 ob[ (neq + 2):(nc + 1) ] = ob[ (neq + 2):(nc + 1) ] - p[ 1:nineq ] 273 274 } 275 276 if( nc > 0 ) { 277 ob[ 2:(nc + 1) ] = ob[ 2:(nc + 1) ] - a %*% p + b 278 j = ob[ 1 ] - t(yy) %*% matrix(ob[ 2:(nc + 1) ], ncol=1) + rho * .vnorm(ob[ 2:(nc + 1) ]) ^ 2 279 } 280 281 minit = 0 282 while( minit < maxit ) { 283 minit = minit + 1 284 285 if( ch > 0 ) { 286 287 for( i in 1:np ) { 288 289 p[ nineq + i ] = p[ nineq + i ] + delta 290 tmpv = p[ (nineq + 1):npic ] * vscale[ (nc + 2):(nc + np + 1) ] 291 funv = .safefunx(tmpv, .solnp_fun, .env, ...) 292 eqv = .solnp_eqfun(tmpv, ...) 293 ineqv = .solnp_ineqfun(tmpv, ...) 294 ctmp = get(".solnp_nfn", envir = .env) 295 assign(".solnp_nfn", ctmp + 1, envir = .env) 296 obm = c(funv, eqv, ineqv) / vscale[ 1:(nc + 1) ] 297 298 if( ind[4] ) { 299 obm[ (neq + 2):(nc + 1)] = obm[ (neq + 2):(nc + 1) ] - p[ 1:nineq ] 300 } 301 302 if( nc > 0 ) { 303 304 obm[ 2:(nc + 1) ] = obm[ 2:(nc + 1) ] - a %*% p + b 305 obm = obm[ 1 ] - t(yy) %*% obm[ 2:(nc + 1) ] + rho * .vnorm(obm[ 2:(nc + 1 ) ]) ^ 2 306 } 307 308 g[ nineq + i ] = (obm - j) / delta 309 p[ nineq + i ] = p[ nineq + i ] - delta 310 } 311 312 if( ind[4] ) { 313 g[ 1:nineq ] = 0 314 } 315 316 } 317 318 if( minit > 1 ) { 319 yg = g - yg 320 sx = p - sx 321 sc[ 1 ] = t(sx) %*% hessv %*% sx 322 sc[ 2 ] = t(sx) %*% yg 323 324 if( (sc[ 1 ] * sc[ 2 ]) > 0 ) { 325 sx = hessv %*% sx 326 hessv = hessv - ( sx %*% t(sx) ) / sc[ 1 ] + ( yg %*% t(yg) ) / sc[ 2 ] 327 } 328 329 } 330 331 dx = 0.01 * .ones(npic, 1) 332 if( ind[11] ) { 333 334 gap = cbind(p[ 1:mm ] - pb[ , 1 ], pb[ , 2 ] - p[ 1:mm ]) 335 gap = t( apply( gap, 1, FUN = function( x ) sort(x) ) ) 336 gap = gap[ , 1 ] + sqrt(.eps) * .ones(mm, 1) 337 dx[ 1:mm, 1 ] = .ones(mm, 1) / gap 338 if( !ind[10] ){ 339 dx[ (mm + 1):npic, 1 ] = min (c( dx[ 1:mm, 1 ], 0.01) ) * .ones(npic - mm, 1) 340 } 341 342 } 343 344 go = -1 345 lambda = lambda / 10 346 while( go <= 0 ) { 347 cz = try(chol( hessv + lambda * diag( as.numeric(dx * dx), length(dx), length(dx) ) ), silent = TRUE) 348 if(inherits(cz, "try-error")){ 349 p = p * vscale[ (neq + 2):(nc + np + 1) ] # unscale the parameter vector 350 if( nc > 0 ) { 351 y = 0 # unscale the lagrange multipliers 352 } 353 hessv = vscale[ 1 ] * hessv / (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1) ]) ) 354 ans = list(p = p, y = y, hessv = hessv, lambda = lambda) 355 assign(".solnp_errors", 1, envir = .env) 356 return(ans) 357 } 358 cz = try(solve(cz), silent = TRUE) 359 if(inherits(cz, "try-error")){ 360 p = p * vscale[ (neq + 2):(nc + np + 1) ] # unscale the parameter vector 361 if( nc > 0 ) { 362 y = 0 # unscale the lagrange multipliers 363 } 364 hessv = vscale[ 1 ] * hessv / (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1) ]) ) 365 ans = list(p = p, y = y, hessv = hessv, lambda = lambda) 366 assign(".solnp_errors", 1, envir = .env) 367 return(ans) 368 } 369 yg = t(cz) %*% g 370 371 if( nc == 0 ) { 372 u = -cz %*% yg 373 } else{ 374 y = try( qr.solve(t(cz) %*% t(a), yg), silent = TRUE ) 375 if(inherits(y, "try-error")){ 376 p = p * vscale[ (neq + 2):(nc + np + 1) ] # unscale the parameter vector 377 if( nc > 0 ) { 378 # y = vscale[ 1 ] * y / vscale[ 2:(nc + 1) ] # unscale the lagrange multipliers 379 y = 0 380 } 381 hessv = vscale[ 1 ] * hessv / (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1) ]) ) 382 ans = list(p = p, y = y, hessv = hessv, lambda = lambda) 383 assign(".solnp_errors", 1, envir = .env) 384 return(ans) 385 } 386 387 u = -cz %*% (yg - ( t(cz) %*% t(a) ) %*% y) 388 } 389 390 p0 = u[ 1:npic ] + p 391 if( !ind[ 11 ] ) { 392 go = 1 393 } else { 394 go = min( c(p0[ 1:mm ] - pb[ , 1 ], pb[ , 2 ] - p0[ 1:mm ]) ) 395 lambda = 3 * lambda 396 } 397 398 } 399 400 alp[ 1 ] = 0 401 ob1 = ob 402 ob2 = ob1 403 sob[ 1 ] = j 404 sob[ 2 ] = j 405 ptt = cbind(p, p) 406 alp[ 3 ] = 1.0 407 ptt = cbind(ptt, p0) 408 tmpv = ptt[ (nineq + 1):npic, 3 ] * vscale[ (nc + 2):(nc + np + 1) ] 409 funv = .safefunx(tmpv, .solnp_fun, .env, ...) 410 eqv = .solnp_eqfun(tmpv, ...) 411 ineqv = .solnp_ineqfun(tmpv, ...) 412 ctmp = get(".solnp_nfn", envir = .env) 413 assign(".solnp_nfn", ctmp + 1, envir = .env) 414 415 ob3 = c(funv, eqv, ineqv) / vscale[ 1:(nc + 1) ] 416 sob[ 3 ] = ob3[ 1 ] 417 418 if( ind[4] ) { 419 ob3[ (neq + 2):(nc + 1) ] = ob3[ (neq + 2):(nc + 1) ] - ptt[ 1:nineq, 3 ] 420 } 421 422 if( nc > 0 ) { 423 ob3[ 2:(nc + 1) ] = ob3[ 2:(nc + 1) ] - a %*% ptt[ , 3 ] + b 424 sob[ 3 ] = ob3[ 1 ] - t(yy) %*% ob3[ 2:(nc + 1) ] + rho * .vnorm(ob3[ 2:(nc + 1) ]) ^ 2 425 } 426 427 go = 1 428 while( go > tol ) { 429 alp[ 2 ] = (alp[ 1 ] + alp[ 3 ]) / 2 430 ptt[ , 2 ] = (1 - alp[ 2 ]) * p + alp[ 2 ] * p0 431 tmpv = ptt[ (nineq + 1):npic, 2 ] * vscale[ (nc + 2):(nc + np + 1) ] 432 funv = .safefunx(tmpv, .solnp_fun, .env, ...) 433 eqv = .solnp_eqfun(tmpv, ...) 434 ineqv = .solnp_ineqfun(tmpv, ...) 435 ctmp = get(".solnp_nfn", envir = .env) 436 assign(".solnp_nfn", ctmp + 1, envir = .env) 437 438 ob2 = c(funv, eqv, ineqv) / vscale[ 1:(nc + 1) ] 439 440 sob[ 2 ] = ob2[ 1 ] 441 442 if( ind[4] ) { 443 ob2[ (neq + 2):(nc + 1) ] = ob2[ (neq + 2):(nc + 1) ] - ptt[ 1:nineq , 2 ] 444 } 445 446 if( nc > 0 ) { 447 ob2[ 2:(nc + 1) ] = ob2[ 2:(nc + 1) ] - a %*% ptt[ , 2 ] + b 448 sob[ 2 ] = ob2[ 1 ] - t(yy) %*% ob2[ 2:(nc + 1) ] + rho * .vnorm(ob2[ 2:(nc + 1) ]) ^ 2 449 } 450 451 obm = max(sob) 452 453 if( obm < j ) { 454 obn = min(sob) 455 go = tol * (obm - obn) / (j - obm) 456 } 457 458 condif1 = sob[ 2 ] >= sob[ 1 ] 459 condif2 = sob[ 1 ] <= sob[ 3 ] && sob[ 2 ] < sob[ 1 ] 460 condif3 = sob[ 2 ] < sob[ 1 ] && sob[ 1 ] > sob[ 3 ] 461 462 if( condif1 ) { 463 sob[ 3 ] = sob[ 2 ] 464 ob3 = ob2 465 alp[ 3 ] = alp[ 2 ] 466 ptt[ , 3 ] = ptt[ , 2 ] 467 } 468 469 if( condif2 ) { 470 sob[ 3 ] = sob[ 2 ] 471 ob3 = ob2 472 alp[ 3 ] = alp[ 2 ] 473 ptt[ , 3 ] = ptt[ , 2 ] 474 } 475 476 if( condif3 ) { 477 sob[ 1 ] = sob[ 2 ] 478 ob1 = ob2 479 alp[ 1 ] = alp[ 2 ] 480 ptt[ , 1 ] = ptt[ , 2 ] 481 } 482 483 if( go >= tol ) { 484 go = alp[ 3 ] - alp[ 1 ] 485 } 486 487 } 488 489 sx = p 490 yg = g 491 ch = 1 492 obn = min(sob) 493 if( j <= obn ) { 494 maxit = minit 495 } 496 497 reduce = (j - obn) / ( 1 + abs(j) ) 498 499 if( reduce < tol ) { 500 maxit = minit 501 } 502 503 condif1 = sob[ 1 ] < sob[ 2 ] 504 condif2 = sob[ 3 ] < sob[ 2 ] && sob[ 1 ] >= sob[ 2 ] 505 condif3 = sob[ 1 ] >= sob[ 2 ] && sob[ 3 ] >= sob[ 2 ] 506 507 if( condif1 ) { 508 j = sob[ 1 ] 509 p = ptt[ , 1 ] 510 ob = ob1 511 } 512 513 if( condif2 ) { 514 j = sob [ 3 ] 515 p = ptt[ , 3 ] 516 ob = ob3 517 } 518 519 if( condif3 ) { 520 j = sob[ 2 ] 521 p = ptt[ , 2 ] 522 ob = ob2 523 } 524 525 } 526 527 p = p * vscale[ (neq + 2):(nc + np + 1) ] # unscale the parameter vector 528 529 if( nc > 0 ) { 530 y = vscale[ 1 ] * y / vscale[ 2:(nc + 1) ] # unscale the lagrange multipliers 531 } 532 533 hessv = vscale[ 1 ] * hessv / (vscale[ (neq + 2):(nc + np + 1) ] %*% t(vscale[ (neq + 2):(nc + np + 1) ]) ) 534 if( reduce > tol ) { 535 if( trace ) .subnpmsg( "m3" ) 536 } 537 538 ans = list(p = p, y = y, hessv = hessv, lambda = lambda) 539 return( ans ) 540} 541 542 543.fdgrad = function(i, p, delta, np, vscale, constraint, j, nineq, npic, nc, .solnp_fun, 544 .solnp_eqfun, .solnp_ineqfun, .env, ...) 545{ 546 ans = list() 547 px = p 548 px[ nineq + i ] = px[ nineq + i ] + delta 549 tmpv = px[ (nineq + 1):npic ] * vscale[ (nc + 2):(nc + np + 1) ] 550 funv = .safefunx(tmpv, .solnp_fun, .env, ...) 551 eqv = .solnp_eqfun(tmpv, ...) 552 ineqv = .solnp_ineqfun(tmpv, ...) 553 ctmp = get(".solnp_nfn", envir = .env) 554 assign(".solnp_nfn", ctmp + 1, envir = .env) 555 ob = c(funv, eqv, ineqv) / vscale[ 1:(nc + 1) ] 556 ans$p = px 557 ans$ob = ob 558 ans$g = (ob[ 1 ] - j) / delta 559 ans$a = (ob[ 2:(nc + 1) ] - constraint) / delta 560 return( ans ) 561} 562 563 564