def casadi(self, x, u, dxdt):
""" write dynamics as first order ODE: dxdt = f(x(t))
x is a 6x1 vector: [x, y, psi, vx, vy, omega]^T
u is a 2x1 vector: [acc/pwm, steer]^T
dxdt is a casadi.SX variable
"""
pwm = u[0]
steer = u[1]
psi = x[2]
vx = x[3]
vy = x[4]
omega = x[5]
vmin = 0.05
vy = cs.if_else(vx < vmin, 0, vy)
omega = cs.if_else(vx < vmin, 0, omega)
steer = cs.if_else(vx < vmin, 0, steer)
vx = cs.if_else(vx < vmin, vmin, vx)
Frx = (self.Cm1 - self.Cm2 * vx) * pwm - self.Cr0 - self.Cr2 * (vx**2)
alphaf = steer - cs.atan2((self.lf * omega + vy), vx)
alphar = cs.atan2((self.lr * omega - vy), vx)
Ffy = self.Df * cs.sin(self.Cf * cs.arctan(self.Bf * alphaf))
Fry = self.Dr * cs.sin(self.Cr * cs.arctan(self.Br * alphar))
dxdt[0] = vx * cs.cos(psi) - vy * cs.sin(psi)
dxdt[1] = vx * cs.sin(psi) + vy * cs.cos(psi)
dxdt[2] = omega
dxdt[3] = 1 / self.mass * (Frx - Ffy * cs.sin(steer)) + vy * omega
dxdt[4] = 1 / self.mass * (Fry + Ffy * cs.cos(steer)) - vx * omega
dxdt[5] = 1 / self.Iz * (Ffy * self.lf * cs.cos(steer) - Fry * self.lr)
return dxdt
def configure_model(self):
"""
Create state and input variables, as well as describe the vehicle's physical properties by using approximative \
motion's equations.
"""
self._model = Model("continuous")
# Create state and input variables
self._position_x = self._model.set_variable(var_type="_x",
var_name="position_x",
shape=(1, 1))
self._position_y = self._model.set_variable(var_type="_x",
var_name="position_y",
shape=(1, 1))
self._heading_angle = self._model.set_variable(
var_type="_x", var_name="heading_angle", shape=(1, 1))
self._velocity = self._model.set_variable(var_type="_u",
var_name="velocity",
shape=(1, 1))
self._steering_angle = self._model.set_variable(
var_type="_u", var_name="steering_angle", shape=(1, 1))
# Next state equations - these mathematical formulas are the core of the model
slip_factor = casadi.arctan(LR * casadi.tan(self._steering_angle) /
WHEELBASE_LENGTH)
self._model.set_rhs(
"position_x",
self._velocity * casadi.cos(self._heading_angle + slip_factor))
self._model.set_rhs(
"position_y",
self._velocity * casadi.sin(self._heading_angle + slip_factor))
self._model.set_rhs(
"heading_angle",
self._velocity * casadi.tan(self._steering_angle) *
casadi.cos(slip_factor) / WHEELBASE_LENGTH)
def __init__(self, N=30, step=0.01):
# We construct the model as a set of differential-algebraic equations (DAE)
self.dae = casadi.DaeBuilder()
# Parameters
self.n = 4
self.m = 2 # controls
self.N = N
self.step = step
self.T = N * step # Time horizon (seconds)
self.u_upper = 2.5
self.fixed_points = dict() # None yet
# Constants
self.lr = 2.10 # distance from CG to back wheel in meters
self.lf = 2.67
# Source, page 40: https://www.diva-portal.org/smash/get/diva2:860675/FULLTEXT01.pdf
# States
z = self.dae.add_x('z', self.n)
x = z[0]
y = z[1]
v = z[2]
psi = z[3]
self.STATE_NAMES = [
'x',
'y',
'v',
'psi',
]
# Controls
u = self.dae.add_u('u', self.m) # acceleration
a = u[0]
delta_f = u[1] # front steering angle
# This is a weird "state".
beta = casadi.arctan(self.lr / (self.lr + self.lf) *
casadi.tan(delta_f))
self.CONTROL_NAMES = ['a', 'delta_f']
# Define ODEs
xdot = v * casadi.cos(psi + beta)
ydot = v * casadi.sin(psi + beta)
vdot = a
psidot = v / self.lr * casadi.sin(beta)
zdot = casadi.vertcat(xdot, ydot, vdot, psidot)
self.dae.add_ode('zdot', zdot)
# Customize Matplotlib:
mpl.rcParams['font.size'] = 18
mpl.rcParams['lines.linewidth'] = 3
mpl.rcParams['axes.grid'] = True
self.state_estimate = None
self.control_estimate = np.zeros((self.m, self.N))
def sigmoid(x,
sigmoid_type: str = "tanh",
normalization_range: Tuple[Union[float, int],
Union[float, int]] = (0, 1)):
"""
A sigmoid function. From Wikipedia (https://en.wikipedia.org/wiki/Sigmoid_function):
A sigmoid function is a mathematical function having a characteristic "S"-shaped curve
or sigmoid curve.
Args:
x: The input
sigmoid_type: Type of sigmoid function to use [str]. Can be one of:
*
*
*
normalization_type: Range in which to normalize the sigmoid, shorthanded here in the
documentation as "N". This parameter is given as a two-element tuple (min, max).
After normalization:
>>> sigmoid(-Inf) == normalization_range[0]
>>> sigmoid(Inf) == normalization_range[1]
* In the special case of N = (0, 1):
>>> sigmoid(-Inf) == 0
>>> sigmoid(Inf) == 1
>>> sigmoid(0) == 0.5
>>> d(sigmoid)/dx at x=0 == 0.5
* In the special case of N = (-1, 1):
>>> sigmoid(-Inf) == -1
>>> sigmoid(Inf) == 1
>>> sigmoid(0) == 0
>>> d(sigmoid)/dx at x=0 == 1
Returns: The value of the sigmoid.
"""
### Sigmoid equations given here under the (-1, 1) normalization:
if sigmoid_type == ("tanh" or "logistic"):
# Note: tanh(x) is simply a scaled and shifted version of a logistic curve; after
# normalization these functions are identical.
s = cas.tanh(x)
elif sigmoid_type == "arctan":
s = 2 / cas.pi * cas.arctan(cas.pi / 2 * x)
elif sigmoid_type == "polynomial":
s = x / (1 + x**2)**0.5
else:
raise ValueError("Bad value of parameter 'type'!")
### Normalize
min = normalization_range[0]
max = normalization_range[1]
s_normalized = s * (max - min) / 2 + (max + min) / 2
return s_normalized
def cubic_spline(self, step=40):
# Loading waypoints from csv file
# input_file = rospy.get_param("~wp_file")
input_file = "/home/lva/data_file/wp-for-mpcc.csv"
data = genfromtxt(input_file, delimiter=',')
x_list = data[:, 0]
y_list = data[:, 1]
self.wp_len = len(x_list)
l_list = np.arange(0, self.wp_len, 1)
self.L = int(self.wp_len / step) * step
self.cs_x = ca.interpolant('cs_x', 'bspline', [l_list[::step]],
x_list[::step])
self.cs_y = ca.interpolant('cs_y', 'bspline', [l_list[::step]],
y_list[::step])
th = ca.MX.sym('th')
# Tangent angle
self.Phi = ca.Function('Phi', [th], [
ca.arctan(
ca.jacobian(self.cs_y(th), th) /
ca.jacobian(self.cs_x(th), th))
])
print(self.L)
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def makeModel(conf,propertiesDir='../properties'):
print "Using the update carousel model"
# Make model
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
# Get variables and outputs from the model
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
z = dae['z']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
zt = conf['zt']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = dae['R_c2b']
rA = conf['rArm']
g = conf['g']
# Rotation matrix to convert from NWU to NED frame type
R_nwu2ned = np.eye( 3 )
R_nwu2ned[1, 1] = R_nwu2ned[2, 2] = -1.0
############################################################################
#
# IMU model
#
############################################################################
# Load IMU position and orientation w.r.t. body frame
pIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat'))))
RIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat'))))
# Define IMU measurement functions
# TODO here is omitted the term: w x w pIMU
# The sign of gravity is negative because of the NED convention (z points down!)
ddpIMU_c = ddp - ddelta ** 2 * C.vertcat([x + rA, y, 0]) + 2 * ddelta * C.vertcat([-dy, dx, 0]) + \
dddelta * C.vertcat([-y, x + rA, 0]) - C.vertcat([0, 0, g])
ddpIMU = C.mul(R, ddpIMU_c)
aBridle = C.cross(ddt_w_bn_b, pIMU)
ddpIMU += aBridle
ddpIMU = C.mul(RIMU,ddpIMU)
# You can add a parameter to conf which will give the model 3 extra states with derivative 0 (to act as parameter) for the bias in the acceleration measurements. If that is present, it should be added to the measurement of the acceleration
if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']:
IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']])
ddpIMU += IMUAccelerationBias
# For the accelerometers
dae['IMU_acceleration'] = ddpIMU
# ... and for the gyroscopes
dae['IMU_angular_velocity'] = C.mul(RIMU, dae['w_bn_b'])
if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:
dae['vel_error_x'] = dae['dx']-dae['dx_IMU']
dae['vel_error_y'] = dae['dy']-dae['dy_IMU']
dae['vel_error_z'] = dae['dz']-dae['dz_IMU']
############################################################################
#
# Stereo vision subsystem modeling
#
############################################################################
# Load calibration data from configuration files
camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/PC1.dat'))),
'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/PC2.dat'))),
'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/RPC1.dat'))),
'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/RPC2.dat'))),
'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'markers/pos_marker_body1.dat'))),
'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'markers/pos_marker_body2.dat'))),
'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'markers/pos_marker_body3.dat')))}
# Construction of the measurement functions
dae['marker_positions'] = camModel.fullCamModel(dae, camConf)
x_tether = (x + zt*e31)
y_tether = (y + zt*e32)
z_tether = (z + zt*e33)
dae['lineAngles'] = C.vertcat([C.arctan(y_tether/x_tether),C.arctan(z_tether/x_tether)])
############################################################################
#
# Constraints in the MHE
#
############################################################################
dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1
dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23']
dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33']
dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1
dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33']
dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1
dae['ConstDelta'] = (dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1)
return dae
def makeDae( conf = None ):
if conf is None:
conf = arianne_conf.makeConf()
# Make model
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
# Get variables and outputs from the model
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
z = dae['z']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
zt = conf['zt']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = dae['R_c2b']
rA = conf['rArm']
g = conf['g']
# Rotation matrix to convert from NWU to NED frame type
R_nwu2ned = np.eye( 3 )
R_nwu2ned[1, 1] = R_nwu2ned[2, 2] = -1.0
############################################################################
#
# IMU model
#
############################################################################
# Load IMU position and orientation w.r.t. body frame
# pIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat'))))
pIMU = C.DMatrix([0,0,0])
pIMU = C.mul(R_nwu2ned, pIMU)
#0
#0
#0
# RIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat'))))
#9.937680e-01 6.949103e-02 8.715574e-02
#-6.975647e-02 9.975641e-01 0
#-8.694344e-02 -6.079677e-03 9.961947e-01
RIMU = C.DMatrix([[9.937680e-01, 6.949103e-02, 8.715574e-02],
[-6.975647e-02, 9.975641e-01, 0],
[-8.694344e-02, -6.079677e-03, 9.961947e-01]])
RIMU = C.mul(R_nwu2ned, RIMU)
# Define IMU measurement functions
# TODO here is omitted the term: w x w pIMU
# The sign of gravity is negative because of the NED convention (z points down!)
ddpIMU_c = ddp - ddelta ** 2 * C.vertcat([x + rA, y, 0]) + 2 * ddelta * C.vertcat([-dy, dx, 0]) + \
dddelta * C.vertcat([-y, x + rA, 0]) - C.vertcat([0, 0, g])
ddpIMU = C.mul(R, ddpIMU_c)
aBridle = C.cross(ddt_w_bn_b, pIMU)
ddpIMU += aBridle
ddpIMU = C.mul(RIMU,ddpIMU)
# You can add a parameter to conf which will give the model 3 extra states with derivative 0 (to act as parameter) for the bias in the acceleration measurements. If that is present, it should be added to the measurement of the acceleration
if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']:
IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']])
ddpIMU += IMUAccelerationBias
# For the accelerometers
dae['IMU_acceleration'] = ddpIMU
dae['IMU_acceleration_x'] = dae['IMU_acceleration'][0]
dae['IMU_acceleration_y'] = dae['IMU_acceleration'][1]
dae['IMU_acceleration_z'] = dae['IMU_acceleration'][2]
# ... and for the gyroscopes
dae['IMU_angular_velocity'] = C.mul(RIMU, dae['w_bn_b'])
dae['IMU_angular_velocity_x'] = dae['IMU_angular_velocity'][0]
dae['IMU_angular_velocity_y'] = dae['IMU_angular_velocity'][1]
dae['IMU_angular_velocity_z'] = dae['IMU_angular_velocity'][2]
if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:
dae['vel_error_x'] = dae['dx']-dae['dx_IMU']
dae['vel_error_y'] = dae['dy']-dae['dy_IMU']
dae['vel_error_z'] = dae['dz']-dae['dz_IMU']
############################################################################
#
# LAS
#
############################################################################
x_tether = (x + zt*e31)
y_tether = (y + zt*e32)
z_tether = (z + zt*e33)
dae['lineAngles'] = C.vertcat([C.arctan(y_tether/x_tether),C.arctan(z_tether/x_tether)])
dae['lineAngle_hor'] = dae['lineAngles'][0]
dae['lineAngle_ver'] = dae['lineAngles'][1]
return dae