def generate_c_func(name, expr, in_args, column_major_storage=True, **kwargs):
""" generates C function code for a sympy exrpession
name is the name of the function
expr is the matrix expression
in_args is a list of tuples containing a variable-name string and the
variable symbol. The symbol can be a matrix or a scalar.
The sequence of the arguments will be the sequence of in_args and the
output (called 'out') is last
"""
if column_major_storage:
# generated C code matrix access is in row-major order
# [source: sympy/printing/ccode.py:215, _print_MatrixElement()]
# this is why we transpose the matrices in column-major mode
expr = expr.transpose()
subs_tab = []
arg_list = []
for argname, arg_sym in in_args:
if isinstance(arg_sym, sp.Matrix):
if column_major_storage:
arg_sym = arg_sym.transpose()
new_sym = sp.MatrixSymbol('__in__' + argname, *arg_sym.shape)
subs_tab += sympy_util.matrix_subs(arg_sym, new_sym)
arg_list.append(cg.InputArgument(new_sym,
dimensions=new_sym.shape))
else:
new_sym = sp.symbols(argname)
subs_tab.append((arg_sym, new_sym))
arg_list.append(cg.InputArgument(new_sym))
out_sym = sp.MatrixSymbol('out', expr.shape[0], expr.shape[1])
out_arg = cg.OutputArgument(out_sym,
out_sym,
expr.subs(subs_tab),
dimensions=out_sym.shape)
arg_list.append(out_arg)
no_return_val = []
no_local_vars = []
no_global_vars = []
code_gen = cg.get_code_generator("C", "projectname")
routines = [
cg.Routine(name, arg_list, no_return_val, no_local_vars,
no_global_vars)
]
[(c_name, c_code), (h_name, c_header)] = code_gen.write(routines,
"prefix",
header=False)
c_code = unused_param_warn_suppr(c_code,
[argname for argname, _sym in in_args])
c_code = clean_c_code(c_code, **kwargs)
return c_code
def sympy_into_c(sympy_functions, global_vars=None):
from sympy.utilities import codegen
routines = []
for name, expr, args in sympy_functions:
r = codegen.make_routine(name,
expr,
language="C99",
global_vars=global_vars)
# argument ordering input to sympy is broken with function with output arguments
nargs = []
# reorder the input arguments
for aa in args:
if aa is None:
nargs.append(
codegen.InputArgument(sp.Symbol('unused'),
dimensions=[1, 1]))
continue
found = False
for a in r.arguments:
if str(aa.name) == str(a.name):
nargs.append(a)
found = True
break
if not found:
# [1,1] is a hack for Matrices
nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
# add the output arguments
for a in r.arguments:
if type(a) == codegen.OutputArgument:
nargs.append(a)
# assert len(r.arguments) == len(args)+1
r.arguments = nargs
# add routine to list
routines.append(r)
[(c_name, c_code), (h_name, c_header)
] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
c_header = '\n'.join(x for x in c_header.split("\n")
if len(x) > 0 and x[0] != '#')
c_code = '\n'.join(x for x in c_code.split("\n")
if len(x) > 0 and x[0] != '#')
c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
return c_header, c_code
def ekf_generate_c_code(f, F, h, H, x, u, f_additional_in=[], h_additional_in=[]):
# generated C code matrix access is in row-major order
# [source: sympy/printing/ccode.py:215, _print_MatrixElement()]
# Since Eigen uses column-major by default we transpose the matrices before
# generating the C code
H = H.transpose()
F = F.transpose()
f_additional_in_sym = [sp.symbols('in{}_{}'.format(i+2, var.name)) for i, var in enumerate(f_additional_in)]
f_add_subs_tab = list(zip(f_additional_in, f_additional_in_sym))
h_additional_in_sym = [sp.symbols('in{}_{}'.format(i+1, var.name)) for i, var in enumerate(h_additional_in)]
h_add_subs_tab = list(zip(h_additional_in, h_additional_in_sym))
x_sym = sp.MatrixSymbol('in0_x', len(x), 1)
x_subs_tab = [(elem_sym, x_sym[i, 0]) for i, elem_sym in enumerate(x)]
u_sym = sp.MatrixSymbol('in1_u', len(u), 1)
u_subs_tab = [(elem_sym, u_sym[i, 0]) for i, elem_sym in enumerate(u)]
subs_tab = x_subs_tab + u_subs_tab + f_add_subs_tab + h_add_subs_tab
# c_code = generate_c_code('f', f.subs(subs_tab)) + '\n'
# c_code += generate_c_code('F', F.subs(subs_tab)) + '\n'
# c_code += generate_c_code('h', h.subs(subs_tab)) + '\n'
# c_code += generate_c_code('H', H.subs(subs_tab)) + '\n'
no_return_val = []
no_local_vars = []
f_additional_in_arg = [cg.InputArgument(sym) for sym in f_additional_in_sym]
h_additional_in_arg = [cg.InputArgument(sym) for sym in h_additional_in_sym]
f_out_sym = sp.MatrixSymbol('f_out', len(x), 1)
f_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape),
cg.InputArgument(u_sym, dimensions=u_sym.shape)]
f_arg_list += f_additional_in_arg
f_arg_list += [cg.OutputArgument(f_out_sym, f_out_sym, f.subs(subs_tab), dimensions=f_out_sym.shape)]
F_out_sym = sp.MatrixSymbol('F_out', F.shape[0], F.shape[1])
F_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape),
cg.InputArgument(u_sym, dimensions=u_sym.shape)]
F_arg_list += f_additional_in_arg
F_arg_list += [cg.OutputArgument(F_out_sym, F_out_sym, F.subs(subs_tab), dimensions=F_out_sym.shape)]
h_out_sym = sp.MatrixSymbol('h_out', len(h), 1)
h_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape)]
h_arg_list += h_additional_in_arg
h_arg_list += [cg.OutputArgument(h_out_sym, h_out_sym, h.subs(subs_tab), dimensions=h_out_sym.shape)]
H_out_sym = sp.MatrixSymbol('H_out', H.shape[0], H.shape[1])
H_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape)]
H_arg_list += h_additional_in_arg
H_arg_list += [cg.OutputArgument(H_out_sym, H_out_sym, H.subs(subs_tab), dimensions=H_out_sym.shape)]
routines = [cg.Routine("f", f_arg_list, no_return_val, no_local_vars),
cg.Routine("F", F_arg_list, no_return_val, no_local_vars),
cg.Routine("h", h_arg_list, no_return_val, no_local_vars),
cg.Routine("H", H_arg_list, no_return_val, no_local_vars)]
code_gen = cg.get_code_generator("C", "projectname")
[(c_name, c_code), (h_name, c_header)] = code_gen.write(routines, "prefix", header=False)
c_code = clean_c_code(c_code, use_single_float=True)
c_code_head = '// This file has been automatically generated\n'
c_code_head += '// DO NOT EDIT!\n\n'
c_code_head += '#include <math.h>\n\n'
c_code_head += 'const int STATE_DIM = {};\n'.format(len(x))
c_code_head += 'const int CONTROL_DIM = {};\n'.format(len(u))
c_code_head += 'const int MEASURE_DIM = {};\n'.format(len(h))
c_code_head += '\n\n'
return c_code_head + c_code