forked from giaf/hpipm
-
Notifications
You must be signed in to change notification settings - Fork 0
/
hpipm_d_cond.h
135 lines (110 loc) · 6.54 KB
/
hpipm_d_cond.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
/**************************************************************************************************
* *
* This file is part of HPIPM. *
* *
* HPIPM -- High-Performance Interior Point Method. *
* Copyright (C) 2019 by Gianluca Frison. *
* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
* All rights reserved. *
* *
* The 2-Clause BSD License *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions are met: *
* *
* 1. Redistributions of source code must retain the above copyright notice, this *
* list of conditions and the following disclaimer. *
* 2. Redistributions in binary form must reproduce the above copyright notice, *
* this list of conditions and the following disclaimer in the documentation *
* and/or other materials provided with the distribution. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND *
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
* *
* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
* *
**************************************************************************************************/
#ifndef HPIPM_D_COND_H_
#define HPIPM_D_COND_H_
#include <blasfeo_target.h>
#include <blasfeo_common.h>
#include "hpipm_d_dense_qp.h"
#include "hpipm_d_dense_qp_sol.h"
#include "hpipm_d_ocp_qp.h"
#include "hpipm_d_ocp_qp_dim.h"
#include "hpipm_d_ocp_qp_sol.h"
#ifdef __cplusplus
extern "C" {
#endif
struct d_cond_qp_arg
{
int cond_last_stage; // condense last stage
int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2
int comp_prim_sol; // primal solution (v)
int comp_dual_sol_eq; // dual solution equality constr (pi)
int comp_dual_sol_ineq; // dual solution inequality constr (lam t)
int square_root_alg; // square root algorithm (faster but requires RSQ>0)
hpipm_size_t memsize;
};
struct d_cond_qp_ws
{
struct blasfeo_dmat *Gamma;
struct blasfeo_dmat *GammaQ;
struct blasfeo_dmat *L;
struct blasfeo_dmat *Lx;
struct blasfeo_dmat *AL;
struct blasfeo_dvec *Gammab;
struct blasfeo_dvec *l;
struct blasfeo_dvec *tmp_nbgM;
struct blasfeo_dvec *tmp_nuxM;
int bs; // block size
hpipm_size_t memsize;
};
//
hpipm_size_t d_cond_qp_arg_memsize();
//
void d_cond_qp_arg_create(struct d_cond_qp_arg *cond_arg, void *mem);
//
void d_cond_qp_arg_set_default(struct d_cond_qp_arg *cond_arg);
// condensing algorithm: 0 N2-nx3, 1 N3-nx2
void d_cond_qp_arg_set_cond_alg(int cond_alg, struct d_cond_qp_arg *cond_arg);
// set riccati-like algorithm: 0 classical, 1 square-root
void d_cond_qp_arg_set_ric_alg(int ric_alg, struct d_cond_qp_arg *cond_arg);
// condense last stage: 0 last stage disregarded, 1 last stage condensed too
void d_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qp_arg *cond_arg);
//
void d_cond_qp_arg_set_comp_prim_sol(int value, struct d_cond_qp_arg *cond_arg);
//
void d_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_cond_qp_arg *cond_arg);
//
void d_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_cond_qp_arg *cond_arg);
//
void d_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, struct d_dense_qp_dim *dense_dim);
//
hpipm_size_t d_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg);
//
void d_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws, void *mem);
//
void d_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
//
void d_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
//
void d_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
//
void d_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
// TODO remove
void d_cond_qp_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
//
void d_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // HPIPM_D_COND_H_