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 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