├── .gitignore ├── ChangeLog ├── Makefile ├── Pending ├── README ├── doc ├── Makefile ├── README ├── fraga_splines_01aug03.pdf ├── inanc_splines-16sep03.pdf └── workshop_15sep03.pdf ├── examples ├── Makefile ├── README ├── kincar.c ├── vanderpol.c ├── vanderpol.m ├── vanderpol.maple └── vanderpol.txt ├── exclude.txt ├── npsol ├── .gitignore ├── ChangeLog ├── Makefile └── README ├── pgs ├── Makefile ├── README ├── l2err.f.diff └── l2main.f.diff └── src ├── Makefile ├── av.h ├── colloc.c ├── colloc.h ├── constraints.c ├── constraints.h ├── cost.c ├── cost.h ├── integrator.c ├── integrator.h ├── matrix.c ├── matrix.h ├── ntg.c ├── ntg.h └── options.c /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | 3 | # / 4 | /include 5 | /lib 6 | 7 | # /examples/ 8 | /examples/*.o 9 | /examples/kincar 10 | /examples/vanderpol 11 | /examples/coef1 12 | 13 | # /npsol/ 14 | /npsol/*.f 15 | /npsol/*.o 16 | /npsol/*.a 17 | /npsol/npsol.??? 18 | /npsol/npmain 19 | 20 | # /pgs/ 21 | /pgs/*.f 22 | /pgs/*.o 23 | /pgs/*.a 24 | /pgs/*ex* 25 | /pgs/reinsch* 26 | /pgs/.datafiles 27 | /pgs/.examples 28 | 29 | # /src/ 30 | /src/*.o 31 | /src/*.a 32 | -------------------------------------------------------------------------------- /ChangeLog: -------------------------------------------------------------------------------- 1 | 2013-01-02 Richard Murray 2 | 3 | * README: added instructions for how to get to compile under OS X. 4 | 5 | * src/matrix.c (SubFMatrix, DoubleFMatrix, DoubleMatrix): malloc -> 6 | calloc 7 | 8 | * src/ntg.c (ntg, NPfunobj): malloc -> calloc 9 | 10 | * src/matrix.c (FMatrixMult): fixed initialization error 11 | 12 | * examples/vanderpol.c (main): changed malloc to calloc throughout 13 | 14 | * src/constraints.h, src/constraints.c: moved malloc.h out of this 15 | constraints.h and into individual files 16 | 17 | * src/matrix.c (DoubleFMatrix): replaced malloc + memset with calloc 18 | 19 | * src/colloc.c: changed malloc to calloc throughout 20 | 21 | * src/colloc.c (CollocMult): fixed error in initialization; 22 | B->elements[j][i] was only being set for j = 0. Didn't fix the 23 | problem with examples/vanderpol getting bad values 24 | 25 | * src/constraints.h: include instead of 26 | 27 | * doc/Makefile: empty file to allow 'make clean' to work from main 28 | directory 29 | 30 | * examples/Makefile, src/Makefile, pgs/Makefile, npsol/Makefile: 31 | fixed small errors and removed CC and F77 assignments (use system 32 | version or overwrite on the command line) 33 | 34 | * Changes: moved contents to ChangeLog 35 | 36 | * README: updated instructions to indicate that PGS files are 37 | automatically grabbed using wget 38 | 39 | ----- Old Changes file ---- 40 | 41 | 3 Feb 02, RMM: added READMEs, updated Makefiles 42 | * Updated all README files in main and sub-directories 43 | * Updated makefiles to allow tar file creation + cleanup 44 | * Created CVS archive for all files 45 | * Added NPSOL directory with README, Makefile (no source) 46 | 47 | 1 Feb 02, RMM: updated Makefiles and directory structure 48 | * moved npsol and pgs to root directory 49 | * create src/, lib/, include/ structure 50 | * updated makefiles to install to NTGLIB (default = ..) 51 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Makefile - main makefile for NTG software package 2 | # RMM, 2 Feb 02 3 | 4 | # Desired installation location of main NTG directory 5 | # 6 | # By default, this is set to the current director. This is useful 7 | # if you are a developer, but probably not what you want to use if 8 | # you are installing it for others to use. 9 | # 10 | NTGDIR = $(CURDIR) 11 | # NTGDIR = /usr/local/ntg 12 | 13 | # Files and directories to include in distribution 14 | FILES = README Makefile Pending 15 | DIRS = src/ doc/ examples/ pgs/ npsol/ 16 | 17 | #! TODO: set compiler flags based on operating system 18 | CFLAGS = -g 19 | FFLAGS = -g -fdefault-real-8 -fdefault-double-8 20 | 21 | # Default rule: make everything in the subdirectories 22 | # 23 | # Note that pgs and npsol are not distributed with NTG, so it's OK 24 | # if these are not in place 25 | # 26 | install: build 27 | mkdir -p $(NTGDIR)/lib 28 | mkdir -p $(NTGDIR)/include 29 | (cd src; make NTGDIR=$(NTGDIR) install) 30 | (cd npsol; make NTGDIR=$(NTGDIR) install) 31 | (cd pgs; make NTGDIR=$(NTGDIR) install) 32 | 33 | # build without installing 34 | build: 35 | (cd src; make CFLAGS="$(CFLAGS)") 36 | (cd npsol; make FFLAGS="$(FFLAGS)") 37 | (cd pgs; make FFLAGS="$(FFLAGS)") 38 | 39 | # Compile the examples 40 | examples: 41 | make -C examples 42 | 43 | # Generate a tar file for distribution to others 44 | tar: distclean 45 | tar zcf ntg-dist.tgz -X exclude.txt $(FILES) $(DIRS) 46 | 47 | clean: 48 | (cd src; make clean) 49 | (cd examples; make clean) 50 | (cd pgs; make clean) 51 | (cd npsol; make clean) 52 | (cd doc; make clean) 53 | 54 | distclean: clean 55 | (cd pgs; make distclean) 56 | (cd npsol; make distclean) 57 | 58 | tidy: 59 | rm *~ 60 | 61 | .PHONY: clean examples 62 | -------------------------------------------------------------------------------- /Pending: -------------------------------------------------------------------------------- 1 | Pending file for NTG 2 | 3 | Order Date Item 4 | ----- ---- ---- 5 | Jul'22 Add documentation for kincar 6 | Jul'22 Add command line interface for kincar 7 | Jul'22 Add python interface for kincar (plotting) 8 | Jul'22 Add sphinx documentation for NTG 9 | Jul'22 Add ipopt interface for NTG 10 | Jul'22 Add python interface for NTG 11 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | This directory contains the source code for the NTG software package. 2 | 3 | Version: 2.2.3 4 | Release Date: 27 June 2022 5 | Contact: murray@cds.caltech.edu 6 | 7 | INSTALLATION 8 | ------------ 9 | In order to install NTG, you will need to install two other software 10 | packages that are licensed separately: NPSOL and PGS. 11 | 12 | NPSOL is a numerical optimization library that uses sequential 13 | quadratic programming to solve nonlinear optimization problems. It is 14 | not in the public domain, but licenses are available at low cost for 15 | university and government usage. More information on NPSOL is 16 | available at 17 | 18 | http://www.sbsi-sol-optimize.com/asp/sol_product_npsol.htm 19 | 20 | PGS is the Practical Guide to Splines companion software. It is in the 21 | public domain and is included in the NTG distribution in the pgs/ 22 | subdirectory. By default, the PGS source code will be installed from the 23 | PGS web site using wget. If this doesn't work, you can grab the source code 24 | from one of the following web sites: 25 | 26 | http://pages.cs.wisc.edu/~deboor/pgs/ 27 | http://www.netlib.org/pppack/ (may require some renaming of files) 28 | 29 | This version of NTG has been tested under linux (RH 7.2) and should 30 | work in most vanilla unix environments. 31 | 32 | To install NTG: 33 | 1. Unpack ntg.tar into the directory of your choice 34 | 2. Edit Makefile and make sure everything is pointed where you want 35 | 3. Type 'make" to make all NTG libraries and programs 36 | 4. If everything works, type 'make install' to install the library 37 | and programs 38 | 39 | Example programs are available in the subdirectory 'examples/'. These 40 | programs should compile correctly after you have installed the software. 41 | You will need to edit the makefile to point to the properly directories for 42 | the include files and libraries. You will also need to have the PGS and 43 | NPSOL package installed on your system. See pgs/README for directions on 44 | installing the version of PGS distributed with NTG. 45 | 46 | NOTE FOR CALTECH CDS USERS: Caltech has a site license for NPSOL. You can 47 | get a copy of the source code from cds:~murray/src/npsol/. 48 | 49 | NOTE FOR OS X USERS (2 Jan 2013): XCode does not come with a FORTRAN 50 | compiler, so you will need to install gfortran and use a compatible 51 | version of gcc (may different than the system default). I have 52 | successfully gfortran 11.0.0 from the MacPorts distribution along with 53 | the XCode gcc compiler (Apple clang version 13.1.6) to get everything 54 | to compile. The following incantation in the main directory generates 55 | everytihng correctly: 56 | 57 | make F77=gfortran CFLAGS=-g FFLAGS="-g -fdefault-real-8 -fdefault-double-8" 58 | 59 | The flags setting the size of the real and double types are (apparently) 60 | required because of the use of doubles in the NTG source code for matrices 61 | that are treated as REAL in PGS. 62 | 63 | -- 64 | RMM, 3 Feb 2002 65 | RMM, 2 Jan 2013 66 | RMM, 27 Jun 2022 (updated for MacOS 12) 67 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Empty file to allow make clean to work one level up 2 | clean: 3 | -------------------------------------------------------------------------------- /doc/README: -------------------------------------------------------------------------------- 1 | This directory contains some (sparse) documentation on the use of NTG. 2 | Here's what here: 3 | 4 | workshop_15sep03.pdf - long tutorial on NTG 5 | inanc_splines-16sep03.pdf - tutorial on B-splines as used by NTG 6 | fraga_splines_01aug03.pdf - more details on splines 7 | 8 | More documentation coming in the future. 9 | 10 | RMM, 25 Apr 2009 11 | -------------------------------------------------------------------------------- /doc/fraga_splines_01aug03.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/murrayrm/ntg/1043cf7c493df154f2a965af84a369152ea5fdec/doc/fraga_splines_01aug03.pdf -------------------------------------------------------------------------------- /doc/inanc_splines-16sep03.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/murrayrm/ntg/1043cf7c493df154f2a965af84a369152ea5fdec/doc/inanc_splines-16sep03.pdf -------------------------------------------------------------------------------- /doc/workshop_15sep03.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/murrayrm/ntg/1043cf7c493df154f2a965af84a369152ea5fdec/doc/workshop_15sep03.pdf -------------------------------------------------------------------------------- /examples/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for NTG examples 2 | # RMM, 1 Feb 02 3 | 4 | NTGDIR = .. 5 | NTGINCDIR = $(NTGDIR)/include 6 | NTGLIBDIR = $(NTGDIR)/lib 7 | 8 | PGMS = vanderpol kincar 9 | 10 | CFLAGS = -g 11 | CPPFLAGS += -I $(NTGINCDIR) 12 | LIB = -lm -lntg -lpgs -lnpsol -lgfortran 13 | 14 | vanderpol: vanderpol.o $(NTGLIBDIR)/*.a 15 | $(CC) $(CFLAGS) $(CPPFLAGS) -L $(NTGLIBDIR) -o $@ vanderpol.o $(LIB) 16 | 17 | kincar: kincar.o $(NTGLIBDIR)/*.a 18 | $(CC) $(CFLAGS) $(CPPFLAGS) -L $(NTGLIBDIR) -o $@ kincar.o $(LIB) 19 | 20 | 21 | clean: 22 | rm -f *.o coef1 *~ 23 | rm -f $(PGMS) 24 | -------------------------------------------------------------------------------- /examples/README: -------------------------------------------------------------------------------- 1 | This directory contains working examples of NTG. Currently the only 2 | example is the van der Pol system. 3 | 4 | For each example, there is a file .txt that describes the 5 | system. Type "make " to compile the example and "" 6 | to run it. 7 | 8 | RMM, 3 Feb 02 9 | 10 | -------------------------------------------------------------------------------- /examples/kincar.c: -------------------------------------------------------------------------------- 1 | /* 2 | * kincar.c - kinematic car example for NTG 3 | * RMM, 27 Jun 2022 4 | * 5 | * This file is an example of how to use NTG to generate an optimal 6 | * trajectory for a kinematic car performing a lane change operation. A 7 | * description of the examples worked out here is available in Chapter 2 of 8 | * the FBS Optimization-Based Control Supplement: 9 | * 10 | * https://fbswiki.org/wiki/index.php/OBC 11 | * 12 | * Usage: 13 | * 14 | * ./kincar [-i] 15 | * 16 | * Options: 17 | * -i interactive mode, prompts for initial and final conditions 18 | * -o file save optimal trajectory to file 19 | * -v turn out verbose output (NTG and solver) 20 | * --ipopt solve using IPOPT (instead of NPLSOL) [not implemented] 21 | * 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include "ntg.h" 28 | 29 | /* 30 | * Vehicle dynamics 31 | * 32 | * This section contains the description of the dynamics of the vehicle in 33 | * terms of the differentially flat outputs, allowing conversion from the 34 | * flat flag to the system state and input. 35 | * 36 | */ 37 | 38 | /* Vehicle dynamics parameters */ 39 | struct kincar_params { 40 | double b; /* wheel based, default = 3 m */ 41 | char dir; /* travel direction ('f' or 'r') */ 42 | }; 43 | struct kincar_params default_params = {3.0, 'f'}; 44 | 45 | /* Function to take states, inputs and return the flat flag */ 46 | void kincar_flat_forward(double *x, double *u, struct kincar_params *params, 47 | double **zflag) { 48 | double b = params->b; 49 | double thdot; 50 | 51 | /* Flat output is the x, y position of the rear wheels */ 52 | zflag[0][0] = x[0]; 53 | zflag[1][0] = x[1]; 54 | 55 | /* First derivatives of the flat output */ 56 | zflag[0][1] = u[0] * cos(x[2]); /* dx/dt */ 57 | zflag[1][1] = u[0] * sin(x[2]); /* dy/dt */ 58 | 59 | /* First derivative of the angle */ 60 | thdot = (u[0]/b) * tan(u[1]); 61 | 62 | /* Second derivatives of the flat output (setting vdot = 0) */ 63 | zflag[0][2] = -u[0] * thdot * sin(x[2]); 64 | zflag[1][2] = u[0] * thdot * cos(x[2]); 65 | } 66 | 67 | /* Function to take the flat flag and return states, inputs */ 68 | void kincar_flat_reverse(double **zflag, struct kincar_params *params, 69 | double *x, double *u) { 70 | double b = params->b; 71 | char dir = params->dir; 72 | double thdot_v; 73 | 74 | /* Given the flat variables, solve for the state */ 75 | x[0] = zflag[0][0]; /* x position */ 76 | x[1] = zflag[1][0]; /* y position */ 77 | 78 | /* Vehicle angle determined by the tangent of the flat output curve */ 79 | if (dir == 'f') { 80 | x[2] = atan2(zflag[1][1], zflag[0][1]); /* tan(theta) = ydot/xdot */ 81 | } else if (dir == 'r') { 82 | /* Angle is flipped by 180 degrees (since v < 0) */; 83 | x[2] = atan2(-zflag[1][1], -zflag[0][1]); 84 | } else { 85 | fprintf(stderr, "unknown direction: %c", dir); 86 | } 87 | 88 | /* Next solve for the inputs, based on curvature */ 89 | thdot_v = zflag[1][2] * cos(x[2]) - zflag[0][2] * sin(x[2]); 90 | u[0] = zflag[0][1] * cos(x[2]) + zflag[1][1] * sin(x[2]); 91 | u[1] = atan2(thdot_v, pow(u[0], 2.0) / b); 92 | } 93 | 94 | /* 95 | * Optimal control problem setup 96 | * 97 | * For this system we solve a point-to-point optimal control problem in 98 | * which we minmimze the integrated curvature of the trajectory between the 99 | * inital and final positions. This corresponds roughly to minimizing the 100 | * inputs to the vehicle (velocity and steering angle). 101 | * 102 | */ 103 | 104 | /* Trajectory cost function (unintegrated) */ 105 | void tcf(int *mode, int *nstate, int *i, double *f, double *df, double **zp) 106 | { 107 | if (*mode == 0 || *mode == 2) { 108 | /* compute cost function: curvature */ 109 | *f = zp[0][2] * zp[0][2] + zp[1][2] * zp[1][2]; 110 | } 111 | 112 | if (*mode == 1 || *mode == 2) { 113 | /* compute gradient of cost function (index = active variables) */ 114 | df[0] = 0; df[1] = 0; df[2] = 2 * zp[0][2]; 115 | df[3] = 0; df[4] = 0; df[5] = 2 * zp[1][2]; 116 | } 117 | } 118 | 119 | /* 120 | * NTG problem setup 121 | * 122 | * This section defaults all of the parameters that are used to set up the 123 | * optimization problem that NTG solves, including the number of intervals, 124 | * order of the splines, and number of free variables. 125 | * 126 | * Note: NTG allows each of the outputs to have different values for the 127 | * number of intervals, multiplicity, order, etc. Here we will use the same 128 | * values for both outputs (the arrays are defined at the top of the main() 129 | * function, below). 130 | * 131 | */ 132 | 133 | #define NOUT 2 /* number of flat outputs, j */ 134 | #define NINTERV 2 /* number of intervals, lj */ 135 | #define MULT 3 /* regularity of splits, mj */ 136 | #define ORDER 5 /* degree of split polynomial, kj */ 137 | #define MAXDERIV 3 /* highest derivative required + 1 */ 138 | 139 | /* 140 | * Total number of coefficients required 141 | * 142 | * Sum_j [lj*(kj-mj) + mj] 143 | * 144 | */ 145 | 146 | #define NCOEF 14 /* total # of coeffs */ 147 | 148 | /* number linear constraints */ 149 | #define NLIC 6 /* linear initial constraints */ 150 | #define NLTC 0 /* linear trajectory constraints */ 151 | #define NLFC 6 /* linear final constraints */ 152 | 153 | /* number nonlinear constraints */ 154 | #define NNLIC 0 /* nonlinear initial constraints */ 155 | #define NNLTC 0 /* nonlinear trajectory constraints */ 156 | #define NNLFC 0 /* nonlinear final constraints */ 157 | 158 | /* 159 | * Nonlinear constraint function active variables 160 | * 161 | * This is the number of active variables that show up in the nonlinear 162 | * constraints. The active variables are defined by their indices into 163 | * the zp[][] array 164 | * 165 | */ 166 | 167 | #define NINITIALCONSTRAV 0 /* active variables, initial */ 168 | #define NTRAJECTORYCONSTRAV 0 /* active variables, trajectory */ 169 | #define NFINALCONSTRAV 0 /* active variables, final */ 170 | 171 | /* number of cost functions */ 172 | #define NICF 0 /* initial */ 173 | #define NTCF 1 /* trajectory (unintegrated) */ 174 | #define NFCF 0 /* final */ 175 | 176 | /* Same as above, now for the cost functions */ 177 | #define NINITIALCOSTAV 0 /* initial */ 178 | #define NTRAJECTORYCOSTAV 2 /* trajectory */ 179 | #define NFINALCOSTAV 0 /* final */ 180 | 181 | /* 182 | * Now declare all of the active variables that are required for the 183 | * nonlinear constraints [none here] and the cost functions 184 | * 185 | */ 186 | 187 | static AV trajectorycostav[NTRAJECTORYCOSTAV] = 188 | {{0, 2}, {1, 2}}; /* second deriv's of flat outputs */ 189 | 190 | /* 191 | * Main program 192 | * 193 | * This is the actual program that is called to compute the optimal 194 | * trajectory and print out the result. 195 | * 196 | */ 197 | 198 | int main(int argc, char **argv) 199 | { 200 | int i, j; /* indices for iterating */ 201 | 202 | /* 203 | * Now define all of the NTG parameters that were listed above 204 | * 205 | */ 206 | int ninterv[NOUT] = {NINTERV, NINTERV}; 207 | int mult[NOUT] = {MULT, MULT}; 208 | int maxderiv[NOUT] = {MAXDERIV, MAXDERIV}; 209 | int order[NOUT] = {ORDER, ORDER}; 210 | double *knots[NOUT]; /* knot points for each output */ 211 | int nbps = 20; /* number of breakpoints */ 212 | double *bps; /* breakpoint times */ 213 | double **lic, **lfc; /* linear initial/final constraints */ 214 | 215 | int ncoef; /* number of B-spline coefficients */ 216 | double *coefficients; /* vector of coefficients (stacked) */ 217 | int *istate; /* NTG internal memory */ 218 | double *clambda; /* NTG internal memory */ 219 | double *R; /* NTG internal memory */ 220 | 221 | /* Upper and lower bound vectors (stacked) */ 222 | double lowerb[NLIC + NLTC + NLFC + NNLIC + NNLTC + NNLFC]; 223 | double upperb[NLIC + NLTC + NLFC + NNLIC + NNLTC + NNLFC]; 224 | 225 | int inform; /* output from NPSOL */ 226 | double objective; /* optimial cost value */ 227 | 228 | /* 229 | * Process command line arguments 230 | * 231 | */ 232 | int opt, interactive = 0, verbose = 0; 233 | FILE *outfp = stdout; 234 | while ((opt = getopt(argc, argv, "io:v")) != -1) { 235 | switch (opt) { 236 | case 'i': /* interactive mode */ 237 | interactive = 1; 238 | break; 239 | 240 | case 'o': /* set output file */ 241 | outfp = fopen(optarg, "w"); 242 | if (outfp == NULL) { 243 | fprintf(stderr, "%s: can't open output file '%s'\n", argv[0], optarg); 244 | exit(EXIT_FAILURE); 245 | } 246 | break; 247 | 248 | case 'v': 249 | verbose = 1; 250 | break; 251 | 252 | default: 253 | fprintf(stderr, "Usage: %s [-i] [-o file] [-v]\n", argv[0]); 254 | exit(EXIT_FAILURE); 255 | } 256 | } 257 | 258 | /* 259 | * Allocate space and initialize the knot points 260 | * 261 | * For this problem, regularly spaced from t=0 to t=5 262 | * 263 | */ 264 | for (i = 0; i < NOUT; ++i) { 265 | knots[i] = calloc((ninterv[i] + 1), sizeof(double)); 266 | linspace(knots[i], 0, 5, ninterv[i] + 1); 267 | } 268 | 269 | /* 270 | * Compute the number of spline coefficients required 271 | * 272 | * This formula was given above (manually) and so this serves as a check 273 | * to make sure everything lines up. 274 | */ 275 | ncoef = 0; 276 | for (i = 0; i < NOUT; ++i) { 277 | ncoef += ninterv[i] * (order[i] - mult[i]) + mult[i]; 278 | } 279 | assert(ncoef == NCOEF); 280 | coefficients = calloc(ncoef, sizeof(double)); 281 | 282 | /* Initial guess for coefficients (all 1s) */ 283 | linspace(coefficients, 1, 1, ncoef); 284 | 285 | /* Allocate space for breakpoints and initialize */ 286 | bps = calloc(nbps, sizeof(double)); 287 | linspace(bps, 0, 5, nbps); 288 | 289 | /* 290 | * NTG internal memory 291 | * 292 | * These calculations do not need to be changed. 293 | */ 294 | istate = calloc((ncoef + NLIC + NLFC + NLTC * nbps + NNLIC + 295 | NNLTC * nbps + NNLFC), sizeof(int)); 296 | clambda = calloc((ncoef + NLIC + NLFC + NLTC * nbps + NNLIC + 297 | NNLTC * nbps + NNLFC), sizeof(double)); 298 | R = calloc((ncoef + 1) * (ncoef + 1), sizeof(double)); 299 | 300 | Matrix *lic_m = MakeMatrix(NLIC, MAXDERIV * NOUT); lic = lic_m->elements; 301 | Matrix *lfc_m = MakeMatrix(NLFC, MAXDERIV * NOUT); lfc = lfc_m->elements; 302 | Matrix *bounds_m = MakeMatrix(NOUT, MAXDERIV); 303 | double **bounds = bounds_m->elements; 304 | 305 | /* 306 | * Define the initial and final conditions 307 | * 308 | * This section defines the initial and final conditions by setting up a 309 | * set of linear initial/final constraints. Since the flat outputs and 310 | * their derivatives are determine by the state and inputs, constraining 311 | * the initial and final state corresponds to constraining all of the flat 312 | * outputs and their derivatives up to third order. 313 | * 314 | * TODO: add trajectory constraints on the input variables (indexed by 315 | * active variable). 316 | */ 317 | 318 | /* Lane change manuever */ 319 | double x0[3] = {0.0, -2.0, 0.0}, u0[2] = {8.0, 0}; 320 | double xf[3] = {40.0, 2.0, 0.0}, uf[2] = {8.0, 0}; 321 | 322 | /* Initial condition constraint: zflag given */ 323 | for (i = 0; i < NLIC; ++i) { lic[i][i] = 1.0; } 324 | kincar_flat_forward(x0, u0, &default_params, bounds); 325 | for (i = 0; i < NOUT; ++i) { 326 | for (j = 0; j < MAXDERIV; ++j) { 327 | lowerb[i * MAXDERIV + j] = upperb[i * MAXDERIV + j] = bounds[i][j]; 328 | } 329 | } 330 | 331 | /* Final condition constraint: zflag given */ 332 | for (i = 0; i < NLFC; ++i) { lfc[i][i] = 1.0; } 333 | kincar_flat_forward(xf, uf, &default_params, bounds); 334 | for (i = 0; i < NOUT; ++i) { 335 | for (j = 0; j < MAXDERIV; ++j) { 336 | lowerb[NLIC + i * MAXDERIV + j] = 337 | upperb[NLIC + i * MAXDERIV + j] = bounds[i][j]; 338 | } 339 | } 340 | 341 | if (verbose) { 342 | fprintf(stdout, "\nLinear initial constraints (LIC) matrix:\n"); 343 | PrintMatrix("stdout", lic_m); 344 | fprintf(stdout, "\nLinear final constraints (LFC) matrix:\n"); 345 | PrintMatrix("stdout", lfc_m); 346 | fprintf(stdout, "\nUpper and Lower Bounds:\n"); 347 | PrintVector("stdout", lowerb, NLIC + NLTC + NLFC + NNLIC + NNLTC + NNLFC); 348 | } 349 | 350 | /* 351 | * Call NTG 352 | * 353 | * Now that the problem is set up, we call the main NTG function. The 354 | * coefficients array contains the initial guess (all ones, set above) and 355 | * returns the final value of the optimal solution (if successful). 356 | * 357 | */ 358 | 359 | /* Set NPSOL options */ 360 | npsoloption("nolist"); /* turn off NPSOL listing */ 361 | if (!verbose) { 362 | npsoloption("print level 0"); /* only print the final solution */ 363 | } 364 | npsoloption("summary file = 0"); /* ??? */ 365 | 366 | /* Call NTG */ 367 | ntg(NOUT, bps, nbps, ninterv, knots, order, mult, maxderiv, 368 | coefficients, 369 | NLIC, lic, 370 | NLTC, NULL, 371 | NLFC, lfc, 372 | NNLIC, NULL, 373 | NNLTC, NULL, 374 | NNLFC, NULL, 375 | NINITIALCONSTRAV, NULL, 376 | NTRAJECTORYCONSTRAV, NULL, 377 | NFINALCONSTRAV, NULL, 378 | lowerb, upperb, 379 | NICF, NULL, 380 | NTCF, tcf, 381 | NFCF, NULL, 382 | NINITIALCOSTAV, NULL, 383 | NTRAJECTORYCOSTAV, trajectorycostav, 384 | NFINALCOSTAV, NULL, 385 | istate, clambda, R, &inform, &objective); 386 | 387 | /* 388 | * Print out/store the results for later use. 389 | * 390 | */ 391 | 392 | /* Print out the trajectory for the states and inputs */ 393 | double **fz = DoubleMatrix(NOUT, MAXDERIV); 394 | double x[3], u[2]; 395 | int ntimepts = 30; 396 | for (i = 0; i < ntimepts; ++i) { 397 | double time = 5.0 * (double) i / (ntimepts-1); 398 | for (j = 0; j < NOUT; ++j) { 399 | SplineInterp(fz[j], time, knots[j], ninterv[j], 400 | &coefficients[j * ncoef/2], ncoef/2, 401 | order[j], mult[j], maxderiv[j]); 402 | } 403 | kincar_flat_reverse(fz, &default_params, x, u); 404 | printf("%8.3g %8.3g %8.3g %8.3g %8.3g %8.3g\n", 405 | time, x[0], x[1], x[2], u[0], u[1]); 406 | } 407 | FreeDoubleMatrix(fz); 408 | 409 | /* Free up storage we used (not really needed here, but why not) */ 410 | FreeDoubleMatrix(lic); 411 | FreeDoubleMatrix(lfc); 412 | FreeDoubleMatrix(bounds); 413 | free(istate); 414 | free(clambda); 415 | free(R); 416 | free(bps); 417 | free(knots[0]); free(knots[1]); 418 | free(coefficients); 419 | 420 | return 0; 421 | } 422 | -------------------------------------------------------------------------------- /examples/vanderpol.c: -------------------------------------------------------------------------------- 1 | /* 2 | * vanderpol.c - example optimization for NTG 3 | * MBM, ?? 4 | * 5 | * This file is an example of how to use NTG. It contains at a 6 | * model for a driven van der Pol oscillator and tries to minimize 7 | * a quadratic cost subject to a set of boundary conditions. See the 8 | * README file for more information. 9 | * 10 | * RMM, 1 Feb 02: added comments to example file 11 | */ 12 | 13 | #include 14 | #include /* math functions */ 15 | #include "ntg.h" /* main NTG declarations */ 16 | 17 | #define NOUT 1 /* number of (flat) outputs */ 18 | #define NINTERV 2 /* number of intervals, lj */ 19 | #define MULT 3 /* regularity of splines, mj */ 20 | #define ORDER 5 /* degree of spline polynomial, kj */ 21 | #define MAXDERIV 3 /* highest derived required + 1 */ 22 | 23 | /* 24 | * Total number of coefficients required 25 | * 26 | * Sum_j [lj*(kj-mj) + mj] 27 | * 28 | */ 29 | #define NCOEF 7 /* total # of coeffs */ 30 | 31 | /* 32 | * Give names to the different outputs and their derivatives 33 | * 34 | * The zp label is used in the function calls below. This allows us to 35 | * use simpler labelling. 36 | */ 37 | #define z zp[0][0] 38 | #define zd zp[0][1] 39 | #define zdd zp[0][2] 40 | 41 | /* number linear constraints */ 42 | #define NLIC 2 /* linear initial constraints */ 43 | #define NLTC 0 /* linear trajectory constraints */ 44 | #define NLFC 1 /* linear final constraints */ 45 | 46 | /* number nonlinear constraints */ 47 | #define NNLIC 0 /* nonlinear initial constraints */ 48 | #define NNLTC 0 /* nonlinear trajectory constraints */ 49 | #define NNLFC 0 /* nonlinear final constraints */ 50 | 51 | /* 52 | * Nonlinear constraint function active variables 53 | * 54 | * This is the number of active variables that show up in the nonlinear 55 | * constraints. The active variables are defined by their indices into 56 | * the zp[][] array 57 | * 58 | */ 59 | #define NINITIALCONSTRAV 0 /* active variables, initial */ 60 | #define NTRAJECTORYCONSTRAV 0 /* active variables, trajectory */ 61 | #define NFINALCONSTRAV 0 /* active variables, final */ 62 | 63 | /* number of cost functions */ 64 | #define NICF 0 /* initial */ 65 | #define NUCF 1 /* unintegrated */ 66 | #define NFCF 0 /* final */ 67 | 68 | /* Same as above, now for the cost functions */ 69 | #define NINITIALCOSTAV 0 /* initial */ 70 | #define NTRAJECTORYCOSTAV 3 /* trajectory */ 71 | #define NFINALCOSTAV 0 /* final */ 72 | 73 | /* 74 | * Now declare all of the active variables that are required for the 75 | * nonlinear constraints [none here] and the cost functions 76 | * 77 | */ 78 | static AV trajectorycostav[NTRAJECTORYCOSTAV]={{0,0},{0,1},{0,2}}; 79 | 80 | /* Declare the function used to compute the unintegrated cost */ 81 | static void ucf(int *mode,int *nstate,int *i,double *f,double *df,double **zp); 82 | 83 | int main(void) 84 | { 85 | double *knots[NOUT]; /* knot points, list of times for 86 | each output */ 87 | 88 | /* 89 | * Now define all of the NTG parameters that were listed above 90 | */ 91 | int ninterv[NOUT]= {NINTERV}; 92 | int mult[NOUT]= {MULT}; 93 | int maxderiv[NOUT]= {MAXDERIV}; 94 | int order[NOUT]= {ORDER}; 95 | int nbps; 96 | double *bps; 97 | double **lic,**lfc; 98 | 99 | /* initial guess size = sum over each output ninterv*(order-mult)+mult */ 100 | int ncoef; 101 | double *coefficients; 102 | int *istate; 103 | double *clambda; 104 | double *R; 105 | 106 | double lowerb[NLIC+NLTC+NLFC+NNLIC+NNLTC+NNLFC]; 107 | double upperb[NLIC+NLTC+NLFC+NNLIC+NNLTC+NNLFC]; 108 | int inform; 109 | double objective; 110 | 111 | /* 112 | * Allocate space and initialize the knot points 113 | * 114 | * For this problem, regularly spaced from t=0 to t=5 115 | * 116 | */ 117 | knots[0]=calloc((ninterv[0]+1), sizeof(double)); 118 | linspace(knots[0], 0, 5, ninterv[0]+1); 119 | 120 | /* 121 | * Compute the number of spline coefficients required 122 | * 123 | * WARNING: this formula was computed above and the formula below 124 | * is specific to this particular case 125 | */ 126 | ncoef=ninterv[0]*(order[0]-mult[0])+mult[0]; 127 | coefficients=calloc(ncoef, sizeof(double)); 128 | 129 | /* Initial guess for coefficients (all 1s) */ 130 | linspace(coefficients,1,1,ncoef); 131 | 132 | /* Allocate space for breakpoints and initialize */ 133 | nbps=20; 134 | bps=calloc(nbps, sizeof(double)); 135 | linspace(bps,0,5,nbps); 136 | 137 | /* 138 | * NTG internal memory 139 | * 140 | * These calculations do not need to be changed. 141 | */ 142 | istate=calloc((ncoef+NLIC+NLFC+NLTC*nbps+NNLIC+NNLTC*nbps+NNLFC),sizeof(int)); 143 | clambda=calloc((ncoef+NLIC+NLFC+NLTC*nbps+NNLIC+NNLTC*nbps+NNLFC),sizeof(double)); 144 | R=calloc((ncoef+1)*(ncoef+1),sizeof(double)); 145 | 146 | lic = DoubleMatrix(NLIC,maxderiv[0]); 147 | lfc = DoubleMatrix(NLFC,maxderiv[0]); 148 | 149 | 150 | /* 151 | * Define the constraints 152 | * 153 | * This section defines the various constraints for the problem. For 154 | * the linear constraints, these are indexed by the active variable number. 155 | * 156 | * Format: [constraint number][active variable number] 157 | */ 158 | 159 | /* Initial condition constraints */ 160 | lic[0][0]=1.0; /* z */ 161 | lowerb[0]=upperb[0]=1.0; /* equality constraint */ 162 | 163 | lic[1][1]=1.0; /* zdot */ 164 | lowerb[1]=upperb[1]=0.0; /* equality constraint */ 165 | 166 | /* Final constraint: -x1(5) + x2(5) - 1 = 0 */ 167 | lfc[0][0]=-1.0; 168 | lfc[0][1]=1.0; 169 | lowerb[2]=upperb[2]=1.0; /* equality constraint */ 170 | 171 | npsoloption("summary file = 0"); 172 | ntg(NOUT,bps,nbps,ninterv,knots,order,mult,maxderiv, 173 | coefficients, 174 | NLIC, lic, 175 | NLTC, NULL, 176 | NLFC, lfc, 177 | NNLIC, NULL, 178 | NNLTC, NULL, 179 | NNLFC, NULL, 180 | NINITIALCONSTRAV, NULL, 181 | NTRAJECTORYCONSTRAV, NULL, 182 | NFINALCONSTRAV, NULL, 183 | lowerb,upperb, 184 | NICF, NULL, 185 | NUCF, ucf, 186 | NFCF, NULL, 187 | NINITIALCOSTAV, NULL, 188 | NTRAJECTORYCOSTAV, trajectorycostav, 189 | NFINALCOSTAV, NULL, 190 | istate,clambda,R,&inform,&objective); 191 | 192 | PrintVector("coef1",coefficients,ncoef); 193 | 194 | FreeDoubleMatrix(lic); 195 | FreeDoubleMatrix(lfc); 196 | free(istate); 197 | free(clambda); 198 | free(R); 199 | free(bps); 200 | free(knots[0]); 201 | free(coefficients); 202 | 203 | return 0; 204 | } 205 | 206 | void ucf(int *mode,int *nstate,int *i,double *f,double *df,double **zp) 207 | { 208 | double t1,t2,t4,t6; 209 | switch(*mode) 210 | { 211 | case 0: 212 | t1 = z*z; 213 | t2 = zd*zd; 214 | t6 = pow(zdd+z-(1.0-t1)*zd,2.0); 215 | *f = t1/2.0+t2/2.0+t6/2.0; 216 | break; 217 | 218 | case 1: 219 | t1 = z*z; 220 | t2 = 1.0-t1; 221 | t4 = zdd+z-t2*zd; 222 | df[0] = z+t4*(1.0+2.0*z*zd); 223 | df[1] = zd-t4*t2; 224 | df[2] = t4; 225 | break; 226 | 227 | case 2: 228 | t1 = z*z; 229 | t2 = zd*zd; 230 | t6 = pow(zdd+z-(1.0-t1)*zd,2.0); 231 | *f = t1/2.0+t2/2.0+t6/2.0; 232 | 233 | /*t1 = z*z; this is calculated above */ 234 | t2 = 1.0-t1; 235 | t4 = zdd+z-t2*zd; 236 | df[0] = z+t4*(1.0+2.0*z*zd); 237 | df[1] = zd-t4*t2; 238 | df[2] = t4; 239 | break; 240 | } 241 | } 242 | -------------------------------------------------------------------------------- /examples/vanderpol.m: -------------------------------------------------------------------------------- 1 | % vanderpol.m - Matlab file to read NTG output 2 | % RMM, 2 Feb 02 (based on previous version by MBM) 3 | % 4 | % This file reads the output from NTG and uses the spline toolbox 5 | % to compute the trajectories for the status and the inputs 6 | 7 | % Define the breakpoints to be used for plotting 8 | More breakpoints for illustration 9 | bps = linspace(0,5,20); 10 | augbps = sort([bps bps bps]); 11 | laugbps = length(augbps); 12 | 13 | % Define the spline used for the output 14 | % Note that this much match the setup in vanderpol.c 15 | order1=5; 16 | mult1=3; 17 | knots1=linspace(0,5,3); 18 | augknots1=augknt(knots1,order1,order1-mult1); 19 | colloc1 = spcol(augknots1,order1,augbps); 20 | 21 | % Load the output from NTG 22 | load coef1 23 | 24 | % Compute the functions defined by the splines 25 | z1=colloc1*coef1'; 26 | 27 | % Now extract them into more useful variables 28 | % This uses the problem setup described in README 29 | 30 | time = bps; 31 | 32 | o1 = z1(1:3:laugbps); % output 33 | o1d = z1(2:3:laugbps); % first deriv 34 | o1dd = z1(3:3:laugbps); % second deriv 35 | 36 | u1= o1dd + o1 - (1-o1.^2).*o1d; % input (see README) 37 | -------------------------------------------------------------------------------- /examples/vanderpol.maple: -------------------------------------------------------------------------------- 1 | # maple code to create the C code for the van der Pol 2 | # non-linear oscillator coded for ntj 3 | # 4 | # written for Maple V Release 5 5 | # 6 | # kcm 10 Feb 2000 7 | 8 | readlib(C): 9 | with(linalg): 10 | 11 | u:=zdd + z - (1-z^2)*zd: 12 | f:=1/2 * (z^2 + zd^2 + u^2): 13 | df:=jacobian([f],[z,zd,zdd]): 14 | C(f,filename="unintegratedcost.raw",optimized,ansi): 15 | C(df,filename="dunintegratedcost.raw",optimized,ansi): 16 | -------------------------------------------------------------------------------- /examples/vanderpol.txt: -------------------------------------------------------------------------------- 1 | van der Pol example 2 | 3 | the driven second order oscillator studied by van der Pol 4 | 5 | problem: 6 | 5 7 | / 8 | | 2 2 2 9 | minimise 1/2 | x1 + x2 + u dt 10 | | 11 | / 12 | 0 13 | subject to the boundary conditions 14 | x1(0)=1 15 | x2(0)=0 16 | 17 | -x1(5) + x2(5) - 1 = 0 18 | 19 | where, 20 | 21 | d 22 | -- x1=x2 23 | dt 24 | and 25 | 26 | d 2 27 | -- x2 = -x1 + (1 - x1 ) x2 + u 28 | dt 29 | 30 | 31 | This problem becomes the following in output space: 32 | 33 | 5 34 | / 35 | | 2 /d \2 2 36 | minimise 1/2 | z + |-- z| + u dt 37 | | \dt / 38 | / 39 | 0 40 | 41 | subject to the boundary conditions 42 | 43 | z(0) = 1 44 | 45 | d 46 | -- z(0) = 0 47 | dt 48 | 49 | d 50 | - z(5) + -- z(5) - 1 = 0 51 | dt 52 | 53 | where 54 | / 2 \ 55 | |d | 2 /d \ 56 | u = |--- z| + z - (1 - z ) |-- z| 57 | | 2 | \dt / 58 | \dt / 59 | -------------------------------------------------------------------------------- /exclude.txt: -------------------------------------------------------------------------------- 1 | .svn 2 | doc/*.pdf 3 | -------------------------------------------------------------------------------- /npsol/.gitignore: -------------------------------------------------------------------------------- 1 | # .gitignore - files to ignore in this directory 2 | # 3 | # Ignore all of the normal source code (copy here instead) 4 | 5 | *.bat 6 | *.com 7 | *.doc 8 | *.f 9 | *.m 10 | *.mak 11 | *.nam 12 | *.opt 13 | *.run 14 | -------------------------------------------------------------------------------- /npsol/ChangeLog: -------------------------------------------------------------------------------- 1 | 2013-01-02 Richard Murray 2 | 3 | * Makefile: created using unix.mak as a starting point. Allows 4 | compilation using gfortran 5 | 6 | * *.for: renamed .for files as .f files for gfortran compatibility 7 | 8 | * README, ChangeLog: created README and ChangeLog files to document 9 | changes 10 | 11 | -------------------------------------------------------------------------------- /npsol/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for NPSOL 2 | # RMM, 1 Feb 02 based on installed copy 3 | 4 | LIBDIR = ../lib 5 | 6 | NPSRC = chsubs.f srsubs.f npsubs.f 7 | LSSRC = mcsubs.f opsubs.f blas1.f blas2.f \ 8 | f06subs.f cmsubs.f qrsubs.f lssubs.f 9 | 10 | libnpsol.a: $(NPSRC:.f=.o) $(LSSRC:.f=.o) 11 | ar ruv $@ *.o 12 | ranlib $@ 13 | 14 | install: 15 | mkdir -p $(LIBDIR) 16 | install libnpsol.a $(LIBDIR) 17 | 18 | test: npmain 19 | npmain: npmain.o 20 | $(F77) $(FFLAGS) -o $@ $< -L . -lnpsol 21 | 22 | .SUFFIXES: .f .o 23 | .f.o: 24 | $(F77) $(FFLAGS) -c $*.f 25 | 26 | clean: 27 | rm -f npmain *.o *~ 28 | rm -f libnpsol.a 29 | 30 | distclean: clean 31 | rm -f *.f npsol.* 32 | 33 | 34 | -------------------------------------------------------------------------------- /npsol/README: -------------------------------------------------------------------------------- 1 | This directory contains the files needed to make NPSOL. The source 2 | code for NPSOL is not distributed with NTG, so you will need to obtain 3 | that separately. Information on NPSOL is available at 4 | 5 | http://www.sbsi-sol-optimize.com/asp/sol_product_npsol.htm 6 | 7 | INSTALLATION 8 | ------------ 9 | To install NPSOL, you must first get the original source code files 10 | (see URL above). The following files are required for NTG: 11 | 12 | blas1.f, blas2.f, chsubs.f, cmsubs.f, f06subs.f, lssolsubs.f, 13 | mcsubs.f, npsolsubs.f, opsubs.f, qrsubs.f, srsubs.f 14 | 15 | Note that in the standard NPSOL distribution, these files have ".for" 16 | as the suffix, so you will need to rename them. Once these files are 17 | installed in the npsol/ directory, perform the following steps to 18 | install NPSOL: 19 | 20 | 1. Edit Makefile and make sure everything is pointed where you want 21 | 2. Type 'make" to make the NPSOL library, libnpsol.a 22 | 3. If everything works, type 'make install' to install the library 23 | 24 | To test the installation, you can copy npmain.f and npsol.opt from the NPSOL 25 | distribution to this directory, run 'make test' and then run 'npmain' to 26 | verify proper operation (you should find an optimimum value). 27 | 28 | If you don't have f77 installed, you can probably use gfortran, using 29 | the following command: 30 | 31 | make -F77=gfortran 32 | 33 | -- 34 | RMM, 3 Feb 2002 35 | RMM, 27 Jun 2022 (updated NPSOL URL) 36 | -------------------------------------------------------------------------------- /pgs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for NPSOL 2 | # RMM, 1 Feb 02 based on installed copy 3 | 4 | # Flags to allow compilation using gfortran 5 | 6 | PGSURL = http://pages.cs.wisc.edu/~deboor/pgs 7 | LIBDIR = ../lib 8 | 9 | # Source code required for NTG 10 | SRC = banfac.f banslv.f bchfac.f bchslv.f bsplpp.f bsplvb.f bsplvb.f \ 11 | bsplvd.f bspp2d.f bvalue.f chol1d.f colloc.f colpnt.f cubspl.f \ 12 | cwidth.f difequ.f eqblok.f interv.f knots.f l2appr.f l2err.f \ 13 | l2knts.f l2main.f newnot.f newnotfk.f ppvalu.f putit.f \ 14 | reinsch.f round.f setdatx2.f setdatx3.f setdatx4.f setupq.f \ 15 | slvblktx.f smooth.f spli2d.f splint.f splopt.f tautsp.f titand.f 16 | 17 | # Examples from de Boor's book (used as tests) 18 | EXAMPLES = iiex.f ivex.f ixex.f xex1.f xex2.f xex3.f xex4.f xiiex2.f \ 19 | xiiex3.f xiiex4.f xiiiex1.f xiiiex2.f xiiiex2m.f xiiiex3.f xivex1.f \ 20 | reinsch.f xvex.f xviex2.f xviex3.f xviiex2.f xviiex3.f 21 | 22 | DATAFILES = xiiex2.dt1 xiiex2.dt2 xiiex3.dt1 xiiex3.dt2 xiiex3.dt3 \ 23 | xiiex4.dt1 xiiex4.dt2 xiiiex1.dt1 xiiiex1.dt2 xiiiex2.dt1 \ 24 | xiiiex2.dt2 xiiiex2.dt3 xiiiex2m.dt1 xiiiex2m.dt2 xiiiex2m.dt3 \ 25 | xivex1.dt xivex2.dt xivex3.dt xivex4.dt xviex2.dt 26 | 27 | OBJ = $(SRC:.f=.o) 28 | 29 | libpgs.a: $(OBJ) 30 | ar rcv $@ $? 31 | ranlib $@ 32 | 33 | install: libpgs.a 34 | mkdir -p $(LIBDIR) 35 | install libpgs.a $(LIBDIR) 36 | 37 | src: $(SRC) 38 | 39 | test: $(EXAMPLES:.f=) $(DATAFILES) 40 | @for i in $(EXAMPLES:.f=); do \ 41 | echo "Executing $$i: (output in $$i.out)"; \ 42 | /bin/rm -f $$i.out; touch $$i.out; \ 43 | if ( [ -e $$i.dt ] || [ -e $$i.dt1 ] ); then \ 44 | for file in $$i.dt*; do \ 45 | ./$$i < $$file >> $$i.out; \ 46 | done; \ 47 | else \ 48 | ./$$i >> $$i.out; \ 49 | fi; \ 50 | done; 51 | 52 | # Rule to compile all of the example programs 53 | $(EXAMPLES:.f=): $(EXAMPLES) 54 | @for i in $(EXAMPLES:.f=); do \ 55 | echo "Compiling $$i:"; \ 56 | $(F77) -o $$i $$i.f -L . -lpgs; \ 57 | done; 58 | 59 | # Grab all of the datafiles from the server 60 | $(DATAFILES): 61 | @for i in $(DATAFILES); do \ 62 | wget $(PGSURL)/$$i; \ 63 | done; 64 | touch .datafiles 65 | 66 | clean: 67 | rm -f $(OBJ) *.out $(EXAMPLES:.f=) *~ 68 | rm -f libpgs.a 69 | 70 | distclean: clean 71 | rm -f *.f *.dt* .datafiles 72 | 73 | # Create a rule to try to get the PGS source code if it is not present 74 | # (and apply local patches, if present) 75 | %.f: 76 | wget $(PGSURL)/$@ 77 | if ( [ -e $@.diff ] ); then patch $@ $@.diff; fi 78 | 79 | .f.o: 80 | $(F77) $(FFLAGS) -c $*.f 81 | -------------------------------------------------------------------------------- /pgs/README: -------------------------------------------------------------------------------- 1 | This directory contains the files needed to make PGS. The source 2 | code for PGS is not distributed with NTG, so you will need to obtain 3 | that separately. The sources files for PGS are available at 4 | 5 | http://pages.cs.wisc.edu/~deboor/pgs/ 6 | 7 | INSTALLATION 8 | ------------ 9 | To install PGS, you must first get the original source code files 10 | (see URL above). The following files are required for NTG: 11 | 12 | banfac.f, banslv.f, bchfac.f, bchslv.f, bsplpp.f, bsplvb.f, bsplvd.f, 13 | bspp2d.f, bvalue.f, chol1d.f, colpnt.f, cubspl.f, difequ.f, eqblok.f, 14 | interv.f, knots.f, l2appr.f, l2err.f, l2knts.f, l2main.f, newnot.f, 15 | newnotfk.f, ppvalu.f, putit.f, round.f, setdatx2.f, setdatx3.f, 16 | setdatx4.f, setupq.f, smooth.f, spli2d.f, splint.f 17 | 18 | If you have wget on your system, the default makefile should install 19 | the latest version of these files if they are not available. 20 | 21 | Once these files are installed in the npsol/ directory, perform the 22 | following steps to install NPSOL: 23 | 24 | 1. Edit Makefile and make sure everything is pointed where you want 25 | 2. Type 'make" to make the PGS library 26 | 3. If everything works, type 'make install' to install the library 27 | 28 | To test the installation, you can run 'make test', which will grab 29 | additional source code and data files from the PGS web site, compile 30 | everything and then run the tests. 31 | 32 | If you don't have f77 on your machine, but instead have gfortran (more 33 | modern) then you can use the command 34 | 35 | make F77=gfortran 36 | 37 | Local changes 38 | ------------- 39 | 40 | 30 Sept 1999, Mark Milam: 41 | * There seemed to be problem with inconsistent array dimensions 42 | in the files bvalue.f l2appr.f l2err.f 43 | * Bad declarations 44 | + bvalue.f line 65 t declared with two different array sizes. 45 | + l2appr.f line 66 same as bvalue.f 46 | + l2err.f line 29 ftau and error declare with 2 different array sizes 47 | I commented out the declarations I though were mistakes and used 48 | the ones that made more sense 49 | * removed colloc.f from the library 50 | * NOTE (RMM, 2 Jan 2013): not sure if this comment is still relevant 51 | 52 | 2 Jan 2013, Richard Murray: 53 | * For gfortran, need to edit l2err.f and l2main.f and change 'ON' to 1: 54 | 55 | data on /'ON'/ ---> data on /1/ 56 | 57 | * This probably breaks some options in PGS, but works OK for NTG 58 | 59 | -------------------------------------------------------------------------------- /pgs/l2err.f.diff: -------------------------------------------------------------------------------- 1 | 26c26,27 2 | < integer prfun, ie,k,l,ll,lpkmax,ltkmax,ntau,ntmax,on 3 | --- 4 | > integer ie,k,l,ll,lpkmax,ltkmax,ntau,ntmax 5 | > character*4 prfun,on 6 | -------------------------------------------------------------------------------- /pgs/l2main.f.diff: -------------------------------------------------------------------------------- 1 | 36c36,37 2 | < * ,ntimes,ntmax,on,prbco,prfun,prpco 3 | --- 4 | > * ,ntimes,ntmax 5 | > character*4 on,prbco,prfun,prpco 6 | -------------------------------------------------------------------------------- /src/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile - NTG source code 2 | # RMM, 1 Feb 02 3 | 4 | # Location to install NTG 5 | NTGDIR = .. 6 | NTGLIB = libntg.a 7 | 8 | # Compiler flags 9 | LINT=lint 10 | LTFLAGS = 11 | 12 | INCFILES = ntg.h integrator.h colloc.h constraints.h cost.h matrix.h av.h 13 | 14 | all: $(NTGLIB) 15 | 16 | install: all 17 | mkdir -p $(NTGDIR)/lib 18 | install $(NTGLIB) $(NTGDIR)/lib 19 | mkdir -p $(NTGDIR)/include 20 | install $(INCFILES) $(NTGDIR)/include 21 | 22 | 23 | libntg.a: colloc.o constraints.o cost.o integrator.o matrix.o ntg.o 24 | ar rcv $@ colloc.o constraints.o cost.o integrator.o matrix.o \ 25 | ntg.o 26 | ranlib $@ 27 | 28 | colloc.o: colloc.c colloc.h av.h matrix.h 29 | constraints.o: constraints.c constraints.h matrix.h colloc.h 30 | cost.o: cost.c cost.h matrix.h integrator.h 31 | integrator.o: integrator.c integrator.h matrix.h 32 | matrix.o: matrix.c matrix.h 33 | ntg.o: ntg.c ntg.h av.h colloc.h constraints.h cost.h matrix.h 34 | test.o: test.c 35 | 36 | test: test.o colloc.o 37 | cc -fast -L. -L$(HOME)/src/pgs/double \ 38 | test.o colloc.o -lntg -lpgs -lM77 -lF77 \ 39 | -lsunmath -lm 40 | 41 | lint4: 42 | $(LINT) -dirout=.lint -errchk=%all -errhdr=%user\ 43 | -errfmt=macro\ 44 | -I$(HOME)/src/ntg/2.1\ 45 | -Ncheck=%all\ 46 | -Nlevel=4 \ 47 | example4/main.c colloc.c constraints.c cost.c integrator.c matrix.c \ 48 | ntg.c 49 | 50 | lint5: 51 | $(LINT) -dirout=.lint -errchk=%all -errhdr=%user\ 52 | -errfmt=macro\ 53 | -I$(HOME)/src/ntg/2.1.debug\ 54 | -Ncheck=%all\ 55 | -Nlevel=4 \ 56 | example5/main.c colloc.c constraints.c cost.c integrator.c matrix.c \ 57 | ntg.c 58 | 59 | clean: 60 | rm -f *.o *~ 61 | rm -f libntg.a 62 | -------------------------------------------------------------------------------- /src/av.h: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | #ifndef _AV_H_ 14 | #define _AV_H_ 15 | 16 | /* active variable */ 17 | 18 | #define AVINITIAL 0 19 | #define AVTRAJECTORY 1 20 | #define AVFINAL 2 21 | 22 | typedef struct AVStruct 23 | { 24 | int output; 25 | int deriv; 26 | } AV; 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /src/colloc.c: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | #include "colloc.h" 14 | 15 | ConcatColloc *ConcatCollocMatrix( 16 | int nout, 17 | double **knots, 18 | int *ninterv, 19 | double *bps, 20 | int nbps, 21 | int *maxderiv, 22 | int *order, 23 | int *mult) 24 | { 25 | ConcatColloc *ccolloc; 26 | int i; 27 | 28 | ccolloc=malloc(sizeof(ConcatColloc)); assert(ccolloc!=NULL); 29 | ccolloc->colloc=malloc(nout*sizeof(Colloc *));assert(ccolloc->colloc!=NULL); 30 | ccolloc->iZ=malloc(nout*sizeof(int)); assert(ccolloc->iZ!=NULL); 31 | ccolloc->iz=malloc(nout*sizeof(int)); assert(ccolloc->iz!=NULL); 32 | ccolloc->iC=malloc(nout*sizeof(int)); assert(ccolloc->iC!=NULL); 33 | 34 | for(i=0,ccolloc->nz=0,ccolloc->iZ[0]=0,ccolloc->iC[0]=0,ccolloc->nC=0, 35 | ccolloc->iz[0]=0; 36 | icolloc[i]=CollocMatrix(knots[i],ninterv[i],bps,nbps, 39 | maxderiv[i],order[i],mult[i]); 40 | assert(ccolloc->colloc[i]!=NULL); 41 | if(i!=0) 42 | { 43 | ccolloc->iZ[i]=ccolloc->iZ[i-1]+ccolloc->colloc[i-1]->rows; 44 | ccolloc->iz[i]=ccolloc->iz[i-1]+ccolloc->colloc[i-1]->maxderiv; 45 | ccolloc->iC[i]=ccolloc->iC[i-1]+ccolloc->colloc[i-1]->cols; 46 | } 47 | ccolloc->nz+=maxderiv[i]; 48 | ccolloc->nC+=ccolloc->colloc[i]->cols; 49 | } 50 | ccolloc->nZ=ccolloc->nz*nbps; 51 | ccolloc->nout=nout; 52 | ccolloc->nbps=nbps; 53 | 54 | return ccolloc; 55 | } 56 | 57 | Colloc *CollocMatrix( 58 | double *knots, 59 | int ninterv, 60 | double *bps, 61 | int nbps, 62 | int maxderiv, 63 | int order, 64 | int mult) 65 | { 66 | Colloc *colloc; 67 | int n=ninterv*(order-mult)+mult; 68 | int nknots=ninterv+1; 69 | int naugknots=n+order; 70 | double *augknots; 71 | double *a; 72 | FMatrix *dbiatx; 73 | double x; 74 | int left; 75 | int mflag; 76 | int i; 77 | 78 | dbiatx=MakeFMatrix(order,maxderiv); 79 | augknots=malloc(naugknots*sizeof(double)); assert(augknots!=NULL); 80 | a=malloc(order*order*sizeof(double)); assert(a!=NULL); 81 | 82 | colloc=malloc(sizeof(Colloc)); assert(colloc!=NULL); 83 | colloc->block=malloc(nbps*sizeof(Block)); assert(colloc->block!=NULL); 84 | colloc->ninterv=ninterv; 85 | colloc->order=order; 86 | colloc->mult=mult; 87 | colloc->maxderiv=maxderiv; 88 | colloc->nbps=nbps; 89 | colloc->rows=maxderiv*nbps; 90 | colloc->cols=n; 91 | 92 | side_.m=mult; 93 | knots_(knots,&ninterv,&order,augknots,&n); 94 | 95 | for(i=0;ielements[0],&maxderiv); 100 | colloc->block[i].matrix=MakeFMatrix(maxderiv,order); 101 | FTranspose(colloc->block[i].matrix,dbiatx); 102 | } 103 | /* place in a separate for loop so interv_ doesn't have to search so much*/ 104 | for(i=0;iblock[i].offset=(left-1)*(order-mult); 109 | /* subtract 1 from left because interv_() is a fortran subroutine 110 | which assumes arrays are indexed from 1*/ 111 | } 112 | free(augknots); 113 | free(a); 114 | FreeFMatrix(dbiatx); 115 | 116 | return colloc; 117 | } 118 | 119 | void FreeConcatColloc(ConcatColloc *ccolloc) 120 | { 121 | int i; 122 | for(i=0;inout;i++) 123 | FreeColloc(ccolloc->colloc[i]); 124 | assert(ccolloc->iZ!=NULL); free(ccolloc->iZ); 125 | assert(ccolloc->iz!=NULL); free(ccolloc->iz); 126 | assert(ccolloc->iC!=NULL); free(ccolloc->iC); 127 | assert(ccolloc->colloc!=NULL); free(ccolloc->colloc); 128 | assert(ccolloc!=NULL); free(ccolloc); 129 | } 130 | 131 | void FreeColloc(Colloc *colloc) 132 | { 133 | int i; 134 | 135 | for(i=0;inbps;i++) 136 | FreeFMatrix(colloc->block[i].matrix); 137 | assert(colloc->block!=NULL); free(colloc->block); 138 | assert(colloc!=NULL); free(colloc); 139 | } 140 | 141 | void CollocMult(FMatrix *B,FMatrix *A,Colloc *colloc) 142 | { 143 | int i,j,k; 144 | for(i=0;irows;i++) 145 | for(j=0,B->elements[j][i]=0;jcols;j++) 146 | for(k=0;krows;k++) 147 | B->elements[j][i]+=A->elements[k][i]*CollocElement(colloc,k,j); 148 | } 149 | 150 | void CollocConcatMult(FMatrix *B,FMatrix *A,ConcatColloc *ccolloc) 151 | { 152 | int i; 153 | FMatrix *m1,*m2; 154 | 155 | if(ccolloc->nout==1) 156 | { 157 | CollocMult(B,A,ccolloc->colloc[0]); 158 | return; 159 | } 160 | 161 | for(i=0;inout;i++) 162 | { 163 | if(i==0) 164 | { 165 | m1=MakeFMatrix(A->rows,ccolloc->colloc[0]->rows); 166 | m2=MakeFMatrix(A->rows,ccolloc->colloc[0]->cols); 167 | } 168 | else 169 | { 170 | m1=ResizeFMatrix(m1,A->rows,ccolloc->colloc[i]->rows); 171 | m2=ResizeFMatrix(m2,A->rows,ccolloc->colloc[i]->cols); 172 | } 173 | FMatrixCopy(m1,0,0,A,0,ccolloc->iZ[i],m1->rows,m1->cols); 174 | CollocMult(m2,m1,ccolloc->colloc[i]); 175 | FMatrixCopy(B,0,ccolloc->iC[i],m2,0,0,m2->rows,m2->cols); 176 | } 177 | FreeFMatrix(m1); 178 | FreeFMatrix(m2); 179 | } 180 | 181 | double CollocElement(Colloc *colloc,int row,int col) 182 | { 183 | int deriv; 184 | int bp; 185 | 186 | lin2db(&deriv,&bp,colloc,row); 187 | if(colblock[bp].offset) 188 | return 0.0; 189 | else if(col>=colloc->block[bp].offset+colloc->order) 190 | return 0.0; 191 | else 192 | return colloc->block[bp].matrix->elements[col-colloc->block[bp].offset][deriv]; 193 | } 194 | 195 | void PrintColloc(char *filename,Colloc *colloc) 196 | { 197 | int i,j; 198 | FILE *file; 199 | 200 | if(!strcmp(filename,"stdout")) 201 | file=stdout; 202 | else if(!strcmp(filename,"stderr")) 203 | file=stderr; 204 | else 205 | { 206 | if((file=fopen(filename,"w"))==NULL) 207 | return; 208 | } 209 | 210 | for(i=0;irows;i++) 211 | { 212 | for(j=0;jcols;j++) 213 | fprintf(file,"%.16e ",CollocElement(colloc,i,j)); 214 | fprintf(file,"\n"); 215 | } 216 | fprintf(file,"\n\n\n"); 217 | if(file!=stdout && file!=stderr) 218 | fclose(file); 219 | } 220 | 221 | /*void CollocBlockMultVC(double *vout,double *v,Colloc *colloc,int b) 222 | { 223 | int offset=colloc->block[b].offset; 224 | int i; 225 | FMatrix *m1,*m2; 226 | 227 | for(i=0;iorder;icols;i++) 230 | vout[i]=0.0; 231 | 232 | m1=MakeFMatrix(1,colloc->order); 233 | m2=MakeFMatrix(1,colloc->maxderiv); 234 | memcpy(m2->elements[0],v,colloc->maxderiv*sizeof(double)); 235 | FMatrixMult(m1,m2,colloc->block[b].matrix); 236 | memcpy(vout+offset,m1->elements[0],colloc->order*sizeof(double)); 237 | 238 | FreeFMatrix(m1); 239 | FreeFMatrix(m2); 240 | }*/ 241 | 242 | /* for the sparse matrices of the form given by the initial constraints */ 243 | void CollocConcatMultI(FMatrix *dIdC,FMatrix *dIdZ,ConcatColloc *ccolloc) 244 | { 245 | int I,J; 246 | int j,k,l; 247 | 248 | assert(dIdZ->cols==ccolloc->nZ); 249 | 250 | for(I=0;Irows;I++) 251 | for(j=0;jnout;j++) 252 | for(k=0;kcolloc[j]->order;k++) 253 | { 254 | J=ccolloc->iC[j]+k; 255 | for(l=0,dIdC->elements[J][I]=0;lcolloc[j]->maxderiv;l++) 256 | dIdC->elements[J][I]+= 257 | dIdZ->elements[ccolloc->iZ[j]+l][I]* 258 | ccolloc->colloc[j]->block[0].matrix->elements[k][l]; 259 | } 260 | } 261 | 262 | /* for the sparse matrices of the form given by the trajectory constraints */ 263 | void CollocConcatMultT(FMatrix *dIdC,FMatrix *dIdZ,ConcatColloc *ccolloc) 264 | { 265 | int I; 266 | int i,j,k,l; 267 | 268 | assert(dIdZ->cols==ccolloc->nZ); 269 | 270 | for(I=0;Irows;I++) 271 | { 272 | i=I%ccolloc->nbps; 273 | for(j=0;jnout;j++) 274 | for(k=ccolloc->colloc[j]->block[i].offset; 275 | kcolloc[j]->block[i].offset+ccolloc->colloc[j]->order;k++) 276 | for(l=0,dIdC->elements[ccolloc->iC[j]+k][I]=0; 277 | lcolloc[j]->maxderiv;l++) 278 | { 279 | dIdC->elements[ccolloc->iC[j]+k][I]+= 280 | dIdZ->elements[ccolloc->iZ[j]+i*ccolloc->colloc[j]->maxderiv+l][I]* 281 | ccolloc->colloc[j]->block[i].matrix->elements 282 | [k-ccolloc->colloc[j]->block[i].offset][l]; 283 | } 284 | } 285 | } 286 | 287 | void CollocConcatMultF(FMatrix *dIdC,FMatrix *dIdZ,ConcatColloc *ccolloc) 288 | { 289 | int I,J; 290 | int j,k,l; 291 | 292 | assert(dIdZ->cols==ccolloc->nZ); 293 | 294 | for(I=0;Irows;I++) 295 | for(j=0;jnout;j++) 296 | for(k=0;kcolloc[j]->order;k++) 297 | { 298 | J=ccolloc->iC[j]+ccolloc->colloc[j]->block[ccolloc->nbps-1].offset+k; 299 | for(l=0,dIdC->elements[J][I]=0;lcolloc[j]->maxderiv;l++) 300 | { 301 | if(j==ccolloc->nout-1) 302 | dIdC->elements[J][I]+=dIdZ->elements 303 | [ccolloc->iZ[j]+ccolloc->colloc[j]->maxderiv* 304 | ccolloc->nbps-ccolloc->colloc[j]->maxderiv+l][I]* 305 | ccolloc->colloc[j]->block[ccolloc->nbps-1].matrix->elements[k][l]; 306 | else 307 | dIdC->elements[J][I]+=dIdZ->elements 308 | [ccolloc->iZ[j+1]-ccolloc->colloc[j]->maxderiv+l][I]* 309 | ccolloc->colloc[j]->block[ccolloc->nbps-1].matrix->elements[k][l]; 310 | } 311 | } 312 | /*for(j=0;jnout;j++) 313 | FMatrixSet(dIdC,0.0, 314 | 0,ccolloc->iC[j], 315 | dIdZ->rows,ccolloc->colloc[j]->cols-ccolloc->colloc[j]->order);*/ 316 | } 317 | 318 | double Zvalue(ConcatColloc *ccolloc,double *C,int output,int deriv,int bp) 319 | { 320 | int j; 321 | double d; 322 | for(j=0,d=0.0;jcolloc[output]->order;j++) 323 | d+=ccolloc->colloc[output]->block[bp].matrix->elements[j][deriv]* 324 | C[ccolloc->iC[output]+ccolloc->colloc[output]->block[bp].offset+j]; 325 | return d; 326 | } 327 | 328 | int odb2lin(ConcatColloc *ccolloc,int output,int deriv,int bp) 329 | { 330 | return ccolloc->iZ[output]+ccolloc->colloc[output]->maxderiv*bp+deriv; 331 | } 332 | 333 | int db2lin(Colloc *colloc,int deriv,int bp) 334 | { 335 | return bp*colloc->maxderiv+deriv; 336 | } 337 | 338 | void lin2db(int *deriv,int *bp,Colloc *colloc,int i) 339 | { 340 | *deriv=i%(colloc->maxderiv); /* row in the block */ 341 | *bp=i/(colloc->maxderiv); /* the number of the block i'm in */ 342 | } 343 | 344 | void updateZ(double *Z,ConcatColloc *ccolloc,double *C,AV *av,int nav,int type) 345 | { 346 | int i,j; 347 | 348 | switch(type) 349 | { 350 | case AVINITIAL: 351 | for(i=0;inbps;j++) 358 | Z[odb2lin(ccolloc,av[i].output,av[i].deriv,j)]= 359 | Zvalue(ccolloc,C,av[i].output,av[i].deriv,j); 360 | break; 361 | case AVFINAL: 362 | for(i=0;inbps-1)]= 364 | Zvalue(ccolloc,C,av[i].output,av[i].deriv,ccolloc->nbps-1); 365 | break; 366 | } 367 | } 368 | 369 | void dIdz2dIdZI(FMatrix *dIdZ,FMatrix *dIdz,ConcatColloc *ccolloc) 370 | { 371 | int i,j,k; 372 | 373 | assert(dIdZ->rows==dIdz->cols); 374 | assert(dIdZ->cols==ccolloc->nZ); 375 | assert(dIdz->rows==ccolloc->nz); 376 | 377 | for(i=0;irows;i++) 378 | for(j=0;jnout;j++) 379 | for(k=0;kcolloc[j]->maxderiv;k++) 380 | dIdZ->elements[ccolloc->iZ[j]+k][i]= 381 | dIdz->elements[i][ccolloc->iz[j]+k]; 382 | } 383 | 384 | void dIdz2dIdZT(FMatrix *dIdZ,FMatrix *dIdz,ConcatColloc *ccolloc,int bp) 385 | { 386 | int i,j,k; 387 | 388 | assert(bpnbps); 389 | assert(dIdZ->rows==ccolloc->nbps*dIdz->cols); 390 | assert(dIdZ->cols==ccolloc->nZ); 391 | assert(dIdz->rows==ccolloc->nz); 392 | 393 | for(i=0;icols;i++) 394 | for(j=0;jnout;j++) 395 | for(k=0;kcolloc[j]->maxderiv;k++) 396 | dIdZ->elements 397 | [ccolloc->iZ[j]+bp*ccolloc->colloc[j]->maxderiv+k][i*ccolloc->nbps+bp]= 398 | dIdz->elements[i][ccolloc->iz[j]+k]; 399 | } 400 | 401 | void dIdz2dIdZF(FMatrix *dIdZ,FMatrix *dIdz,ConcatColloc *ccolloc) 402 | { 403 | int i,j,k; 404 | 405 | assert(dIdZ->rows==dIdz->cols); 406 | assert(dIdZ->cols==ccolloc->nZ); 407 | assert(dIdz->rows==ccolloc->nz); 408 | 409 | for(i=0;irows;i++) 410 | { 411 | if(ccolloc->nout!=1) 412 | { 413 | for(j=0;jnout-1;j++) 414 | for(k=0;kcolloc[j]->maxderiv;k++) 415 | dIdZ->elements[ccolloc->iZ[j+1]-ccolloc->colloc[j]->maxderiv+k][i]= 416 | dIdz->elements[i][ccolloc->iz[j]+k]; 417 | } 418 | for(k=0;kcolloc[ccolloc->nout-1]->maxderiv;k++) 419 | dIdZ->elements 420 | [ccolloc->nZ-ccolloc->colloc[ccolloc->nout-1]->maxderiv+k][i]= 421 | dIdz->elements[i][ccolloc->iz[ccolloc->nout-1]+k]; 422 | } 423 | } 424 | 425 | void Z2zpI(double **zp,double *Z,ConcatColloc *ccolloc) 426 | { 427 | int i; 428 | for(i=0;inout;i++) 429 | zp[i]=&(Z[ccolloc->iZ[i]]); 430 | } 431 | 432 | void Z2zpT(double **zp,double *Z,ConcatColloc *ccolloc,int bp) 433 | { 434 | int i; 435 | for(i=0;inout;i++) 436 | zp[i]=&(Z[ccolloc->iZ[i]+bp*ccolloc->colloc[i]->maxderiv]); 437 | } 438 | 439 | void Z2zpF(double **zp,double *Z,ConcatColloc *ccolloc) 440 | { 441 | int i; 442 | if(ccolloc->nout!=1) 443 | for(i=0;inout-1;i++) 444 | zp[i]=&(Z[ccolloc->iZ[i+1]-ccolloc->colloc[i]->maxderiv]); 445 | zp[ccolloc->nout-1]= 446 | &(Z[ccolloc->nZ-ccolloc->colloc[ccolloc->nout-1]->maxderiv]); 447 | } 448 | 449 | void SplineInterp( 450 | double *f,double x,double *knots,int ninterv,double *coefs,int ncoefs, 451 | int order,int mult,int maxderiv) 452 | { 453 | int nknots=ninterv+1; 454 | int n=ninterv*(order-mult)+mult; 455 | int naugknots=n+order; 456 | double *augknots=malloc(naugknots*sizeof(double)); 457 | double *a=malloc(order*order*sizeof(double)); 458 | double *dbiatx=malloc(order*maxderiv*sizeof(double)); 459 | static int left1; 460 | static int left2; 461 | int mflag; 462 | int i,j; 463 | int offset,i1; 464 | 465 | assert(n==ncoefs); 466 | side_.m=mult; 467 | knots_(knots,&ninterv,&order,augknots,&n); 468 | 469 | interv_(augknots,&naugknots,&x,&left1,&mflag); 470 | bsplvd_(augknots,&order,&x,&left1,a,dbiatx,&maxderiv); 471 | 472 | interv_(knots,&nknots,&x,&left2,&mflag); 473 | offset=(left2-1)*(order-mult); 474 | 475 | for(i=0;i 26 | #include 27 | #include 28 | #include "matrix.h" 29 | #include "av.h" 30 | 31 | extern void interv_(); 32 | extern void knots_(); 33 | extern void bsplvd_(); 34 | 35 | extern struct side 36 | { 37 | int m; 38 | int iside; 39 | double xside[10]; 40 | }side_; 41 | 42 | typedef struct BlockStruct 43 | { 44 | FMatrix *matrix; 45 | int offset; 46 | }Block; 47 | 48 | typedef struct CollocStruct 49 | { 50 | Block *block; 51 | int ninterv; 52 | int order; 53 | int mult; 54 | int maxderiv; 55 | int nbps; 56 | int rows; 57 | int cols; 58 | } Colloc; 59 | 60 | typedef struct ConcatCollocStruct 61 | { 62 | Colloc **colloc; 63 | int nout; 64 | int nbps; 65 | int nz; 66 | int nZ; 67 | int nC; 68 | int *iZ; 69 | int *iz; 70 | int *iC; 71 | } ConcatColloc; 72 | 73 | ConcatColloc *ConcatCollocMatrix( 74 | int nout,double **knots,int *ninterv, double *bps, int nbps, 75 | int *maxderiv,int *order,int *mult); 76 | Colloc *CollocMatrix( 77 | double *knots,int ninterv,double *bps, 78 | int nbps,int maxderiv,int order,int mult); 79 | void FreeConcatColloc(ConcatColloc *ccolloc); 80 | void FreeColloc(Colloc *colloc); 81 | void CollocMult(FMatrix *B,FMatrix *A,Colloc *colloc); 82 | void CollocConcatMult(FMatrix *B,FMatrix *A,ConcatColloc *ccolloc); 83 | void CollocConcatMultT(FMatrix *dIdC,FMatrix *dIdZ,ConcatColloc *ccolloc); 84 | void CollocConcatMultI(FMatrix *dIdC,FMatrix *dIdZ,ConcatColloc *ccolloc); 85 | void CollocConcatMultF(FMatrix *dIdC,FMatrix *dIdZ,ConcatColloc *colloc); 86 | double CollocElement(Colloc *colloc,int row,int col); 87 | void CollocBlockMultVC(double *vout,double *v,Colloc *colloc,int b); 88 | void PrintColloc(char *filename,Colloc *colloc); 89 | 90 | double Zvalue(ConcatColloc *ccolloc,double *C,int output,int deriv,int bp); 91 | int odb2lin(ConcatColloc *ccolloc,int output,int deriv,int bp); 92 | int db2lin(Colloc *colloc,int deriv,int bp); 93 | void lin2db(int *deriv,int *bp,Colloc *colloc,int i); 94 | void updateZ(double *Z,ConcatColloc *ccolloc,double *C,AV *av,int nav,int type); 95 | 96 | void dIdz2dIdZI(FMatrix *dIdZ,FMatrix *dIdz,ConcatColloc *ccolloc); 97 | void dIdz2dIdZT(FMatrix *dIdZ,FMatrix *dIdz,ConcatColloc *ccolloc,int bp); 98 | void dIdz2dIdZF(FMatrix *dIdZ,FMatrix *dIdz,ConcatColloc *ccolloc); 99 | void Z2zpI(double **zp,double *Z,ConcatColloc *ccolloc); 100 | void Z2zpT(double **zp,double *Z,ConcatColloc *ccolloc,int bp); 101 | void Z2zpF(double **zp,double *Z,ConcatColloc *ccolloc); 102 | 103 | void SplineInterp( 104 | double *f,double x,double *knots,int ninterv,double *coeffs,int ncoeffs, 105 | int order,int mult,int maxderiv); 106 | 107 | /* 108 | CollocMatrix() 109 | -------------- 110 | returns an array of sub blocks representing the collocation 111 | matrix. The number of sub blocks is equal to the number 112 | of breakpoints. 113 | 114 | FreeColloc() 115 | ------------ 116 | frees memory allocated for a collocation matrix created using 117 | CollocMatrix(). 118 | 119 | CollocElement() 120 | --------------- 121 | double CollocElement(Colloc *colloc,int row,int col); 122 | return the element of the collocation matrix at position 123 | row,col 124 | 125 | CollocMultAC1() 126 | -------------- 127 | void CollocMultAC1(Matrix *B,Matrix *A,Colloc *colloc) 128 | multiplies the matrix A and the collocation matrix. stores the result in B. 129 | 130 | so, A->cols=colloc->rows and B assumed to be of size [A->rows,colloc->cols] 131 | 132 | CollocMultAC2() 133 | -------------- 134 | void CollocMultAC2(Matrix *B,Matrix *A,Colloc *colloc,int *k,int nk) 135 | multiplies a matrix of special structure A and the collocation matrix 136 | stores the result in B. 137 | 138 | The structure of A is such that only certain columns in A have non-zero 139 | elements. All the elements the other columns are. 140 | 141 | cols[ncols] is an array of integers that describe which columns in A 142 | are non-zero. The list of columns does not have to be ordered, but 143 | of course all integers in the list must be 0 <= k < A->cols. 144 | 145 | so, A->cols=colloc->rows and B assumed to be of size [A->rows,colloc->cols] 146 | 147 | CollocConcatMult() 148 | -------------- 149 | void CollocConcatMult(Matrix *B,Matrix *A,Colloc *colloc,int n) 150 | multiplies a matrix A by the concatenated collocation matrix. 151 | stores thre result in B. This is the brute force method. Assumes 152 | the matrices to be dense. Use this routine if you think one 153 | of the sparse routines might be wrong. 154 | 155 | The concatenated collocation matrix is the matrix with the collocation 156 | matrix n times on the diagonal, with zero entries elsewhere. 157 | 158 | so, A->cols=n*colloc->rows and B assumed to be of size [A->rows,n*colloc->cols] 159 | 160 | CollocConcatMultT() 161 | -------------- 162 | void CollocConcatMultT(Matrix *B,Matrix *A,Colloc **colloc,int n, int nC) 163 | multiplies a matrix A by the concatenated collocation matrix. 164 | stores the result in B. 165 | 166 | This method assumes A is sparse (ie doesn't perform every multiplication 167 | Matrix A should be of the form created of dIdZ in 168 | NonLinearTrajectoryConstraints() or TrajectoryConstraintsMatrix() 169 | 170 | The concatenated collocation matrix is the matrix with the collocation 171 | matrix n times on the diagonal, with zero entries elsewhere. 172 | 173 | so, A->cols=n*colloc->rows and B assumed to be of size [A->rows,n*colloc->cols] 174 | 175 | CollocConcatMultI() 176 | -------------- 177 | void CollocConcatMultI(Matrix *dIdc,Matrix *dIdZ,Colloc **colloc,int nout) 178 | multiplies a matrix A by the concatenated collocation matrix. 179 | stores the result in B. 180 | 181 | This method assumes A is sparse (ie doesn't perform every multiplication 182 | Matrix A should be of the form created of dIdZ in 183 | NonLinearInitialConstraints() or InitialConstraintsMatrix() 184 | 185 | The concatenated collocation matrix is the matrix with the collocation 186 | matrix n times on the diagonal, with zero entries elsewhere. 187 | 188 | so, A->cols=n*colloc->rows and B assumed to be of size [A->rows,n*colloc->cols] 189 | 190 | CollocConcatMultF() 191 | --------------------- 192 | void CollocConcatMultF(Matrix *B,Matrix *A,Colloc **colloc,int nout); 193 | multiplies a matrix A by the concatenated collocation matrix. 194 | stores the result in B. 195 | 196 | This method assumes A is sparse (ie doesn't perform every multiplication 197 | Matrix A should be of the form created of dIdZ in 198 | NonLinearFinalConstraints() or FinalConstraintsMatrix() 199 | 200 | The concatenated collocation matrix is the matrix with the collocation 201 | matrix n times on the diagonal, with zero entries elsewhere. 202 | 203 | so, A->cols=n*colloc->rows and B assumed to be of size [A->rows,n*colloc->cols] 204 | 205 | CollocBlockMultVC() 206 | ------------------- 207 | void CollocBlockMultVC(double *vout,double *v,Colloc *colloc,int b); 208 | multiplies the vector by the submatrix of colloc that is all the columns in the 209 | collocation matrix by the same rows that contain block b. 210 | 211 | vector: place to store the result. size [colloc->cols] 212 | v: vector to multiply. size [colloc->maxderiv] 213 | colloc: the collocation matrix 214 | b: the number of the block in the collocation matrix 215 | 216 | Zvalue() 217 | -------- 218 | double zvalue(ConcatColloc *ccolloc,double *C,int output,int deriv,int bp) 219 | calculates a specific value in the z vector. 220 | 221 | inputs 222 | colloc: the collocation matrix. The matrix multiplying c is assumed 223 | to consist of the collocation matrix noutput times on the diagonal 224 | with zeros off the diagonal 225 | c: the bspline coefficient vector for noutput outputs. 226 | Assumed to be of size [colloc->rows*noutputs,1] 227 | output,deriv,breakpoint: of the value of z that you want 228 | 229 | output 230 | the value z. 231 | 232 | use the funtion odb2lin() to find the correct position of the 233 | value in the z vector. 234 | 235 | odb2lin() 236 | --------- 237 | int odb2lin(ConcatColloc *ccolloc,int output,int deriv,int bp) 238 | 239 | converts an output deriv breakpoint specification into a linear 240 | array index 241 | 242 | lin2db() 243 | -------- 244 | void lin2db(int *deriv,int *bp,Colloc *colloc,int i); 245 | converts a linear array index into a breakpoint deriv specification. 246 | 247 | updateZ() 248 | --------- 249 | void updateZ(double *Z,ConcatColloc *ccolloc,double *C,AV *av,int nav,int type) 250 | calculates the values of z specified in the array av 251 | 252 | Z: the z vector of all the outputs. values of the active variables 253 | are stored in their appropriate places and can be accessed using odb2lin(). 254 | ccolloc: the collocation matrix 255 | C: the b-spline coefficients 256 | av: the z's to calculate 257 | nav: the number of active variables 258 | type: 259 | AVINTIAL to evaluate the active variables at the first breakpoint 260 | of each output. 261 | AVTRAJECTORY all the breakpoints 262 | AVFINAL the final breakpoint 263 | */ 264 | 265 | #endif 266 | -------------------------------------------------------------------------------- /src/constraints.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "constraints.h" 3 | 4 | /* checked */ 5 | void bounds( 6 | double *bbar,double *b,int nc, 7 | int nlic,int nltc,int nlfc, 8 | int nnlic,int nnltc,int nnlfc, 9 | int nbps,double bigbnd) 10 | { 11 | int i,j; 12 | 13 | /* the values of the coefficients are unbounded */ 14 | for(i=0;inbps; 58 | } 59 | if(nnlfc!=0) 60 | NonLinearFinalConstraints(mode,nstate,nnlfc,nlc+i1,NULL,nlfcf,ccolloc,Z); 61 | } 62 | else 63 | { 64 | assert(dnlc->rows==nnlic+nnltc*ccolloc->nbps+nnlfc); 65 | assert(dnlc->cols==ccolloc->nC); 66 | if(nnlic!=0) 67 | { 68 | NonLinearInitialConstraints(mode,nstate,nnlic,nlc,dnlc,nlicf,ccolloc,Z); 69 | i1=nnlic; 70 | } 71 | if(nnltc!=0) 72 | { 73 | m1=SubFMatrix(dnlc,i1,0,nnltc*ccolloc->nbps,ccolloc->nC); 74 | NonLinearTrajectoryConstraints(mode,nstate,nnltc,nlc+i1,m1,nltcf,ccolloc,Z); 75 | FreeSubFMatrix(m1); 76 | i1+=nnltc*ccolloc->nbps; 77 | } 78 | if(nnlfc!=0) 79 | { 80 | m1=SubFMatrix(dnlc,i1,0,nnlfc,ccolloc->nC); 81 | NonLinearFinalConstraints(mode,nstate,nnlfc,nlc+i1,m1,nlfcf,ccolloc,Z); 82 | FreeSubFMatrix(m1); 83 | } 84 | } 85 | } 86 | 87 | /* checked */ 88 | void NonLinearInitialConstraints( 89 | int *mode,int *nstate, 90 | int nnlic,double *nlic,FMatrix *dIdC, 91 | void (*nlicf)(int *,int *,double *,double **,double **), 92 | ConcatColloc *ccolloc,double *Z) 93 | { 94 | FMatrix *dIdz; 95 | FMatrix *dIdZ; 96 | double **zp=malloc(ccolloc->nout*sizeof(double *)); 97 | 98 | Z2zpI(zp,Z,ccolloc); 99 | 100 | if(*mode==0) 101 | { 102 | (*nlicf)(mode,nstate,nlic,NULL,zp); 103 | free(zp); 104 | } 105 | else if(*mode==1 || *mode==2) 106 | { 107 | dIdz=MakeFMatrix(ccolloc->nz,nnlic); 108 | (*nlicf)(mode,nstate,nlic,dIdz->elements,zp); 109 | free(zp); 110 | 111 | dIdZ=MakeFMatrix(nnlic,ccolloc->nZ); 112 | dIdz2dIdZI(dIdZ,dIdz,ccolloc); 113 | FreeFMatrix(dIdz); 114 | CollocConcatMultI(dIdC,dIdZ,ccolloc); 115 | FreeFMatrix(dIdZ); 116 | } 117 | } 118 | 119 | /* checked */ 120 | void NonLinearTrajectoryConstraints( 121 | int *mode,int *nstate, 122 | int nnltc,double *nltc,FMatrix *dIdC, 123 | void (*nltcf)(int *,int *,int *,double *,double **,double **), 124 | ConcatColloc *ccolloc,double *Z) 125 | { 126 | FMatrix *dIdz; 127 | FMatrix *dIdZ; 128 | double *tmp=malloc(nnltc*sizeof(double)); 129 | double **zp=malloc(ccolloc->nout*sizeof(double *)); 130 | int i,j; 131 | 132 | if(*mode==0) 133 | { 134 | for(i=0;inbps;i++) 135 | { 136 | Z2zpT(zp,Z,ccolloc,i); 137 | (*nltcf)(mode,nstate,&i,tmp,NULL,zp); 138 | for(j=0;jnbps+i]=tmp[j]; 140 | } 141 | free(tmp); 142 | free(zp); 143 | } 144 | else if(*mode==1 || *mode==2) 145 | { 146 | dIdz=MakeFMatrix(ccolloc->nz,nnltc); 147 | dIdZ=MakeFMatrix(ccolloc->nbps*nnltc,ccolloc->nZ); 148 | for(i=0;inbps;i++) 149 | { 150 | Z2zpT(zp,Z,ccolloc,i); 151 | (*nltcf)(mode,nstate,&i,tmp,dIdz->elements,zp); 152 | for(j=0;jnbps+i]=tmp[j]; 154 | dIdz2dIdZT(dIdZ,dIdz,ccolloc,i); 155 | } 156 | FreeFMatrix(dIdz); 157 | free(tmp); 158 | free(zp); 159 | CollocConcatMultT(dIdC,dIdZ,ccolloc); 160 | FreeFMatrix(dIdZ); 161 | } 162 | } 163 | 164 | /* checked */ 165 | void NonLinearFinalConstraints( 166 | int *mode,int *nstate, 167 | int nnlfc,double *nlfc,FMatrix *dIdC, 168 | void (*nlfcf)(int *,int *,double *,double **,double **), 169 | ConcatColloc *ccolloc,double *Z) 170 | { 171 | FMatrix *dIdz; 172 | FMatrix *dIdZ; 173 | double **zp=malloc(ccolloc->nout*sizeof(double *)); 174 | 175 | Z2zpF(zp,Z,ccolloc); 176 | 177 | if(*mode==0) 178 | { 179 | (*nlfcf)(mode,nstate,nlfc,NULL,zp); 180 | free(zp); 181 | } 182 | else if(*mode==1 || *mode==2) 183 | { 184 | dIdz=MakeFMatrix(ccolloc->nz,nnlfc); 185 | (*nlfcf)(mode,nstate,nlfc,dIdz->elements,zp); 186 | free(zp); 187 | 188 | dIdZ=MakeFMatrix(nnlfc,ccolloc->nZ); 189 | dIdz2dIdZF(dIdZ,dIdz,ccolloc); 190 | FreeFMatrix(dIdz); 191 | 192 | CollocConcatMultF(dIdC,dIdZ,ccolloc); 193 | FreeFMatrix(dIdZ); 194 | } 195 | } 196 | 197 | /* checked */ 198 | void LinearConstraintsMatrix( 199 | FMatrix *lc,FMatrix *lic,FMatrix *ltc,FMatrix *lfc,ConcatColloc *ccolloc) 200 | { 201 | FMatrix *m1; 202 | int i1=0; 203 | 204 | if(lic!=NULL) 205 | { 206 | InitialConstraintsMatrix(lc,lic,ccolloc); 207 | i1=lic->cols; 208 | } 209 | if(ltc!=NULL) 210 | { 211 | m1=SubFMatrix(lc,i1,0,ccolloc->nbps*ltc->cols,ccolloc->nC); 212 | TrajectoryConstraintsMatrix(m1,ltc,ccolloc); 213 | FreeSubFMatrix(m1); 214 | i1+=ltc->cols*ccolloc->nbps; 215 | } 216 | if(lfc!=NULL) 217 | { 218 | m1=SubFMatrix(lc,i1,0,lfc->cols,ccolloc->nC); 219 | FinalConstraintsMatrix(m1,lfc,ccolloc); 220 | FreeSubFMatrix(m1); 221 | } 222 | } 223 | 224 | /* checked */ 225 | void InitialConstraintsMatrix(FMatrix *dIdC,FMatrix *dIdz,ConcatColloc *ccolloc) 226 | { 227 | FMatrix *dIdZ=MakeFMatrix(dIdz->cols,ccolloc->nZ); 228 | 229 | dIdz2dIdZI(dIdZ,dIdz,ccolloc); 230 | /*cbug*/ 231 | /*PrintFMatrix("stdout",dIdZ);*/ 232 | CollocConcatMultI(dIdC,dIdZ,ccolloc); 233 | FreeFMatrix(dIdZ); 234 | } 235 | 236 | /* checked */ 237 | void TrajectoryConstraintsMatrix(FMatrix *dIdC,FMatrix *dIdz, 238 | ConcatColloc *ccolloc) 239 | { 240 | FMatrix *dIdZ=MakeFMatrix(ccolloc->nbps*dIdz->cols,ccolloc->nZ); 241 | int i; 242 | 243 | for(i=0;inbps;i++) 244 | dIdz2dIdZT(dIdZ,dIdz,ccolloc,i); 245 | /*cbug*/ 246 | /*PrintFMatrix("stdout",dIdZ);*/ 247 | CollocConcatMultT(dIdC,dIdZ,ccolloc); 248 | FreeFMatrix(dIdZ); 249 | } 250 | 251 | /* checked */ 252 | void FinalConstraintsMatrix(FMatrix *dIdC,FMatrix *dIdz,ConcatColloc *ccolloc) 253 | { 254 | FMatrix *dIdZ=MakeFMatrix(dIdz->cols,ccolloc->nZ); 255 | 256 | dIdz2dIdZF(dIdZ,dIdz,ccolloc); 257 | /*cbug*/ 258 | /*PrintFMatrix("stdout",dIdZ);*/ 259 | CollocConcatMultF(dIdC,dIdZ,ccolloc); 260 | FreeFMatrix(dIdZ); 261 | } 262 | -------------------------------------------------------------------------------- /src/constraints.h: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | /* 15 | * constraints.h 16 | * 17 | * functions to calculate the linear constraint matrix and bounds 18 | * 19 | */ 20 | 21 | #ifndef _CONSTRAINTS_H_ 22 | #define _CONSTRAINTS_H_ 23 | 24 | #include "colloc.h" 25 | #include "matrix.h" 26 | 27 | void bounds( 28 | double *bbar,double *b,int nC, 29 | int nlic,int nltc,int nlfc, 30 | int nnlic,int nnltc,int nnlfc, 31 | int nbps,double bigbnd); 32 | 33 | void NonLinearConstraints( 34 | int *mode,int *nstate, 35 | double *nlc,FMatrix *dnlc, 36 | int nnlic,void (*nlicf)(int *,int *,double *,double **,double **), 37 | int nnltc,void (*nltcf)(int *,int *,int *,double *,double **,double **), 38 | int nnlfc,void (*nlfcf)(int *,int *,double *,double **,double **), 39 | ConcatColloc *ccolloc,double *Z); 40 | 41 | void NonLinearInitialConstraints( 42 | int *mode,int *nstate, 43 | int nnlic,double *nlic,FMatrix *dIdC, 44 | void (*nlicf)(int *,int *,double *,double **,double **), 45 | ConcatColloc *ccolloc,double *Z); 46 | 47 | void NonLinearTrajectoryConstraints( 48 | int *mode,int *nstate, 49 | int nnltc,double *nltc,FMatrix *dnltc, 50 | void (*nltcf)(int *,int *,int *,double *,double **,double **), 51 | ConcatColloc *ccolloc,double *Z); 52 | 53 | void NonLinearFinalConstraints( 54 | int *mode,int *nstate,int nnlfc,double *nlfc,FMatrix *dnlfc, 55 | void (*nlfcf)(int *,int *,double *,double **,double **), 56 | ConcatColloc *ccolloc,double *Z); 57 | 58 | void LinearConstraintsMatrix( 59 | FMatrix *lc,FMatrix *lic,FMatrix *ltc,FMatrix *lfc,ConcatColloc *ccolloc); 60 | 61 | void InitialConstraintsMatrix(FMatrix *dIdC,FMatrix *dIdz,ConcatColloc *ccolloc); 62 | void TrajectoryConstraintsMatrix(FMatrix *dIdC,FMatrix *dIdz, 63 | ConcatColloc *ccolloc); 64 | void FinalConstraintsMatrix(FMatrix *dIdC,FMatrix *dIdz,ConcatColloc *colloc); 65 | 66 | /* 67 | bounds() 68 | -------- 69 | void bounds(double *bbar,double *b,int nc,int nlic,int nltc,int nlfc, 70 | int nnlic,int nnltc,int nnlfc,int nbps,double bigbnd) 71 | fills the bounds vector that will be passed to NPSOL 72 | recall the actual values of the coefficients are unconstrained so 73 | set the bounds to +/-bigbnd 74 | 75 | bbar: vector to be filled. size [nc+nlic+nltc*nbps+nlfc+nnlic+nnltc*nbps+nnlfc] 76 | b: user defined bounds. size [nlic+nltc+nlfc+nnlic+nnltc+nnlfc] 77 | nc: number of coefficients 78 | nlic,nltc,nlfc: number of linear (initial,trajectory,final) constraints. 79 | nnlic,nnltc,nnlfc: number of nonlinear (initial,trajectory,final) constraints. 80 | nbps: number of breakpoints 81 | bigbnd: the value to set the first nc elements to. 82 | 83 | NonLinearConstraints() 84 | ---------------------- 85 | fills the nonlinear constraints vector with values of the constraints 86 | 87 | mode: action to take. If mode is 0... 88 | nlc: values of the nonlinear constraints. size(nlc)=[nnlic+nnltc*nbps+nnlfc]. 89 | dnlc: Jacobian of nlc. size(dnlc)=[size(nlc),ccolloc->nC] 90 | nnlic, nlicf: number of nonlinear initial constraints and the user defined 91 | nonlinear intial constraints function. 92 | nnltc, nltcf: number of nonlinear trajectory constraints and the user defined 93 | nonlinear trajectory constraints function. 94 | nnlfc, nlfcf: number of nonlinear final constraints and the user defined 95 | nonlinear final constraints function. 96 | ccolloc,Z: the concatenated collocation matrix and the vector of output Z's 97 | 98 | NonLinearInitialConstraints() 99 | ----------------------------- 100 | fills the nonlinear initial constraints vector values of the constraints 101 | 102 | mode: action to take 103 | nnlic,nlic,dnlic: place to store function values and their derivatives. 104 | size(nlic)=nnlic. size(dnlic)=[nnlic,ccolloc->nC] 105 | nlicf: user defined function to calculate nonlinear initial constraints 106 | ccolloc,Z: concatenated collocation matrix and vector of output Z's 107 | 108 | NonLinearTrajectoryConstraints() 109 | -------------------------------- 110 | fills a vector with the values of the nonlinear trajectory constraints. 111 | 112 | mode: action to take 113 | nnltc,nltc,dnltc: place to store function values and their derivatives. 114 | size(nltc)=nbps*nnltc. size(dnltc)=[nbps*nnltc,ccolloc->nC] 115 | nltcf: user defined function to calculate nonlinear trajectory constraints 116 | ccolloc,Z: concatenated collocation matrix and vector of output Z's 117 | 118 | NonLinearFinalConstraints 119 | ------------------------- 120 | fills a vector with the values of the nonlinear final constraints. 121 | 122 | mode: action to take 123 | nnlfc,nlfc,dnlfc: place to store function values and their derivatives. 124 | size(nlfc)=nnlfc. size(dnlfc)=[nnlfc,ccolloc->nC] 125 | nlfcf: user defined function to calculate nonlinear final constraints. 126 | ccolloc,Z: collocation matrix and vector of output Z's 127 | 128 | LinearConstraintsMatrix() 129 | ----------------- 130 | fills the linear constraint matrix that will be passed to npsol 131 | arguments 132 | lc: pointer to the matrix to be filled. Must be of 133 | size [nlic+nltc*nbps+nlfc][nout*colloc->cols] 134 | lic: initial time linear constraints. size [nlci,ccolloc->nz]. 135 | ltc: trajectory linear constraints. size [nlcj,colloc->nz]. 136 | lfc: final time linear constraints. size [nlcf,colloc->nz]. 137 | ccolloc: concatenated collocation matrix 138 | 139 | InitialConstraintsMatrix() 140 | ----------------------------- 141 | takes the user specified trajectory constraint matrix 142 | and returns the linear trajectory constraint matrix 143 | 144 | dIdC: (output) linear constraints matrix. Must point to a matrix of size 145 | [nbps*nlic,ccolloc->nC] 146 | dIdz: (input) user specified constraint matrix. These 147 | linear constraints are applied only to the first breakpoint. 148 | Assumed to be of size [nlic,ccolloc->nz] 149 | ccolloc: (input) the collocation matrix 150 | 151 | TrajectoryConstraintsMatrix() 152 | ----------------------------- 153 | takes the user specified trajectory constraint matrix 154 | and returns the linear trajectory constraint matrix 155 | 156 | dIdC: (output) linear tj constr mat. Must point to a matrix of size 157 | [nbps*nltj,ccolloc->nC] 158 | dIdz: (input) user specified constraint matrix. These 159 | linear constraints are applied to every breakpoint. 160 | Assumed to be of size [nltj,ccolloc->nz] 161 | ccolloc: (input) the concatenated collocation matrix 162 | nout: (input) the number of outputs 163 | 164 | FinalConstraintsMatrix() 165 | ----------------------------- 166 | takes the user specified trajectory constraint matrix 167 | and returns the linear trajectory constraint matrix 168 | 169 | dIdC: (output) linear constraints matrix. Must point to a matrix of size 170 | [nbps*nlfc,ccolloc->nC] 171 | dIdz: (input) user specified constraint matrix. These 172 | linear constraints are applied only to the last breakpoint of each output. 173 | Assumed to be of size [nlic,ccolloc->nz] 174 | ccolloc: (input) the concatenated collocation matrix 175 | */ 176 | 177 | #endif 178 | -------------------------------------------------------------------------------- /src/cost.c: -------------------------------------------------------------------------------- 1 | 2 | #include "cost.h" 3 | 4 | void InitialCost( 5 | int *mode,int *nstate,double *I,double *dI, 6 | void (*func)(int *,int *,double *,double *,double **), 7 | ConcatColloc *ccolloc,double *Z) 8 | { 9 | FMatrix *dIdz; 10 | FMatrix *dIdZ; 11 | FMatrix *dIdC; 12 | double **zp=malloc(ccolloc->nout*sizeof(double *)); 13 | 14 | Z2zpI(zp,Z,ccolloc); 15 | if(*mode==0) 16 | { 17 | (*func)(mode,nstate,I,NULL,zp); 18 | free(zp); 19 | } 20 | else if(*mode==1 || *mode==2) 21 | { 22 | dIdz=MakeFMatrix(ccolloc->nz,1); 23 | (*func)(mode,nstate,I,dIdz->elements[0],zp); 24 | free(zp); 25 | 26 | dIdZ=MakeFMatrix(1,ccolloc->nZ); 27 | dIdz2dIdZI(dIdZ,dIdz,ccolloc); 28 | FreeFMatrix(dIdz); 29 | 30 | dIdC=MakeFMatrix(1,ccolloc->nC); 31 | CollocConcatMultI(dIdC,dIdZ,ccolloc); 32 | memcpy(dI,dIdC->elements[0],ccolloc->nC*sizeof(double)); 33 | FreeFMatrix(dIdZ); 34 | FreeFMatrix(dIdC); 35 | } 36 | } 37 | 38 | void IntegratedCost( 39 | int *mode,int *nstate, 40 | double *I,double *dI,double *bps, 41 | void (*func)(int *,int *,int *,double *,double *,double **), 42 | ConcatColloc *ccolloc,double *Z) 43 | { 44 | double **zp=malloc(ccolloc->nout*sizeof(double *)); 45 | double *d1; 46 | double *f; 47 | 48 | FMatrix *dIdC; 49 | FMatrix *dIdz; 50 | int i,j,k,l,offset; 51 | 52 | switch(*mode) 53 | { 54 | case 0: 55 | f=malloc(ccolloc->nbps*sizeof(double)); 56 | for(i=0;inbps;i++) 57 | { 58 | Z2zpT(zp,Z,ccolloc,i); 59 | (*func)(mode,nstate,&i,f+i,NULL,zp); 60 | } 61 | IntegrateVector(I,f,bps,ccolloc->nbps,TRAPEZOID); 62 | /*cbug 63 | PrintVector("data/Z",Z,ccolloc->nZ); 64 | PrintVector("data/f",f,ccolloc->nbps); 65 | PrintVector("data/I",I,1);*/ 66 | free(f); 67 | break; 68 | case 1: 69 | d1=malloc(ccolloc->nz*sizeof(double)); 70 | dIdz=MakeFMatrix(ccolloc->nbps,ccolloc->nz); 71 | for(i=0;inbps;i++) 72 | { 73 | Z2zpT(zp,Z,ccolloc,i); 74 | (*func)(mode,nstate,&i,NULL,d1,zp); 75 | for(j=0;jnz;j++) 76 | dIdz->elements[j][i]=d1[j]; 77 | } 78 | free(d1); 79 | dIdC=MakeFMatrix(ccolloc->nbps,ccolloc->nC); 80 | for(i=0;inout;i++) 81 | for(j=0;jnbps;j++) 82 | { 83 | offset=ccolloc->colloc[i]->block[j].offset; 84 | for(k=0;kelements[ccolloc->iC[i]+k][j]=0; 86 | for(;kcolloc[i]->order;k++) 87 | for(l=0,dIdC->elements[ccolloc->iC[i]+k][j]=0; 88 | lcolloc[i]->maxderiv;l++) 89 | dIdC->elements[ccolloc->iC[i]+k][j]+= 90 | dIdz->elements[ccolloc->iz[i]+l][j]* 91 | ccolloc->colloc[i]->block[j].matrix->elements[k-offset][l]; 92 | for(;kcolloc[i]->cols;k++) 93 | dIdC->elements[ccolloc->iC[i]+k][j]=0; 94 | } 95 | FreeFMatrix(dIdz); 96 | IntegrateFMatrixCols(dI,dIdC,bps,TRAPEZOID); 97 | FreeFMatrix(dIdC); 98 | break; 99 | case 2: 100 | d1=malloc(ccolloc->nz*sizeof(double)); 101 | dIdz=MakeFMatrix(ccolloc->nbps,ccolloc->nz); 102 | f=malloc(ccolloc->nbps*sizeof(double)); 103 | for(i=0;inbps;i++) 104 | { 105 | Z2zpT(zp,Z,ccolloc,i); 106 | (*func)(mode,nstate,&i,f+i,d1,zp); 107 | for(j=0;jnz;j++) 108 | dIdz->elements[j][i]=d1[j]; 109 | } 110 | free(d1); 111 | IntegrateVector(I,f,bps,ccolloc->nbps,TRAPEZOID); 112 | /*cbug 113 | PrintVector("data/Z",Z,ccolloc->nZ); 114 | PrintVector("data/f",f,ccolloc->nbps); 115 | PrintVector("data/I",I,1);*/ 116 | free(f); 117 | dIdC=MakeFMatrix(ccolloc->nbps,ccolloc->nC); 118 | for(i=0;inout;i++) 119 | for(j=0;jnbps;j++) 120 | { 121 | offset=ccolloc->colloc[i]->block[j].offset; 122 | for(k=0;kelements[ccolloc->iC[i]+k][j]=0; 124 | for(;kcolloc[i]->order;k++) 125 | for(l=0,dIdC->elements[ccolloc->iC[i]+k][j]=0; 126 | lcolloc[i]->maxderiv;l++) 127 | dIdC->elements[ccolloc->iC[i]+k][j]+= 128 | dIdz->elements[ccolloc->iz[i]+l][j]* 129 | ccolloc->colloc[i]->block[j].matrix->elements[k-offset][l]; 130 | for(;kcolloc[i]->cols;k++) 131 | dIdC->elements[ccolloc->iC[i]+k][j]=0; 132 | } 133 | FreeFMatrix(dIdz); 134 | IntegrateFMatrixCols(dI,dIdC,bps,TRAPEZOID); 135 | FreeFMatrix(dIdC); 136 | break; 137 | } 138 | free(zp); 139 | } 140 | 141 | void FinalCost( 142 | int *mode,int *nstate,double *I,double *dI, 143 | void (*func)(int *,int *,double *,double *,double **), 144 | ConcatColloc *ccolloc,double *Z) 145 | { 146 | double **zp=malloc(ccolloc->nout*sizeof(double *)); 147 | FMatrix *dIdz; 148 | FMatrix *dIdZ; 149 | FMatrix *dIdC; 150 | 151 | Z2zpF(zp,Z,ccolloc); 152 | 153 | if(*mode==0) 154 | { 155 | (*func)(mode,nstate,I,NULL,zp); 156 | free(zp); 157 | } 158 | else if(*mode==1 || *mode==2) 159 | { 160 | dIdz=MakeFMatrix(ccolloc->nz,1); 161 | (*func)(mode,nstate,I,dIdz->elements[0],zp); 162 | free(zp); 163 | 164 | dIdZ=MakeFMatrix(1,ccolloc->nZ); 165 | dIdz2dIdZF(dIdZ,dIdz,ccolloc); 166 | FreeFMatrix(dIdz); 167 | 168 | dIdC=MakeFMatrix(1,ccolloc->nC); 169 | CollocConcatMultF(dIdC,dIdZ,ccolloc); 170 | memcpy(dI,dIdC->elements[0],ccolloc->nC*sizeof(double)); 171 | FreeFMatrix(dIdC); 172 | FreeFMatrix(dIdZ); 173 | } 174 | } 175 | -------------------------------------------------------------------------------- /src/cost.h: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | /* 15 | costs.h 16 | */ 17 | 18 | #ifndef _COSTS_H_ 19 | #define _COSTS_H_ 20 | 21 | #include "av.h" 22 | #include "colloc.h" 23 | #include "integrator.h" 24 | #include "matrix.h" 25 | 26 | 27 | void InitialCost( 28 | int *mode,int *nstate, 29 | double *I,double *dI, 30 | void (*func)(int *,int *,double *,double *,double **), 31 | ConcatColloc *ccolloc,double *Z); 32 | 33 | void IntegratedCost( 34 | int *mode,int *nstate, 35 | double *I,double *dI,double *bps, 36 | void (*func)(int *,int *,int *,double *,double *,double **), 37 | ConcatColloc *ccolloc,double *Z); 38 | 39 | void FinalCost( 40 | int *mode,int *nstate, 41 | double *F,double *dF, 42 | void (*func)(int *,int *,double *,double *,double **), 43 | ConcatColloc *ccolloc,double *Z); 44 | 45 | /* 46 | InitialCost() 47 | ------------- 48 | computes the value of the initial cost function 49 | 50 | mode: action to take 51 | nstate: is this first time the cost is being calculated? 52 | I: value of the initial cost functions size(I)=[1] 53 | dI: Jacobian of I size(dI)=[1,nout*colloc->cols] 54 | func: the initial cost function 55 | colloc,Z: the collocation matrix and the vector of outputs Z. 56 | nout: number of outputs 57 | nz: Sum of the number of variables (sum over i max_deriv(i)) 58 | nZ: Sum of the number of breakpoints (nz*nbps) 59 | nc: Sum of the number of B-spline coefficients 60 | 61 | IntegratedCost() 62 | ---------------- 63 | computes the value of the integrated cost function 64 | 65 | mode: action to take 66 | I: value of the integrated cost functions size(I)=[1] 67 | dI: place to store Jacobian of I. size(dI)=[1,nout*colloc->cols] 68 | bps: the breakpoints. size [colloc->nbps] 69 | func: the unintegrated cost function 70 | colloc,Z: the collocation matrix and the vector of outputs Z. 71 | bps: breakpoints vector. size [colloc->nbps] 72 | nout: number of outputs 73 | nz: Sum of the number of variables (sum over i max_deriv(i)) 74 | nc: Sum of the number of B-spline coefficients 75 | 76 | 77 | FinalCost() 78 | ----------- 79 | computes the value of the final cost function 80 | 81 | mode: action to take 82 | F: place to store value of the final cost function size(F)=1 83 | dF: place to store jacobian of F. size(dF)=[1,nout*colloc->cols] 84 | func: fincal cost function 85 | Colloc,Z: the collocation Matrix and the vector of outputs Z. 86 | nout: number of outputs 87 | nz: Sum of the number of variables (sum over i max_deriv(i)) 88 | nZ: Sum of the number of breakpoints (nz*nbps) 89 | nc: Sum of the number of B-spline coefficients 90 | 91 | 92 | */ 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /src/integrator.c: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | #include "integrator.h" 15 | 16 | void IntegrateVector(double *I,double *f,double *t,int n,int type) 17 | { 18 | int i; 19 | switch(type) 20 | { 21 | case TRAPEZOID: 22 | for(i=0,*I=0.0;icols;i++) 46 | for(j=0,I[i]=0.0;jrows-1;j++) 47 | I[i]+=(t[j+1]-t[j])*(f->elements[i][j+1]+f->elements[i][j])/2; 48 | break; 49 | /* forward euler */ 50 | case FEULER: 51 | for(i=0;icols;i++) 52 | for(j=0,I[i]=0.0;jrows-1;j++) 53 | I[i]+=(t[j+1]-t[j])*f->elements[i][j]; 54 | break; 55 | /* backward euler */ 56 | case BEULER: 57 | for(i=0;icols;i++) 58 | for(j=0,I[i]=0.0;jrows-1;j++) 59 | I[i]+=(t[j+1]-t[j])*f->elements[i][j+1]; 60 | break; 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/integrator.h: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | #ifndef _INTEGRATOR_H_ 15 | #define _INTEGRATOR_H_ 16 | 17 | #include "matrix.h" 18 | 19 | #define FEULER 0 20 | #define BEULER 1 21 | #define TRAPEZOID 2 22 | 23 | void IntegrateVector(double *I,double *f,double *t,int n,int type); 24 | void IntegrateFMatrixCols(double *I,FMatrix *f,double *t,int type); 25 | 26 | /* 27 | IntegrateVector() 28 | ----------------- 29 | IntegrateVector(double *I,double *f,double *t,int n,int type) 30 | integrate a vector of values 31 | 32 | I: place to store the integral. size(I)=[1] 33 | f: function values size(f)=[n] 34 | t: independent variable values size(t)=[n] 35 | n: length of the arrays f and x 36 | type: type of integrator to use 37 | 38 | IntegrateMatrixCols() 39 | --------------------- 40 | IntegrateMatrixCols(double *I,Matrix *f,double *t,int type) 41 | 42 | integrate each column of f 43 | 44 | I: place to store the integral. size(I)=[matrix->cols] 45 | f: function values 46 | t: independent variable values size(t)=[matrix->rows] 47 | type: type of integrator 48 | 49 | */ 50 | 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /src/matrix.c: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | #include "matrix.h" 15 | 16 | FMatrix *ResizeFMatrix(FMatrix *m,int rows,int cols) 17 | { 18 | double *d1; 19 | size_t s1; 20 | int i,j; 21 | 22 | s1=rows*cols*sizeof(double); 23 | d1=realloc(m->elements[0],s1); 24 | memset(d1,0,s1); /*bzero(d1,s1);*/ 25 | m->elements=realloc(m->elements,cols*sizeof(double *)); 26 | m->elements[0]=d1; 27 | m->rows=rows; 28 | m->cols=cols; 29 | for(j=1,i=rows;jelements[j]=&(m->elements[0][i]); 31 | return m; 32 | } 33 | 34 | FMatrix *SubFMatrix(FMatrix *m,int row,int col,int rows,int cols) 35 | { 36 | FMatrix *m1; 37 | int j; 38 | 39 | m1=malloc(sizeof(Matrix)); 40 | m1->rows=rows; 41 | m1->cols=cols; 42 | m1->elements=calloc(cols,sizeof(double *)); 43 | 44 | for(j=0;jelements[j]=&(m->elements[j+col][row]); 46 | return m1; 47 | } 48 | 49 | void FreeSubFMatrix(FMatrix *m) 50 | { 51 | free(m->elements); 52 | free(m); 53 | } 54 | 55 | void FMatrixSet(FMatrix *m,double d,int row,int col,int rows,int cols) 56 | { 57 | int i,j; 58 | for(i=0;ielements[col+j][row+i]=d; 61 | } 62 | 63 | void FMatrixCopy( 64 | FMatrix *B,int rowb,int colb, 65 | FMatrix *A,int rowa,int cola,int rows,int cols) 66 | { 67 | int j; 68 | for(j=0;jelements[colb+j][rowb]), 71 | &(A->elements[cola+j][rowa]), 72 | rows*sizeof(double)); 73 | } 74 | 75 | void FTranspose(FMatrix *out,FMatrix *in) 76 | { 77 | int i,j; 78 | 79 | for(i=0;irows;i++) 80 | for(j=0;jcols;j++) 81 | out->elements[i][j]=in->elements[j][i]; 82 | } 83 | 84 | int Fsub2ind(int rows,int cols,int i,int j) 85 | { 86 | return j*rows+i; 87 | } 88 | 89 | FMatrix *MakeFMatrix(int rows,int cols) 90 | { 91 | FMatrix *m; 92 | 93 | m=malloc(sizeof(FMatrix)); 94 | m->elements=DoubleFMatrix(rows,cols); 95 | m->rows=rows; 96 | m->cols=cols; 97 | return m; 98 | } 99 | 100 | void FreeFMatrix(FMatrix *m) 101 | { 102 | FreeDoubleFMatrix(m->elements); 103 | free(m); 104 | } 105 | 106 | double **DoubleFMatrix(int rows,int cols) 107 | { 108 | double **ppd; 109 | int i,j; 110 | size_t s1; 111 | 112 | ppd=malloc(cols*sizeof(double *)); 113 | ppd[0]=calloc(rows*cols,sizeof(double)); 114 | for(j=1,i=rows;jcols;j++) 129 | for(i=0;irows;i++) 130 | out->elements[i][j]=in->elements[j][i]; 131 | } 132 | 133 | void C2FMatrix(FMatrix *out,Matrix *in) 134 | { 135 | int i,j; 136 | for(j=0;jcols;j++) 137 | for(i=0;irows;i++) 138 | out->elements[j][i]=in->elements[i][j]; 139 | } 140 | 141 | void PrintFMatrix(char *filename,FMatrix *matrix) 142 | { 143 | int i,j; 144 | FILE *file; 145 | 146 | if(!strcmp(filename,"stdout")) 147 | file=stdout; 148 | else if(!strcmp(filename,"stderr")) 149 | file=stderr; 150 | else 151 | { 152 | if((file=fopen(filename,"w"))==NULL) 153 | return; 154 | } 155 | 156 | for(i=0;irows;i++) 157 | { 158 | for(j=0;jcols;j++) 159 | fprintf(file,"%.16e ",matrix->elements[j][i]); 160 | fprintf(file,"\n"); 161 | } 162 | fprintf(file,"\n\n\n"); 163 | 164 | if(file!=stdout && file!=stderr) 165 | fclose(file); 166 | } 167 | 168 | void FMatrixMult(FMatrix *mout,FMatrix *m1,FMatrix *m2) 169 | { 170 | int i,j,k; 171 | for(i=0;irows;i++) 172 | for(j=0;mout->elements[j][i]=0,jcols;j++) 173 | for(k=0;kcols;k++) 174 | mout->elements[j][i]+=m1->elements[k][i]*m2->elements[j][k]; 175 | } 176 | 177 | void Vector3Add(double *vout,double *v1,double *v2,double *v3,int n) 178 | { 179 | int i; 180 | for(i=0;ielements[row+i][col+j]=d; 191 | } 192 | 193 | void MatrixCopy( 194 | Matrix *B,int rowb,int colb, 195 | Matrix *A,int rowa,int cola,int rows,int cols) 196 | { 197 | int i; 198 | for(i=0;ielements[rowb+i][colb]), 201 | &(A->elements[rowa+i][cola]), 202 | cols*sizeof(double)); 203 | } 204 | 205 | Matrix *MakeMatrix(int rows,int cols) 206 | { 207 | Matrix *m; 208 | 209 | m=malloc(sizeof(Matrix)); assert(m!=NULL); 210 | m->elements=DoubleMatrix(rows,cols); 211 | m->rows=rows; 212 | m->cols=cols; 213 | return m; 214 | } 215 | 216 | void FreeMatrix(Matrix *matrix) 217 | { 218 | FreeDoubleMatrix(matrix->elements); 219 | assert(matrix!=NULL); free(matrix); 220 | } 221 | 222 | double **DoubleMatrix(int rows,int cols) 223 | { 224 | double **tmp; 225 | size_t sizeofmatrix; 226 | int i,j; 227 | 228 | tmp=malloc(rows*sizeof(double *)); assert(tmp!=NULL); 229 | tmp[0]=calloc(rows*cols,sizeof(double)); assert(tmp[0]!=NULL); 230 | for(i=1,j=cols;irows;i++) 266 | { 267 | for(j=0;jcols;j++) 268 | fprintf(file,"%f ",matrix->elements[i][j]); 269 | fprintf(file,"\n"); 270 | } 271 | fprintf(file,"\n\n\n"); 272 | 273 | if(file!=stdout && file!=stderr) 274 | fclose(file); 275 | } 276 | 277 | void PrintVector(char *filename,double *f,int nf) 278 | { 279 | int i; 280 | FILE *file; 281 | 282 | if(!strcmp(filename,"stdout")) 283 | file=stdout; 284 | else if(!strcmp(filename,"stderr")) 285 | file=stderr; 286 | else 287 | { 288 | if((file=fopen(filename,"w"))==NULL) 289 | return; 290 | } 291 | for(i=0;irows;i++) 323 | for(j=0;jcols;j++) 324 | { 325 | mout->elements[i][j]=0.0; 326 | for(k=0;kcols;k++) 327 | mout->elements[i][j]+=m1->elements[i][k]*m2->elements[k][j]; 328 | } 329 | } 330 | -------------------------------------------------------------------------------- /src/matrix.h: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | /* 15 | * matrix.h 16 | * matrix handling routines 17 | */ 18 | 19 | #ifndef _MATRIX_H_ 20 | #define _MATRIX_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | typedef struct MatrixStruct 28 | { 29 | double **elements; 30 | int rows,cols; 31 | } Matrix; 32 | 33 | void MatrixCopy( 34 | Matrix *B,int rowb,int colb, 35 | Matrix *A,int rowa,int cola,int rows,int cols); 36 | void MatrixSet(Matrix *m,double d,int row,int col,int rows,int cols); 37 | Matrix *MakeMatrix(int rows,int cols); 38 | void MatrixMult(Matrix *mout,Matrix *m1,Matrix *m2); 39 | void FreeMatrix(Matrix *matrix); 40 | void PrintMatrix(char *filename,Matrix *matrix); 41 | void PrintVector(char *filename,double *f,int nf); 42 | void PrintiVector(char *filename,int *f,int nf); 43 | 44 | double **DoubleMatrix(int rows,int cols); 45 | void FreeDoubleMatrix(double **d); 46 | 47 | /* 48 | * stores and manipulate matrices columnwise (fortran style). 49 | */ 50 | 51 | typedef struct FMatrixStruct 52 | { 53 | double **elements; 54 | int rows,cols; 55 | } FMatrix; 56 | 57 | void FMatrixCopy( 58 | FMatrix *B,int rowb,int colb, 59 | FMatrix *A,int rowa,int cola,int rows,int cols); 60 | void FMatrixSet(FMatrix *m,double d,int row,int col,int rows,int cols); 61 | FMatrix *ResizeFMatrix(FMatrix *m,int rows,int cols); 62 | FMatrix *MakeFMatrix(int rows,int cols); 63 | FMatrix *SubFMatrix(FMatrix *m,int row,int col,int rows,int cols); 64 | void FreeSubFMatrix(FMatrix *m); 65 | void FMatrixMult(FMatrix *mout,FMatrix *m1,FMatrix *m2); 66 | void FreeFMatrix(FMatrix *matrix); 67 | void PrintFMatrix(char *filename,FMatrix *matrix); 68 | 69 | double **DoubleFMatrix(int rows,int cols); 70 | void FreeDoubleFMatrix(double **d); 71 | 72 | int FSub2ind(int rows,int cols,int i,int j); 73 | void FTranspose(FMatrix *out,FMatrix *in); 74 | void F2CMatrix(Matrix *out,FMatrix *in); 75 | void C2FMatrix(FMatrix *out,Matrix *in); 76 | 77 | void Vector3Add(double *vout,double *v1,double *v2,double *v3,int n); 78 | double dot(double *v1,double *v2,int n); 79 | 80 | /************************************************** 81 | 82 | Description of functions 83 | ----------------------- 84 | 85 | Matrix *MakeMatrix(int rows,int cols); 86 | void FreeMatrix(Matrix *matrix); 87 | FMatrix *MakeFMatrix(int rows,int cols); 88 | void FreeFMatrix(FMatrix *matrix); 89 | 90 | Make/Free a matrix of size [rows,cols]. and initialises all elements to 0. 91 | -------------------------------------------------------------- 92 | 93 | MatrixCopy() 94 | FMatrixCopy() 95 | 96 | copy the submatrix in A of size [rows,cols] at position [rowa,cola] 97 | in the the matrix B at position [rowb,colb] 98 | make sure B has enough memory already allocated 99 | ------------------------------------------------------- 100 | 101 | void MatrixSet(Matrix *m,double d,int row,int col,int rows,int cols); 102 | void FMatrixSet(FMatrix *m,double d,int row,int col,int rows,int cols); 103 | 104 | sets all the elements in the submatrix of size [rows,cols] positioned 105 | at [row,col] in m to the number d 106 | ------------------------------------------------------- 107 | 108 | void Transpose(Matrix *out,Matrix *in); 109 | void FTranspose(FMatrix *out,FMatrix *in); 110 | 111 | returns the transpose of in in out. 112 | ------------------------------------------ 113 | 114 | int Fsub2ind(int rows,int cols,int i,int j); 115 | 116 | determines the equivalent single index corresponding 117 | subscript values row i, column j for a matrix of size 118 | "rows","cols" stored columnwise in a 1D array 119 | (ie Fortran style). 120 | see matlabs function sub2ind 121 | -------------------------------------------- 122 | 123 | 124 | void F2CMatrix(Matrix *out,FMatrix *in) 125 | void C2FMatrix(FMatrix *out,Matrix *in) 126 | 127 | copies data between the two types of matrices. in and out must 128 | be of the same size. ie out->cols=in->cols and out->rows=in->rows 129 | --------------------------------------- 130 | 131 | void MatrixMult(Matrix *mout,Matrix *m1,Matrix *m2); 132 | void FMatrixMult(FMatrix *mout,FMatrix *m1,FMatrix *m2); 133 | 134 | multiplies the two matrices 135 | mout: result stored here size(mout)=[m1->rows,m2->cols] 136 | m1,m2: matrices to multiply. note this should hold m1->cols=m2->rows 137 | ------------------------------------------------------- 138 | 139 | void Vector3Add(double *vout,double *v1,double *v2,double *v3,int n) 140 | 141 | adds three vectors element by elements 142 | vout: space to store result 143 | v1,v2,v3: the 3 vectors to add 144 | n: the length of vout,v1,v2,v3 145 | 146 | ********************************************************/ 147 | 148 | #endif 149 | -------------------------------------------------------------------------------- /src/ntg.c: -------------------------------------------------------------------------------- 1 | /*********************************************************/ 2 | /* */ 3 | /* NTG 2.2 */ 4 | /* Copyright (c) 2000 by */ 5 | /* California Institute of Technology */ 6 | /* Control and Dynamical Systems */ 7 | /* Mark Milam, Kudah Mushambi, and Richard Murray */ 8 | /* All right reserved. */ 9 | /* */ 10 | /*********************************************************/ 11 | 12 | 13 | 14 | #include "ntg.h" 15 | 16 | /* globals */ 17 | static ConcatColloc *Gccolloc; 18 | static double *GZ; 19 | static double *Gbps; 20 | 21 | static FMatrix *GcJac; 22 | 23 | static int Gnlic,Gnltc,Gnlfc; 24 | 25 | static AV *Ginitialconstrav; static int Gninitialconstrav; 26 | static AV *Gtrajectoryconstrav; static int Gntrajectoryconstrav; 27 | static AV *Gfinalconstrav; static int Gnfinalconstrav; 28 | 29 | static AV *Ginitialcostav; static int Gninitialcostav; 30 | static AV *Gtrajectorycostav; static int Gntrajectorycostav; 31 | static AV *Gfinalcostav; static int Gnfinalcostav; 32 | 33 | static int Gnnlic,Gnnltc,Gnnlfc; 34 | static void (*Gnlicf)(int *,int *,double *,double **,double **); 35 | static void (*Gnltcf)(int *,int *,int *,double *,double **,double **); 36 | static void (*Gnlfcf)(int *,int *,double *,double **,double **); 37 | 38 | static int Gnicf,Gnucf,Gnfcf; 39 | static void (*Gicf)(int *,int *,double *,double *,double **); 40 | static void (*Gucf)(int *,int *,int *,double *,double *,double **); 41 | static void (*Gfcf)(int *,int *,double *,double *,double **); 42 | 43 | extern void npsol_(); 44 | extern void npfile_(); 45 | extern void npoptn_(); 46 | extern void fopen_(); 47 | extern void fclose_(); 48 | 49 | static void NPfuncon(); 50 | static void NPfunobj(); 51 | 52 | /* Trajectory generation parameters */ 53 | 54 | void ntg( 55 | int nout, /* number of outputs */ 56 | double *bps, /* breakpoint sequence */ 57 | int nbps, /* number of breakpoints (same for all outputs ) */ 58 | int *kninterv, /* knot point intervals */ 59 | double **knots, /* Knot points for each output */ 60 | int *order, /* orders of the outputs */ 61 | int *mult, /* multiplicities of the outputs */ 62 | int *maxderiv, /* Max derivative + 1 occurring in each output */ 63 | double *initialguess, /* initial b-spline coefficient guess */ 64 | int nlic,double **lic, 65 | int nltc,double **ltc, 66 | int nlfc,double **lfc, 67 | int nnlic, void (*nlicf)(int *,int *,double *,double **,double **), 68 | int nnltc, void (*nltcf)(int *,int *,int *,double *,double **,double **), 69 | int nnlfc, void (*nlfcf)(int *,int *,double *,double **,double **), 70 | int ninitialconstrav, AV *initialconstrav, 71 | int ntrajectoryconstrav, AV *trajectoryconstrav, 72 | int nfinalconstrav, AV *finalconstrav, 73 | double *lowerb, 74 | double *upperb, 75 | int nicf, void (*icf)(int *,int *,double *,double *,double **), 76 | int nucf, void (*ucf)(int *,int *,int *,double *,double *,double **), 77 | int nfcf, void (*fcf)(int *,int *,double *,double *,double **), 78 | int ninitialcostav, AV *initialcostav, 79 | int ntrajectorycostav, AV *trajectorycostav, 80 | int nfinalcostav, AV *finalcostav, 81 | int *istate,double *clambda,double *R, 82 | int *inform,double *objective 83 | ) 84 | { 85 | int i; 86 | 87 | int NPn; 88 | int NPnclin; 89 | int NPncnln; 90 | int NPldA; 91 | int NPldJ; 92 | int NPldR; 93 | int *NPinform; 94 | int NPiter; 95 | int NPleniw; 96 | int NPlenw; 97 | int *NPistate; 98 | int *NPiw; 99 | 100 | double *NPf; 101 | double *NPA; 102 | double *NPbl; 103 | double *NPbu; 104 | double *NPc; 105 | double *NPcJac; 106 | double *NPclambda; 107 | double *NPg; 108 | double *NPR; 109 | double *NPx=initialguess; 110 | double *NPw; 111 | 112 | FMatrix *Matlc,*Matlic,*Matltc,*Matlfc; 113 | 114 | Gccolloc=ConcatCollocMatrix(nout,knots,kninterv,bps, 115 | nbps,maxderiv,order,mult); 116 | /*cbug*/ 117 | /*PrintColloc("colloc",Gccolloc->colloc[0]);*/ 118 | 119 | GZ=calloc(Gccolloc->nZ, sizeof(double)); 120 | Gbps=bps; 121 | 122 | Gnlic=nlic; 123 | Gnltc=nltc; 124 | Gnlfc=nlfc; 125 | Gnnlic=nnlic; 126 | Gnnltc=nnltc; 127 | Gnnlfc=nnlfc; 128 | 129 | Gnlicf=nlicf; 130 | Gnltcf=nltcf; 131 | Gnlfcf=nlfcf; 132 | 133 | Ginitialconstrav=initialconstrav; 134 | Gtrajectoryconstrav=trajectoryconstrav; 135 | Gfinalconstrav=finalconstrav; 136 | Gninitialconstrav=ninitialconstrav; 137 | Gntrajectoryconstrav=ntrajectoryconstrav; 138 | Gnfinalconstrav=nfinalconstrav; 139 | 140 | Gnicf=nicf; 141 | Gnucf=nucf; 142 | Gnfcf=nfcf; 143 | Gicf=icf; 144 | Gucf=ucf; 145 | Gfcf=fcf; 146 | 147 | Ginitialcostav=initialcostav; 148 | Gtrajectorycostav=trajectorycostav; 149 | Gfinalcostav=finalcostav; 150 | Gninitialcostav=ninitialcostav; 151 | Gntrajectorycostav=ntrajectorycostav; 152 | Gnfinalcostav=nfinalcostav; 153 | 154 | /* npsol parameters */ 155 | NPn=Gccolloc->nC; 156 | NPnclin=Gnlic+Gnltc*nbps+Gnlfc; 157 | NPncnln=Gnnlic+Gnnltc*nbps+Gnnlfc; 158 | NPinform=inform; 159 | NPf=objective; 160 | 161 | printNTGBanner(); 162 | if(NPnclin==0) 163 | { 164 | NPldA=1; 165 | Matlc=MakeFMatrix(NPldA,1); 166 | } 167 | else 168 | { 169 | NPldA=NPnclin; 170 | Matlc=MakeFMatrix(NPldA,NPn); 171 | 172 | if(nlic!=0) 173 | { 174 | Matlic=malloc(sizeof(FMatrix)); 175 | Matlic->elements=lic; 176 | Matlic->rows=Gccolloc->nz; 177 | Matlic->cols=nlic; 178 | } 179 | else 180 | Matlic=NULL; 181 | 182 | if(nltc!=0) 183 | { 184 | Matltc=malloc(sizeof(FMatrix)); 185 | Matltc->elements=ltc; 186 | Matltc->rows=Gccolloc->nz; 187 | Matltc->cols=nltc; 188 | } 189 | else 190 | Matltc=NULL; 191 | 192 | if(nlfc!=0) 193 | { 194 | Matlfc=malloc(sizeof(FMatrix)); 195 | Matlfc->elements=lfc; 196 | Matlfc->rows=Gccolloc->nz; 197 | Matlfc->cols=nlfc; 198 | } 199 | else 200 | Matlfc=NULL; 201 | LinearConstraintsMatrix(Matlc,Matlic,Matltc,Matlfc,Gccolloc); 202 | if(nlic!=0) free(Matlic); 203 | if(nltc!=0) free(Matltc); 204 | if(nlfc!=0) free(Matlfc); 205 | } 206 | NPA=Matlc->elements[0]; 207 | /*cbug*/ 208 | /*PrintFMatrix("NPA",Matlc);*/ 209 | 210 | if(NPncnln==0) 211 | { 212 | NPldJ=1; 213 | GcJac=MakeFMatrix(NPldJ,1); 214 | } 215 | else 216 | { 217 | NPldJ=NPncnln; 218 | GcJac=MakeFMatrix(NPldJ,NPn); 219 | } 220 | NPcJac=GcJac->elements[0]; 221 | 222 | NPldR=NPn; 223 | 224 | NPbu=calloc((NPn+NPnclin+NPncnln),sizeof(double)); 225 | NPbl=calloc((NPn+NPnclin+NPncnln),sizeof(double)); 226 | bounds(NPbu,upperb,Gccolloc->nC, 227 | nlic,nltc,nlfc,nnlic,nnltc,nnlfc,Gccolloc->nbps,DBL_MAX); 228 | bounds(NPbl,lowerb,Gccolloc->nC, 229 | nlic,nltc,nlfc,nnlic,nnltc,nnlfc,Gccolloc->nbps,-DBL_MAX); 230 | 231 | NPistate=istate; 232 | NPc=calloc(NPncnln,sizeof(double)); 233 | NPclambda=clambda; 234 | NPg=calloc(NPn,sizeof(double)); 235 | NPR=R; 236 | 237 | NPleniw=(3*NPn)+NPnclin+(2*NPncnln); 238 | NPiw=calloc(NPleniw,sizeof(int)); 239 | if(NPnclin==0 && NPncnln==0) 240 | NPlenw=20*NPn; 241 | else if(NPncnln==0) 242 | NPlenw=(2*NPn*NPn)+(20*NPn)+(11*NPnclin); 243 | else 244 | NPlenw=(2*NPn*NPn)+(NPn*NPnclin)+(2*NPn*NPncnln)+(20*NPn)\ 245 | +(11*NPnclin)+(21*NPncnln); 246 | NPw=calloc(NPlenw,sizeof(double)); 247 | 248 | npsoloption("nolist"); 249 | npsoloption("derivative level = 3"); 250 | npsol_(&NPn,&NPnclin,&NPncnln,&NPldA,&NPldJ,&NPldR,NPA,NPbl, 251 | NPbu,NPfuncon,NPfunobj,NPinform,&NPiter,NPistate, 252 | NPc,NPcJac,NPclambda,NPf,NPg,NPR,NPx,NPiw,&NPleniw, 253 | NPw,&NPlenw); 254 | 255 | free(NPbu); 256 | free(NPbl); 257 | free(NPc); 258 | free(NPg); 259 | free(NPiw); 260 | free(NPw); 261 | 262 | FreeConcatColloc(Gccolloc); 263 | FreeFMatrix(Matlc); 264 | FreeFMatrix(GcJac); 265 | 266 | free(GZ); 267 | } 268 | 269 | void npsoloption(char *option) 270 | { 271 | npoptn_(option,(long)strlen(option)); 272 | } 273 | 274 | void NPfunobj( 275 | int *mode, 276 | int *n, 277 | double *x, 278 | double *y, 279 | double *yprime, 280 | int *nstate) 281 | { 282 | size_t i1; 283 | double I=0.0,*dI; 284 | double In=0.0,*dIn; 285 | double F=0.0,*dF; 286 | 287 | if(Gnicf!=0) 288 | updateZ(GZ,Gccolloc,x,Ginitialcostav,Gninitialcostav,AVINITIAL); 289 | if(Gnucf!=0) 290 | updateZ(GZ,Gccolloc,x,Gtrajectorycostav,Gntrajectorycostav,AVTRAJECTORY); 291 | if(Gnfcf!=0) 292 | updateZ(GZ,Gccolloc,x,Gfinalcostav,Gnfinalcostav,AVFINAL); 293 | 294 | switch(*mode) 295 | { 296 | case 0: /* only compute objective function */ 297 | if(Gnicf==1) 298 | InitialCost(mode,nstate,&I,NULL,Gicf,Gccolloc,GZ); 299 | if(Gnucf==1) 300 | IntegratedCost(mode,nstate,&In,NULL,Gbps,Gucf,Gccolloc,GZ); 301 | if(Gnfcf==1) 302 | FinalCost(mode,nstate,&F,NULL,Gfcf,Gccolloc,GZ); 303 | *y=I+In+F; 304 | break; 305 | case 1: /* only compute objective gradient */ 306 | dI=calloc(Gccolloc->nC,sizeof(double)); 307 | dIn=calloc(Gccolloc->nC,sizeof(double)); 308 | dF=calloc(Gccolloc->nC,sizeof(double)); 309 | if(Gnicf!=0) 310 | InitialCost(mode,nstate,NULL,dI,Gicf,Gccolloc,GZ); 311 | if(Gnucf!=0) 312 | IntegratedCost(mode,nstate,NULL,dIn,Gbps,Gucf,Gccolloc,GZ); 313 | if(Gnfcf!=0) 314 | FinalCost(mode,nstate,NULL,dF,Gfcf,Gccolloc,GZ); 315 | Vector3Add(yprime,dI,dIn,dF,Gccolloc->nC); 316 | free(dI);free(dIn);free(dF); 317 | break; 318 | case 2: /* compute both objective function and gradients */ 319 | dI=calloc(Gccolloc->nC,sizeof(double)); 320 | dIn=calloc(Gccolloc->nC,sizeof(double)); 321 | dF=calloc(Gccolloc->nC,sizeof(double)); 322 | if(Gnicf!=0) 323 | InitialCost(mode,nstate,&I,dI,Gicf,Gccolloc,GZ); 324 | if(Gnucf!=0) 325 | IntegratedCost(mode,nstate,&In,dIn,Gbps,Gucf,Gccolloc,GZ); 326 | if(Gnfcf!=0) 327 | FinalCost(mode,nstate,&F,dF,Gfcf,Gccolloc,GZ); 328 | *y=I+In+F; 329 | Vector3Add(yprime,dI,dIn,dF,Gccolloc->nC); 330 | free(dI);free(dIn);free(dF); 331 | break; 332 | default: 333 | *nstate=-1; 334 | } 335 | } 336 | 337 | void NPfuncon( 338 | int *mode, 339 | int *ncnln, 340 | int *n, 341 | int *nrowj, 342 | int *needc, 343 | double *x, 344 | double *C, 345 | double *Cjac, 346 | int *nstate) 347 | { 348 | if(Gnnlic!=0) 349 | updateZ(GZ,Gccolloc,x,Ginitialconstrav,Gninitialconstrav,AVINITIAL); 350 | if(Gnnltc!=0) 351 | updateZ(GZ,Gccolloc,x,Gtrajectoryconstrav,Gntrajectoryconstrav,AVTRAJECTORY); 352 | if(Gnnlfc!=0) 353 | updateZ(GZ,Gccolloc,x,Gfinalconstrav,Gnfinalconstrav,AVFINAL); 354 | switch(*mode) 355 | { 356 | case 0: /* only compute constraint functions */ 357 | NonLinearConstraints(mode,nstate,C,NULL, 358 | Gnnlic,Gnlicf,Gnnltc,Gnltcf,Gnnlfc,Gnlfcf,Gccolloc,GZ); 359 | break; 360 | case 1: /* only compute constraint gradient */ 361 | NonLinearConstraints(mode,nstate,NULL,GcJac, 362 | Gnnlic,Gnlicf,Gnnltc,Gnltcf,Gnnlfc,Gnlfcf,Gccolloc,GZ); 363 | break; 364 | case 2: /* compute both constraint function and gradients */ 365 | NonLinearConstraints(mode,nstate,C,GcJac, 366 | Gnnlic,Gnlicf,Gnnltc,Gnltcf,Gnnlfc,Gnlfcf,Gccolloc,GZ); 367 | break; 368 | default: 369 | *mode=-1; 370 | } 371 | } 372 | 373 | /* see matlab's linspace() */ 374 | void linspace(double *v,double d0,double d1,int n) 375 | { 376 | int i; 377 | double interv; 378 | if(d0==d1) 379 | { 380 | for(i=0;i 26 | #include 27 | #include "av.h" 28 | #include "colloc.h" 29 | #include "constraints.h" 30 | #include "cost.h" 31 | #include "matrix.h" 32 | 33 | /* 34 | nout: number of outputs 35 | bps,nbps: an array with the breakpoint sequence and the number of breakpoints 36 | ninterv: number of intervals for each output 37 | knots: knot points for each output 38 | order: order of spline outputs 39 | mult: continuity condition of the outputs 40 | maxderiv: maximum derivative required for each output 41 | initialguess: self-explanatory 42 | nlic: number of linear initial constraints. 43 | nlic,lic: linear initial constraints. size[nlic,sum(maxderiv,over all outputs)] 44 | nltc,ltc: linear trajectory constraints. size(nltc,sum(maxderiv)); 45 | nlfc,lfc: linear final constraints. size(nlfc,sum(maxderiv)); 46 | 47 | nnlic: no. of nonlinear initial constraints. 48 | nlicf: nonlinear initial constraint function. 49 | nnltc: no. of nonlinear trajectory constraints. 50 | nltcf: nonlinear trajectory constraint function. 51 | nnlfc: no. of nonlinear final constraints. 52 | nlfcf: nonlinear final constraint function. 53 | constrAV,nconstrAV: the "active" variables used in evaluating the 54 | constraints. 55 | 56 | upperb,lowerb: upper and lower bounds on the constraints. 57 | size[nlic+nltc+nlfc+nnlic+nnltc+nnlfc] 58 | 59 | icf: initial cost function 60 | ucf: unintegrated cost function 61 | fcf: final cost function 62 | costAV,ncostAV: the variables used in evaluating the costs. 63 | 64 | istate (int *) : an array of length 65 | ncoeff + nlic+nltc*nbps+nlfc + nnlic+nnltc*nbps+nnlfc 66 | clambda (double *): an array of the same length as istate. 67 | R (double *) : 68 | an array length ncoef*ncoef 69 | 70 | */ 71 | 72 | void ntg( 73 | int nout,double *bps, int nbps,int *kninterv, double **knots, 74 | int *order,int *mult,int *max_deriv, 75 | double *initialguess, 76 | 77 | int nlic,double **lic, 78 | int nltc,double **ltc, 79 | int nlfc,double **lfc, 80 | 81 | int nnlic,void (*nlicf)(int *,int *,double *,double **,double **), 82 | int nnltc,void (*nltcf)(int *,int *,int *,double *,double **,double **), 83 | int nnlfc,void (*nlfcf)(int *,int *,double *,double **,double **), 84 | int ninitialconstrav,AV *initialconstrav, 85 | int ntrajectoryconstrav,AV *trajectoryconstrav, 86 | int nfinalconstrav,AV *finalconstrav, 87 | 88 | double *lowerb,double *upperb, 89 | 90 | int nicf,void (*icf)(int *,int *,double *,double *,double **), 91 | int nucf,void (*ucf)(int *,int *,int *,double *,double *,double **), 92 | int nfcf,void (*fcf)(int *,int *,double *,double *,double **), 93 | int ninitialcostav,AV *initialcostav, 94 | int ntrajectorycostav,AV *trajectorycostav, 95 | int nfinalcostav,AV *finalcostav, 96 | 97 | int *istate,double *clambda,double *R, 98 | int *inform,double *objective 99 | ); 100 | void npsoloption(char *type); 101 | 102 | /* see matlab's linspace() */ 103 | void linspace(double *v,double d0,double d1,int n); 104 | void printNTGBanner(void); 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /src/options.c: -------------------------------------------------------------------------------- 1 | #define MXPARM 30 2 | 3 | extern struct NPPAR2 4 | { 5 | double rpsvnp[MXPARM], 6 | double cdint, /* Central difference interval */ 7 | double ctol, /* Nonlinear feasibility */ 8 | double dxlim, /* Step Limit */ 9 | double epsrf, /* Function Precision */ 10 | double eta, /* Line search tolerance */ 11 | double fdint, /* Difference interval */ 12 | double ftol, /* Optimality tolerance */ 13 | double hcndbd, 14 | double rpadnp[22] 15 | } nppar2_ 16 | --------------------------------------------------------------------------------