ocra-recipes
Doxygen documentation for the ocra-recipes repository
OFSQP.h
Go to the documentation of this file.
1 #ifndef OFSQP_H
2 #define OFSQP_H
3 #pragma once
4 
5 
6 #include <stdio.h>
7 #include <math.h>
8 #include <stdlib.h>
9 
10 
11 namespace ocra
12 {
13  namespace fsqpDetails
14  {
15  struct Info
16  {
17  int nnineq, M, ncallg, ncallf, mode, modec;
18  int tot_actf_sip,tot_actg_sip,nfsip,ncsipl,ncsipn; /* SIP */
19  };
20 
21  struct Prnt
22  {
23  int iprint,info,ipd,iter,initvl,iter_mod;
24  FILE *io;
25  };
26 
27  struct Grd
28  {
29  double epsmac,rteps,udelta,valnom;
30  };
31 
32  struct Log
33  {
34  bool rhol_is1,get_ne_mult,first,local,update,d0_is0,dlfeas;
35  };
36  }
37 
38  class OFSQPProblem
39  {
40  public:
41  OFSQPProblem();
42 
43  virtual void obj(int nparam, int j, double* x, double* fj, void* cd)= 0;
44  virtual void constr(int nparam, int j, double* x, double* gj, void* cd) = 0;
45  virtual void gradob(int nparam, int j, double* x, double* gradfj, void* cd);
46  virtual void gradcn(int nparam, int j, double* x, double* gradgj, void* cd);
47 
48  void invalidateX();
49  void validateX();
50  bool isXNew();
51 
52  void setFSQPstruct(fsqpDetails::Grd& grd, fsqpDetails::Prnt& prnt);
53  void resetFSQPstruct();
54 
55  private:
56  bool x_is_new;
57  fsqpDetails::Grd* glob_grd;
58  fsqpDetails::Prnt* glob_prnt;
59  };
60 
61 /**************************************************************/
62 /* Gradients - Finite Difference */
63 /**************************************************************/
64 
65 class OFSQP
66 {
67 
68 public:
69  OFSQP(void);
70  ~OFSQP(void);
71 
72 public:
73  bool isXNew(void);
74  void resetNewX(void);
75 
76 
77 private:
78  enum eFnType
79  {
80  NONE = 0,
81  OBJECT = 1,
82  CONSTR = 2
83  };
84 
85  struct _objective {
86  double val;
87  double *grad;
88  double mult;
89  double mult_L; /* mode A=1 */
90  bool act_sip; /* SIP */
91  };
92 
93  struct _constraint {
94  double val;
95  double *grad;
96  double mult;
97  bool act_sip; /* SIP */
98  bool d1bind; /* SR constraints */
99  };
100 
101  struct _parameter {
102  double *x;
103  double *bl;
104  double *bu;
105  double *mult;
106  void *cd; /* Client data pointer */
107  };
108 
109  struct _violation { /* SIP */
110  int type;
111  int index;
112  };
113 
114 private:
115  fsqpDetails::Info glob_info;
116  fsqpDetails::Prnt glob_prnt;
117  fsqpDetails::Grd glob_grd;
118  fsqpDetails::Log glob_log;
119 
120 
121 public :
122  /**************************************************************/
123  /* Prototype for CFSQP - */
124  /**************************************************************/
125 
126  void
127  cfsqp(int nparam,int nf,int nfsr,int nineqn,int nineq,int neqn,
128  int neq,int ncsrl,int ncsrn,int *mesh_pts,
129  int mode,int iprint,int miter,int *inform,double bigbnd,
130  double eps,double epseqn,double udelta,double *bl,double *bu,
131  double *x,double *f,double *g,double *lambda,
132  OFSQPProblem& problem, void *cd);
133 
134 
135 private:
136  /****jrl : change form char[] to char* ****/
137  const char* cfsqp_version;
138  double bgbnd,tolfea;
139  int maxit;
140 
141  OFSQPProblem* pb;
142 
143  /* Declare and initialize user-accessible flag indicating */
144  /* whether x sent to user functions has been changed within */
145  /* CFSQP. */
146 private:
147  //bool x_is_new;
148 
149 private:
150  /* Declare and initialize user-accessible stopping criterion */
151  double objeps;
152  double objrep;
153  double gLgeps;
154  int nstop;
155 
156  /* Workspace */
157  int *iw;
158  double *w;
159  int lenw, leniw;
160 
161  /***************************************************************/
162  /* Memory Utilities */
163  /***************************************************************/
164 private:
165  static int *make_iv(int);
166  static double *make_dv(int);
167  static double **make_dm(int, int);
168  static void free_iv(int *);
169  static void free_dv(double *);
170  static void free_dm(double **, int);
171  static double *convert(double **, int, int);
172 
173  /***************************************************************/
174  /* Utility Subroutines */
175  /***************************************************************/
176 
177 /* int
178  ql0001_(int *,int *,int *,int *,int *,int *,double *,double *,
179  double *,double *,double *,double *,double *,double *,
180  int *,int *,int *,double *,int *,int *,int *,double *);*/
181  static void diagnl(int, double, double **);
182  void error(const char string[],int *);
183  void
184  estlam(int,int,int *,double,double **,double *,double *,double *,
185  struct _constraint *,double *,double *,double *,double *);
186  static double *colvec(double **,int,int);
187  static double scaprd(int,double *,double *);
188  static double small(void);
189  static bool fuscmp(double,double);
190  static int indexs(int,int);
191  static void matrcp(int,double **,int,double **);
192  static void matrvc(int,int,double **,double *,double *);
193  static void nullvc(int,double *);
194  void
195  resign(int,int,double *,double *,double *,struct _constraint *,
196  double *,int,int);
197  static void sbout1(FILE *,int,const char *,double,double *,int,int);
198  static void sbout2(FILE *,int,int,const char *,const char *,double *);
199  static void shift(int,int,int *);
200  double
201  slope(int,int,int,int,int,struct _objective *,double *,double *,
202  double *,double,double,int,double *,int);
203  static bool element(int *,int,int);
204 
205 
206  /**************************************************************/
207  /* Main routines for optimization - */
208  /**************************************************************/
209 
210  void
211  cfsqp1(int,int,int,int,int,int,int,int,int,int,int *,int,
212  int,int,int,double,double,int *,int *,struct _parameter *,
213  struct _constraint *,struct _objective *,double *);
214  void
215  check(int,int,int,bool *,int,int,int,int,int,int,int,int *,double,
216  double,struct _parameter *);
217  void
218  initpt(int,int,int,int,int,int,int,struct _parameter *,struct _constraint *);
219  void
220  dir(int,int,int,int,int,int,int,int,int,int,int,int,double *,
221  double,double,double *,double *,double,double *,double *,int *,
222  int *,int *,int *,int *,int *,struct _parameter *,double *,
223  double *,struct _constraint *,struct _objective *,double *,
224  double *,double *,double *,double *,double *,double **,double *,
225  double *,double *,double *,double **,double **,double *,
226  double *,struct _violation *);
227  void
228  step1(int,int,int,int,int,int,int,int,int,int,int,int *,int *,int *,
229  int *,int *,int *,int *,int *,int,double,struct _objective *,
230  double *,double *,double *,double *,double *,double *,double *,
231  double *,double *,double *,double *,struct _constraint *,
232  double *,double *,struct _violation *viol,void *);
233  void
234  hessian(int,int,int,int,int,int,int,int,int,int,int,int *,int,
235  double *,struct _parameter *,struct _objective *,
236  double,double *,double *,double *,double *,double *,
237  struct _constraint *,double *,int *,int *,double *,
238  double *,double *,double **,double *,double,int *,
239  double *,double *,double **,double *,double *,struct _violation *);
240  void
241  out(int,int,int,int,int,int,int,int,int,int,int,int *,double *,
242  struct _constraint *,struct _objective *,double,
243  double,double,double,double,int);
244  void
245  update_omega(int,int,int,int *,int,int,int,int,double,double,
246  struct _constraint *,struct _objective *,double *,
247  struct _violation *,void *,int);
248 
249  static void
250  dealloc(int,int,double *,int *,int *,struct _constraint *cs,
251  struct _parameter *);
252 
253  static void
254  dealloc1(int,int,double **,double **,double **,double *,double *,
255  double *,double *,double *,double *,double *,double *,
256  double *,double *,double *,double *,int *,int *,int *);
257 
258  void
259  dqp(int,int,int,int,int,int,int,int,int,int,int,int,int,
260  int,int,int *,struct _parameter *,double *,int,
261  struct _objective *,double,double *,struct _constraint *,
262  double **,double *,double *,double *,double *,
263  double **,double **,double *,double,int);
264  void
265  di1(int,int,int,int,int,int,int,int,int,int,int,int,int *,
266  int,struct _parameter *,double *,struct _objective *,
267  double,double *,struct _constraint *,double *,
268  double *,double *,double *,double **,double *,double);
269 
270 
271  /************************************************************/
272  /* Utility functions used by CFSQP - */
273  /* Available functions: */
274  /* diagnl error estlam */
275  /* colvec scaprd small */
276  /* fool matrvc matrcp */
277  /* nullvc resign sbout1 */
278  /* sbout2 shift slope */
279  /* fuscmp indexs element */
280  /************************************************************/
281 
282  static void fool(double, double, double *);
283 
284 };
285 
286 void testOFSQP01();
287 void testOFSQP02a();
288 void testOFSQP02b();
289 void testOFSQP03();
290 void testOFSQP04();
291 void testOFSQP05();
292 
293 }//ocra
294 
295 
296 #endif //OFSQP_H
297 
298 // cmake:sourcegroup=Solvers
ocra::testOFSQP04
void testOFSQP04()
Definition: OFSQP.cpp:4077
ocra::testOFSQP02a
void testOFSQP02a()
Definition: OFSQP.cpp:3807
ocra::testOFSQP01
void testOFSQP01()
Definition: OFSQP.cpp:3722
ocra
Optimization-based Robot Controller namespace. a library of classes to write and solve optimization p...
Definition: ContactAvoidanceConstraint.h:14
ocra::utils::convert
void convert(const LinearConstraint &cstr, const std::vector< int > &mapping, eConstraintOutput type, MatrixBase< Derived > &A, VectorBase1 &b, VectorBase2 &l, double infinity=0.)
Definition: SolverUtilities.hxx:478
ocra::testOFSQP03
void testOFSQP03()
Definition: OFSQP.cpp:3966
ocra::testOFSQP02b
void testOFSQP02b()
Definition: OFSQP.cpp:3859
ocra::testOFSQP05
void testOFSQP05()
Definition: OFSQP.cpp:4167
Generated on Wed Mar 21 2018 09:00:38 for ocra-recipes by   doxygen 1.8.11