def aero_prop():
pos_ENU = ca.SX.sym('pos_ENU', 3)
vel_ENU = ca.SX.sym('vel_ENU', 3)
q_ENU_TRF = ca.SX.sym('q_ENU_TRF', 4)
p = ca.SX.sym('p', 15)
u = ca.SX.sym('u', 4)
C_NED_ENU = np.array([
[0, 1, 0],
[1, 0, 0],
[0, 0, -1]
])
C_TRF_FRD = np.array([
[0, 0, -1],
[0, 1, 0],
[1, 0, 0]
])
so3.Mr
F_aero = ca.vertcat(p[0], p[1], p[2])
F_prop = ca.vertcat(x[3], x[4], x[5])
f = ca.Function('double_this', [x, u, p], [F_aero, F_prop],
['x', 'u', 'p'], ['F_aero', 'F_prop'])
gen = ca.CodeGenerator('casadi_gen.c', {'main': False, 'mex': False, 'with_header': True, 'with_mem': True})
gen.add(f)
gen.generate()
def code_generation():
x = ca.SX.sym('x', 14)
x_gz = ca.SX.sym('x_gz', 14)
p = ca.SX.sym('p', 16)
u = ca.SX.sym('u', 4)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
gz_eqs = gazebo_equations()
f_state = gz_eqs['state_from_gz']
eqs = rocket_equations()
C_FLT_FRB = gz_eqs['C_FLT_FRB']
F_FRB, M_FRB = eqs['force_moment'](x, u, p)
F_FLT = ca.mtimes(C_FLT_FRB, F_FRB)
M_FLT = ca.mtimes(C_FLT_FRB, M_FRB)
f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'],
['fin'])
f_force_moment = ca.Function('rocket_force_moment', [x, u, p],
[F_FLT, M_FLT], ['x', 'u', 'p'],
['F_FLT', 'M_FLT'])
u_control = eqs['control'](x, p, t, dt)
f_control = ca.Function('rocket_control', [x, p, t, dt], [u_control],
['x', 'p', 't', 'dt'], ['u'])
gen = ca.CodeGenerator('casadi_gen.c', {
'main': False,
'mex': False,
'with_header': True,
'with_mem': True
})
gen.add(f_state)
gen.add(f_force_moment)
gen.add(f_control)
gen.add(f_u_to_fin)
gen.generate()
def generate(self,
filename,
variable_names,
input_parameter_order=None,
cg_options=None):
"""
Generate the model in C, using CasADi.
Parameters
----------
filename : str
Name of the file to which to save the code
variable_names : list
Variables to be exported alongside the model structure
input_parameter_order : list, optional
Order in which the input parameters should be stacked. If None, the order
returned by :meth:`BaseModel.input_parameters` is used
cg_options : dict
Options to pass to the code generator.
See https://web.casadi.org/docs/#generating-c-code
"""
model = self.export_casadi_objects(variable_names,
input_parameter_order)
# Read the exported objects
t, x, z, p = model["t"], model["x"], model["z"], model["inputs"]
x0, z0 = model["x0"], model["z0"]
rhs, alg = model["rhs"], model["algebraic"]
variables = model["variables"]
jac_rhs, jac_alg = model["jac_rhs"], model["jac_algebraic"]
# Create functions
rhs_fn = casadi.Function("rhs_", [t, x, z, p], [rhs])
alg_fn = casadi.Function("alg_", [t, x, z, p], [alg])
jac_rhs_fn = casadi.Function("jac_rhs", [t, x, z, p], [jac_rhs])
jac_alg_fn = casadi.Function("jac_alg", [t, x, z, p], [jac_alg])
# Call these functions to initialize initial conditions
# (initial conditions are not yet consistent at this stage)
x0_fn = casadi.Function("x0", [p], [x0])
z0_fn = casadi.Function("z0", [p], [z0])
# Variables
variables_stacked = casadi.vertcat(*variables.values())
variables_fn = casadi.Function("variables", [t, x, z, p],
[variables_stacked])
# Write C files
cg_options = cg_options or {}
C = casadi.CodeGenerator(filename, cg_options)
C.add(rhs_fn)
C.add(alg_fn)
C.add(jac_rhs_fn)
C.add(jac_alg_fn)
C.add(x0_fn)
C.add(z0_fn)
C.add(variables_fn)
C.generate()
def code_generation():
x = ca.SX.sym('x', 14)
x_gz = ca.SX.sym('x_gz', 14)
p = ca.SX.sym('p', 16)
u = ca.SX.sym('u', 4)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
gz_eqs = gazebo_equations()
f_state = gz_eqs['state_from_gz']
eqs = rocket_equations()
ctrl_eqs = control_equations()
f_control = ctrl_eqs['pitch_control']
C_FLT_FRB = gz_eqs['C_FLT_FRB']
F_FRB, M_FRB = eqs['force_moment'](x, u, p)
F_FLT = ca.mtimes(C_FLT_FRB, F_FRB)
M_FLT = ca.mtimes(C_FLT_FRB, M_FRB)
#Casadi functions------------------------------------------------------------------
f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'],
['fin'])
#Rocket force & moment calculations
f_force_moment = ca.Function('rocket_force_moment', [x, u, p],
[F_FLT, M_FLT], ['x', 'u', 'p'],
['F_FLT', 'M_FLT'])
#----------------------------------------------------------------------------------
pitch_ctrl = control_rocket()
theta_ref = ca.SX.sym('theta_ref', 1)
mrp = ca.SX.sym('mrp', 4)
euler = so3.Euler.from_mrp(mrp)
theta_cur = euler[1]
theta_err = theta_ref - theta_cur
x_ctrl = ca.SX.sym('x_ctrl', 2)
x1_ctrl, u_elv = pitch_ctrl(x_ctrl, theta_err)
f_control = ca.Function('pitch_ctrl', [x_ctrl, mrp, theta_ref],
[x1_ctrl, u_elv], ['x_ctrl', 'mrp', 'theta_ref'],
['x1_ctrl', 'u_elv'])
gen = ca.CodeGenerator('casadi_gen.c', {
'main': False,
'mex': False,
'with_header': True,
'with_mem': True
})
gen.add(f_state)
gen.add(f_force_moment)
gen.add(f_control)
gen.add(f_u_to_fin)
gen.generate()
def _codegen_model(model_folder: str, f: ca.Function, library_name: str):
# Avoid locating compiler when loading from cache by loading
# distutils.ccompiler here on-demand.
import distutils.ccompiler
# Compile shared libraries
if os.name == 'posix':
compiler_flags = ['-O2', '-fPIC']
linker_flags = ['-fPIC']
else:
compiler_flags = ['/O2',
'/wd4101'] # Shut up unused local variable warnings.
linker_flags = ['/DLL']
# Generate C code
logger.debug("Generating {}".format(library_name))
cg = ca.CodeGenerator(library_name)
cg.add(f, True) # Nondifferentiated function
cg.add(f.forward(1), True) # Jacobian-times-vector product
cg.add(f.reverse(1), True) # vector-times-Jacobian product
cg.add(f.reverse(1).forward(1), True) # Hessian-times-vector product
cg.generate(model_folder + '/')
compiler = distutils.ccompiler.new_compiler()
file_name = os.path.relpath(os.path.join(model_folder,
library_name + '.c'))
object_name = compiler.object_filenames([file_name])[0]
library = os.path.join(model_folder,
library_name + compiler.shared_lib_extension)
try:
# NOTE: For some reason running in debug mode in PyCharm (2017.1)
# on Windows causes cl.exe to fail on its own binary name (?!) and
# the include paths. This does not happen when running directly
# from cmd.exe / PowerShell or e.g. with debug mode in VS Code.
compiler.compile([file_name], extra_postargs=compiler_flags)
# We do not want the "lib" prefix on POSIX systems, so we call
# link() directly with our desired filename instead of
# link_shared_lib().
compiler.link(compiler.SHARED_LIBRARY, [object_name],
library,
extra_preargs=linker_flags)
except:
raise
finally:
with contextlib.suppress(FileNotFoundError):
os.remove(file_name)
os.remove(object_name)
return library
def compile(self,compilation_folder):
# creates a clib (named: <self.name>.so) in <compilation_folder> with the above functions named: simplified_<func_name>
# (the order of the inputs is the same of the one of the casadi functions defined above)
funs = [self.eval,self.eval_jac]+self.eval_hes
os.makedirs(compilation_folder,exist_ok=True)
cg = cs.CodeGenerator(self.name)
cg.add_include("stdlib.h")
for f in funs:
cg.add(f)
cg.generate(compilation_folder+os.sep)
with open(os.path.join(compilation_folder,self.name+".c"),"a") as out:
out.write("""
typedef struct {
const double** arg;
double** res;
casadi_int* iw;
double* w;
void* mem;
} casadi_mem;
""")
for f in funs:
in_arg = ", ".join(["const double * in%d" % i for i in range(f.n_in())])
out_arg = ", ".join(["double * out%d" % i for i in range(f.n_out())])
set_in = "\n ".join(["mem.arg[{i}] = in{i};".format(i=i) for i in range(f.n_in())])
set_out = "\n ".join(["mem.res[{i}] = out{i};".format(i=i) for i in range(f.n_out())])
out.write("""
CASADI_SYMBOL_EXPORT int simplified_{name}({in_arg}, {out_arg}) {{
static int initialized = 0;
static casadi_mem mem;
if (!initialized) {{
casadi_int sz_arg, sz_res, sz_iw, sz_w;
{name}_work(&sz_arg, &sz_res, &sz_iw, &sz_w);
mem.arg = malloc(sizeof(void*)*sz_arg);
mem.res = malloc(sizeof(void*)*sz_res);
mem.iw = malloc(sizeof(casadi_int)*sz_iw);
mem.w = malloc(sizeof(casadi_int)*sz_w);
{name}_incref();
mem.mem = 0;//{name}_checkout();
}};
{set_in}
{set_out}
return {name}(mem.arg, mem.res, mem.iw, mem.w, mem.mem);
}}
""".format(name=f.name(), in_arg=in_arg, out_arg=out_arg, set_in=set_in, set_out=set_out))
subprocess.Popen(["gcc","-O0","-shared","-fPIC",os.path.join(compilation_folder,self.name+".c"),"-o",os.path.join(compilation_folder,self.name+".so")]).wait()
self.lib_path = compilation_folder+'/'+self.name+'.so'
def generate_code(eqs, dest_dir, **kwargs):
p = {
'main': False,
'mex': False,
'with_header': True,
'with_mem': True
}
for k, v in kwargs.items():
assert k in p.keys()
p[k] = v
# generate code
# Code Generation
for name, eqs in eqs.items():
filename = 'casadi_{:s}.c'.format(name)
gen = ca.CodeGenerator(filename, p)
for f_name in eqs:
gen.add(eqs[f_name])
os.makedirs(dest_dir, exist_ok=True)
gen.generate(dest_dir + os.path.sep)
def code_generation():
x = ca.SX.sym('x', 14)
x_gz = ca.SX.sym('x_gz', 14)
p = ca.SX.sym('p', 16)
u = ca.SX.sym('u', 4)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
gz_eqs = gazebo_equations()
f_state = gz_eqs['state_from_gz']
eqs = rocket_equations()
C_FLT_FRB = gz_eqs['C_FLT_FRB']
F_FRB, M_FRB = eqs['force_moment'](x, u, p)
F_FLT = ca.mtimes(C_FLT_FRB, F_FRB)
M_FLT = ca.mtimes(C_FLT_FRB, M_FRB)
f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'],
['fin'])
f_force_moment = ca.Function('rocket_force_moment', [x, u, p],
[F_FLT, M_FLT], ['x', 'u', 'p'],
['F_FLT', 'M_FLT'])
u_control = eqs['control'](x, p, t, dt)
f_control = ca.Function('rocket_control', [x, p, t, dt], [u_control],
['x', 'p', 't', 'dt'], ['u'])
gen = ca.CodeGenerator('casadi_gen.c', {
'main': False,
'mex': False,
'with_header': True,
'with_mem': True
})
x0, u0, p0 = do_trim(vt=100, gamma_deg=60, m_fuel=0.8, z=60)
x1, u1, p1 = do_trim(vt=100, gamma_deg=75, m_fuel=0.6, z=90)
x2, u2, p2 = do_trim(vt=100, gamma_deg=90, m_fuel=0.3, z=100)
# x3, u3, p3 = do_trim(vt=100, gamma_deg=85, m_fuel=0, z=100)
lin = linearize()
import control
sys1 = control.ss(*lin(x0, u0, p0))
sys2 = control.ss(*lin(x1, u1, p1))
sys3 = control.ss(*lin(x2, u2, p2))
# sys4 = control.ss(*lin(x3, u3, p3))
G1 = control.ss2tf(sys1[1, 2])
G2 = control.ss2tf(sys2[1, 2])
G3 = control.ss2tf(sys3[1, 2])
# G4 = control.ss2tf(sys4[1, 2])
G1 = control.c2d(G1, .01)
G2 = control.c2d(G2, .01)
G3 = control.c2d(G3, .01)
# G4 = control.c2d(G4, .01)
s = control.tf([1, 0], [0, 1])
Hd1 = (20 + 3 / s)
Hd2 = (20 + 3 / s)
Hd3 = (20 + 3 / s)
G1 = control.minreal(G1 * Hd1)
G2 = control.minreal(G2 * Hd2)
G3 = control.minreal(G3 * Hd3)
G1 = control.tf2ss(G1)
G2 = control.tf2ss(G2)
G3 = control.tf2ss(G3)
# G4 = control.minreal(G4)
#print (G3)
x0 = ca.SX.sym('x0', 8)
u = ca.SX.sym('u', 1)
x = ca.mtimes(G1.A, x0) + ca.mtimes(G1.B, u)
y = ca.mtimes(G1.C, x0) + ca.mtimes(G1.D, u)
controller1 = ca.Function('controller1', [x0, u], [x, y])
x = ca.mtimes(G2.A, x0) + ca.mtimes(G2.B, u)
y = ca.mtimes(G2.C, x0) + ca.mtimes(G2.D, u)
controller2 = ca.Function('controller2', [x0, u], [x, y])
x0 = ca.SX.sym('x', 4)
x = ca.mtimes(G3.A, x0) + ca.mtimes(G3.B, u)
y = ca.mtimes(G3.C, x0) + ca.mtimes(G3.D, u)
controller3 = ca.Function('controller3', [x0, u], [x, y])
gen.add(controller1)
gen.add(controller2)
gen.add(controller3)
gen.add(f_state)
gen.add(f_force_moment)
gen.add(f_control)
gen.add(f_u_to_fin)
#gen.generate()
return {
'controller1': controller1,
'controller2': controller2,
'controller3': controller3
}
def _save_model(model_folder: str, model_name: str, model: Model):
# Compile shared libraries
if os.name == 'posix':
compiler_flags = ['-O2', '-fPIC']
linker_flags = ['-fPIC']
else:
compiler_flags = ['/O2',
'/wd4101'] # Shut up unused local variable warnings.
linker_flags = ['/DLL']
objects = {
'dae_residual': ObjectData('dae_residual', ''),
'initial_residual': ObjectData('initial_residual', ''),
'variable_metadata': ObjectData('variable_metadata', '')
}
for o, d in objects.items():
f = getattr(model, o + '_function')
# Generate C code
library_name = '{}_{}'.format(model_name, o)
logger.debug("Generating {}".format(library_name))
cg = ca.CodeGenerator(library_name)
cg.add(f, True) # Nondifferentiated function
cg.add(f.forward(1), True) # Jacobian-times-vector product
cg.add(f.reverse(1), True) # vector-times-Jacobian product
cg.add(f.reverse(1).forward(1), True) # Hessian-times-vector product
cg.generate(model_folder + '/')
compiler = distutils.ccompiler.new_compiler()
file_name = os.path.relpath(
os.path.join(model_folder, library_name + '.c'))
object_name = compiler.object_filenames([file_name])[0]
d.library = os.path.join(model_folder,
library_name + compiler.shared_lib_extension)
try:
# NOTE: For some reason running in debug mode in PyCharm (2017.1)
# on Windows causes cl.exe to fail on its own binary name (?!) and
# the include paths. This does not happen when running directly
# from cmd.exe / PowerShell or e.g. with debug mode in VS Code.
compiler.compile([file_name], extra_postargs=compiler_flags)
# We do not want the "lib" prefix on POSIX systems, so we call
# link() directly with our desired filename instead of
# link_shared_lib().
compiler.link(compiler.SHARED_LIBRARY, [object_name],
d.library,
extra_preargs=linker_flags)
except:
raise
finally:
with contextlib.suppress(FileNotFoundError):
os.remove(file_name)
os.remove(object_name)
# Output metadata
db_file = os.path.join(model_folder, model_name)
with open(db_file, 'wb') as f:
db = {}
# Store version
db['version'] = __version__
# Include references to the shared libraries
for o, d in objects.items():
db[d.key] = d.library
db['library_os'] = os.name
# Describe variables per category
for key in [
'states', 'der_states', 'alg_states', 'inputs', 'parameters',
'constants'
]:
db[key] = [e.to_dict() for e in getattr(model, key)]
db['outputs'] = model.outputs
db['delayed_states'] = model.delayed_states
pickle.dump(db, f)
import casadi as ca
x = ca.SX.sym('x')
y = 2 * x
f = ca.Function('double_this', [x], [y], ['x'], ['y'])
gen = ca.CodeGenerator('casadi_gen.c', {
'main': False,
'mex': False,
'with_header': True,
'with_mem': True
})
gen.add(f)
gen.generate()
with open(os.path.join(root, items), 'r') as f:
if ast is None:
ast = parser.parse(f.read())
else:
ast.extend(parser.parse(f.read()))
# Compile
if options.flatten_only:
ast = tree.flatten(ast, model_name)
print(ast)
else:
model = gen_casadi.generate(ast, model_name)
f = model.get_function()
# Generate C code
cg = ca.CodeGenerator(model_name)
cg.add(f)
cg.add(f.forward(1))
cg.add(f.reverse(1))
cg.add(f.reverse(1).forward(1))
cg.generate()
file_name = model_name + '.c'
# Compile shared library
if os.name == 'posix':
ext = 'so'
else:
ext = 'dll'
try:
os.system("clang -shared {} -o lib{}.{}".format(file_name, model_name, ext))
