#!/usr/bin/env python3
import sympy as sp
import numpy as np
def cross ( x ) :
ret = sp . Matrix ( np . zeros ( ( 3 , 3 ) ) )
ret [ 0 , 1 ] , ret [ 0 , 2 ] = - x [ 2 ] , x [ 1 ]
ret [ 1 , 0 ] , ret [ 1 , 2 ] = x [ 2 ] , - x [ 0 ]
ret [ 2 , 0 ] , ret [ 2 , 1 ] = - x [ 1 ] , x [ 0 ]
return ret
def euler_rotate ( roll , pitch , yaw ) :
# make symbolic rotation matrix from eulers
matrix_roll = sp . Matrix ( [ [ 1 , 0 , 0 ] ,
[ 0 , sp . cos ( roll ) , - sp . sin ( roll ) ] ,
[ 0 , sp . sin ( roll ) , sp . cos ( roll ) ] ] )
matrix_pitch = sp . Matrix ( [ [ sp . cos ( pitch ) , 0 , sp . sin ( pitch ) ] ,
[ 0 , 1 , 0 ] ,
[ - sp . sin ( pitch ) , 0 , sp . cos ( pitch ) ] ] )
matrix_yaw = sp . Matrix ( [ [ sp . cos ( yaw ) , - sp . sin ( yaw ) , 0 ] ,
[ sp . sin ( yaw ) , sp . cos ( yaw ) , 0 ] ,
[ 0 , 0 , 1 ] ] )
return matrix_yaw * matrix_pitch * matrix_roll
def quat_rotate ( q0 , q1 , q2 , q3 ) :
# make symbolic rotation matrix from quat
return sp . Matrix ( [ [ q0 * * 2 + q1 * * 2 - q2 * * 2 - q3 * * 2 , 2 * ( q1 * q2 + q0 * q3 ) , 2 * ( q1 * q3 - q0 * q2 ) ] ,
[ 2 * ( q1 * q2 - q0 * q3 ) , q0 * * 2 - q1 * * 2 + q2 * * 2 - q3 * * 2 , 2 * ( q2 * q3 + q0 * q1 ) ] ,
[ 2 * ( q1 * q3 + q0 * q2 ) , 2 * ( q2 * q3 - q0 * q1 ) , q0 * * 2 - q1 * * 2 - q2 * * 2 + q3 * * 2 ] ] ) . T
def quat_matrix_l ( p ) :
return sp . Matrix ( [ [ p [ 0 ] , - p [ 1 ] , - p [ 2 ] , - p [ 3 ] ] ,
[ p [ 1 ] , p [ 0 ] , - p [ 3 ] , p [ 2 ] ] ,
[ p [ 2 ] , p [ 3 ] , p [ 0 ] , - p [ 1 ] ] ,
[ p [ 3 ] , - p [ 2 ] , p [ 1 ] , p [ 0 ] ] ] )
def quat_matrix_r ( p ) :
return sp . Matrix ( [ [ p [ 0 ] , - p [ 1 ] , - p [ 2 ] , - p [ 3 ] ] ,
[ p [ 1 ] , p [ 0 ] , p [ 3 ] , - p [ 2 ] ] ,
[ p [ 2 ] , - p [ 3 ] , p [ 0 ] , p [ 1 ] ] ,
[ p [ 3 ] , p [ 2 ] , - p [ 1 ] , p [ 0 ] ] ] )
def sympy_into_c ( sympy_functions ) :
from sympy . utilities import codegen
routines = [ ]
for name , expr , args in sympy_functions :
r = codegen . make_routine ( name , expr , language = " C99 " )
# 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 " )
getting ready for Python 3 (#619)
* tabs to spaces
python 2 to 3: https://portingguide.readthedocs.io/en/latest/syntax.html#tabs-and-spaces
* use the new except syntax
python 2 to 3: https://portingguide.readthedocs.io/en/latest/exceptions.html#the-new-except-syntax
* make relative imports absolute
python 2 to 3: https://portingguide.readthedocs.io/en/latest/imports.html#absolute-imports
* Queue renamed to queue in python 3
Use the six compatibility library to support both python 2 and 3: https://portingguide.readthedocs.io/en/latest/stdlib-reorg.html#renamed-modules
* replace dict.has_key() with in
python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#removed-dict-has-key
* make dict views compatible with python 3
python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#dict-views-and-iterators
Where needed, wrapping things that will be a view in python 3 with a list(). For example, if it's accessed with []
Python 3 has no iter*() methods, so just using the values() instead of itervalues() as long as it's not too performance intensive. Note that any minor performance hit of using a list instead of a view will go away when switching to python 3. If it is intensive, we could use the six version.
* Explicitly use truncating division
python 2 to 3: https://portingguide.readthedocs.io/en/latest/numbers.html#division
python 3 treats / as float division. When we want the result to be an integer, use //
* replace map() with list comprehension where a list result is needed.
In python 3, map() returns an iterator.
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter
* replace filter() with list comprehension
In python 3, filter() returns an interatoooooooooooor.
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter
* wrap zip() in list() where we need the result to be a list
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-zip
* clean out some lint
Removes these pylint warnings:
************* Module selfdrive.car.chrysler.chryslercan
W: 15, 0: Unnecessary semicolon (unnecessary-semicolon)
W: 16, 0: Unnecessary semicolon (unnecessary-semicolon)
W: 25, 0: Unnecessary semicolon (unnecessary-semicolon)
************* Module common.dbc
W:101, 0: Anomalous backslash in string: '\?'. String constant might be missing an r prefix. (anomalous-backslash-in-string)
************* Module selfdrive.car.gm.interface
R:102, 6: Redefinition of ret.minEnableSpeed type from float to int (redefined-variable-type)
R:103, 6: Redefinition of ret.mass type from int to float (redefined-variable-type)
************* Module selfdrive.updated
R: 20, 6: Redefinition of r type from int to str (redefined-variable-type)
old-commit-hash: 9dae0bfac4e54ec2b2e488d2b4ead1495c8f56d8
6 years ago
c_code = ' \n ' . join ( x for x in c_code . split ( " \n " ) if len ( x ) > 0 and x [ 0 ] != ' # ' )
c_header = ' \n ' . join ( x for x in c_header . split ( " \n " ) if len ( x ) > 0 and x [ 0 ] != ' # ' )
return c_header , c_code
