コード例 #1
0
    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
コード例 #2
0
    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)
コード例 #3
0
    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))
コード例 #4
0
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
コード例 #5
0
 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)
コード例 #6
0
ファイル: sympy_interop.py プロジェクト: tmmsartor/casadi
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)))
コード例 #7
0
ファイル: sympy_interop.py プロジェクト: ml-lab/casadi
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)))
コード例 #8
0
ファイル: carouselModel.py プロジェクト: drewm1980/planepower
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
コード例 #9
0
ファイル: carousel_dae.py プロジェクト: ghorn/rawesome
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