gradgen 0.1.0__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
gradgen-0.1.0/LICENSE ADDED
@@ -0,0 +1,21 @@
1
+ MIT License
2
+
3
+ Copyright (c) 2023 Pantelis Sopasakis
4
+
5
+ Permission is hereby granted, free of charge, to any person obtaining a copy
6
+ of this software and associated documentation files (the "Software"), to deal
7
+ in the Software without restriction, including without limitation the rights
8
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9
+ copies of the Software, and to permit persons to whom the Software is
10
+ furnished to do so, subject to the following conditions:
11
+
12
+ The above copyright notice and this permission notice shall be included in all
13
+ copies or substantial portions of the Software.
14
+
15
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21
+ SOFTWARE.
@@ -0,0 +1,4 @@
1
+ include gradgen/templates/*
2
+ include gradgen/templates/**/*
3
+ include VERSION
4
+
gradgen-0.1.0/PKG-INFO ADDED
@@ -0,0 +1,38 @@
1
+ Metadata-Version: 2.1
2
+ Name: gradgen
3
+ Version: 0.1.0
4
+ Summary: Gradient computation with AD for optimal control
5
+ Home-page: https://github.com/alphaville/gradgen
6
+ Author: ['Jie Lin', 'Pantelis Sopasakis']
7
+ Author-email: p.sopasakis@gmail.com
8
+ License: MIT License
9
+ Keywords: optimization,nonconvex,embedded
10
+ Classifier: Development Status :: 4 - Beta
11
+ Classifier: License :: OSI Approved :: MIT License
12
+ Classifier: Programming Language :: Python
13
+ Classifier: Programming Language :: Rust
14
+ Classifier: Intended Audience :: Science/Research
15
+ Classifier: Topic :: Software Development :: Libraries
16
+ Classifier: Topic :: Scientific/Engineering
17
+ Classifier: Topic :: Scientific/Engineering :: Mathematics
18
+ Classifier: Topic :: Software Development :: Code Generators
19
+ Classifier: Topic :: Software Development :: Embedded Systems
20
+ Description-Content-Type: text/markdown
21
+ License-File: LICENSE
22
+
23
+
24
+ # GradGen
25
+
26
+ Code generation for cost gradient
27
+
28
+ ### Installation instructions
29
+
30
+ To build this project locally:
31
+
32
+ - Create a virtual environment: `virtualenv -p python3 venv`
33
+ - Activate the virtual environment. On Linux/MacOS, run `source venv/bin/activate`
34
+ - Install the project: `pip install .`
35
+
36
+ The folder `playground` can be used during the design phase of the project to try out different things, but it should be removed for release.
37
+
38
+
@@ -0,0 +1,15 @@
1
+ # GradGen
2
+
3
+ Code generation for cost gradient
4
+
5
+ ### Installation instructions
6
+
7
+ To build this project locally:
8
+
9
+ - Create a virtual environment: `virtualenv -p python3 venv`
10
+ - Activate the virtual environment. On Linux/MacOS, run `source venv/bin/activate`
11
+ - Install the project: `pip install .`
12
+
13
+ The folder `playground` can be used during the design phase of the project to try out different things, but it should be removed for release.
14
+
15
+
gradgen-0.1.0/VERSION ADDED
@@ -0,0 +1 @@
1
+ 0.1.0
@@ -0,0 +1,2 @@
1
+ from gradgen.cost_gradient import *
2
+ from gradgen.definitions import *
@@ -0,0 +1,227 @@
1
+ import casadi.casadi as cs
2
+ import subprocess as subp
3
+ import os
4
+ import shutil
5
+ import jinja2
6
+ from gradgen.definitions import *
7
+
8
+
9
+ class CostGradient:
10
+
11
+ def __init__(self, x, u, f, ell, vf, N):
12
+ """
13
+ Create new CostGradient object
14
+
15
+ :param x: state symbol
16
+ :param u: input symbol
17
+ :param f: system dynamics symbol (depends on x, u)
18
+ :param ell: cost function symbol (depends on x, u)
19
+ :param vf: terminal cost symbol (depends on x)
20
+ :param N: prediction horizon (int)
21
+ """
22
+ self.__x = x
23
+ self.__u = u
24
+ self.__f = f
25
+ self.__ell = ell
26
+ self.__vf = vf
27
+ self.__N = N
28
+ self.__nx = x.size()[0]
29
+ self.__nu = u.size()[0]
30
+ self.__d = cs.SX.sym('d', self.__nx)
31
+ self.__jfx = None
32
+ self.__jfu = None
33
+ self.__ellx = None
34
+ self.__ellu = None
35
+ self.__vfx = None
36
+ self.__f_fun = None
37
+ self.__jfx_fun = None
38
+ self.__jfu_fun = None
39
+ self.__ell_fun = None
40
+ self.__ellx_fun = None
41
+ self.__ellu_fun = None
42
+ self.__vf_fun = None
43
+ self.__vfx_fun = None
44
+ self.__name = 'gradgenz'
45
+ self.__destination_path = 'codegenz'
46
+
47
+ def __target_root_dir(self):
48
+ trgt_root_abspath = os.path.join(self.__destination_path, self.__name)
49
+ return os.path.abspath(trgt_root_abspath)
50
+
51
+ def __target_externc_dir(self):
52
+ dest_abspath = os.path.join(
53
+ self.__destination_path, self.__name, 'casadi_'+self.__name, 'extern')
54
+ return os.path.abspath(dest_abspath)
55
+
56
+ def __target_casadirs_dir(self):
57
+ casadirs_abspath = os.path.join(
58
+ self.__destination_path, self.__name, 'casadi_'+self.__name)
59
+ return os.path.abspath(casadirs_abspath)
60
+
61
+ def __create_dirs(self):
62
+ if not os.path.exists(self.__target_externc_dir()):
63
+ os.makedirs(self.__target_externc_dir())
64
+ casadi_src_path = os.path.join(
65
+ self.__destination_path, self.__name, 'casadi_'+self.__name, 'src')
66
+ main_src_path = os.path.join(
67
+ self.__destination_path, self.__name, 'src')
68
+ if not os.path.exists(casadi_src_path):
69
+ os.makedirs(casadi_src_path)
70
+ if not os.path.exists(main_src_path):
71
+ os.makedirs(main_src_path)
72
+
73
+ @staticmethod
74
+ def __get_template(name, subdir=None):
75
+ subdir_path = templates_subdir(subdir)
76
+ file_loader = jinja2.FileSystemLoader(subdir_path)
77
+ env = jinja2.Environment(loader=file_loader, autoescape=True)
78
+ return env.get_template(name)
79
+
80
+ def with_name(self, name):
81
+ self.__name = name
82
+ return self
83
+
84
+ def with_target_path(self, dst_path):
85
+ self.__destination_path = dst_path
86
+ return self
87
+
88
+ def __create_gradients(self):
89
+ self.__jfx = cs.jacobian(self.__f, self.__x).T @ self.__d
90
+ self.__jfu = cs.jacobian(self.__f, self.__u).T @ self.__d
91
+ self.__ellx = cs.jacobian(self.__ell, self.__x).T
92
+ self.__ellu = cs.jacobian(self.__ell, self.__u).T
93
+ self.__vfx = cs.jacobian(self.__vf, self.__x).T
94
+
95
+ def __function_name(self, fname):
96
+ return 'casadi_' + self.__name + '_' + fname
97
+
98
+ def __generate_casadi_functions(self):
99
+ self.__f_fun = cs.Function(self.__function_name('f'), [self.__x, self.__u], [
100
+ self.__f], ['x', 'u'], ['f'])
101
+ self.__jfx_fun = cs.Function(self.__function_name(
102
+ 'jfx'), [self.__x, self.__u, self.__d], [self.__jfx], ['x', 'u', 'd'], ['jfx'])
103
+ self.__jfu_fun = cs.Function(self.__function_name(
104
+ 'jfu'), [self.__x, self.__u, self.__d], [self.__jfu], ['x', 'u', 'd'], ['jfu'])
105
+ self.__ell_fun = cs.Function(self.__function_name(
106
+ 'ell'), [self.__x, self.__u], [self.__ell], ['x', 'u'], ['ell'])
107
+ self.__ellx_fun = cs.Function(self.__function_name('ellx'), [self.__x, self.__u], [
108
+ self.__ellx], ['x', 'u'], ['ellx'])
109
+ self.__ellu_fun = cs.Function(self.__function_name('ellu'), [self.__x, self.__u], [
110
+ self.__ellu], ['x', 'u'], ['ellu'])
111
+ self.__vf_fun = cs.Function(self.__function_name(
112
+ 'vf'), [self.__x], [self.__vf], ['x'], ['vf'])
113
+ self.__vfx_fun = cs.Function(self.__function_name(
114
+ 'vfx'), [self.__x], [self.__vfx], ['x'], ['vfx'])
115
+
116
+ def __generate_c_code(self):
117
+ c_code_filename = 'casadi_functions.c'
118
+ codegen = cs.CodeGenerator(c_code_filename)
119
+ codegen.add(self.__f_fun)
120
+ codegen.add(self.__jfx_fun)
121
+ codegen.add(self.__jfu_fun)
122
+ codegen.add(self.__ell_fun)
123
+ codegen.add(self.__ellx_fun)
124
+ codegen.add(self.__ellu_fun)
125
+ codegen.add(self.__vf_fun)
126
+ codegen.add(self.__vfx_fun)
127
+ codegen.generate()
128
+ # Move generated C code to destination directory
129
+ target_dir = self.__target_externc_dir()
130
+ shutil.move(c_code_filename, os.path.join(target_dir, c_code_filename))
131
+
132
+ def __generate_glob_header(self):
133
+ global_header_template = CostGradient.__get_template(
134
+ 'global_header.h.tmpl', subdir='c')
135
+ global_header_rendered = global_header_template.render(
136
+ name=self.__name,
137
+ f=self.__f_fun,
138
+ jfx=self.__jfx_fun,
139
+ jfu=self.__jfu_fun,
140
+ ell=self.__ell_fun,
141
+ ellx=self.__ellx_fun,
142
+ ellu=self.__ellu_fun,
143
+ vf=self.__vf_fun,
144
+ vfx=self.__vfx_fun,
145
+ N=self.__N,
146
+ nx=self.__nx,
147
+ nu=self.__nu
148
+ )
149
+ glob_header_target_path = os.path.join(
150
+ self.__target_externc_dir(), "glob_header.h")
151
+ with open(glob_header_target_path, "w") as fh:
152
+ fh.write(global_header_rendered)
153
+
154
+ def __generate_c_interface(self):
155
+ c_interface_template = CostGradient.__get_template(
156
+ 'autograd_interface.c.tmpl', subdir='c')
157
+ c_interface_rendered = c_interface_template.render(name=self.__name)
158
+ c_interface_target_path = os.path.join(
159
+ self.__target_externc_dir(), "interface.c")
160
+ with open(c_interface_target_path, "w") as fh:
161
+ fh.write(c_interface_rendered)
162
+
163
+ def __prepare_casadi_rs(self):
164
+ # Cargo.toml [casadi]
165
+ cargo_template = CostGradient.__get_template(
166
+ 'Cargo.toml', subdir='casadi-rs')
167
+ cargo_rendered = cargo_template.render(name=self.__name)
168
+ cargo_target_path = os.path.join(
169
+ self.__target_casadirs_dir(), "Cargo.toml")
170
+ with open(cargo_target_path, "w") as fh:
171
+ fh.write(cargo_rendered)
172
+ # build.rs
173
+ build_rs_template = CostGradient.__get_template(
174
+ 'build.rs', subdir='casadi-rs')
175
+ build_rs_rendered = build_rs_template.render(name=self.__name)
176
+ build_rs_target_path = os.path.join(
177
+ self.__target_casadirs_dir(), "build.rs")
178
+ with open(build_rs_target_path, "w") as fh:
179
+ fh.write(build_rs_rendered)
180
+ # lib.rs
181
+ casadi_lib_rs_template = CostGradient.__get_template(
182
+ 'lib.rs', subdir='casadi-rs')
183
+ casadi_lib_rs_rendered = casadi_lib_rs_template.render(
184
+ name=self.__name,
185
+ nx=self.__nx,
186
+ nu=self.__nu,
187
+ N=self.__N)
188
+ casadi_lib_rs_target_path = os.path.join(
189
+ self.__target_casadirs_dir(), "src", "lib.rs")
190
+ with open(casadi_lib_rs_target_path, "w") as fh:
191
+ fh.write(casadi_lib_rs_rendered)
192
+
193
+ def __generate_rust_lib(self):
194
+ # Cargo
195
+ cargo_template = CostGradient.__get_template(
196
+ 'Cargo.toml', subdir='rust')
197
+ cargo_rendered = cargo_template.render(name=self.__name)
198
+ cargo_target_path = os.path.join(
199
+ self.__target_root_dir(), "Cargo.toml")
200
+ with open(cargo_target_path, "w") as fh:
201
+ fh.write(cargo_rendered)
202
+ # lib
203
+ lib_template = CostGradient.__get_template('lib.rs', subdir='rust')
204
+ lib_rendered = lib_template.render(name=self.__name)
205
+ lib_target_path = os.path.join(
206
+ self.__target_root_dir(), "src", "lib.rs")
207
+ with open(lib_target_path, "w") as fh:
208
+ fh.write(lib_rendered)
209
+
210
+ def __cargo_build(self):
211
+ cmd = ['cargo', 'build', '-q']
212
+ p = subp.Popen(cmd, cwd=self.__target_root_dir())
213
+ process_completion = p.wait()
214
+ if process_completion != 0:
215
+ raise Exception('Rust build failed')
216
+
217
+ def build(self, no_rust_build=False):
218
+ self.__create_dirs()
219
+ self.__create_gradients()
220
+ self.__generate_casadi_functions()
221
+ self.__generate_c_code()
222
+ self.__generate_glob_header()
223
+ self.__generate_c_interface()
224
+ self.__prepare_casadi_rs()
225
+ self.__generate_rust_lib()
226
+ if not no_rust_build:
227
+ self.__cargo_build()
@@ -0,0 +1,16 @@
1
+ import pkg_resources
2
+
3
+
4
+ def templates_dir():
5
+ """Directory where the templates are found (for internal use, mainly)"""
6
+ return pkg_resources.resource_filename('gradgen', 'templates/')
7
+
8
+
9
+ def templates_subdir(subdir=None):
10
+ """
11
+ Directory where the templates are found and subfolder relative
12
+ to that path(for internal use, mainly)
13
+ """
14
+ if subdir is None:
15
+ return templates_dir()
16
+ return pkg_resources.resource_filename('gradgen', 'templates/%s/' % subdir)
@@ -0,0 +1,390 @@
1
+ /*
2
+ clang -fPIC -shared casadi_functions.c -o casadi_functions.so
3
+ clang -DTEST_INTERFACE -o test interface.c casadi_functions.so -lm
4
+ */
5
+
6
+ #include <stdlib.h>
7
+
8
+ /*
9
+ * This is to be used ONLY for DEBUG purposes
10
+ * Compile with -DTEST_INTERFACE
11
+ */
12
+ #ifdef TEST_INTERFACE
13
+ #include <stdio.h>
14
+ #endif
15
+
16
+ #include "glob_header.h"
17
+
18
+ #ifndef casadi_real
19
+ #define casadi_real double
20
+ #endif
21
+
22
+ #ifndef casadi_int
23
+ #define casadi_int long long int
24
+ #endif
25
+
26
+ extern int casadi_{{name}}_f(
27
+ const casadi_real **arg,
28
+ casadi_real **res,
29
+ casadi_int *iw,
30
+ casadi_real *w,
31
+ int mem);
32
+
33
+ extern int casadi_{{name}}_jfx(
34
+ const casadi_real **arg,
35
+ casadi_real **res,
36
+ casadi_int *iw,
37
+ casadi_real *w,
38
+ int mem);
39
+
40
+ extern int casadi_{{name}}_jfu(
41
+ const casadi_real **arg,
42
+ casadi_real **res,
43
+ casadi_int *iw,
44
+ casadi_real *w,
45
+ int mem);
46
+
47
+ extern int casadi_{{name}}_ell(
48
+ const casadi_real **arg,
49
+ casadi_real **res,
50
+ casadi_int *iw,
51
+ casadi_real *w,
52
+ int mem);
53
+
54
+ extern int casadi_{{name}}_ellx(
55
+ const casadi_real **arg,
56
+ casadi_real **res,
57
+ casadi_int *iw,
58
+ casadi_real *w,
59
+ int mem);
60
+
61
+ extern int casadi_{{name}}_ellu(
62
+ const casadi_real **arg,
63
+ casadi_real **res,
64
+ casadi_int *iw,
65
+ casadi_real *w,
66
+ int mem);
67
+
68
+ extern int casadi_{{name}}_vf(
69
+ const casadi_real **arg,
70
+ casadi_real **res,
71
+ casadi_int *iw,
72
+ casadi_real *w,
73
+ int mem);
74
+
75
+ extern int casadi_{{name}}_vfx(
76
+ const casadi_real **arg,
77
+ casadi_real **res,
78
+ casadi_int *iw,
79
+ casadi_real *w,
80
+ int mem);
81
+
82
+
83
+ /* -------------------------- */
84
+ /* Integer Workspaces */
85
+ /* -------------------------- */
86
+
87
+ #if F_SZ_IW_{{ name | upper}} > 0
88
+ static casadi_int iws_f[F_SZ_IW_{{ name | upper}}];
89
+ #else
90
+ static casadi_int *iws_f = NULL;
91
+ #endif
92
+
93
+ #if JFX_SZ_IW_{{ name | upper}} > 0
94
+ static casadi_int iws_jfx[JFX_SZ_IW_{{ name | upper}}];
95
+ #else
96
+ static casadi_int *iws_jfx = NULL;
97
+ #endif
98
+
99
+
100
+ #if JFU_SZ_IW_{{ name | upper}} > 0
101
+ static casadi_int iws_jfu[JFU_SZ_IW_{{ name | upper}}];
102
+ #else
103
+ static casadi_int *iws_jfu = NULL;
104
+ #endif
105
+
106
+ #if ELL_SZ_IW_{{ name | upper}} > 0
107
+ static casadi_int iws_ell[ELL_SZ_IW_{{ name | upper}}];
108
+ #else
109
+ static casadi_int *iws_ell = NULL;
110
+ #endif
111
+
112
+ #if ELLX_SZ_IW_{{ name | upper}} > 0
113
+ static casadi_int iws_ellx[ELLX_SZ_IW_{{ name | upper}}];
114
+ #else
115
+ static casadi_int *iws_ellx = NULL;
116
+ #endif
117
+
118
+ #if ELLU_SZ_IW_{{ name | upper}} > 0
119
+ static casadi_int iws_ellu[ELLU_SZ_IW_{{ name | upper}}];
120
+ #else
121
+ static casadi_int *iws_ellu = NULL;
122
+ #endif
123
+
124
+ #if VF_SZ_IW_{{ name | upper}} > 0
125
+ static casadi_int iws_vf[VF_SZ_IW_{{ name | upper}}];
126
+ #else
127
+ static casadi_int *iws_vf = NULL;
128
+ #endif
129
+
130
+ #if VFX_SZ_IW_{{ name | upper}} > 0
131
+ static casadi_int iws_vfx[VFX_SZ_IW_{{ name | upper}}];
132
+ #else
133
+ static casadi_int *iws_vfx = NULL;
134
+ #endif
135
+
136
+
137
+ /* -------------------------- */
138
+ /* Real Workspaces */
139
+ /* -------------------------- */
140
+
141
+ #if F_SZ_W_{{ name | upper}} > 0
142
+ static casadi_real ws_f[F_SZ_W_{{ name | upper}}];
143
+ #else
144
+ static casadi_real *ws_f = NULL;
145
+ #endif
146
+
147
+ #if JFX_SZ_W_{{ name | upper}} > 0
148
+ static casadi_real ws_jfx[JFX_SZ_W_{{ name | upper}}];
149
+ #else
150
+ static casadi_real *ws_jfx = NULL;
151
+ #endif
152
+
153
+ #if JFU_SZ_W_{{ name | upper}} > 0
154
+ static casadi_real ws_jfu[JFU_SZ_W_{{ name | upper}}];
155
+ #else
156
+ static casadi_real *ws_jfu = NULL;
157
+ #endif
158
+
159
+ #if ELL_SZ_W_{{ name | upper}} > 0
160
+ static casadi_real ws_ell[ELL_SZ_W_{{ name | upper}}];
161
+ #else
162
+ static casadi_real *ws_ell = NULL;
163
+ #endif
164
+
165
+ #if ELLX_SZ_W_{{ name | upper}} > 0
166
+ static casadi_real ws_ellx[ELLX_SZ_W_{{ name | upper}}];
167
+ #else
168
+ static casadi_real *ws_ellx = NULL;
169
+ #endif
170
+
171
+ #if ELLU_SZ_W_{{ name | upper}} > 0
172
+ static casadi_real ws_ellu[ELLU_SZ_W_{{ name | upper}}];
173
+ #else
174
+ static casadi_real *ws_ellu = NULL;
175
+ #endif
176
+
177
+ #if VF_SZ_W_{{ name | upper}} > 0
178
+ static casadi_real ws_vf[VF_SZ_W_{{ name | upper}}];
179
+ #else
180
+ static casadi_real *ws_vf = NULL;
181
+ #endif
182
+
183
+ #if VFX_SZ_W_{{ name | upper}} > 0
184
+ static casadi_real ws_vfx[VFX_SZ_W_{{ name | upper}}];
185
+ #else
186
+ static casadi_real *ws_vfx = NULL;
187
+ #endif
188
+
189
+
190
+ /* -------------------------- */
191
+ /* Result Spaces */
192
+ /* -------------------------- */
193
+ static casadi_real *f_res[F_SZ_RES_{{ name | upper}}];
194
+ static casadi_real *jfx_res[JFX_SZ_RES_{{ name | upper}}];
195
+ static casadi_real *jfu_res[JFU_SZ_RES_{{ name | upper}}];
196
+ static casadi_real *ell_res[ELL_SZ_RES_{{ name | upper}}];
197
+ static casadi_real *ellx_res[ELLX_SZ_RES_{{ name | upper}}];
198
+ static casadi_real *ellu_res[ELLU_SZ_RES_{{ name | upper}}];
199
+ static casadi_real *vf_res[VF_SZ_RES_{{ name | upper}}];
200
+ static casadi_real *vfx_res[VFX_SZ_RES_{{ name | upper}}];
201
+
202
+
203
+ /* ------------------------------------ */
204
+ /* Persistent (x, u, d) memory */
205
+ /* ------------------------------------ */
206
+ #define N_PERSISTENT_MEM_{{ name | upper}} (2 * NX_{{ name | upper}} + NU_{{ name | upper}})
207
+ static casadi_real persistent_memory[N_PERSISTENT_MEM_{{ name | upper}}]; /* (x, u, d) */
208
+ #define IDX_X_{{ name | upper}} 0
209
+ #define IDX_U_{{ name | upper}} IDX_X_{{ name | upper}} + NX_{{ name | upper}}
210
+ #define IDX_D_{{ name | upper}} IDX_U_{{ name | upper}} + NU_{{ name | upper}}
211
+
212
+
213
+ void init_interface_{{ name }}(void) {
214
+ /* nothing */
215
+ }
216
+
217
+ /* Copy (x) into persistent_memory; arg = {x} */
218
+ static void copy_x_to_persistent(const casadi_real** arg) {
219
+ unsigned int i;
220
+ for (i=0; i<NX_{{ name | upper}}; i++)
221
+ persistent_memory[IDX_X_{{ name | upper}} + i] = arg[0][i]; /* copy x */
222
+ }
223
+
224
+ /* Copy (x, u) into persistent_memory; arg = {x, u} */
225
+ static void copy_xu_to_persistent(const casadi_real** arg) {
226
+ unsigned int i;
227
+ copy_x_to_persistent(arg); /* copy x */
228
+ for (i=0; i<NU_{{ name | upper}}; i++)
229
+ persistent_memory[IDX_U_{{ name | upper}} + i] = arg[1][i]; /* copy u */
230
+ }
231
+
232
+ /* Copy (x, u, d) into persistent_memory; arg = {x, u, d} */
233
+ static void copy_xud_to_persistent(const casadi_real** arg) {
234
+ unsigned int i;
235
+ copy_xu_to_persistent(arg);
236
+ for (i=0; i<NX_{{ name | upper}}; i++)
237
+ persistent_memory[IDX_D_{{ name | upper}} + i] = arg[2][i]; /* copy d */
238
+ }
239
+
240
+ int {{ name }}_f(const casadi_real** arg, casadi_real** res) {
241
+ copy_xu_to_persistent(arg); /* arg = {x, u} */
242
+ const casadi_real* args__[F_SZ_ARG_{{ name | upper}}] = {
243
+ persistent_memory + IDX_X_{{ name | upper}},
244
+ persistent_memory + IDX_U_{{ name | upper}}};
245
+ f_res[0] = res[0];
246
+ return casadi_{{ name }}_f(args__, f_res, iws_f, ws_f, 0);
247
+ }
248
+
249
+ int {{ name }}_jfx(const casadi_real** arg, casadi_real** res) {
250
+ copy_xud_to_persistent(arg); /* arg = {x, u, d} */
251
+ const casadi_real* args__[JFX_SZ_ARG_{{ name | upper}}] = {
252
+ persistent_memory + IDX_X_{{ name | upper}},
253
+ persistent_memory + IDX_U_{{ name | upper}},
254
+ persistent_memory + IDX_D_{{ name | upper}}};
255
+ jfx_res[0] = res[0];
256
+ return casadi_{{ name }}_jfx(args__, jfx_res, iws_jfx, ws_jfx, 0);
257
+ }
258
+
259
+ int {{ name }}_jfu(const casadi_real** arg, casadi_real** res) {
260
+ copy_xud_to_persistent(arg); /* arg = {x, u, d} */
261
+ const casadi_real* args__[JFU_SZ_ARG_{{ name | upper}}] = {
262
+ persistent_memory + IDX_X_{{ name | upper}},
263
+ persistent_memory + IDX_U_{{ name | upper}},
264
+ persistent_memory + IDX_D_{{ name | upper}}};
265
+ jfu_res[0] = res[0];
266
+ return casadi_{{ name }}_jfu(args__, jfu_res, iws_jfu, ws_jfu, 0);
267
+ }
268
+
269
+ int {{ name }}_ellx(const casadi_real** arg, casadi_real** res) {
270
+ copy_xu_to_persistent(arg); /* arg = {x, u} */
271
+ const casadi_real* args__[ELLX_SZ_ARG_{{ name | upper}}] = {
272
+ persistent_memory + IDX_X_{{ name | upper}},
273
+ persistent_memory + IDX_U_{{ name | upper}}};
274
+ ellx_res[0] = res[0];
275
+ return casadi_{{ name }}_ellx(args__, ellx_res, iws_ellx, ws_ellx, 0);
276
+ }
277
+
278
+ int {{ name }}_ellu(const casadi_real** arg, casadi_real** res) {
279
+ copy_xu_to_persistent(arg); /* arg = {x, u} */
280
+ const casadi_real* args__[ELLU_SZ_ARG_{{ name | upper}}] = {
281
+ persistent_memory + IDX_X_{{ name | upper}},
282
+ persistent_memory + IDX_U_{{ name | upper}}};
283
+ ellu_res[0] = res[0];
284
+ return casadi_{{ name }}_ellu(args__, ellu_res, iws_ellu, ws_ellu, 0);
285
+ }
286
+
287
+ int {{ name }}_vfx(const casadi_real** arg, casadi_real** res) {
288
+ copy_x_to_persistent(arg); /* arg = {x} */
289
+ const casadi_real* args__[VFX_SZ_ARG_{{ name | upper}}] = {persistent_memory + IDX_X_{{ name | upper}}};
290
+ vfx_res[0] = res[0];
291
+ return casadi_{{ name }}_vfx(args__, vfx_res, iws_vfx, ws_vfx, 0);
292
+ }
293
+
294
+ #ifdef TEST_INTERFACE
295
+ /* -------------------------- */
296
+ /* Testing */
297
+ /* -------------------------- */
298
+
299
+ void call_f_test(){
300
+ double x[NX_{{ name | upper}}] = {1.570796326794897, 10., 2.};
301
+ double u[NU_{{ name | upper}}] = {1.5, 2.1};
302
+ double x_next[NX_{{ name | upper}}];
303
+ const double *arg[2] = {x, u};
304
+ double *res[1] = {x_next};
305
+ {{ name }}_f(arg, res);
306
+
307
+ int i;
308
+ for (i = 0; i < NX_{{ name | upper}}; i++)
309
+ {
310
+ printf("x+ >> %g\n", x_next[i]);
311
+ }
312
+ }
313
+
314
+ void call_jfx_test()
315
+ {
316
+ double x[NX_{{ name | upper}}] = {1.570796326794897, 10., 2.};
317
+ double u[NU_{{ name | upper}}] = {1.5, 2.1};
318
+ double d[NX_{{ name | upper}}] = {0, 1, 0};
319
+
320
+ double jfx[NX_{{ name | upper}}];
321
+ const double *arg[3] = {x, u, d};
322
+ double *res[1] = {jfx};
323
+ {{ name }}_jfx(arg, res);
324
+
325
+ for (unsigned int i = 0; i < NX_{{ name | upper}}; i++)
326
+ {
327
+ printf("Jxf >> %g\n", jfx_res[0][i]);
328
+ }
329
+ }
330
+
331
+ void call_jfu_test()
332
+ {
333
+ double x[NX_{{ name | upper}}] = {1.570796326794897, 10., 2.};
334
+ double u[NU_{{ name | upper}}] = {1.5, 2.1};
335
+ double d[NX_{{ name | upper}}] = {1, 1, 0};
336
+
337
+ double jfu[NX_{{ name | upper}}];
338
+ const double *arg[3] = {x, u, d};
339
+ double *res[1] = {jfu};
340
+ {{ name }}_jfu(arg, res);
341
+
342
+ for (unsigned int i = 0; i < NX_{{ name | upper}}; i++)
343
+ {
344
+ printf("Juf >> %g\n", jfu_res[0][i]);
345
+ }
346
+ }
347
+
348
+ void call_ellx_test()
349
+ {
350
+ double x[NX_{{ name | upper}}] = {1.570796326794897, 10., 2.};
351
+ double u[NU_{{ name | upper}}] = {1.5, 2.1};
352
+
353
+ double ellx[NX_{{ name | upper}}];
354
+ const double *arg[2] = {x, u};
355
+ double *res[1] = {ellx};
356
+ {{ name }}_ellx(arg, res);
357
+
358
+ for (unsigned int i = 0; i < NX_{{ name | upper}}; i++)
359
+ {
360
+ printf("ell_x >> %g\n", ellx_res[0][i]);
361
+ }
362
+ }
363
+
364
+ void call_vfx_test()
365
+ {
366
+ double x[NX_{{ name | upper}}] = {1.570796326794897, 10., 2.};
367
+
368
+ double vfx[NX_{{ name | upper}}];
369
+ const double *arg[1] = {x};
370
+ double *res[1] = {vfx};
371
+ {{ name }}_vfx(arg, res);
372
+
373
+ for (unsigned int i = 0; i < NX_{{ name | upper}}; i++)
374
+ {
375
+ printf("vfx >> %g\n", vfx_res[0][i]);
376
+ }
377
+ }
378
+
379
+ /* for testing purposes only! */
380
+ int main()
381
+ {
382
+ call_f_test();
383
+ call_jfx_test();
384
+ call_jfu_test();
385
+ call_ellx_test();
386
+ call_vfx_test();
387
+ return 0;
388
+ }
389
+ #endif
390
+
@@ -0,0 +1,66 @@
1
+ /*
2
+ * Header file with sizes of arrays that need to be allocated
3
+ * (statically) once
4
+ *
5
+ *
6
+ * This file is autogenerated by Gradgen
7
+ *
8
+ * Metadata:
9
+ * + name: {{ name }}
10
+ *
11
+ */
12
+
13
+ #pragma once
14
+
15
+ #define NPRED_{{ name | upper}} {{ N }}
16
+ #define NX_{{ name | upper}} {{ nx }}
17
+ #define NU_{{ name | upper}} {{ nu }}
18
+
19
+ /* f */
20
+ #define F_SZ_ARG_{{ name | upper}} {{ f.sz_arg() }}
21
+ #define F_SZ_IW_{{ name | upper}} {{ f.sz_iw() }}
22
+ #define F_SZ_W_{{ name | upper}} {{ f.sz_w() }}
23
+ #define F_SZ_RES_{{ name | upper}} {{ f.sz_res() }}
24
+
25
+ /* jfx */
26
+ #define JFX_SZ_ARG_{{ name | upper}} {{ jfx.sz_arg() }}
27
+ #define JFX_SZ_IW_{{ name | upper}} {{ jfx.sz_iw() }}
28
+ #define JFX_SZ_W_{{ name | upper}} {{ jfx.sz_w() }}
29
+ #define JFX_SZ_RES_{{ name | upper}} {{ jfx.sz_res() }}
30
+
31
+ /* jfu */
32
+ #define JFU_SZ_ARG_{{ name | upper}} {{ jfu.sz_arg() }}
33
+ #define JFU_SZ_IW_{{ name | upper}} {{ jfu.sz_iw() }}
34
+ #define JFU_SZ_W_{{ name | upper}} {{ jfu.sz_w() }}
35
+ #define JFU_SZ_RES_{{ name | upper}} {{ jfu.sz_res() }}
36
+
37
+ /* ell */
38
+ #define ELL_SZ_ARG_{{ name | upper}} {{ ell.sz_arg() }}
39
+ #define ELL_SZ_IW_{{ name | upper}} {{ ell.sz_iw() }}
40
+ #define ELL_SZ_W_{{ name | upper}} {{ ell.sz_w() }}
41
+ #define ELL_SZ_RES_{{ name | upper}} {{ ell.sz_res() }}
42
+
43
+ /* ellx */
44
+ #define ELLX_SZ_ARG_{{ name | upper}} {{ ellx.sz_arg() }}
45
+ #define ELLX_SZ_IW_{{ name | upper}} {{ ellx.sz_iw() }}
46
+ #define ELLX_SZ_W_{{ name | upper}} {{ ellx.sz_w() }}
47
+ #define ELLX_SZ_RES_{{ name | upper}} {{ ellx.sz_res() }}
48
+
49
+ /* ellu */
50
+ #define ELLU_SZ_ARG_{{ name | upper}} {{ ellu.sz_arg() }}
51
+ #define ELLU_SZ_IW_{{ name | upper}} {{ ellu.sz_iw() }}
52
+ #define ELLU_SZ_W_{{ name | upper}} {{ ellu.sz_w() }}
53
+ #define ELLU_SZ_RES_{{ name | upper}} {{ ellu.sz_res() }}
54
+
55
+ /* vf */
56
+ #define VF_SZ_ARG_{{ name | upper}} {{ vf.sz_arg() }}
57
+ #define VF_SZ_IW_{{ name | upper}} {{ vf.sz_iw() }}
58
+ #define VF_SZ_W_{{ name | upper}} {{ vf.sz_w() }}
59
+ #define VF_SZ_RES_{{ name | upper}} {{ vf.sz_res() }}
60
+
61
+ /* vfx */
62
+ #define VFX_SZ_ARG_{{ name | upper}} {{ vfx.sz_arg() }}
63
+ #define VFX_SZ_IW_{{ name | upper}} {{ vfx.sz_iw() }}
64
+ #define VFX_SZ_W_{{ name | upper}} {{ vfx.sz_w() }}
65
+ #define VFX_SZ_RES_{{ name | upper}} {{ vfx.sz_res() }}
66
+
@@ -0,0 +1,33 @@
1
+ # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2
+ #
3
+ # Autogenerated Cargo.toml configuration file
4
+ # This file was generated by GradGen
5
+ #
6
+ # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
7
+ [package]
8
+ authors = ["GradGen"]
9
+ name = "casadi_{{name}}"
10
+ categories = [
11
+ "api-bindings",
12
+ "no-std",
13
+ "science",
14
+ "science::robotics",
15
+ "simulation",
16
+ ]
17
+ keywords = ["casadi", "optimization", "interface"]
18
+ description = "Rust interface to CasADi functions"
19
+ license = "MIT"
20
+ version = "0.2.1"
21
+ edition = "2018"
22
+
23
+ [dependencies]
24
+ libc = { version = "0.2.0", default-features = false }
25
+
26
+ [build-dependencies]
27
+ cc = "1.0"
28
+
29
+ [profile.dev]
30
+ opt-level = 0
31
+
32
+ [profile.release]
33
+ opt-level = 3
@@ -0,0 +1,27 @@
1
+ use std::path::Path;
2
+
3
+ fn main() {
4
+ assert!(
5
+ Path::new("extern/casadi_functions.c").exists(),
6
+ "extern/casadi_functions.c is missing"
7
+ );
8
+ assert!(
9
+ Path::new("extern/interface.c").exists(),
10
+ "extern/interface.c is missing"
11
+ );
12
+
13
+ cc::Build::new()
14
+ .flag_if_supported("-Wall")
15
+ .flag_if_supported("-Wno-unused-variable")
16
+ .flag_if_supported("-Wno-long-long")
17
+ .flag_if_supported("-Wno-unused-parameter")
18
+ .pic(true)
19
+ .include("src")
20
+ .file("extern/casadi_functions.c")
21
+ .file("extern/interface.c")
22
+ .compile("icasadi");
23
+
24
+ // Rerun if these autogenerated files change
25
+ println!("cargo:rerun-if-changed=extern/casadi_functions.c");
26
+ println!("cargo:rerun-if-changed=extern/interface.c");
27
+ }
@@ -0,0 +1,123 @@
1
+ use libc::{c_double, c_int};
2
+
3
+ /// number of states
4
+ pub const NX: usize = {{ nx }};
5
+ /// number of inputs
6
+ pub const NU: usize = {{ nu }};
7
+ /// prediction horizon
8
+ pub const NPRED: usize = {{ N }};
9
+
10
+
11
+ extern "C" {
12
+ fn init_interface_{{name}}();
13
+
14
+ fn {{name}}_f(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int;
15
+ fn {{name}}_jfx(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int;
16
+ fn {{name}}_jfu(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int;
17
+ fn {{name}}_ellx(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int;
18
+ fn {{name}}_ellu(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int;
19
+ fn {{name}}_vfx(arg: *const *const c_double, casadi_results: *mut *mut c_double) -> c_int;
20
+ }
21
+
22
+
23
+ pub fn init_{{name}}() {
24
+ unsafe {
25
+ init_interface_{{name}}();
26
+ }
27
+ }
28
+
29
+ pub fn f(x: &[f64], u: &[f64], fxu: &mut [f64]) -> i32 {
30
+ let arguments = &[x.as_ptr(), u.as_ptr()];
31
+ let result = &mut [fxu.as_mut_ptr()];
32
+ unsafe { {{name}}_f(arguments.as_ptr(), result.as_mut_ptr()) as i32}
33
+ }
34
+
35
+ pub fn jfx(x: &[f64], u: &[f64], d: &[f64], jfxd: &mut [f64]) -> i32 {
36
+ let arguments = &[x.as_ptr(), u.as_ptr(), d.as_ptr()];
37
+ let result = &mut [jfxd.as_mut_ptr()];
38
+ unsafe { {{name}}_jfx(arguments.as_ptr(), result.as_mut_ptr()) as i32 }
39
+ }
40
+
41
+ pub fn jfu(x: &[f64], u: &[f64], d: &[f64], jfud: &mut [f64]) -> i32 {
42
+ let arguments = &[x.as_ptr(), u.as_ptr(), d.as_ptr()];
43
+ let result = &mut [jfud.as_mut_ptr()];
44
+ unsafe { {{name}}_jfu(arguments.as_ptr(), result.as_mut_ptr()) as i32 }
45
+ }
46
+
47
+ pub fn ellx(x: &[f64], u: &[f64], ellx_out: &mut [f64]) -> i32 {
48
+ let arguments = &[x.as_ptr(), u.as_ptr()];
49
+ let result = &mut [ellx_out.as_mut_ptr()];
50
+ unsafe { {{name}}_ellx(arguments.as_ptr(), result.as_mut_ptr()) as i32 }
51
+ }
52
+
53
+ pub fn ellu(x: &[f64], u: &[f64], ellu_out: &mut [f64]) -> i32 {
54
+ let arguments = &[x.as_ptr(), u.as_ptr()];
55
+ let result = &mut [ellu_out.as_mut_ptr()];
56
+ unsafe { {{name}}_ellu(arguments.as_ptr(), result.as_mut_ptr()) as i32 }
57
+ }
58
+
59
+ pub fn vfx(x: &[f64], vfx_out: &mut [f64]) -> i32 {
60
+ let arguments = &[x.as_ptr()];
61
+ let result = &mut [vfx_out.as_mut_ptr()];
62
+ unsafe { {{name}}_vfx(arguments.as_ptr(), result.as_mut_ptr()) as i32 }
63
+ }
64
+
65
+ #[cfg(test)]
66
+ mod tests {
67
+ use super::*;
68
+
69
+ #[test]
70
+ fn tst_init() {
71
+ init_{{name}}();
72
+ }
73
+
74
+ #[test]
75
+ fn tst_f(){
76
+ let x = [0.1; NX];
77
+ let u = [0.1; NU];
78
+ let mut fxu = [0.0; NX];
79
+ assert_eq!(0, f(&x, &u, &mut fxu));
80
+ }
81
+
82
+ #[test]
83
+ fn tst_jfx() {
84
+ let x = [0.1, 0.2, 0.3];
85
+ let u = [1.1, 2.2];
86
+ let d = [-0.8, 6.2, 0.0];
87
+ let mut jfx_d = [0.0; NX];
88
+ assert_eq!(0, jfx(&x, &u, &d, &mut jfx_d));
89
+ }
90
+
91
+ #[test]
92
+ fn tst_jfu() {
93
+ let x = [0.1, 0.2, 0.3];
94
+ let u = [1.1, 2.2];
95
+ let d = [-0.8, 6.2, 0.0];
96
+ let mut jfu_d = [0.0; NX];
97
+ assert_eq!(0, jfu(&x, &u, &d, &mut jfu_d));
98
+ }
99
+
100
+ #[test]
101
+ fn tst_ellx() {
102
+ let x = [0.1, 0.2, 0.3];
103
+ let u = [1.1, 2.2];
104
+ let mut ellx_res = [0.0; NX];
105
+ assert_eq!(0, ellx(&x, &u, &mut ellx_res));
106
+ }
107
+
108
+ #[test]
109
+ fn tst_ellu() {
110
+ let x = [0.1, 0.2, 0.3];
111
+ let u = [1.1, 2.2];
112
+ let mut ellu_res = [0.0; NU];
113
+ assert_eq!(0, ellu(&x, &u, &mut ellu_res));
114
+ }
115
+
116
+ #[test]
117
+ fn tst_vfx() {
118
+ let x = [0.1, 0.2, 0.3];
119
+ let u = [1.1, 2.2];
120
+ let mut vfx_res = [0.0; NX];
121
+ assert_eq!(0, vfx(&x, &mut vfx_res));
122
+ }
123
+ }
@@ -0,0 +1,21 @@
1
+ # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2
+ #
3
+ # Autogenerated Cargo.toml configuration file
4
+ # This file was generated by GradGen
5
+ #
6
+ # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
7
+ [package]
8
+ name = "{{name}}"
9
+ version = "0.0.0"
10
+ license = "MIT"
11
+ authors = ["GradGen"]
12
+ edition = "2018"
13
+ publish = false
14
+
15
+
16
+ [dependencies]
17
+ casadi_{{name}} = { path = "./casadi_{{name}}/" }
18
+
19
+
20
+ [lib]
21
+ crate-type = ["rlib"]
@@ -0,0 +1,117 @@
1
+ // nothing
2
+ use casadi_{{name}}::*;
3
+
4
+ pub fn num_states() -> usize {
5
+ return NX;
6
+ }
7
+
8
+ pub fn num_inputs() -> usize {
9
+ return NU;
10
+ }
11
+
12
+ #[derive(Debug)]
13
+ pub struct BackwardGradientWorkspace {
14
+ pub(crate) w: Vec<f64>,
15
+ pub(crate) w_new: Vec<f64>,
16
+ pub(crate) x_seq: Vec<f64>,
17
+ pub(crate) temp_nu: Vec<f64>,
18
+ pub(crate) temp_nx: Vec<f64>,
19
+ }
20
+
21
+ impl BackwardGradientWorkspace {
22
+ pub fn new(n_pred: usize) -> BackwardGradientWorkspace {
23
+ BackwardGradientWorkspace {
24
+ w: vec![0.0; NX],
25
+ w_new : vec![0.0; NX],
26
+ x_seq: vec![0.0; NX * (n_pred + 1)],
27
+ temp_nu: vec! [0.0; NU],
28
+ temp_nx: vec! [0.0; NX],
29
+ }
30
+ }
31
+ }
32
+
33
+
34
+ /// a = a + b
35
+ fn add(a: &mut [f64], b: &[f64]) {
36
+ a.iter_mut().zip(b.iter()).for_each(|(ai, bi)| *ai += *bi);
37
+ }
38
+
39
+
40
+
41
+ pub fn total_cost_gradient_bw(
42
+ x0: &[f64],
43
+ u_seq: &[f64],
44
+ grad: &mut [f64],
45
+ workspace: &mut BackwardGradientWorkspace,
46
+ n: usize
47
+ ) {
48
+
49
+ /*
50
+ * Simulate the system sxtarting from x0 and using the
51
+ * sequence of inputs, u_seq = (u(0), u(1), .., U(N-1))
52
+ * and store the states, x_seq = (x(0), x(1),..., x(N))
53
+ * in workspace. x_seq = (x(0), x(1), . . . , x (N))
54
+ */
55
+ workspace.x_seq[0..NX].copy_from_slice(x0);
56
+ /* Simulation */
57
+ for t in 0..= n - 1 {
58
+ let xt = &workspace.x_seq[t * NX.. (t + 1) * NX];
59
+ let ut = &u_seq[t * NU.. (t + 1) * NU];
60
+ // W= f(xt, ut)
61
+ f(xt, ut, &mut workspace.w);
62
+ // x seq[next]<- W (CODY)
63
+ workspace.x_seq[(t + 1) * NX.. (t + 2) * NX].copy_from_slice (&workspace.w);
64
+ }
65
+
66
+ /* Store Vfx in w */
67
+ let xn = &workspace.x_seq[n * NX..(n + 1) * NX];
68
+ vfx(xn, &mut workspace.w);
69
+
70
+ /* backward for-loop */
71
+ for j in 1..=n{
72
+
73
+ let xnj = &workspace.x_seq[(n-j)* NX.. (n-j+ 1) * NX];
74
+ let unj = &u_seq[(n-j) * NU.. (n-j+1)* NU];
75
+ let gradnj = &mut grad[(n-j) * NU.. (n-j+1)* NU];
76
+
77
+ // gradnj := f^u(w) at t=N-j
78
+ jfu(xnj, unj, &mut workspace.w, gradnj);
79
+ // temp_nu := ellu at t=N-j
80
+ ellu(xnj, unj,&mut workspace.temp_nu );
81
+ // gradnj += temp_nu
82
+ add(gradnj, &workspace.temp_nu);
83
+
84
+
85
+ // gradnj += &mut workspace.temp_nx
86
+ jfx(xnj, unj, &mut workspace.w, &mut workspace.w_new);
87
+ // temp_nx <-- ell^x_N_minus_j
88
+ ellx(xnj, unj,&mut workspace.temp_nx );
89
+ // grad_V_N_minus_j += temp_nx
90
+ add(&mut workspace.w_new, &workspace.temp_nx);
91
+ workspace.w.copy_from_slice (&workspace.w_new);
92
+ }
93
+
94
+
95
+
96
+
97
+ }
98
+
99
+ #[cfg(test)]
100
+ mod tests {
101
+
102
+ use casadi_{{name}}::*;
103
+
104
+ #[test]
105
+ fn tst_nothing() {
106
+ let n: usize= 6 ;
107
+ let mut ws = super::BackwardGradientWorkspace::new(n);
108
+ let x0 = vec![1.0; NX];
109
+ let u_seq = vec![1.0; NU * n];
110
+ let mut grad = vec![0.0; NU * n];
111
+ super::total_cost_gradient_bw(&x0, &u_seq, &mut grad, &mut ws, n);
112
+ println!("The gradient of total cost function is {:?}", grad);
113
+ println!("The state variable x is {:?}", ws.x_seq);
114
+ }
115
+ }
116
+
117
+
File without changes
@@ -0,0 +1,62 @@
1
+ import os
2
+ import unittest
3
+ import casadi.casadi as cs
4
+ from gradgen.cost_gradient import *
5
+ import logging
6
+ import numpy as np
7
+ import subprocess
8
+
9
+ logger = logging.getLogger(__name__)
10
+
11
+
12
+ class GradgenTestCase(unittest.TestCase):
13
+
14
+ @classmethod
15
+ def create_example(cls):
16
+ nx, nu = 10, 3
17
+ k1, k2, Ts = 252, 20, 1 / 125
18
+ Gamma_u = np.diag([0, 0, 124.659])
19
+ Gamma_n = np.diag([2.287469, 1.35699886, -0.42388942])
20
+ I = np.diag([0.01788, 0.03014, 0.04614])
21
+ Iinv = np.linalg.inv(I)
22
+ x = cs.SX.sym('x', nx)
23
+ u = cs.SX.sym('u', nu)
24
+ (q0, q1, q2, q3, wx, wy, wz, nx, ny, nz) = (
25
+ x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8], x[9])
26
+ Q_bar_q = cs.vertcat(cs.vertcat(0, -x[5], -x[6], -x[7]).T,
27
+ cs.vertcat(x[5], 0, x[7], -x[6]).T,
28
+ cs.vertcat(x[6], -x[7], 0, x[5]).T,
29
+ cs.vertcat(x[7], x[6], -x[5], 0).T)
30
+ tau = cs.sqrt(wx ** 2 + wy ** 2 + wz ** 2)
31
+ expQbar = cs.vertcat(
32
+ cs.vertcat(cs.cosh(tau), -wx * tau, -wy * tau, -wz * tau).T,
33
+ cs.vertcat(wx * tau, cs.cosh(tau), wz * tau, wy * tau).T,
34
+ cs.vertcat(wy * tau, -wz * tau, cs.cosh(tau), wx * tau).T,
35
+ cs.vertcat(wz * tau, wy * tau, -wx * tau, cs.cosh(tau)).T)
36
+ q = cs.vertcat(q0, q1, q2, q3)
37
+ w = cs.vertcat(wx, wy, wz)
38
+ n = cs.vertcat(nx, ny, nz)
39
+ f = cs.vertcat((Ts / 2) * expQbar @ q,
40
+ w + Ts * (Gamma_n @ n + Gamma_u @ u -
41
+ Iinv @ cs.cross(w, I @ w)),
42
+ n + Ts * (k1 * k2 * u - k2 * n)
43
+ )
44
+
45
+ # Stage cost function, ell
46
+ ell = 5*x[0]**2 + 0.01*x[1]**2 + 0.01*x[2]**2
47
+
48
+ # terminal cost function, vf
49
+ vf = 0.5 * (x[0]**2 + 50 * x[1]**2 + 100 * x[2]**2)
50
+ return x, u, f, ell, vf
51
+
52
+ def test_generate_code_and_build(self):
53
+ x, u, f, ell, vf = GradgenTestCase.create_example()
54
+ N = 15
55
+ gradObj = CostGradient(x, u, f, ell, vf, N).with_name(
56
+ "quadcopter_test").with_target_path(".")
57
+ gradObj.build(no_rust_build=True)
58
+
59
+
60
+ if __name__ == '__main__':
61
+ logger.setLevel(logging.ERROR)
62
+ unittest.main()
@@ -0,0 +1,38 @@
1
+ Metadata-Version: 2.1
2
+ Name: gradgen
3
+ Version: 0.1.0
4
+ Summary: Gradient computation with AD for optimal control
5
+ Home-page: https://github.com/alphaville/gradgen
6
+ Author: ['Jie Lin', 'Pantelis Sopasakis']
7
+ Author-email: p.sopasakis@gmail.com
8
+ License: MIT License
9
+ Keywords: optimization,nonconvex,embedded
10
+ Classifier: Development Status :: 4 - Beta
11
+ Classifier: License :: OSI Approved :: MIT License
12
+ Classifier: Programming Language :: Python
13
+ Classifier: Programming Language :: Rust
14
+ Classifier: Intended Audience :: Science/Research
15
+ Classifier: Topic :: Software Development :: Libraries
16
+ Classifier: Topic :: Scientific/Engineering
17
+ Classifier: Topic :: Scientific/Engineering :: Mathematics
18
+ Classifier: Topic :: Software Development :: Code Generators
19
+ Classifier: Topic :: Software Development :: Embedded Systems
20
+ Description-Content-Type: text/markdown
21
+ License-File: LICENSE
22
+
23
+
24
+ # GradGen
25
+
26
+ Code generation for cost gradient
27
+
28
+ ### Installation instructions
29
+
30
+ To build this project locally:
31
+
32
+ - Create a virtual environment: `virtualenv -p python3 venv`
33
+ - Activate the virtual environment. On Linux/MacOS, run `source venv/bin/activate`
34
+ - Install the project: `pip install .`
35
+
36
+ The folder `playground` can be used during the design phase of the project to try out different things, but it should be removed for release.
37
+
38
+
@@ -0,0 +1,23 @@
1
+ LICENSE
2
+ MANIFEST.in
3
+ README.md
4
+ VERSION
5
+ setup.py
6
+ gradgen/__init__.py
7
+ gradgen/cost_gradient.py
8
+ gradgen/definitions.py
9
+ gradgen.egg-info/PKG-INFO
10
+ gradgen.egg-info/SOURCES.txt
11
+ gradgen.egg-info/dependency_links.txt
12
+ gradgen.egg-info/not-zip-safe
13
+ gradgen.egg-info/requires.txt
14
+ gradgen.egg-info/top_level.txt
15
+ gradgen/templates/c/autograd_interface.c.tmpl
16
+ gradgen/templates/c/global_header.h.tmpl
17
+ gradgen/templates/casadi-rs/Cargo.toml
18
+ gradgen/templates/casadi-rs/build.rs
19
+ gradgen/templates/casadi-rs/lib.rs
20
+ gradgen/templates/rust/Cargo.toml
21
+ gradgen/templates/rust/lib.rs
22
+ gradgen/test/__init__.py
23
+ gradgen/test/test_gradgen.py
@@ -0,0 +1,3 @@
1
+ jinja2
2
+ casadi
3
+ numpy
@@ -0,0 +1 @@
1
+ gradgen
@@ -0,0 +1,4 @@
1
+ [egg_info]
2
+ tag_build =
3
+ tag_date = 0
4
+
gradgen-0.1.0/setup.py ADDED
@@ -0,0 +1,56 @@
1
+ #!/usr/bin/env python
2
+
3
+ from setuptools import setup, find_packages
4
+ import io
5
+ import os
6
+
7
+ here = os.path.abspath(os.path.dirname(__file__))
8
+
9
+ NAME = 'gradgen'
10
+
11
+ # Import version from file
12
+ version_file = open(os.path.join(here, 'VERSION'))
13
+ VERSION = version_file.read().strip()
14
+
15
+ DESCRIPTION = 'Gradient computation with AD for optimal control'
16
+
17
+
18
+ # Import the README and use it as the long-description.
19
+ # Note: this will only work if 'README.md' is present in your MANIFEST.in file!
20
+ try:
21
+ with io.open(os.path.join(here, 'README.md'), encoding='utf-8') as f:
22
+ long_description = '\n' + f.read()
23
+ except FileNotFoundError:
24
+ long_description = DESCRIPTION
25
+
26
+ setup(name=NAME,
27
+ version=VERSION,
28
+ description=DESCRIPTION,
29
+ long_description=long_description,
30
+ long_description_content_type='text/markdown',
31
+ author=['Jie Lin', 'Pantelis Sopasakis'],
32
+ author_email='p.sopasakis@gmail.com',
33
+ license='MIT License',
34
+ packages=find_packages(
35
+ exclude=["templates", "test"]),
36
+ include_package_data=True,
37
+ install_requires=[
38
+ 'jinja2', 'casadi', 'numpy'
39
+ ],
40
+ classifiers=[
41
+ 'Development Status :: 4 - Beta',
42
+ 'License :: OSI Approved :: MIT License',
43
+ 'Programming Language :: Python',
44
+ 'Programming Language :: Rust',
45
+ 'Intended Audience :: Science/Research',
46
+ 'Topic :: Software Development :: Libraries',
47
+ 'Topic :: Scientific/Engineering',
48
+ 'Topic :: Scientific/Engineering :: Mathematics',
49
+ 'Topic :: Software Development :: Code Generators',
50
+ 'Topic :: Software Development :: Embedded Systems'
51
+ ],
52
+ keywords=['optimization', 'nonconvex', 'embedded'],
53
+ url=(
54
+ 'https://github.com/alphaville/gradgen'
55
+ ),
56
+ zip_safe=False)