Example #1
0
def pendulum_model():
    """ Nonlinear inverse pendulum model. """
    M = 1    # mass of the cart [kg]
    m = 0.1  # mass of the ball [kg]
    g = 9.81 # gravity constant [m/s^2]
    l = 0.8  # length of the rod [m]

    p = SX.sym('p')         # horizontal displacement [m]
    theta = SX.sym('theta') # angle with the vertical [rad]
    v = SX.sym('v')         # horizontal velocity [m/s]
    omega = SX.sym('omega') # angular velocity [rad/s]
    F = SX.sym('F')         # horizontal force [N]

    ode_rhs = vertcat(v,
                      omega,
                      (- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
                      (- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))

    nx = 4
    # for IRK
    xdot = SX.sym('xdot', nx, 1)
    z = SX.sym('z',0,1)
    return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
            nx, # number of states
            1,  # number of controls
            Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
                                    ['x', 'u','xdot','z'], ['rhs']))
Example #2
0
    def setTrajectory(ocp, t0):
        tk = t0
        for k in range(N):
            ocp.y[k,0] = A*C.sin(omega*tk)
            ocp.y[k,1] = A*C.cos(omega*tk)*omega
            tk += ts
        ocp.yN[0] = A*C.sin(omega*tk)
        ocp.yN[1] = A*C.cos(omega*tk)*omega

        refLog.append(numpy.copy(numpy.vstack((ocp.y[:,:2], ocp.yN.T))))
Example #3
0
 def _plot_arrows_3D(name, ax, x, y, phi, psi):
     x = ca.veccat(x)
     y = ca.veccat(y)
     z = ca.DMatrix.zeros(x.size())
     phi = ca.veccat(phi)
     psi = ca.veccat(psi)
     x_vec = ca.cos(psi) * ca.cos(phi)
     y_vec = ca.cos(psi) * ca.sin(phi)
     z_vec = ca.sin(psi)
     ax.quiver(x + x_vec, y + y_vec, z + z_vec,
               x_vec, y_vec, z_vec,
               color='r', alpha=0.8)
     return [Patch(color='red', label=name)]
Example #4
0
def plot_arrows3D(name, ax, x, y, phi, theta):
    x = ca.veccat(x)
    y = ca.veccat(y)
    z = ca.DMatrix.zeros(x.size())
    phi = ca.veccat(phi)
    theta = ca.veccat(theta)
    x_vec = ca.cos(theta)*ca.cos(phi)
    y_vec = ca.cos(theta)*ca.sin(phi)
    z_vec = ca.sin(theta)
    length = 2.0
    ax.quiver(x+length*x_vec, y+length*y_vec, z+length*z_vec,
              x_vec, y_vec, z_vec,
              length=length, arrow_length_ratio=0.5)
    return [Patch(color='red', label=name)]
Example #5
0
    def _create_continuous_dynamics(self):
        # Unpack arguments
        [x_b, y_b, z_b, vx_b, vy_b, vz_b,
         x_c, y_c, vx_c, vy_c, phi, psi] = self.x[...]
        [F_c, w_phi, w_psi, theta] = self.u[...]

        # Define the governing ordinary differential equation (ODE)
        rhs = cat.struct_SX(self.x)
        rhs['x_b'] = vx_b
        rhs['y_b'] = vy_b
        rhs['z_b'] = vz_b
        rhs['vx_b'] = 0
        rhs['vy_b'] = 0
        rhs['vz_b'] = -self.g
        rhs['x_c'] = vx_c
        rhs['y_c'] = vy_c
        rhs['vx_c'] = F_c * ca.cos(phi + theta) - self.mu * vx_c
        rhs['vy_c'] = F_c * ca.sin(phi + theta) - self.mu * vy_c
        rhs['phi'] = w_phi
        rhs['psi'] = w_psi

        op = {'input_scheme': ['x', 'u'],
              'output_scheme': ['x_dot']}
        return ca.SXFunction('Continuous dynamics',
                             [self.x, self.u], [rhs], op)
Example #6
0
def continuous_dynamics(state, control):
    # Unpack arguments
    [x_b, y_b, z_b, vx_b, vy_b, vz_b, x_c, y_c, phi] = state[...]
    [v, w, psi] = control[...]

    # Define right-hand side
    rhs = cat.struct_SX(state)
    rhs["x_b"] = vx_b
    rhs["y_b"] = vy_b
    rhs["z_b"] = vz_b
    rhs["vx_b"] = 0
    rhs["vy_b"] = 0
    rhs["vz_b"] = -g0
    rhs["x_c"] = v * (ca.cos(psi) * ca.cos(phi) - ca.sin(psi) * ca.sin(phi))
    rhs["y_c"] = v * (ca.cos(psi) * ca.sin(phi) + ca.sin(psi) * ca.cos(phi))
    rhs["phi"] = w
    return ca.SXFunction("Continuous dynamics", [state, control], [rhs])
Example #7
0
def continuous_dynamics(state, control):
    # Unpack arguments
    [x_b,y_b,vx_b,vy_b,x_c,y_c,phi] = state[...]
    [v,w,psi] = control[...]    
    
    # Define right-hand side
    rhs = cat.struct_SX(state)
    rhs['x_b'] = vx_b
    rhs['y_b'] = vy_b
    rhs['vx_b'] = 0
    rhs['vy_b'] = 0
    rhs['x_c'] = v * (ca.cos(psi) * ca.cos(phi) - \
                      ca.sin(psi) * ca.sin(phi))
    rhs['y_c'] = v * (ca.cos(psi) * ca.sin(phi) + \
                      ca.sin(psi) * ca.cos(phi))
    rhs['phi'] = w
    return ca.SXFunction('Continuous dynamics',[state,control],[rhs])
Example #8
0
def plot_arrows(name, ax, x, y, phi, theta):
    x_vec = ca.cos(theta)*ca.cos(phi)
    y_vec = ca.cos(theta)*ca.sin(phi)
    ax.quiver(x, y, x_vec, y_vec,
              units='xy', angles='xy', scale=1, width=0.1,
              headwidth=4, headlength=6, headaxislength=4,
              color='r', lw='0.1')
    return [Patch(color='red', label=name)]
Example #9
0
 def _plot_arrows(name, ax, x, y, phi):
     x_vec = ca.cos(phi)
     y_vec = ca.sin(phi)
     ax.quiver(x, y, x_vec, y_vec,
               units='xy', angles='xy', scale=1, width=0.08,
               headwidth=4, headlength=6, headaxislength=5,
               color='r', alpha=0.8, lw=0.1)
     return [Patch(color='red', label=name)]
Example #10
0
    def init(self, horizon_times=None):
        ObstaclexD.init(self, horizon_times=horizon_times)
        if self.signals['angular_velocity'][:, -1] == 0.:
            self.cos = cos(self.signals['orientation'][:, -1][0])
            self.sin = sin(self.signals['orientation'][:, -1][0])
            self.gon_weight = 1.
            return
        # theta, omega
        theta = self.define_parameter('theta', 1)
        omega = self.signals['angular_velocity'][:, -1][0]
        # theta, omega at time zero of time horizon
        theta0 = theta - self.t*omega
        # cos, sin, weight spline over time horizon
        Ts = 2.*np.pi/abs(omega)
        if self.options['horizon_time'] is None:
            raise ValueError(
                'You need to provide a horizon time when using rotating obstacles!')
        else:
            T = self.options['horizon_time']
        n_quarters = int(np.ceil(4*T/Ts))
        knots_theta = np.r_[np.zeros(3), np.hstack(
            [0.25*k*np.ones(2) for k in range(1, n_quarters+1)]), 0.25*n_quarters]*(Ts/T)
        Tf, knots = get_interval_T(BSplineBasis(knots_theta, 2), 0, 1.)
        basis = BSplineBasis(knots, 2)
        # coefficients based on nurbs representation of circle
        cos_cfs = np.r_[1., np.sqrt(
            2.)/2., 0., -np.sqrt(2.)/2., -1., -np.sqrt(2.)/2.,  0.,  np.sqrt(2.)/2., 1.]
        sin_cfs = np.r_[0., np.sqrt(
            2.)/2., 1.,  np.sqrt(2.)/2.,  0., -np.sqrt(2.)/2., -1., -np.sqrt(2.)/2., 0.]
        weight_cfs = np.r_[1., np.sqrt(
            2.)/2., 1.,  np.sqrt(2.)/2.,  1.,  np.sqrt(2.)/2.,  1.,  np.sqrt(2.)/2., 1.]
        cos_cfs = Tf.dot(np.array([cos_cfs[k % 8] for k in range(len(basis))]))
        sin_cfs = Tf.dot(np.array([sin_cfs[k % 8] for k in range(len(basis))]))
        weight_cfs = Tf.dot(
            np.array([weight_cfs[k % 8] for k in range(len(basis))]))

        cos_wt = BSpline(basis, cos_cfs)
        sin_wt = BSpline(basis, sin_cfs)*np.sign(omega)
        self.cos = cos_wt*cos(theta0) - sin_wt*sin(theta0)
        self.sin = cos_wt*sin(theta0) + sin_wt*cos(theta0)
        self.gon_weight = BSpline(basis, weight_cfs)
Example #11
0
    def _create_objective_function(model, V, warm_start):
        [final_cost] = model.cl([V['X', model.n]])
        running_cost = 0
        for k in range(model.n):
            [stage_cost] = model.c([V['X', k], V['U', k]])

            # Encourage looking at the ball
            d = ca.veccat([ca.cos(V['X', k, 'psi'])*ca.cos(V['X', k, 'phi']),
                           ca.cos(V['X', k, 'psi'])*ca.sin(V['X', k, 'phi']),
                           ca.sin(V['X', k, 'psi'])])
            r = ca.veccat([V['X', k, 'x_b'] - V['X', k, 'x_c'],
                           V['X', k, 'y_b'] - V['X', k, 'y_c'],
                           V['X', k, 'z_b']])
            r_cos_omega = ca.mul(d.T, r)
            if warm_start:
                cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
                stage_cost += 1e-1 * (1 - cos_omega)
            else:
                stage_cost -= 1e-1 * r_cos_omega * model.dt

            running_cost += stage_cost
        return final_cost + running_cost
Example #12
0
def apply_unary_opcode(code, p):
	assert code in unary_opcodes, "Opcode not recognized!"
	if code==5:
		return -p
	elif code==10:
		return casadi.sqrt(p)
	elif code==11:
		return casadi.sin(p)
	elif code==12:
		return casadi.cos(p)
	elif code==13:
		return casadi.tan(p)
	assert False
Example #13
0
def observation_covariance(state, observation):
    d = ca.veccat([ca.cos(state["phi"]), ca.sin(state["phi"])])
    r = ca.veccat([state["x_b"] - state["x_c"], state["y_b"] - state["y_c"]])
    r_cos_theta = ca.mul(d.T, r)
    cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)

    # Look at the ball and be close to the ball
    nz = observation.size
    R = observation.squared(ca.SX.zeros(nz, nz))
    R["x_b", "x_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
    R["y_b", "y_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2

    # state -> R
    return ca.SXFunction("Observation covariance", [state], [R])
Example #14
0
def R_from_roll_pitch_yaw(roll, pitch, yaw):
	ca1 = cos(yaw)
	sa1 = sin(yaw)
	cb1 = cos(pitch)
	sb1 = sin(pitch)
	cc1 = cos(roll)
	sc1 = sin(roll)
	ca1sb1 = ca1*sb1
	sa1sb1 = sa1*sb1
	if casadi.SXMatrix in map(type,[roll,pitch,yaw]):
		R = SXMatrix(3,3)
	else:
		R = numpy.zeros((3,3))
	R[0,0] = ca1*cb1
	R[0,1] = ca1sb1*sc1-sa1*cc1
	R[0,2] = ca1sb1*cc1+sa1*sc1
	R[1,0] = sa1*cb1
	R[1,1] = sa1sb1*sc1+ca1*cc1
	R[1,2] = sa1sb1*cc1-ca1*sc1
	R[2,0] = -sb1
	R[2,1] = cb1*sc1
	R[2,2] = cb1*cc1
	return R
Example #15
0
def Rz(theta):
	if type(theta)==SXMatrix:
		R = SXMatrix(4,4)
	R[0,0] = 1
	R[1,1] = 1
	R[2,2] = 1
	R[3,3] = 1
	c = cos(theta)
	s = sin(theta)
	R[0,0] = c
	R[1,0] = s
	R[0,1] = -s 
	R[1,1] = c
	return R
Example #16
0
    def _create_observation_covariance_function(self, N_min, N_max):
        d = ca.veccat([ca.cos(self.x['psi']) * ca.cos(self.x['phi']),
                       ca.cos(self.x['psi']) * ca.sin(self.x['phi']),
                       ca.sin(self.x['psi'])])
        r = ca.veccat([self.x['x_b'] - self.x['x_c'],
                       self.x['y_b'] - self.x['y_c'],
                       self.x['z_b']])
        r_cos_omega = ca.mul(d.T, r)
        cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)

        # Look at the ball
        N = self.z.squared(ca.SX.zeros(self.nz, self.nz))
        # variance = N_max * (1 - cos_omega) + N_min
        # variance = ca.mul(r.T, r) * (N_max * (1 - cos_omega) + N_min)
        variance = ca.norm_2(r) * (N_max * (1 - cos_omega) + N_min)
        N['x_b', 'x_b'] = variance
        N['y_b', 'y_b'] = variance
        N['z_b', 'z_b'] = variance

        op = {'input_scheme': ['x'],
              'output_scheme': ['N']}
        return ca.SXFunction('Observation covariance',
                             [self.x], [N], op)
Example #17
0
def observation_covariance(state, observation):
    d = ca.veccat([ca.cos(state['phi']), ca.sin(state['phi'])])
    r = ca.veccat([state['x_b']-state['x_c'], state['y_b']-state['y_c']])
    r_cos_theta = ca.mul(d.T,r)
    cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
    
    # Look at the ball and be close to the ball
    nz = observation.size
    R = observation.squared(ca.SX.zeros(nz,nz))
    R['x_b','x_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
    R['y_b','y_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
    
    # state -> R
    return ca.SXFunction('Observation covariance',[state],[R])
Example #18
0
def create_simple_plan(x0, F, dt, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=state),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])

    # Final position
    x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dx_bc = x_bN - x_cN
    
    # Final velocity
    v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
    v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
    dv_bc = v_bN - v_cN
    
    # Angle between gaze and ball velocity
    theta = V['X',N_sim,'theta']
    phi = V['X',N_sim,'phi']
    d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
    r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
Example #19
0
    def setPhase(self,phase):
        maxPoly = max([max(self.fits[name].polyOrder) for name in self.fits])
        maxCos = max([max(self.fits[name].cosOrder) for name in self.fits])
        maxSin = max([max(self.fits[name].sinOrder) for name in self.fits])

        # all bases
        polyBases = [phase**po for po in range(maxPoly+1)]
        cosBases = [C.cos(co*phase) for co in range(maxCos+1)]
        sinBases = [C.sin(co*phase) for si in range(maxSin+1)]

        self.fitsWithPhase = {}
        for name in self.fits:
            fit = self.fits[name]

            polys = [coeff*polyBases[order] for coeff,order in zip(fit.polyCoeffs, fit.polyOrder)]
            coses = [coeff*cosBases[order]  for coeff,order in zip(fit.cosCoeffs,  fit.cosOrder)]
            sins  = [coeff*sinBases[order]  for coeff,order in zip(fit.sinCoeffs,  fit.sinOrder)]

            self.fitsWithPhase[name] = sum(polys+coses+sins)
Example #20
0
def carouselModel(conf):
    '''
    pass this a conf, and it'll return you a dae
    '''
    # empty Dae
    dae = Dae()

    # add some differential states/algebraic vars/controls/params
    dae.addZ("nu")
    dae.addX( [ "x"
              , "y"
              , "z"
              , "e11"
              , "e12"
              , "e13"
              , "e21"
              , "e22"
              , "e23"
              , "e31"
              , "e32"
              , "e33"
              , "dx"
              , "dy"
              , "dz"
              , "w_bn_b_x"
              , "w_bn_b_y"
              , "w_bn_b_z"
              , "ddelta"
              , "r"
              , "dr"
              , "aileron"
              , "elevator"
              , "motor_torque"
              , "ddr"
              ] )
    if 'cn_rudder' in conf:
        dae.addX('rudder')
        dae.addU('drudder')
    if 'cL_flaps' in conf:
        dae.addX('flaps')
        dae.addU('dflaps')
    if conf['delta_parameterization'] == 'linear':
        dae.addX('delta')
        dae['cos_delta'] = C.cos(dae['delta'])
        dae['sin_delta'] = C.sin(dae['delta'])
        dae_delta_residual = dae.ddt('delta') - dae['ddelta']

    elif conf['delta_parameterization'] == 'cos_sin':
        dae.addX("cos_delta")
        dae.addX("sin_delta")
        norm = dae['cos_delta']**2 + dae['sin_delta']**2

        if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
            pole_delta = 0.5    
        else:
            pole_delta = 0.0

        cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
        sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
        dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
                                       dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
    else:
        raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')

    dae.addU( [ "daileron"
              , "delevator"
              , "dmotor_torque"
              , 'dddr'
              ] )
    # add wind parameter if wind shear is in configuration
    if 'wind_model' in conf:
        if conf['wind_model']['name'] == 'hardcoded':
            dae['w0'] = conf['wind_model']['hardcoded_value']
        elif conf['wind_model']['name'] != 'random_walk':
            dae.addP( ['w0'] )

    # set some state derivatives as outputs
    dae['ddx'] = dae.ddt('dx')
    dae['ddy'] = dae.ddt('dy')
    dae['ddz'] = dae.ddt('dz')
    dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
    dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
    dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
    dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])

    # some outputs in for plotting
    dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
    dae['aileron_deg'] = dae['aileron']*180/C.pi
    dae['elevator_deg'] = dae['elevator']*180/C.pi
    dae['daileron_deg_s'] = dae['daileron']*180/C.pi
    dae['delevator_deg_s'] = dae['delevator']*180/C.pi

    # tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
    dae['tether_tension'] = dae['r']*dae['nu']

    # theoretical mechanical power
    dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']

    # carousel2 motor model from thesis data
    dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
    dae['torque'] = dae['tether_tension']*0.1
    dae['electrical_winch_power'] =  293.5816373499238 + \
                                     0.0003931623408*dae['rpm']*dae['rpm'] + \
                                     0.0665919381751*dae['torque']*dae['torque'] + \
                                     0.1078628659825*dae['rpm']*dae['torque']

    dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
                               [dae['e21'],dae['e22'],dae['e23']],
                               [dae['e31'],dae['e32'],dae['e33']]])
    
    dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
    dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
    dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi

    # line angle
    dae['cos_line_angle'] = \
      -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
    dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi

    (massMatrix, rhs, dRexp) = setupModel(dae, conf)

    if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
        RotPole = 0.5
    else:
        RotPole = 0.0
    Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )

    ode = C.veccat([
        C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
        C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
                                             "e21","e22","e23",
                                             "e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
        dae_delta_residual,
#        C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
#        C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
#        dae.ddt('ddelta') - dae['dddelta'],
        dae.ddt('r') - dae['dr'],
        dae.ddt('dr') - dae['ddr'],
        dae.ddt('aileron') - dae['daileron'],
        dae.ddt('elevator') - dae['delevator'],
        dae.ddt('motor_torque') - dae['dmotor_torque'],
        dae.ddt('ddr') - dae['dddr']
        ])
    if 'rudder' in dae:
        ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
    if 'flaps' in dae:
        ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
        
    if 'useVirtualTorques' in conf:
        _v = conf[ 'useVirtualTorques' ]
        if isinstance(_v, str):
            _type = _v
            _which = True, True, True
        else:
            assert isinstance(_v, dict)
            _type = _v["type"]
            _which = _v["which"]
        
        if _type == 'random_walk':
            if _which[ 0 ]:
                ode = C.veccat([ode,
                                dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
            if _which[ 1 ]:
                ode = C.veccat([ode,
                                dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
            if _which[ 2 ]:
                ode = C.veccat([ode,
                                dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                ode = C.veccat([ode, dae.ddt('t1_disturbance')])
            if _which[ 1 ]:
                ode = C.veccat([ode, dae.ddt('t2_disturbance')])
            if _which[ 2 ]:
                ode = C.veccat([ode, dae.ddt('t3_disturbance')])
        
    if 'useVirtualForces' in conf:
        _v = conf[ 'useVirtualForces' ]
        if isinstance(_v, str):
            _type = _v
            _which = True, True, True
        else:
            assert isinstance(_v, dict)
            _type = _v["type"]
            _which = _v["which"]
        
        if _type is 'random_walk':

            if _which[ 0 ]:
                ode = C.veccat([ode,
                                dae.ddt('f1_disturbance') - dae['df1_disturbance']])
            if _which[ 1 ]:
                ode = C.veccat([ode,
                                dae.ddt('f2_disturbance') - dae['df2_disturbance']])
            if _which[ 2 ]:
                ode = C.veccat([ode,
                                dae.ddt('f3_disturbance') - dae['df3_disturbance']])
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                ode = C.veccat([ode, dae.ddt('f1_disturbance')])
            if _which[ 1 ]:
                ode = C.veccat([ode, dae.ddt('f2_disturbance')])
            if _which[ 2 ]:
                ode = C.veccat([ode, dae.ddt('f3_disturbance')])
        
    if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
        tau_wind = conf['wind_model']['tau_wind']
        
        ode = C.veccat([ode,
                        dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
                        , dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
                        , dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
                        ])

    if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
        cPole = 0.5
    else:
        cPole = 0.0
    rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']

    psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
    alg = C.mul(massMatrix, psuedoZVec) - rhs
    dae.setResidual([ode,alg])

    return dae
Example #21
0
    def build_opti0_solver(self, agt_idx):
        self.opti0 = ca.Opti()

        self.x0 = self.opti0.variable(self.n_x, self.N + 1)
        self.u0 = self.opti0.variable(self.n_u, self.N)
        self.x_s0 = self.opti0.parameter(self.n_x)
        self.x_f0 = self.opti0.parameter(self.n_x)

        self.agt_x0 = []
        for i in range(agt_idx):
            self.agt_x0.append(self.opti0.parameter(self.n_x, self.N + 1))

        self.opti0.set_value(self.x_f0, self.x_refs[self.x_refs_idx])

        da_lim = self.agent.da_lim
        ddf_lim = self.agent.ddf_lim
        r = self.agent.r

        self.opti0.subject_to(self.x0[0, 0] == self.x_s0[0])
        self.opti0.subject_to(self.x0[1, 0] == self.x_s0[1])
        # self.opti0.subject_to(self.opti0.bounded(-2*np.pi, self.x0[2,0], 2*np.pi))
        self.opti0.subject_to(self.x0[2, 0] == self.x_s0[2])
        self.opti0.subject_to(self.x0[3, 0] == self.x_s0[3])

        stage_cost = 0
        for i in range(self.N):
            stage_cost += ca.bilin(self.Q, self.x0[:, i + 1] - self.x_f0,
                                   self.x0[:, i + 1] - self.x_f0) + ca.bilin(
                                       self.R, self.u0[:, i], self.u0[:, i])

            beta = ca.atan2(self.l_r * ca.tan(self.u0[0, i]),
                            self.l_f + self.l_r)
            self.opti0.subject_to(
                self.x0[0, i + 1] == self.x0[0, i] +
                self.dt * self.x0[3, i] * ca.cos(self.x0[2, i] + beta))
            self.opti0.subject_to(
                self.x0[1, i + 1] == self.x0[1, i] +
                self.dt * self.x0[3, i] * ca.sin(self.x0[2, i] + beta))
            self.opti0.subject_to(self.x0[2, i + 1] == self.x0[2, i] +
                                  self.dt * self.x0[3, i] * ca.sin(beta))
            self.opti0.subject_to(self.x0[3, i + 1] == self.x0[3, i] +
                                  self.dt * self.u0[1, i])

            if self.F is not None:
                self.opti0.subject_to(
                    ca.mtimes(self.F, self.x0[:, i + 1]) <= self.b)
            if self.H is not None:
                self.opti0.subject_to(
                    ca.mtimes(self.H, self.u0[:, i]) <= self.g)

            # Treat init and final states of agents before as obstacles
            for j in range(agt_idx):
                self.opti0.subject_to(
                    ca.bilin(np.eye(2), self.x0[:2, i + 1] -
                             self.agt_x0[j][:2, i + 1], self.x0[:2, i + 1] -
                             self.agt_x0[j][:2, i + 1]) >= (1.5 * 2 * r)**2)

            if i < self.N - 1:
                stage_cost += ca.bilin(self.Rd,
                                       self.u0[:, i + 1] - self.u0[:, i],
                                       self.u0[:, i + 1] - self.u0[:, i])
                self.opti0.subject_to(
                    self.opti0.bounded(ddf_lim[0] * self.dt,
                                       self.u0[0, i + 1] - self.u0[0, i],
                                       ddf_lim[1] * self.dt))
                self.opti0.subject_to(
                    self.opti0.bounded(da_lim[0] * self.dt,
                                       self.u0[1, i + 1] - self.u0[1, i],
                                       da_lim[1] * self.dt))

        self.cost0 = stage_cost
        self.opti0.minimize(self.cost0)

        solver_opts = {
            "mu_strategy": "adaptive",
            "mu_init": 1e-5,
            "mu_min": 1e-15,
            "barrier_tol_factor": 1,
            "print_level": 0,
            "linear_solver": "ma27"
        }
        # solver_opts = {}
        plugin_opts = {
            "verbose": False,
            "print_time": False,
            "print_out": False
        }
        self.opti0.solver('ipopt', plugin_opts, solver_opts)
Example #22
0
def setupModel(dae, conf):
    #  PARAMETERS OF THE KITE :
    #  ##############
    m = conf["kite"]["mass"]  #  mass of the kite               #  [ kg    ]

    #   PHYSICAL CONSTANTS :
    #  ##############
    g = conf["env"]["g"]  #  gravitational constant         #  [ m /s^2]

    #  PARAMETERS OF THE CABLE :
    #  ##############

    # INERTIA MATRIX (Kurt's direct measurements)
    j1 = conf["kite"]["j1"]
    j31 = conf["kite"]["j31"]
    j2 = conf["kite"]["j2"]
    j3 = conf["kite"]["j3"]

    # Carousel Friction & inertia
    jCarousel = conf["carousel"]["jCarousel"]
    cfric = conf["carousel"]["cfric"]

    zt = conf["kite"]["zt"]
    rA = conf["carousel"]["rArm"]

    ###########     model integ ###################
    e11 = dae.x("e11")
    e12 = dae.x("e12")
    e13 = dae.x("e13")

    e21 = dae.x("e21")
    e22 = dae.x("e22")
    e23 = dae.x("e23")

    e31 = dae.x("e31")
    e32 = dae.x("e32")
    e33 = dae.x("e33")

    x = dae.x("x")
    y = dae.x("y")
    z = dae.x("z")

    dx = dae.x("dx")
    dy = dae.x("dy")
    dz = dae.x("dz")

    w1 = dae.x("w1")
    w2 = dae.x("w2")
    w3 = dae.x("w3")

    delta = 0
    ddelta = 0

    r = dae.x("r")
    dr = dae.x("dr")

    ddr = dae.u("ddr")

    # wind
    z0 = conf["wind shear"]["z0"]
    zt_roughness = conf["wind shear"]["zt_roughness"]
    zsat = 0.5 * (z + C.sqrt(z * z))
    wind_x = dae.p("w0") * C.log((zsat + zt_roughness + 2) / zt_roughness) / C.log(z0 / zt_roughness)
    dae.addOutput("wind at altitude", wind_x)
    dae.addOutput("w0", dae.p("w0"))

    dp_carousel_frame = C.veccat([dx - ddelta * y, dy + ddelta * (rA + x), dz]) - C.veccat(
        [C.cos(delta) * wind_x, C.sin(delta) * wind_x, 0]
    )
    R_c2b = C.veccat(dae.x(["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"])).reshape((3, 3))

    # Aircraft velocity w.r.t. inertial frame, given in its own reference frame
    # (needed to compute the aero forces and torques !)
    dpE = C.mul(R_c2b, dp_carousel_frame)

    (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(
        dae,
        conf,
        dp_carousel_frame,
        dpE,
        dae.x(("w1", "w2", "w3")),
        dae.x(("e21", "e22", "e23")),
        dae.u(("aileron", "elevator")),
    )

    # mass matrix
    mm = C.SXMatrix(7, 7)
    mm[0, 0] = m
    mm[0, 1] = 0
    mm[0, 2] = 0
    mm[0, 3] = 0
    mm[0, 4] = 0
    mm[0, 5] = 0
    mm[0, 6] = x + zt * e31

    mm[1, 0] = 0
    mm[1, 1] = m
    mm[1, 2] = 0
    mm[1, 3] = 0
    mm[1, 4] = 0
    mm[1, 5] = 0
    mm[1, 6] = y + zt * e32

    mm[2, 0] = 0
    mm[2, 1] = 0
    mm[2, 2] = m
    mm[2, 3] = 0
    mm[2, 4] = 0
    mm[2, 5] = 0
    mm[2, 6] = z + zt * e33

    mm[3, 0] = 0
    mm[3, 1] = 0
    mm[3, 2] = 0
    mm[3, 3] = j1
    mm[3, 4] = 0
    mm[3, 5] = j31
    mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)

    mm[4, 0] = 0
    mm[4, 1] = 0
    mm[4, 2] = 0
    mm[4, 3] = 0
    mm[4, 4] = j2
    mm[4, 5] = 0
    mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)

    mm[5, 0] = 0
    mm[5, 1] = 0
    mm[5, 2] = 0
    mm[5, 3] = j31
    mm[5, 4] = 0
    mm[5, 5] = j3
    mm[5, 6] = 0

    mm[6, 0] = x + zt * e31
    mm[6, 1] = y + zt * e32
    mm[6, 2] = z + zt * e33
    mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
    mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
    mm[6, 5] = 0
    mm[6, 6] = 0

    # right hand side
    zt2 = zt * zt
    rhs = C.veccat(
        [
            f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
            f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m,
            f3 - g * m,
            t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
            t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
            t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
            ddr * r
            - (
                zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
                + zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
            )
            * (w3 - ddelta * e33)
            - dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 * (w2 - ddelta * e23))
            - dy * (dy - zt * e22 * (w1 - ddelta * e13) + zt * e12 * (w2 - ddelta * e23))
            - dz * (dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 * (w2 - ddelta * e23))
            + dr * dr
            + (w1 - ddelta * e13)
            * (
                e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
                + e22 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
                + zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
                + zt
                * e33
                * (
                    w1 * z
                    + zt * e33 * w1
                    + ddelta * e11 * x
                    + ddelta * e12 * y
                    + zt * ddelta * e11 * e31
                    + zt * ddelta * e12 * e32
                )
                + zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
                + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
            )
            - (w2 - ddelta * e23)
            * (
                e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
                + e12 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
                + zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
                - zt
                * e33
                * (
                    w2 * z
                    + zt * e33 * w2
                    + ddelta * e21 * x
                    + ddelta * e22 * y
                    + zt * ddelta * e21 * e31
                    + zt * ddelta * e22 * e32
                )
                - zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
                - zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
            ),
        ]
    )

    dRexp = C.SXMatrix(3, 3)

    dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
    dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
    dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)

    dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
    dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
    dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)

    dRexp[2, 0] = e23 * w3 - e33 * w2
    dRexp[2, 1] = e33 * w1 - e13 * w3
    dRexp[2, 2] = e13 * w2 - e23 * w1

    c = (x + zt * e31) ** 2 / 2 + (y + zt * e32) ** 2 / 2 + (z + zt * e33) ** 2 / 2 - r ** 2 / 2

    cdot = (
        dx * (x + zt * e31)
        + dy * (y + zt * e32)
        + dz * (z + zt * e33)
        + zt * (w2 - ddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
        - zt * (w1 - ddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
        - r * dr
    )

    ddx = dae.z("ddx")
    ddy = dae.z("ddy")
    ddz = dae.z("ddz")
    dw1 = dae.z("dw1")
    dw2 = dae.z("dw2")
    dddelta = 0

    cddot = (
        -(w1 - ddelta * e13)
        * (
            zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
            + zt
            * e33
            * (
                w1 * z
                + zt * e33 * w1
                + ddelta * e11 * x
                + ddelta * e12 * y
                + zt * ddelta * e11 * e31
                + zt * ddelta * e12 * e32
            )
            + zt * e21 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
            + zt * e22 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
            + zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
            + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
        )
        + (w2 - ddelta * e23)
        * (
            zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
            - zt
            * e33
            * (
                w2 * z
                + zt * e33 * w2
                + ddelta * e21 * x
                + ddelta * e22 * y
                + zt * ddelta * e21 * e31
                + zt * ddelta * e22 * e32
            )
            + zt * e11 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
            + zt * e12 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
            - zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
            - zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
        )
        - ddr * r
        + (
            zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
            + zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
        )
        * (w3 - ddelta * e33)
        + dx * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
        + dy * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
        + dz * (dz + zt * e13 * w2 - zt * e23 * w1)
        + ddx * (x + zt * e31)
        + ddy * (y + zt * e32)
        + ddz * (z + zt * e33)
        - dr * dr
        + zt * (dw2 - dddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
        - zt * (dw1 - dddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
        - zt
        * dddelta
        * (
            e11 * e23 * x
            - e13 * e21 * x
            + e12 * e23 * y
            - e13 * e22 * y
            + zt * e11 * e23 * e31
            - zt * e13 * e21 * e31
            + zt * e12 * e23 * e32
            - zt * e13 * e22 * e32
        )
    )

    #    cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
    #      where
    #        dw1 = dw @> 0
    #        dw2 = dw @> 1
    #        {-
    #        dw3 = dw @> 2
    #        -}
    #        ddx = ddX @> 0
    #        ddy = ddX @> 1
    #        ddz = ddX @> 2
    #        dddelta = dddelta' @> 0

    dae.addOutput("c", c)
    dae.addOutput("cdot", cdot)
    dae.addOutput("cddot", cddot)
    return (mm, rhs, dRexp)
Example #23
0
# Horizontal theta angle of the tether behind the arm
# a.k.a. theta
theta = ssym('theta') 
dtheta = ssym('dtheta')
ddtheta = ssym('ddtheta')

# Our generalized coordinate is theta, i.e. the azimuth angle referenced
# to world frame x

q  = casadi.vertcat([theta])
dq  = casadi.vertcat([dtheta])
ddq  = casadi.vertcat([ddtheta])
nq = q.size()

# some subexpressions
s_delta = sin(delta)
c_delta = cos(delta)
s_theta = sin(theta)
c_theta = cos(theta)

# stone position in world coordinates.  Maps theta,t -> x
x_w = R*casadi.vertcat([c_delta, s_delta]) \
        + r*casadi.vertcat([c_theta, s_theta])

v_w = R*ddelta*casadi.vertcat([-s_delta,c_delta]) \
        + r*dtheta*casadi.vertcat([-s_theta,c_theta])

# Assume we are spinning a 7kg sphere of water
m = 7.0 # kg
Cd = 0.47 # Drag coefficient for a sphere
rho_water = 1000. # kg / m^2
Example #24
0
    print '='*80

    # set the mpc weights
    xRms = 0.1
    vRms = 1.5
    fRms = 5.0
    mpc.S[0,0] = (1.0/xRms)**2/N
    mpc.S[1,1] = (1.0/vRms)**2/N
    mpc.S[2,2] = (1.0/fRms)**2/N
    mpc.SN[0,0] = (1.0/0.01)**2
    mpc.SN[1,1] = (1.0/0.01)**2

    # initial guess
    tk = 0
    for k in range(N+1):
        mpc.x[k,0] = A*C.sin(omega*tk)
        mpc.x[k,1] = A*C.cos(omega*tk)*omega
        tk += ts

    # set tracking trajectory
    t = 0
    refLog = []
    def setTrajectory(ocp, t0):
        tk = t0
        for k in range(N):
            ocp.y[k,0] = A*C.sin(omega*tk)
            ocp.y[k,1] = A*C.cos(omega*tk)*omega
            tk += ts
        ocp.yN[0] = A*C.sin(omega*tk)
        ocp.yN[1] = A*C.cos(omega*tk)*omega
Example #25
0
    x, y, theta = states[...]
    n_states = states.size

    controls  = ca_tools.struct_symSX([
        (
            ca_tools.entry('v'),
            ca_tools.entry('omega')
        )
    ])
    v, omega = controls[...]
    n_controls = controls.size

    ## rhs
    rhs = ca_tools.struct_SX(states)
    rhs['x'] = v*ca.cos(theta)
    rhs['y'] = v*ca.sin(theta)
    rhs['theta'] = omega

    ## function
    f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])

    ## for MPC
    optimizing_target = ca_tools.struct_symSX([
        (
            ca_tools.entry('U', repeat=N, struct=controls),
            ca_tools.entry('X', repeat=N+1, struct=states)
        )
    ])
    U, X, = optimizing_target[...] # data are stored in list [], notice that ',' cannot be missed

    # X = ca.SX.sym('X', n_states, N+1)
Example #26
0
def create_plan_pc(b0, BF, dt, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=state),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])
    
    # Final coordinate
    x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dx_bc = x_bN - x_cN
    
    # Final velocity
    v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
    v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
    dv_bc = v_bN - v_cN
    
    # Angle between gaze and ball velocity
    theta = V['X',N_sim,'theta']
    phi = V['X',N_sim,'phi']
    d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
    r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
    r_unit = r / (ca.norm_2(r) + 1e-2)
    d_gaze = d - r_unit
    
    
    # Regularize controls
    J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
    
    # Multiple shooting constraints and running costs
    bk = cat.struct_SX(belief)
    bk['S'] = b0['S']
    g, lbg, ubg = [], [], []
    for k in range(N_sim):
        # Belief propagation
        bk['m'] = V['X',k]
        [bk_next] = BF([ bk, V['U',k] ])
        bk_next = belief(bk_next) # simplify indexing
        
        # Multiple shooting
        g.append(bk_next['m'] - V['X',k+1])
        lbg.append(ca.DMatrix.zeros(nx))
        ubg.append(ca.DMatrix.zeros(nx))

        # Control constraints
        g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
        lbg.append(-ca.inf)
        ubg.append(ca.DMatrix([0]))
        
        # Advance time
        bk = bk_next
    g = ca.vertcat(g)
    lbg = ca.veccat(lbg)
    ubg = ca.veccat(ubg)
    
    # Simple cost
    J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
    J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
    #J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
    J += 1e1 * ca.trace(bk_next['S']) # uncertainty
    
    # log-probability cost
    #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
    #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
    
    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
    
    # Create solver
    opts = {}
    opts['linear_solver'] = 'ma97'
    #opts['hessian_approximation'] = 'limited-memory'
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # State constraints
    # catcher can look within the upper hemisphere
    lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
    
    # Control constraints
    # 0 <= F
    lbx['U',:,'F'] = 0
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    # -pi <= psi <= pi
    lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
    
    # m(t=0) = m0
    lbx['X',0] = ubx['X',0] = b0['m']
    
    # Take care of the time
    #lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
    # Simulation ends when the ball touches the ground
    #lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
    
    # Solve the NLP
    sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
    return V(sol['x'])
Example #27
0
def dynamic_model(modelparams):

    # define casadi struct
    model = casadi.types.SimpleNamespace()
    constraints = casadi.types.SimpleNamespace()

    model_name = "f110_dynamic_model"
    model.name = model_name

    #loadparameters
    with open(modelparams) as file:
        params = yaml.load(file, Loader=yaml.FullLoader)

    m = params['m']  #[kg]
    lf = params['lf']  #[m]
    lr = params['lr']  #[m]
    Iz = params['Iz']  #[kg*m^3]

    #pajecka and motor coefficients
    Bf = params['Bf']
    Br = params['Br']
    Cf = params['Cf']
    Cr = params['Cr']
    Cm1 = params['Cm1']
    Cm2 = params['Cm2']
    Croll = params['Croll']
    Cd = params['Cd']
    Df = params['Df']
    Dr = params['Dr']

    print("CasADi model created with the following parameters: filename: ",
          modelparams, "\n values:", params)

    xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
    uvars = ['ddot', 'deltadot', 'thetadot']
    pvars = [
        'xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql',
        'Q_theta', 'R_d', 'R_delta', 'r'
    ]
    #parameter vector, contains linearization poitns
    xt = casadi.SX.sym("xt")
    yt = casadi.SX.sym("yt")
    phit = casadi.SX.sym("phit")
    sin_phit = casadi.SX.sym("sin_phit")
    cos_phit = casadi.SX.sym("cos_phit")
    #stores linearization point
    theta_hat = casadi.SX.sym("theta_hat")
    Qc = casadi.SX.sym("Qc")
    Ql = casadi.SX.sym("Ql")
    Q_theta = casadi.SX.sym("Q_theta")

    #cost on smoothnes of motorinput
    R_d = casadi.SX.sym("R_d")
    #cost on smoothness of steering_angle
    R_delta = casadi.SX.sym("R_delta")
    #trackwidth
    r = casadi.SX.sym("r")

    #pvars = ['xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql', 'Q_theta', 'R_d', 'R_delta', 'r']
    p = casadi.vertcat(xt, yt, phit, sin_phit, cos_phit, theta_hat, Qc, Ql,
                       Q_theta, R_d, R_delta, r)

    #single track model with pajecka tireforces as in  Optimization-Based Autonomous Racing of 1:43 Scale RC Cars Alexander Liniger, Alexander Domahidi and Manfred Morari
    #pose
    posx = casadi.SX.sym("posx")
    posy = casadi.SX.sym("posy")

    #vel (long and lateral)
    vx = casadi.SX.sym("vx")
    vy = casadi.SX.sym("vy")

    #body angular Rate
    omega = casadi.SX.sym("omega")
    #heading
    phi = casadi.SX.sym("phi")
    #steering_angle
    delta = casadi.SX.sym("delta")
    #motorinput
    d = casadi.SX.sym("d")

    #dynamic forces
    Frx = casadi.SX.sym("Frx")
    Fry = casadi.SX.sym("Fry")
    Ffx = casadi.SX.sym("Ffx")
    Ffy = casadi.SX.sym("Ffy")

    #arclength progress
    theta = casadi.SX.sym("theta")

    #temporal derivatives
    posxdot = casadi.SX.sym("xdot")
    posydot = casadi.SX.sym("ydot")
    vxdot = casadi.SX.sym("vxdot")
    vydot = casadi.SX.sym("vydot")
    phidot = casadi.SX.sym("phidot")
    omegadot = casadi.SX.sym("omegadot")
    deltadot = casadi.SX.sym("deltadot")
    thetadot = casadi.SX.sym("thetadot")
    ddot = casadi.SX.sym("ddot")

    #inputvector
    #uvars = ['ddot', 'deltadot', 'thetadot']
    u = casadi.vertcat(ddot, deltadot, thetadot)

    #car state Dynamics
    #xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
    x = casadi.vertcat(posx, posy, phi, vx, vy, omega, d, delta, theta)

    xdot = casadi.vertcat(posxdot, posydot, phidot, vxdot, vydot, omegadot,
                          ddot, deltadot, thetadot)

    #build CasADi expressions for dynamic model
    #front lateral tireforce
    alphaf = -casadi.atan2((omega * lf + vy), vx) + delta
    Ffy = Df * casadi.sin(Cf * casadi.atan(Bf * alphaf))

    #rear lateral tireforce
    alphar = casadi.atan2((omega * lr - vy), vx)

    Fry = Dr * casadi.sin(Cr * casadi.atan(Br * alphar))

    #rear longitudinal forces

    Frx = (Cm1 - Cm2 * vx) * d - Croll - Cd * vx * vx

    f_expl = casadi.vertcat(
        vx * casadi.cos(phi) - vy * casadi.sin(phi),  #posxdot
        vx * casadi.sin(phi) + vy * casadi.cos(phi),  #posydot
        omega,  #phidot
        1 / m * (Frx - Ffy * casadi.sin(delta) + m * vy * omega),  #vxdot
        1 / m * (Fry + Ffy * casadi.cos(delta) - m * vx * omega),  #vydot
        1 / Iz * (Ffy * lf * casadi.cos(delta) - Fry * lr),  #omegadot
        ddot,
        deltadot,
        thetadot)

    # algebraic variables
    z = casadi.vertcat([])

    model.f_expl_expr = f_expl
    model.f_impl_expr = xdot - f_expl
    model.x = x
    model.xdot = xdot
    model.u = u
    model.p = p
    model.z = z

    #boxconstraints
    model.ddot_min = -10.0  #min change in d [-]
    model.ddot_max = 10.0  #max change in d [-]

    model.d_min = -0.1  #min d [-]
    model.d_max = 1  #max d [-]

    model.delta_min = -0.40  # minimum steering angle [rad]
    model.delta_max = 0.40  # maximum steering angle [rad]

    model.deltadot_min = -2  # minimum steering angle cahgne[rad/s]
    model.deltadot_max = 2  # maximum steering angle cahgne[rad/s]

    model.omega_min = -100  # minimum yawrate [rad/sec]
    model.omega_max = 100  # maximum yawrate [rad/sec]

    model.thetadot_min = 0.05  # minimum adv param speed [m/s]
    model.thetadot_max = 5  # maximum adv param speed [m/s]

    model.theta_min = 0.00  # minimum adv param [m]
    model.theta_max = 1000  # maximum adv param  [m]

    model.vx_max = 3.5  # max long vel [m/s]
    model.vx_min = -0.5  #0.05 # min long vel [m/s]

    model.vy_max = 3  # max lat vel [m/s]
    model.vy_min = -3  # min lat vel [m/s]

    model.x0 = np.array([0, 0, 0, 1, 0.01, 0, 0, 0, 0])

    #compute approximate linearized contouring and lag error
    xt_hat = xt + cos_phit * (theta - theta_hat)
    yt_hat = yt + sin_phit * (theta - theta_hat)

    e_cont = sin_phit * (xt_hat - posx) - cos_phit * (yt_hat - posy)
    e_lag = cos_phit * (xt_hat - posx) + sin_phit * (yt_hat - posy)

    cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot

    #error = casadi.vertcat(e_cont, e_lag)
    #set up stage cost
    #Q = diag(vertcat(Qc, Ql))
    model.con_h_expr = (xt_hat - posx)**2 + (yt_hat - posy)**2 - r**2
    model.stage_cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot

    return model, constraints
Example #28
0
def kinematic_model(modelparams):
    # define casadi struct
    # define casadi struct
    model = casadi.types.SimpleNamespace()
    constraints = casadi.types.SimpleNamespace()

    model_name = "f110_kinematic_model"
    model.name = model_name

    #loadparameters
    with open(modelparams) as file:
        params = yaml.load(file, Loader=yaml.FullLoader)

    lf = params['lf']  #[m]
    lr = params['lr']  #[m]
    lwb = lf + lr

    print("CasADi model created with the following parameters: filename: ",
          modelparams, "\n values:", params)

    #parameter vector, contains linearization poitns
    xt = casadi.SX.sym("xt")
    yt = casadi.SX.sym("yt")
    phit = casadi.SX.sym("phit")
    sin_phit = casadi.SX.sym("sin_phit")
    cos_phit = casadi.SX.sym("cos_phit")
    gt_upper = casadi.SX.sym("gt_upper")
    gt_lower = casadi.SX.sym("gt_lower")
    #stores linearization point
    theta_hat = casadi.SX.sym("theta_hat")
    Qc = casadi.SX.sym("Qc")
    Ql = casadi.SX.sym("Ql")
    Q_theta = casadi.SX.sym("Q_theta")

    #cost on smoothnes of motorinput
    R_d = casadi.SX.sym("R_d")

    #cost on smoothness of steering_angle
    R_delta = casadi.SX.sym("R_delta")

    r = casadi.SX.sym("r")
    p = casadi.vertcat(xt, yt, phit, sin_phit, cos_phit, theta_hat, Qc, Ql,
                       Q_theta, R_d, R_delta, r)

    #single track model
    #pose
    posx = casadi.SX.sym("posx")
    posy = casadi.SX.sym("posy")

    #longitudinal velocity
    vx = casadi.SX.sym("vx")
    #heading
    phi = casadi.SX.sym("phi")
    #steering_angle
    delta = casadi.SX.sym("delta")
    #motorinput
    d = casadi.SX.sym("d")
    #arclength progress
    theta = casadi.SX.sym("theta")
    #temporal derivatives
    posxdot = casadi.SX.sym("xdot")
    posydot = casadi.SX.sym("ydot")
    vxdot = casadi.SX.sym("vxdot")
    phidot = casadi.SX.sym("phidot")
    deltadot = casadi.SX.sym("deltadot")
    thetadot = casadi.SX.sym("thetadot")
    ddot = casadi.SX.sym("ddot")

    #inputvector
    u = casadi.vertcat(ddot, deltadot, thetadot)

    #car state Dynamics
    x = casadi.vertcat(posx, posy, phi, vx, theta, d, delta)

    xdot = casadi.vertcat(posxdot, posydot, phidot, vxdot, thetadot, ddot,
                          deltadot)

    f_expl = casadi.vertcat(vx * casadi.cos(phi), vx * casadi.sin(phi),
                            vx / lwb * casadi.tan(delta), d, thetadot, ddot,
                            deltadot)

    # algebraic variables
    z = casadi.vertcat([])

    model.f_expl_expr = f_expl
    model.f_impl_expr = xdot - f_expl
    model.x = x
    model.xdot = xdot
    model.u = u
    model.p = p
    model.z = z
    #boxconstraints
    model.d_min = -3.0
    model.d_max = 5.0

    model.ddot_min = -10.0
    model.ddot_max = 10.0

    model.delta_min = -0.40  # minimum steering angle [rad]
    model.delta_max = 0.40  # maximum steering angle [rad]

    model.deltadot_min = -2  # minimum steering angle cahgne[rad/s]
    model.deltadot_max = 2  # maximum steering angle cahgne[rad/s]

    model.thetadot_min = -0.1  # minimum adv param speed [m/s]
    model.thetadot_max = 5  # maximum adv param speed [m/s]

    model.theta_min = 0.00  # minimum adv param [m]
    model.theta_max = 100  # maximum adv param  [m]

    model.vx_max = 2  # max long vel [m/s]
    model.vx_min = -1  # min long vel [m/s]

    model.x0 = np.array([0, 0, 0, 1, 0, 0, 0])

    #halfspace constraints on x capturing the track at each stage
    #n = casadi.vertcat(-sin_phit, cos_phit)
    #constraints.h_upper = casadi.vertcat(0,0)
    #g_upper = casadi.vertcat(gt_upper, -gt_lower)
    #halfspace constriants for track boundaries, con_expr <= 0
    #model.con_h_expr = casadi.vertcat(n[0]*x[0]+n[1]*x[1]-g_upper[0], -n[0]*x[0]-n[1]*x[1]-g_upper[1])

    #compute approximate linearized contouring and lag error
    xt_hat = xt + cos_phit * (theta - theta_hat)
    yt_hat = yt + sin_phit * (theta - theta_hat)

    e_cont = sin_phit * (xt_hat - posx) - cos_phit * (yt_hat - posy)
    e_lag = cos_phit * (xt_hat - posx) + sin_phit * (yt_hat - posy)

    model.con_h_expr = e_cont**2 + e_lag**2 - (r)**2
    #virtual turn radius
    #error = casadi.vertcat(e_cont, e_lag)
    #set up stage cost
    #Q = diag(vertcat(Qc, Ql))
    model.stage_cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot
    return model, constraints
Example #29
0
def firefly_CLA_and_CDA_fuse_hybrid(  # TODO remove
    fuse_fineness_ratio,
    fuse_boattail_angle,
    fuse_TE_diameter,
    fuse_length,
    fuse_diameter,
    alpha,
    V,
    mach,
    rho,
    mu,
):
    """
    Estimated equiv. lift area and equiv. drag area of the Firefly fuselage, component buildup.
    :param fuse_fineness_ratio: Fineness ratio of the fuselage nose (length / diameter)
    :param fuse_boattail_angle: Boattail half-angle [deg]
    :param fuse_TE_diameter: Diameter of the fuselage's base at the "trailing edge" [m]
    :param fuse_length: Length of the fuselage [m]
    :param fuse_diameter: Diameter of the fuselage [m]
    :param alpha: Angle of attack [deg]
    :param V: Airspeed [m/s]
    :param mach: Mach number [unitless]
    :param rho: Air density [kg/m^3]
    :param mu: Dynamic viscosity of air [Pa*s]
    :return: A tuple of (CLA, CDA) [m^2]
    """
    alpha_rad = alpha * np.pi / 180
    sin_alpha = cas.sin(alpha_rad)
    cos_alpha = cas.cos(alpha_rad)
    """
    Lift of a truncated fuselage, following slender body theory.
    """
    separation_location = 0.3  # 0 for separation at trailing edge, 1 for separation at start of boat-tail. Calibrate this.
    diameter_at_separation = (
        1 - separation_location
    ) * fuse_TE_diameter + separation_location * fuse_diameter

    fuse_TE_area = np.pi / 4 * diameter_at_separation**2
    CLA_slender_body = 2 * fuse_TE_area * alpha_rad  # Derived from FVA 6.6.5, Eq. 6.75
    """
    Crossflow force, following the method of Jorgensen, 1977: "Prediction of Static Aero. Chars. for Slender..."
    """
    V_crossflow = V * sin_alpha
    Re_crossflow = rho * cas.fabs(V_crossflow) * fuse_diameter / mu
    mach_crossflow = mach * cas.fabs(sin_alpha)
    eta = 1  # TODO make this a function of overall fineness ratio
    Cdn = 0.2  # Taken from suggestion for supercritical cylinders in Hoerner's Fluid Dynamic Drag, pg. 3-11
    S_planform = fuse_diameter * fuse_length
    CNA = eta * Cdn * S_planform * sin_alpha * cas.fabs(sin_alpha)
    CLA_crossflow = CNA * cos_alpha
    CDA_crossflow = CNA * sin_alpha

    CLA = CLA_slender_body + CLA_crossflow
    r"""
    Zero-lift_force drag_force
    Model derived from high-fidelity data at: C:\Projects\GitHub\firefly_aerodynamics\Design_Opt\studies\Circular Firefly Fuse CFD\Zero-Lift Drag
    """
    c = np.array([
        112.57153128951402720758778741583,
        -7.1720570832587240417410612280946,
        -0.01765596807595304351679033061373,
        0.0026135564778264172743071913629365,
        550.8775012129947299399645999074,
        3.3166868391027000129156476759817,
        11774.081980549422951298765838146,
        3073258.204571904614567756652832,
        0.0299,
    ])  # coefficients
    fuse_Re = rho * cas.fabs(V) * fuse_length / mu
    CDA_zero_lift = (
        (c[0] * cas.exp(c[1] * fuse_fineness_ratio) +
         c[2] * fuse_boattail_angle + c[3] * fuse_boattail_angle**2 +
         c[4] * fuse_TE_diameter**2 + c[5]) / c[6] *
        (fuse_Re / c[7])**(-1 / 7) * (fuse_length * fuse_diameter) / c[8])
    r"""
    Assumes a ring-shaped wake projection into the Trefftz plane with a sinusoidal circulation distribution.
    This results in a uniform grad(phi) within the ring in the Trefftz plane.
    Derivation at C:\Projects\GitHub\firefly_aerodynamics\Gists and Ideas\Ring Wing Potential Flow
    """
    fuse_oswalds_efficiency = 0.5
    CDiA_slender_body = CLA**2 / (
        diameter_at_separation**2 * np.pi *
        fuse_oswalds_efficiency) / 2  # or equivalently, use the next line
    # CDiA_slender_body = fuse_TE_area * alpha_rad ** 2 / 2

    CDA = CDA_crossflow + CDiA_slender_body + CDA_zero_lift

    return CLA, CDA
Example #30
0
def solveProblem():
    print_time = rospy.Time().now().to_sec()

    N = 51
    U = ca.SX.sym('U', N)
    x0 = ca.SX.sym('x0', 5)  #初始条件
    x0[0] = -1
    x0[1] = 1
    x0[2] = 0
    x0[3] = 0
    x0[4] = 0
    xf = ca.SX.sym('xf', 5)  #终端状态
    xf[0] = x0[0]
    xf[1] = x0[1]
    xf[2] = x0[2]
    xf[3] = x0[3]
    xf[4] = x0[4]
    obj = U[-1]
    g = []
    det_t = ca.SX.sym('det_t', 1)
    det_t = U[-1] / ((N - 1) / 2)
    for i in range((N - 1) / 2):
        xf[4] += det_t * U[2 * i + 1]
        xf[3] += det_t * U[2 * i]
        xf[2] += det_t * xf[4]
        xf[1] += det_t * xf[3] * ca.sin(xf[2])
        xf[0] += det_t * xf[3] * ca.cos(xf[2])
    g.append(xf[0])
    g.append(xf[1])
    g.append(xf[2])
    g.append(xf[3])
    g.append(xf[4])
    nlp_prob = {'f': obj, 'x': U, 'g': ca.vertcat(*g)}
    opts_setting = {
        'ipopt.max_iter': 100,
        'ipopt.print_level': 0,
        'print_time': 0,
        'ipopt.acceptable_tol': 1e-8,
        'ipopt.acceptable_obj_change_tol': 1e-6
    }
    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
    lbx = []
    ubx = []
    lbg = []
    ubg = []
    for i in range((N - 1) / 2):
        lbx.append(-1)
        ubx.append(1)
        lbx.append(-3)
        ubx.append(3)
    lbx.append(0)
    ubx.append(100)
    for i in range(5):
        lbg.append(0)  #终端约束
        ubg.append(0)  #终端约束
    res = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)

    print('cost time of casadi opt problem: ',
          rospy.Time().now().to_sec() - print_time)
    print(res['f'], res['x'])
    #print(xf)
    UU = res['x']

    points = PointCloud()
    points.header.frame_id = 'map'
    xf[0] = x0[0]
    xf[1] = x0[1]
    xf[2] = x0[2]
    xf[3] = x0[3]
    xf[4] = x0[4]
    points.points.append(Point32(xf[0], xf[1], 0))
    det_t = ca.SX.sym('det_t', 1)
    det_t = UU[-1] / ((N - 1) / 2)
    for i in range((N - 1) / 2):
        xf[4] += det_t * UU[2 * i + 1]
        xf[3] += det_t * UU[2 * i]
        xf[2] += det_t * xf[4]
        xf[1] += det_t * xf[3] * ca.sin(xf[2])
        xf[0] += det_t * xf[3] * ca.cos(xf[2])
        points.points.append(Point32(xf[0], xf[1], 0))
    g_pub_clothoid.publish(points)
    # #while not rospy.is_shutdown():
    for i in range(100000):
        g_pub_clothoid.publish(points)
        #rospy.sleep(0.1)
    print('end pub')
Example #31
0
    def _convert(self, symbol, t, y, y_dot, inputs):
        """ See :meth:`CasadiConverter.convert()`. """
        if isinstance(
                symbol,
            (
                pybamm.Scalar,
                pybamm.Array,
                pybamm.Time,
                pybamm.InputParameter,
                pybamm.ExternalVariable,
            ),
        ):
            return casadi.MX(symbol.evaluate(t, y, y_dot, inputs))

        elif isinstance(symbol, pybamm.StateVector):
            if y is None:
                raise ValueError(
                    "Must provide a 'y' for converting state vectors")
            return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices])

        elif isinstance(symbol, pybamm.StateVectorDot):
            if y_dot is None:
                raise ValueError(
                    "Must provide a 'y_dot' for converting state vectors")
            return casadi.vertcat(
                *[y_dot[y_slice] for y_slice in symbol.y_slices])

        elif isinstance(symbol, pybamm.BinaryOperator):
            left, right = symbol.children
            # process children
            converted_left = self.convert(left, t, y, y_dot, inputs)
            converted_right = self.convert(right, t, y, y_dot, inputs)

            if isinstance(symbol, pybamm.Modulo):
                return casadi.fmod(converted_left, converted_right)
            if isinstance(symbol, pybamm.Minimum):
                return casadi.fmin(converted_left, converted_right)
            if isinstance(symbol, pybamm.Maximum):
                return casadi.fmax(converted_left, converted_right)

            # _binary_evaluate defined in derived classes for specific rules
            return symbol._binary_evaluate(converted_left, converted_right)

        elif isinstance(symbol, pybamm.UnaryOperator):
            converted_child = self.convert(symbol.child, t, y, y_dot, inputs)
            if isinstance(symbol, pybamm.AbsoluteValue):
                return casadi.fabs(converted_child)
            if isinstance(symbol, pybamm.Floor):
                return casadi.floor(converted_child)
            if isinstance(symbol, pybamm.Ceiling):
                return casadi.ceil(converted_child)
            return symbol._unary_evaluate(converted_child)

        elif isinstance(symbol, pybamm.Function):
            converted_children = [
                self.convert(child, t, y, y_dot, inputs)
                for child in symbol.children
            ]
            # Special functions
            if symbol.function == np.min:
                return casadi.mmin(*converted_children)
            elif symbol.function == np.max:
                return casadi.mmax(*converted_children)
            elif symbol.function == np.abs:
                return casadi.fabs(*converted_children)
            elif symbol.function == np.sqrt:
                return casadi.sqrt(*converted_children)
            elif symbol.function == np.sin:
                return casadi.sin(*converted_children)
            elif symbol.function == np.arcsinh:
                return casadi.arcsinh(*converted_children)
            elif symbol.function == np.arccosh:
                return casadi.arccosh(*converted_children)
            elif symbol.function == np.tanh:
                return casadi.tanh(*converted_children)
            elif symbol.function == np.cosh:
                return casadi.cosh(*converted_children)
            elif symbol.function == np.sinh:
                return casadi.sinh(*converted_children)
            elif symbol.function == np.cos:
                return casadi.cos(*converted_children)
            elif symbol.function == np.exp:
                return casadi.exp(*converted_children)
            elif symbol.function == np.log:
                return casadi.log(*converted_children)
            elif symbol.function == np.sign:
                return casadi.sign(*converted_children)
            elif symbol.function == special.erf:
                return casadi.erf(*converted_children)
            elif isinstance(symbol, pybamm.Interpolant):
                return casadi.interpolant(
                    "LUT", "bspline", symbol.x,
                    symbol.y.flatten())(*converted_children)
            elif symbol.function.__name__.startswith("elementwise_grad_of_"):
                differentiating_child_idx = int(symbol.function.__name__[-1])
                # Create dummy symbolic variables in order to differentiate using CasADi
                dummy_vars = [
                    casadi.MX.sym("y_" + str(i))
                    for i in range(len(converted_children))
                ]
                func_diff = casadi.gradient(
                    symbol.differentiated_function(*dummy_vars),
                    dummy_vars[differentiating_child_idx],
                )
                # Create function and evaluate it using the children
                casadi_func_diff = casadi.Function("func_diff", dummy_vars,
                                                   [func_diff])
                return casadi_func_diff(*converted_children)
            # Other functions
            else:
                return symbol._function_evaluate(converted_children)
        elif isinstance(symbol, pybamm.Concatenation):
            converted_children = [
                self.convert(child, t, y, y_dot, inputs)
                for child in symbol.children
            ]
            if isinstance(symbol,
                          (pybamm.NumpyConcatenation, pybamm.SparseStack)):
                return casadi.vertcat(*converted_children)
            # DomainConcatenation specifies a particular ordering for the concatenation,
            # which we must follow
            elif isinstance(symbol, pybamm.DomainConcatenation):
                slice_starts = []
                all_child_vectors = []
                for i in range(symbol.secondary_dimensions_npts):
                    child_vectors = []
                    for child_var, slices in zip(converted_children,
                                                 symbol._children_slices):
                        for child_dom, child_slice in slices.items():
                            slice_starts.append(
                                symbol._slices[child_dom][i].start)
                            child_vectors.append(
                                child_var[child_slice[i].start:child_slice[i].
                                          stop])
                    all_child_vectors.extend([
                        v for _, v in sorted(zip(slice_starts, child_vectors))
                    ])
                return casadi.vertcat(*all_child_vectors)

        else:
            raise TypeError("""
                Cannot convert symbol of type '{}' to CasADi. Symbols must all be
                'linear algebra' at this stage.
                """.format(type(symbol)))
Example #32
0
def opt_mintime(reftrack: np.ndarray,
                coeffs_x: np.ndarray,
                coeffs_y: np.ndarray,
                normvectors: np.ndarray,
                pars: dict,
                tpamap_path: str,
                tpadata_path: str,
                export_path: str,
                print_debug: bool = False,
                plot_debug: bool = False) -> tuple:
    """
    Created by:
    Fabian Christ

    Documentation:
    The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using
    direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing
    times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using
    the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The
    vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and
    nonlinear tire model.

    Please refer to our paper for further information:
    Christ, Wischnewski, Heilmeier, Lohmann
    Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients

    Inputs:
    reftrack:       track [x_m, y_m, w_tr_right_m, w_tr_left_m]
    coeffs_x:       coefficient matrix of the x splines with size (no_splines x 4)
    coeffs_y:       coefficient matrix of the y splines with size (no_splines x 4)
    normvectors:    array containing normalized normal vectors for every traj. point [x_component, y_component]
    pars:           parameters dictionary
    tpamap_path:    file path to tpa map (required for friction map loading)
    tpadata_path:   file path to tpa data (required for friction map loading)
    export_path:    path to output folder for warm start files and solution files
    print_debug:    determines if debug messages are printed
    plot_debug:     determines if debug plots are shown

    Outputs:
    alpha_opt:      solution vector of the optimization problem containing the lateral shift in m for every point
    v_opt:          velocity profile for the raceline
    """

    # ------------------------------------------------------------------------------------------------------------------
    # PREPARE TRACK INFORMATION ----------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # spline lengths
    spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(coeffs_x=coeffs_x,
                                                                         coeffs_y=coeffs_y)

    # calculate heading and curvature (numerically)
    kappa_refline = tph.calc_head_curv_num. \
        calc_head_curv_num(path=reftrack[:, :2],
                           el_lengths=spline_lengths_refline,
                           is_closed=True,
                           stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"],
                           stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"],
                           stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"],
                           stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1]

    # close track
    kappa_refline_cl = np.append(kappa_refline, kappa_refline[0])
    w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3])
    w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2])

    # step size along the reference line
    h = pars["stepsize_opts"]["stepsize_reg"]

    # optimization steps (0, 1, 2 ... end point/start point)
    steps = [i for i in range(kappa_refline_cl.size)]

    # number of control intervals
    N = steps[-1]

    # station along the reference line
    s_opt = np.linspace(0.0, N * h, N + 1)

    # interpolate curvature of reference line in terms of steps
    kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps], kappa_refline_cl)

    # interpolate track width (left and right to reference line) in terms of steps
    w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps], w_tr_left_cl)
    w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps], w_tr_right_cl)

    # describe friction coefficients from friction map with linear equations or gaussian basis functions
    if pars["optim_opts"]["var_friction"] is not None:
        w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \
            approx_friction_map.approx_friction_map(reftrack=reftrack,
                                                    normvectors=normvectors,
                                                    tpamap_path=tpamap_path,
                                                    tpadata_path=tpadata_path,
                                                    pars=pars,
                                                    dn=pars["optim_opts"]["dn"],
                                                    n_gauss=pars["optim_opts"]["n_gauss"],
                                                    print_debug=print_debug,
                                                    plot_debug=plot_debug)

    # ------------------------------------------------------------------------------------------------------------------
    # DIRECT GAUSS-LEGENDRE COLLOCATION --------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # degree of interpolating polynomial
    d = 3

    # legendre collocation points
    tau = np.append(0, ca.collocation_points(d, 'legendre'))

    # coefficient matrix for formulating the collocation equation
    C = np.zeros((d + 1, d + 1))

    # coefficient matrix for formulating the collocation equation
    D = np.zeros(d + 1)

    # coefficient matrix for formulating the collocation equation
    B = np.zeros(d + 1)

    # construct polynomial basis
    for j in range(d + 1):
        # construct Lagrange polynomials to get the polynomial basis at the collocation point
        p = np.poly1d([1])
        for r in range(d + 1):
            if r != j:
                p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])

        # evaluate polynomial at the final time to get the coefficients of the continuity equation
        D[j] = p(1.0)

        # evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation
        p_der = np.polyder(p)
        for r in range(d + 1):
            C[j, r] = p_der(tau[r])

        # evaluate integral of the polynomial to get the coefficients of the quadrature function
        pint = np.polyint(p)
        B[j] = pint(1.0)

    # ------------------------------------------------------------------------------------------------------------------
    # STATE VARIABLES --------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # number of state variables
    nx = 5

    # velocity [m/s]
    v_n = ca.SX.sym('v_n')
    v_s = 50
    v = v_s * v_n

    # side slip angle [rad]
    beta_n = ca.SX.sym('beta_n')
    beta_s = 0.5
    beta = beta_s * beta_n

    # yaw rate [rad/s]
    omega_z_n = ca.SX.sym('omega_z_n')
    omega_z_s = 1
    omega_z = omega_z_s * omega_z_n

    # lateral distance to reference line (positive = left) [m]
    n_n = ca.SX.sym('n_n')
    n_s = 5.0
    n = n_s * n_n

    # relative angle to tangent on reference line [rad]
    xi_n = ca.SX.sym('xi_n')
    xi_s = 1.0
    xi = xi_s * xi_n

    # scaling factors for state variables
    x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s])

    # put all states together
    x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n)

    # ------------------------------------------------------------------------------------------------------------------
    # CONTROL VARIABLES ------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # number of control variables
    nu = 4

    # steer angle [rad]
    delta_n = ca.SX.sym('delta_n')
    delta_s = 0.5
    delta = delta_s * delta_n

    # positive longitudinal force (drive) [N]
    f_drive_n = ca.SX.sym('f_drive_n')
    f_drive_s = 7500.0
    f_drive = f_drive_s * f_drive_n

    # negative longitudinal force (brake) [N]
    f_brake_n = ca.SX.sym('f_brake_n')
    f_brake_s = 20000.0
    f_brake = f_brake_s * f_brake_n

    # lateral wheel load transfer [N]
    gamma_y_n = ca.SX.sym('gamma_y_n')
    gamma_y_s = 5000.0
    gamma_y = gamma_y_s * gamma_y_n

    # scaling factors for control variables
    u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s])

    # put all controls together
    u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n)

    # ------------------------------------------------------------------------------------------------------------------
    # MODEL EQUATIONS --------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # extract vehicle and tire parameters
    veh = pars["vehicle_params_mintime"]
    tire = pars["tire_params_mintime"]

    # general constants
    g = pars["veh_params"]["g"]
    mass = pars["veh_params"]["mass"]

    # curvature of reference line [rad/m]
    kappa = ca.SX.sym('kappa')

    # drag force [N]
    f_xdrag = pars["veh_params"]["dragcoeff"] * v ** 2

    # rolling resistance forces [N]
    f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
    f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
    f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
    f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
    f_xroll = tire["c_roll"] * mass * g

    # static normal tire forces [N]
    f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
    f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
    f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
    f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]

    # dynamic normal tire forces (aerodynamic downforces) [N]
    f_zlift_fl = 0.5 * veh["liftcoeff_front"] * v ** 2
    f_zlift_fr = 0.5 * veh["liftcoeff_front"] * v ** 2
    f_zlift_rl = 0.5 * veh["liftcoeff_rear"] * v ** 2
    f_zlift_rr = 0.5 * veh["liftcoeff_rear"] * v ** 2

    # dynamic normal tire forces (load transfers) [N]
    f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
                 - veh["k_roll"] * gamma_y)
    f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
                 + veh["k_roll"] * gamma_y)
    f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
                 - (1.0 - veh["k_roll"]) * gamma_y)
    f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
                 + (1.0 - veh["k_roll"]) * gamma_y)

    # sum of all normal tire forces [N]
    f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl
    f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr
    f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl
    f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr

    # slip angles [rad]
    alpha_fl = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
                               (v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z))
    alpha_fr = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
                               (v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z))
    alpha_rl = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
                       (v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z))
    alpha_rr = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
                       (v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z))

    # lateral tire forces [N]
    f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl * (1 + tire["eps_front"] * f_z_fl / tire["f_z0"])
              * ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fl - tire["E_front"]
                                                 * (tire["B_front"] * alpha_fl - ca.atan(tire["B_front"] * alpha_fl)))))
    f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr * (1 + tire["eps_front"] * f_z_fr / tire["f_z0"])
              * ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fr - tire["E_front"]
                                                 * (tire["B_front"] * alpha_fr - ca.atan(tire["B_front"] * alpha_fr)))))
    f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl * (1 + tire["eps_rear"] * f_z_rl / tire["f_z0"])
              * ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"]
                                                * (tire["B_rear"] * alpha_rl - ca.atan(tire["B_rear"] * alpha_rl)))))
    f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr * (1 + tire["eps_rear"] * f_z_rr / tire["f_z0"])
              * ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"]
                                                * (tire["B_rear"] * alpha_rr - ca.atan(tire["B_rear"] * alpha_rr)))))

    # longitudinal tire forces [N]
    f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fl
    f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fr
    f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rl
    f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rr

    # longitudinal acceleration [m/s²]
    ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) - (f_y_fl + f_y_fr) * ca.sin(delta)
          - pars["veh_params"]["dragcoeff"] * v ** 2) / mass

    # lateral acceleration [m/s²]
    ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr + (f_y_fl + f_y_fr) * ca.cos(delta)) / mass

    # time-distance scaling factor (dt/ds)
    sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta)))

    # model equations for two track model (ordinary differential equations)
    dv = (sf / mass) * ((f_x_rl + f_x_rr) * ca.cos(beta) + (f_x_fl + f_x_fr) * ca.cos(delta - beta)
                        + (f_y_rl + f_y_rr) * ca.sin(beta) - (f_y_fl + f_y_fr) * ca.sin(delta - beta)
                        - f_xdrag * ca.cos(beta))

    dbeta = sf * (-omega_z + (-(f_x_rl + f_x_rr) * ca.sin(beta) + (f_x_fl + f_x_fr) * ca.sin(delta - beta)
                              + (f_y_rl + f_y_rr) * ca.cos(beta) + (f_y_fl + f_y_fr) * ca.cos(delta - beta)
                              + f_xdrag * ca.sin(beta)) / (mass * v))

    domega_z = (sf / veh["I_z"]) * ((f_x_rr - f_x_rl) * veh["track_width_rear"] / 2
                                    - (f_y_rl + f_y_rr) * veh["wheelbase_rear"]
                                    + ((f_x_fr - f_x_fl) * ca.cos(delta)
                                       + (f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2
                                    + ((f_y_fl + f_y_fr) * ca.cos(delta)
                                       + (f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"])

    dn = sf * v * ca.sin(xi + beta)

    dxi = sf * omega_z - kappa

    # put all ODEs together
    dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s

    # ------------------------------------------------------------------------------------------------------------------
    # CONTROL BOUNDARIES -----------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    delta_min = -veh["delta_max"] / delta_s         # min. steer angle [rad]
    delta_max = veh["delta_max"] / delta_s          # max. steer angle [rad]
    f_drive_min = 0.0                               # min. longitudinal drive force [N]
    f_drive_max = veh["f_drive_max"] / f_drive_s    # max. longitudinal drive force [N]
    f_brake_min = -veh["f_brake_max"] / f_brake_s   # min. longitudinal brake force [N]
    f_brake_max = 0.0                               # max. longitudinal brake force [N]
    gamma_y_min = -np.inf                           # min. lateral wheel load transfer [N]
    gamma_y_max = np.inf                            # max. lateral wheel load transfer [N]

    # ------------------------------------------------------------------------------------------------------------------
    # STATE BOUNDARIES -------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    v_min = 1.0 / v_s                               # min. velocity [m/s]
    v_max = pars["veh_params"]["v_max"] / v_s       # max. velocity [m/s]
    beta_min = -0.5 * np.pi / beta_s                # min. side slip angle [rad]
    beta_max = 0.5 * np.pi / beta_s                 # max. side slip angle [rad]
    omega_z_min = - 0.5 * np.pi / omega_z_s         # min. yaw rate [rad/s]
    omega_z_max = 0.5 * np.pi / omega_z_s           # max. yaw rate [rad/s]
    xi_min = - 0.5 * np.pi / xi_s                   # min. relative angle to tangent on reference line [rad]
    xi_max = 0.5 * np.pi / xi_s                     # max. relative angle to tangent on reference line [rad]

    # ------------------------------------------------------------------------------------------------------------------
    # INITIAL GUESS FOR DECISION VARIABLES -----------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------
    v_guess = 20.0 / v_s

    # ------------------------------------------------------------------------------------------------------------------
    # HELPER FUNCTIONS -------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # continuous time dynamics
    f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'], ['dx', 'sf'])

    # longitudinal tire forces [N]
    f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr],
                       ['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr'])
    # lateral tire forces [N]
    f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr],
                       ['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr'])
    # vertical tire forces [N]
    f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr],
                       ['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr'])

    # longitudinal and lateral acceleration [m/s²]
    f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay'])

    # ------------------------------------------------------------------------------------------------------------------
    # FORMULATE NONLINEAR PROGRAM --------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # initialize NLP vectors
    w = []
    w0 = []
    lbw = []
    ubw = []
    J = 0
    g = []
    lbg = []
    ubg = []

    # initialize ouput vectors
    x_opt = []
    u_opt = []
    dt_opt = []
    tf_opt = []
    ax_opt = []
    ay_opt = []
    ec_opt = []

    # initialize control vectors (for regularization)
    delta_p = []
    F_p = []

    # boundary constraint: lift initial conditions
    Xk = ca.MX.sym('X0', nx)
    w.append(Xk)
    n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s
    n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s
    lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
    ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
    w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
    x_opt.append(Xk * x_s)

    # loop along the racetrack and formulate path constraints & system dynamic
    for k in range(N):
        # add decision variables for the control
        Uk = ca.MX.sym('U_' + str(k), nu)
        w.append(Uk)
        lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min])
        ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max])
        w0.append([0.0] * nu)

        # add decision variables for the state at collocation points
        Xc = []
        for j in range(d):
            Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx)
            Xc.append(Xkj)
            w.append(Xkj)
            lbw.append([-np.inf] * nx)
            ubw.append([np.inf] * nx)
            w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])

        # loop over all collocation points
        Xk_end = D[0] * Xk
        sf_opt = []
        for j in range(1, d + 1):
            # calculate the state derivative at the collocation point
            xp = C[0, j] * Xk
            for r in range(d):
                xp = xp + C[r + 1, j] * Xc[r]

            # interpolate kappa at the collocation point
            kappa_col = kappa_interp(k + tau[j])

            # append collocation equations (system dynamic)
            fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col)
            g.append(h * fj - xp)
            lbg.append([0.0] * nx)
            ubg.append([0.0] * nx)

            # add contribution to the end state
            Xk_end = Xk_end + D[j] * Xc[j - 1]

            # add contribution to quadrature function
            J = J + B[j] * qj * h

            # add contribution to scaling factor (for calculating lap time)
            sf_opt.append(B[j] * qj * h)

        # calculate used energy 
        dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2])
        ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1])

        # add new decision variables for state at end of the collocation interval
        Xk = ca.MX.sym('X_' + str(k + 1), nx)
        w.append(Xk)
        n_min = (-w_tr_right_interp(k + 1) + pars["optim_opts"]["width_opt"] / 2.0) / n_s
        n_max = (w_tr_left_interp(k + 1) - pars["optim_opts"]["width_opt"] / 2.0) / n_s
        lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
        ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
        w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])

        # add equality constraint
        g.append(Xk_end - Xk)
        lbg.append([0.0] * nx)
        ubg.append([0.0] * nx)

        # get tire forces
        f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk)
        f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk)
        f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk)

        # get accelerations (longitudinal + lateral)
        axk, ayk = f_a(Xk, Uk)

        # path constraint: limitied engine power
        g.append(Xk[0] * Uk[1])
        lbg.append([-np.inf])
        ubg.append([veh["power_max"] / (f_drive_s * v_s)])

        # get constant friction coefficient
        if pars["optim_opts"]["var_friction"] is None:
            mue_fl = pars["optim_opts"]["mue"]
            mue_fr = pars["optim_opts"]["mue"]
            mue_rl = pars["optim_opts"]["mue"]
            mue_rr = pars["optim_opts"]["mue"]

        # calculate variable friction coefficients along the reference line (regression with linear equations)
        elif pars["optim_opts"]["var_friction"] == "linear":
            # friction coefficient for each tire
            mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1]
            mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1]
            mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1]
            mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1]

        # calculate variable friction coefficients along the reference line (regression with gaussian basis functions)
        elif pars["optim_opts"]["var_friction"] == "gauss":
            # gaussian basis functions
            sigma = 2.0 * center_dist[k + 1, 0]
            n_gauss = pars["optim_opts"]["n_gauss"]
            n_q = np.linspace(-n_gauss, n_gauss, 2 * n_gauss + 1) * center_dist[k + 1, 0]

            gauss_basis = []
            for i in range(2 * n_gauss + 1):
                gauss_basis.append(ca.exp(-(Xk[3] * n_s - n_q[i]) ** 2 / (2 * (sigma ** 2))))
            gauss_basis = ca.vertcat(*gauss_basis)

            mue_fl = ca.dot(w_mue_fl[k + 1, :-1], gauss_basis) + w_mue_fl[k + 1, -1]
            mue_fr = ca.dot(w_mue_fr[k + 1, :-1], gauss_basis) + w_mue_fr[k + 1, -1]
            mue_rl = ca.dot(w_mue_rl[k + 1, :-1], gauss_basis) + w_mue_rl[k + 1, -1]
            mue_rr = ca.dot(w_mue_rr[k + 1, :-1], gauss_basis) + w_mue_rr[k + 1, -1]

        else:
            raise ValueError("No friction coefficients are available!")

        # path constraint: Kamm's Circle for each wheel
        g.append(((f_x_flk / (mue_fl * f_z_flk)) ** 2 + (f_y_flk / (mue_fl * f_z_flk)) ** 2))
        g.append(((f_x_frk / (mue_fr * f_z_frk)) ** 2 + (f_y_frk / (mue_fr * f_z_frk)) ** 2))
        g.append(((f_x_rlk / (mue_rl * f_z_rlk)) ** 2 + (f_y_rlk / (mue_rl * f_z_rlk)) ** 2))
        g.append(((f_x_rrk / (mue_rr * f_z_rrk)) ** 2 + (f_y_rrk / (mue_rr * f_z_rrk)) ** 2))
        lbg.append([0.0] * 4)
        ubg.append([1.0] * 4)

        # path constraint: lateral wheel load transfer
        g.append(((f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk
                  + (f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s))
                 * veh["cog_z"] / ((veh["track_width_front"] + veh["track_width_rear"]) / 2) - Uk[3] * gamma_y_s)
        lbg.append([0.0])
        ubg.append([0.0])

        # path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal)
        g.append(Uk[1] * Uk[2])
        lbg.append([-20000.0 / (f_drive_s * f_brake_s)])
        ubg.append([0.0])

        # path constraint: actor dynamic
        if k > 0:
            sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s)
            g.append((Uk - w[1 + (k - 1) * nx]) / (pars["stepsize_opts"]["stepsize_reg"] * sigma))
            lbg.append([delta_min / (veh["t_delta"]), -np.inf, f_brake_min / (veh["t_brake"]), -np.inf])
            ubg.append([delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]), np.inf, np.inf])

        # path constraint: safe trajectories with acceleration ellipse
        if pars["optim_opts"]["safe_traj"]:
            g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"]) ** 2
                     + (ayk / pars["optim_opts"]["ay_safe"]) ** 2)
            g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"]) ** 2
                     + (ayk / pars["optim_opts"]["ay_safe"]) ** 2)
            lbg.append([0.0] * 2)
            ubg.append([1.0] * 2)

        # append controls (for regularization)
        delta_p.append(Uk[0] * delta_s)
        F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0)

        # append outputs
        x_opt.append(Xk * x_s)
        u_opt.append(Uk * u_s)
        tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk])
        tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk])
        ax_opt.append(axk)
        ay_opt.append(ayk)

    # boundary constraint: start states = final states
    g.append(w[0] - Xk)
    lbg.append([0.0] * nx)
    ubg.append([0.0] * nx)

    # path constraint: limited energy consumption
    if pars["optim_opts"]["limit_energy"]:
        g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0)
        lbg.append([0])
        ubg.append([pars["optim_opts"]["energy_limit"]])

    # formulate differentiation matrix (for regularization)
    diff_matrix = np.eye(N)
    for i in range(N - 1):
        diff_matrix[i, i + 1] = -1.0
    diff_matrix[N - 1, 0] = -1.0

    # regularization (delta)
    delta_p = ca.vertcat(*delta_p)
    Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p)
    Jp_delta = ca.dot(Jp_delta, Jp_delta)

    # regularization (f_drive + f_brake)
    F_p = ca.vertcat(*F_p)
    Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p)
    Jp_f = ca.dot(Jp_f, Jp_f)

    # formulate objective
    J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"]["penalty_delta"] * Jp_delta

    # concatenate NLP vectors
    w = ca.vertcat(*w)
    g = ca.vertcat(*g)
    w0 = np.concatenate(w0)
    lbw = np.concatenate(lbw)
    ubw = np.concatenate(ubw)
    lbg = np.concatenate(lbg)
    ubg = np.concatenate(ubg)

    # concatenate output vectors
    x_opt = ca.vertcat(*x_opt)
    u_opt = ca.vertcat(*u_opt)
    tf_opt = ca.vertcat(*tf_opt)
    dt_opt = ca.vertcat(*dt_opt)
    ax_opt = ca.vertcat(*ax_opt)
    ay_opt = ca.vertcat(*ay_opt)
    ec_opt = ca.vertcat(*ec_opt)

    # ------------------------------------------------------------------------------------------------------------------
    # CREATE NLP SOLVER ------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # fill nlp structure
    nlp = {'f': J, 'x': w, 'g': g}

    # solver options
    opts = {"expand": True,
            "verbose": print_debug,
            "ipopt.max_iter": 2000,
            "ipopt.tol": 1e-7}

    # solver options for warm start
    if pars["optim_opts"]["warm_start"]:
        opts_warm_start = {"ipopt.warm_start_init_point": "yes",
                           "ipopt.warm_start_bound_push": 1e-9,
                           "ipopt.warm_start_mult_bound_push": 1e-9,
                           "ipopt.warm_start_slack_bound_push": 1e-9,
                           "ipopt.mu_init": 1e-9}
        opts.update(opts_warm_start)

    # load warm start files
    if pars["optim_opts"]["warm_start"]:
        try:
            w0 = np.loadtxt(os.path.join(export_path, 'w0.csv'))
            lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv'))
            lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv'))
        except IOError:
            print('\033[91m' + 'WARNING: Failed to load warm start files!' + '\033[0m')
            sys.exit(1)

    # check warm start files
    if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw):
        print('\033[91m' + 'WARNING: Warm start files do not fit to the dimension of the NLP!' + '\033[0m')
        sys.exit(1)

    # create solver instance
    solver = ca.nlpsol("solver", "ipopt", nlp, opts)

    # ------------------------------------------------------------------------------------------------------------------
    # SOLVE NLP --------------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # start time measure
    t0 = time.perf_counter()

    # solve NLP
    if pars["optim_opts"]["warm_start"]:
        sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0)
    else:
        sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)

    # end time measure
    tend = time.perf_counter()

    # ------------------------------------------------------------------------------------------------------------------
    # EXTRACT SOLUTION -------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # helper function to extract solution for state variables, control variables, tire forces, time
    f_sol = ca.Function('f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt],
                        ['w'], ['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt'])

    # extract solution
    x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x'])

    # solution for state variables
    x_opt = np.reshape(x_opt, (N + 1, nx))

    # solution for control variables
    u_opt = np.reshape(u_opt, (N, nu))

    # solution for tire forces
    tf_opt = np.append(tf_opt[-12:], tf_opt[:])
    tf_opt = np.reshape(tf_opt, (N + 1, 12))

    # solution for time
    t_opt = np.hstack((0.0, np.cumsum(dt_opt)))

    # solution for acceleration
    ax_opt = np.append(ax_opt[-1], ax_opt)
    ay_opt = np.append(ay_opt[-1], ay_opt)
    atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2))

    # solution for energy consumption
    ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0

    # ------------------------------------------------------------------------------------------------------------------
    # EXPORT SOLUTION --------------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    # export data to CSVs
    opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(file_path=export_path,
                                                                         s=s_opt,
                                                                         t=t_opt,
                                                                         x=x_opt,
                                                                         u=u_opt,
                                                                         tf=tf_opt,
                                                                         ax=ax_opt,
                                                                         ay=ay_opt,
                                                                         atot=atot_opt,
                                                                         w0=sol["x"],
                                                                         lam_x0=sol["lam_x"],
                                                                         lam_g0=sol["lam_g"])

    # ------------------------------------------------------------------------------------------------------------------
    # PLOT & PRINT RESULTS ---------------------------------------------------------------------------------------------
    # ------------------------------------------------------------------------------------------------------------------

    if plot_debug:
        opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(pars=pars,
                                                                       reftrack=reftrack,
                                                                       s=s_opt,
                                                                       t=t_opt,
                                                                       x=x_opt,
                                                                       u=u_opt,
                                                                       ax=ax_opt,
                                                                       ay=ay_opt,
                                                                       atot=atot_opt,
                                                                       tf=tf_opt,
                                                                       ec=ec_opt_cum)

    if print_debug:
        print("INFO: Laptime: %.3fs" % t_opt[-1])
        print("INFO: NLP solving time: %.3fs" % (tend - t0))
        print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt))
        print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt))
        print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt))
        print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt))
        print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1])

    return -x_opt[:-1, 3], x_opt[:-1, 0]
Example #33
0
u = casadi.MX.sym('u', nu)
# p = casadi.MX.sym('p', np.size(parameter))

# ode = casadi.vertcat(
#     x[3]                                         ,\
#     x[4]                                         ,\
#     x[5]                                         ,\
#     -1/p[0]*(u[0] + u[1])*casadi.sin(x[2])       ,\
#      1/p[0]*(u[0] + u[1])*casadi.cos(x[2]) - p[3],\
#     p[2]/p[1]*(u[0] - u[1])
# )
ode = casadi.vertcat(
    x[3]                                   ,\
    x[4]                                   ,\
    x[5]                                   ,\
    -1/m*(u[0] + u[1])*casadi.sin(x[2])    ,\
     1/m*(u[0] + u[1])*casadi.cos(x[2]) - g,\
    l/I*(u[0] - u[1])
)

f = casadi.Function('f', [x,u], [ode], ['x','u'], ['ode'])

# === create integrator expression ===
intg_options = dict(tf = dt)            # define integration step
dae = dict(x = x, p = u, ode = f(x,u))  # define the dae to integrate (ZOH)

# define the integrator
intg = casadi.integrator('intg', 'rk', dae, intg_options)
res = intg(x0 = x, p = u)
# define symbolic expression of integrator
x_next = res['xf']
Example #34
0
def circle_path(s, centre, radius, rot_mat=np.eye(3)):

    T_goal = cs.vertcat(centre[0] + radius * (cos(s) - 1),
                        centre[1] + radius * sin(s), centre[2])

    return T_goal
Example #35
0
    def compute_vel_cmds(self):
        if self.goal is not None and self.MO_obs is not None and self.SO_obs is not None:

            x0 = self.pose
            x_goal = self.goal
            self.p[0:6] = np.append(x0, x_goal)

            for k in range(N + 1):
                for i in range(n_MO):
                    i_pos = n_MOst * n_MO * (k + 1) + 6 - (n_MO - i) * n_MOst

                    self.p[i_pos + 2:i_pos + 5] = np.array([self.MO_obs[i].velocities.twist.linear.x, self.MO_obs[i].orientation.z, self.MO_obs[i].radius])

                    t_predicted = k * Ts

                    obs_x = self.MO_obs[i].polygon.points[0].x + t_predicted * self.MO_obs[i].velocities.twist.linear.x * ca.cos(self.MO_obs[i].orientation.z)
                    obs_y = self.MO_obs[i].polygon.points[0].y + t_predicted * self.MO_obs[i].velocities.twist.linear.x * ca.sin(self.MO_obs[i].orientation.z)

                    self.p[i_pos:i_pos + 2] = [obs_x, obs_y]
            i_pos += 5

            self.p[i_pos:i_pos+n_SO*3] = closest_n_obs(self.SO_obs, x0, n_SO)


            x0k = np.append(self.x_st_0.reshape(3 * (N + 1), 1), self.u0.reshape(2 * N, 1))
            x0k = x0k.reshape(x0k.shape[0], 1)

            sol = solver(x0=x0k, lbx=self.lbw, ubx=self.ubw, lbg=self.lbg, ubg=self.ubg, p=self.p)

            u_sol = sol.get('x')[3 * (N + 1):].reshape((N, 2))

            self.u0 = shift(u_sol)

            self.x_st_0 = np.reshape(sol.get('x')[0:3 * (N + 1)], (N + 1, 3))
            self.x_st_0 = np.append(self.x_st_0[1:, :], self.x_st_0[-1, :].reshape((1, 3)), axis=0)
            print(u_sol)
            cmd_vel = Twist()
            cmd_vel.linear.x = u_sol[0]
            cmd_vel.angular.z = u_sol[1]
            self.pub.publish(cmd_vel)

            self.mpc_i = self.mpc_i + 1
            print(self.mpc_i)
            print('This is the set of control solutions: ', u_sol)
        else:
            print("Goal has not been received yet. Waiting.")
Example #36
0
def MinCurvature():
    track = run_map_gen()
    txs = track[:, 0]
    tys = track[:, 1]
    nvecs = track[:, 2:4]
    th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]

    n_max = 3
    N = len(track)

    n_f_a = ca.MX.sym('n_f', N)
    n_f = ca.MX.sym('n_f', N - 1)
    th_f = ca.MX.sym('n_f', N - 1)

    x0_f = ca.MX.sym('x0_f', N - 1)
    x1_f = ca.MX.sym('x1_f', N - 1)
    y0_f = ca.MX.sym('y0_f', N - 1)
    y1_f = ca.MX.sym('y1_f', N - 1)
    th1_f = ca.MX.sym('y1_f', N - 1)
    th2_f = ca.MX.sym('y1_f', N - 1)
    th1_f1 = ca.MX.sym('y1_f', N - 2)
    th2_f1 = ca.MX.sym('y1_f', N - 2)

    o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
    o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
    o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
    o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])

    dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
                      [ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])

    track_length = ca.Function('length', [n_f_a], [
        dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
            o_y_e(n_f_a[1:]))
    ])

    real = ca.Function(
        'real', [th1_f, th2_f],
        [ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
    im = ca.Function(
        'im', [th1_f, th2_f],
        [-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])

    sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
                            [ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])
    get_th_n = ca.Function(
        'gth', [th_f],
        [sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])

    d_n = ca.Function('d_n', [n_f_a, th_f],
                      [track_length(n_f_a) / ca.tan(get_th_n(th_f))])

    # define symbols
    n = ca.MX.sym('n', N)
    th = ca.MX.sym('th', N)


    nlp = {\
    'x': ca.vertcat(n, th),
    'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
    'g': ca.vertcat(
                # dynamic constraints
                n[1:] - (n[:-1] + d_n(n, th[:-1])),

                # boundary constraints
                n[0], th[0],
                n[-1], #th[-1],
            ) \

    }

    S = ca.nlpsol('S', 'ipopt', nlp)

    ones = np.ones(N)
    n0 = ones * 0

    th0 = []
    for i in range(N - 1):
        th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
        th0.append(th_00)

    th0.append(0)
    th0 = np.array(th0)

    x0 = ca.vertcat(n0, th0)

    lbx = [-n_max] * N + [-np.pi] * N
    ubx = [n_max] * N + [np.pi] * N

    r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)

    x_opt = r['x']

    n_set = np.array(x_opt[:N])
    thetas = np.array(x_opt[1 * N:2 * N])

    plot_race_line(np.array(track), n_set, wait=True)

    return n_set
Example #37
0
def plt_fnc(state, predict, goal, t, u_cl, SO_init, MO_init):

    plt.figure(1)
    plt.grid()
    plt.text(goal[0] - 0.15, goal[1] - 0.2, 'Goal', style='oblique', fontsize=10)
    plt.text(-0.1, 0.15, 'Start', style='oblique', fontsize=10)
    plt.plot(state.T[:, 0], state.T[:, 1])
    plt.plot(0, 0, 'bo')
    # plt.plot(predict[:, 0], predict[:, 1])
    plt.plot(goal[0], goal[1], 'ro')
    plt.xlabel('X-position [Meters]')
    plt.ylabel('Y-position [Meters]')
    plt.title('MPC in python')
    for obs in range(len(SO_init[:])):
        c1 = plt.Circle((SO_init[obs][0], SO_init[obs][1]), radius=SO_init[obs][2], alpha=0.5)
        plt.gcf().gca().add_artist(c1)
    for obs in range(len(MO_init[:])):
        plt.quiver(MO_init[obs][0], MO_init[obs][1], 1 * ca.cos(MO_init[obs][2]), 1 * ca.sin(MO_init[obs][2]))
        c2 = plt.Circle((MO_init[obs][0], MO_init[obs][1]), radius=MO_init[obs][4], alpha=0.5, color='r')
        plt.gcf().gca().add_artist(c2)
    # plt.xlim(-10, 10)
    # plt.ylim(-10, 10)

    fig, (ax1, ax2) = plt.subplots(2)
    fig.suptitle('Control Signals From MPC Solution')
    ax1.plot(t, u_cl[:, 0])
    ax1.grid()
    ax2.plot(t, u_cl[:, 1])
    ax2.grid()
    ax2.set_ylabel('Angular Velocity [m/s]')
    ax2.set_xlabel('Time [s]')
    ax1.set_ylabel('Linear Velocity [rad/s]')
    ax1.set_xlabel('Time [s]')

    plt.show()
    return state, predict, goal, t
Example #38
0
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)))
Example #39
0
n_SO = 1

# System Model
x = ca.SX.sym('x')
y = ca.SX.sym('y')
theta = ca.SX.sym('theta')
states = ca.vertcat(x, y, theta)
n_states = 3  # len([states])

# Control system
v = ca.SX.sym('v')
omega = ca.SX.sym('omega')
controls = ca.vertcat(v, omega)
n_controls = 2  # len([controls])

rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega)

# Obstacle states in each predictions
MOx = ca.SX.sym('MOx')
MOy = ca.SX.sym('MOy')
MOth = ca.SX.sym('MOth')
MOv = ca.SX.sym('MOv')
MOr = ca.SX.sym('MOr')
Ost = [MOx, MOy, MOth, MOv, MOr]

n_MOst = len(Ost)

# System setup for casadi
mapping_func = ca.Function('f', [states, controls], [rhs])

# Declare empty system matrices
    n_controls = 2
    opt_controls = opti.variable(N, n_controls)
    v = opt_controls[:, 0]
    omega = opt_controls[:, 1]
    n_states = 3
    opt_states = opti.variable(N + 1, n_states)
    x = opt_states[:, 0]
    y = opt_states[:, 1]
    theta = opt_states[:, 2]

    # parameters
    opt_x0 = opti.parameter(3)
    opt_xs = opti.parameter(3)
    # create model
    f = lambda x_, u_: ca.vertcat(
        *[u_[0] * ca.cos(x_[2]), u_[0] * ca.sin(x_[2]), u_[1]])
    f_np = lambda x_, u_: np.array(
        [u_[0] * np.cos(x_[2]), u_[0] * np.sin(x_[2]), u_[1]])

    ## init_condition
    opti.subject_to(opt_states[0, :] == opt_x0.T)
    for i in range(N):
        x_next = opt_states[i, :] + f(opt_states[i, :],
                                      opt_controls[i, :]).T * T
        opti.subject_to(opt_states[i + 1, :] == x_next)

    ## define the cost function
    ### some addition parameters
    Q = np.array([[1.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, .1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    #### cost function
Example #41
0
def MinCurvatureTrajectory(pts, nvecs, ws):
    """
    This function uses optimisation to minimise the curvature of the path
    """
    w_min = -ws[:, 0] * 0.9
    w_max = ws[:, 1] * 0.9
    th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]

    N = len(pts)

    n_f_a = ca.MX.sym('n_f', N)
    n_f = ca.MX.sym('n_f', N - 1)
    th_f = ca.MX.sym('n_f', N - 1)

    x0_f = ca.MX.sym('x0_f', N - 1)
    x1_f = ca.MX.sym('x1_f', N - 1)
    y0_f = ca.MX.sym('y0_f', N - 1)
    y1_f = ca.MX.sym('y1_f', N - 1)
    th1_f = ca.MX.sym('y1_f', N - 1)
    th2_f = ca.MX.sym('y1_f', N - 1)
    th1_f1 = ca.MX.sym('y1_f', N - 2)
    th2_f1 = ca.MX.sym('y1_f', N - 2)

    o_x_s = ca.Function('o_x', [n_f], [pts[:-1, 0] + nvecs[:-1, 0] * n_f])
    o_y_s = ca.Function('o_y', [n_f], [pts[:-1, 1] + nvecs[:-1, 1] * n_f])
    o_x_e = ca.Function('o_x', [n_f], [pts[1:, 0] + nvecs[1:, 0] * n_f])
    o_y_e = ca.Function('o_y', [n_f], [pts[1:, 1] + nvecs[1:, 1] * n_f])

    dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
                      [ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])

    track_length = ca.Function('length', [n_f_a], [
        dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
            o_y_e(n_f_a[1:]))
    ])

    real = ca.Function(
        'real', [th1_f, th2_f],
        [ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
    im = ca.Function(
        'im', [th1_f, th2_f],
        [-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])

    sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
                            [ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])

    get_th_n = ca.Function(
        'gth', [th_f],
        [sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
    d_n = ca.Function('d_n', [n_f_a, th_f],
                      [track_length(n_f_a) / ca.tan(get_th_n(th_f))])

    # objective
    real1 = ca.Function(
        'real1', [th1_f1, th2_f1],
        [ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
    im1 = ca.Function(
        'im1', [th1_f1, th2_f1],
        [-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])

    sub_cmplx1 = ca.Function(
        'a_cpx1', [th1_f1, th2_f1],
        [ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])

    # define symbols
    n = ca.MX.sym('n', N)
    th = ca.MX.sym('th', N - 1)

    nlp = {\
    'x': ca.vertcat(n, th),
    'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
    # 'f': ca.sumsqr(track_length(n)),
    'g': ca.vertcat(
                # dynamic constraints
                n[1:] - (n[:-1] + d_n(n, th)),

                # boundary constraints
                n[0], #th[0],
                n[-1], #th[-1],
            ) \

    }

    # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})
    S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}})

    ones = np.ones(N)
    n0 = ones * 0

    th0 = []
    for i in range(N - 1):
        th_00 = lib.get_bearing(pts[i, 0:2], pts[i + 1, 0:2])
        th0.append(th_00)

    th0 = np.array(th0)

    x0 = ca.vertcat(n0, th0)

    lbx = list(w_min) + [-np.pi] * (N - 1)
    ubx = list(w_max) + [np.pi] * (N - 1)

    r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)

    x_opt = r['x']

    n_set = np.array(x_opt[:N])
    # thetas = np.array(x_opt[1*N:2*(N-1)])

    return n_set
Example #42
0
    deg = 4
    newton = Newton(LagrangePoly,dae,nk,nicp,deg,'RADAU')
    dt = 0.025
    endTime = dt*nk
    newton.setupStuff(endTime)

    r = 0.3
    x0 = [r,0,0,0]

    xTraj = [np.array(x0)]
    uTraj = []
    p = np.array([0.3])
    
    # make a trajectory
    for k in range(nk):
        uTraj.append(np.array([5*C.sin(k/float(nk))]))
#        uTraj.append(np.array([0]))
        newton.isolver.setInput(xTraj[-1],0)
        newton.isolver.setInput(uTraj[-1],1)
        newton.isolver.setInput(p[-1],2)
        newton.isolver.output().set(1)
        newton.isolver.evaluate()
        xTraj.append(np.array(newton.isolver.output(1)).squeeze())

#    for k in range(nk):
#        print xTraj[k],uTraj[k]
#    print xTraj[-1]

    # setup MHE
    nmhe = Nmhe(dae,nk)
Example #43
0
# Identity matrix
Inx = ca.DMatrix.eye(nx)


# ----------------------------------------------------------------------------
#                                 Dynamics
# ----------------------------------------------------------------------------

# Continuous dynamics
dt_sym = ca.SX.sym('dt')
state = cat.struct_symSX(['x', 'y', 'phi'])
control = cat.struct_symSX(['v', 'w'])
rhs = cat.struct_SX(state)
rhs['x'] = control['v'] * ca.cos(state['phi'])
rhs['y'] = control['v'] * ca.sin(state['phi'])
rhs['phi'] = control['w']
f = ca.SXFunction('Continuous dynamics', [state, control], [rhs])

# Discrete dynamics
state_next = state + dt_sym * f([state, control])[0]
op = {'input_scheme': ['state', 'control', 'dt'],
      'output_scheme': ['state_next']}
F = ca.SXFunction('Discrete dynamics',
                  [state, control, dt_sym], [state_next], op)
Fj_x = F.jacobian('state')
Fj_u = F.jacobian('control')
F_xx = ca.SXFunction('F_xx', [state, control, dt_sym],
                     [ca.jacobian(F.jac('state')[i, :].T, state) for i in
                      range(nx)])
F_uu = ca.SXFunction('F_uu', [state, control, dt_sym],
Example #44
0
    def plot_heuristics(model, x_all, u_all, n_last=2):
        n_interm_points = 301
        n = len(x_all[:])
        t_all = np.linspace(0, (n - 1) * model.dt, n)
        t_all_dense = np.linspace(t_all[0], t_all[-1], n_interm_points)

        fig, ax = plt.subplots(2, 2, figsize=(10, 10))

        # ---------------- Optic acceleration cancellation ----------------- #
        oac = []
        for k in range(n):
            x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
            x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
            r_bc_xy = ca.norm_2(x_b - x_c)
            z_b = x_all[k, 'z_b']
            tan_phi = ca.arctan2(z_b, r_bc_xy)
            oac.append(tan_phi)

        # Fit a line for OAC
        fit_oac = np.polyfit(t_all[:-n_last], oac[:-n_last], 1)
        fit_oac_fn = np.poly1d(fit_oac)

        # Plot OAC
        ax[0, 0].plot(t_all[:-n_last], oac[:-n_last],
                      label='Simulation', lw=3)
        ax[0, 0].plot(t_all, fit_oac_fn(t_all), '--k', label='Linear fit')



        # ------------------- Constant bearing angle ----------------------- #
        cba = []
        d = ca.veccat([ca.cos(model.m0['phi']),
                       ca.sin(model.m0['phi'])])
        for k in range(n):
            x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
            x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
            r_cb = x_b - x_c
            cos_gamma = ca.mul(d.T, r_cb) / ca.norm_2(r_cb)
            cba.append(np.rad2deg(np.float(ca.arccos(cos_gamma))))

        # Fit a const for CBA
        fit_cba = np.polyfit(t_all[:-n_last], cba[:-n_last], 0)
        fit_cba_fn = np.poly1d(fit_cba)

        # Smoothen the trajectory
        t_part_dense = np.linspace(t_all[0], t_all[-n_last-1], 301)
        cba_smooth = spline(t_all[:-n_last], cba[:-n_last], t_part_dense)
        ax[1, 0].plot(t_part_dense, cba_smooth, lw=3, label='Simulation')

        # Plot CBA
        # ax[1, 0].plot(t_all[:-n_last], cba[:-n_last],
        #               label='$\gamma \\approx const$')
        ax[1, 0].plot(t_all, fit_cba_fn(t_all), '--k', label='Constant fit')




        # ---------- Generalized optic acceleration cancellation ----------- #
        goac_smooth = spline(t_all,
                            model.m0['phi'] - x_all[:, 'phi'],
                            t_all_dense)

        n_many_last = n_last *\
                      n_interm_points / (t_all[-1] - t_all[0]) * model.dt
        # Delta
        ax[0, 1].plot(t_all_dense[:-n_many_last],
                      np.rad2deg(goac_smooth[:-n_many_last]), lw=3,
                      label=r'Rotation angle $\delta$')
        # Gamma
        ax[0, 1].plot(t_all[:-n_last], cba[:-n_last], '--', lw=2,
                      label=r'Bearing angle $\gamma$')
        # ax[0, 1].plot([t_all[0], t_all[-1]], [30, 30], 'k--',
        #               label='experimental bound')
        # ax[0, 1].plot([t_all[0], t_all[-1]], [-30, -30], 'k--')
        # ax[0, 1].yaxis.set_ticks(range(-60, 70, 30))



        # Derivative of delta
        # ax0_twin = ax[0, 1].twinx()
        # ax0_twin.step(t_all,
        #               np.rad2deg(np.array(ca.veccat([0, u_all[:, 'w_phi']]))),
        #               'g-', label='derivative $\mathrm{d}\delta/\mathrm{d}t$')
        # ax0_twin.set_ylim(-90, 90)
        # ax0_twin.yaxis.set_ticks(range(-90, 100, 30))
        # ax0_twin.set_ylabel('$\mathrm{d}\delta/\mathrm{d}t$, deg/s')
        # ax0_twin.yaxis.label.set_color('g')
        # ax0_twin.legend(loc='lower right')

        # -------------------- Linear optic trajectory --------------------- #
        lot_beta = []
        x_b = model.m0[ca.veccat, ['x_b', 'y_b']]
        for k in range(n):
            x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
            d = ca.veccat([ca.cos(x_all[k, 'phi']),
                           ca.sin(x_all[k, 'phi'])])
            r = x_b - x_c
            cos_beta = ca.mul(d.T, r) / ca.norm_2(r)

            beta = ca.arccos(cos_beta)
            tan_beta = ca.tan(beta)
            lot_beta.append(tan_beta)

            # lot_beta.append(np.rad2deg(np.float(ca.arccos(cos_beta))))
        # lot_alpha = np.rad2deg(np.array(x_all[:, 'psi']))

        lot_alpha = ca.tan(x_all[:, 'psi'])

        # Fit a line for LOT
        fit_lot = np.polyfit(lot_alpha[model.n_delay:-n_last],
                             lot_beta[model.n_delay:-n_last], 1)
        fit_lot_fn = np.poly1d(fit_lot)

        # Plot
        ax[1, 1].scatter(lot_alpha[model.n_delay:-n_last],
                         lot_beta[model.n_delay:-n_last],
                         label='Simulation')
        ax[1, 1].plot(lot_alpha[model.n_delay:-n_last],
                      fit_lot_fn(lot_alpha[model.n_delay:-n_last]),
                      '--k', label='Linear fit')

        fig.tight_layout()
        return fig, ax
Example #45
0
def forcesTorques(state, u, p, outputs):
    rho =    1.23      #  density of the air             #  [ kg/m^3]
    rA = 1.085 #(dixit Kurt)
    alpha0 = -0*C.pi/180 
    
    #TAIL LENGTH
    lT = 0.4
    
    #ROLL DAMPING
    rD = 1e-2 
    pD = 0*1e-3
    yD = 0*1e-3
    
    #WIND-TUNNEL PARAMETERS
    #Lift (report p. 67)
    cLA = 5.064
    
    cLe = -1.924
    
    cL0 = 0.239
    
    #Drag (report p. 70)
    cDA = -0.195
    cDA2 = 4.268
    cDB2 = 5

#    cDe = 0.044
#    cDr = 0.111

    cD0 = 0.026
    
    #Roll (report p. 72)
    cRB = -0.062
    cRAB = -0.271 
    cRr = -5.637e-1
    
    #Pitch (report p. 74)
    cPA = 0.293
    cPe = -4.9766e-1
    
    cP0 = 0.03
    
    #Yaw (report p. 76)
    cYB = 0.05
    cYAB = 0.229

    # rudder (fudged by Greg)
    cYr = 0.02
    
    span = 0.96
    chord = 0.1
    
    ###########     model integ ###################
    x =   state['x']
    y =   state['y']

    e11 = state['e11']
    e12 = state['e12']
    e13 = state['e13']

    e21 = state['e21']
    e22 = state['e22']
    e23 = state['e23']

    e31 = state['e31']
    e32 = state['e32']
    e33 = state['e33']
                   
    dx  =  state['dx']
    dy  =  state['dy']
    dz  =  state['dz']

    w1  =  state['w1']
    w2  =  state['w2']
    w3  =  state['w3']

    delta = 0
    ddelta = 0

    aileron = u['aileron']
    elevator = u['elevator']
    rudder = u['rudder']
    
    ####### kinfile ######
    dpE = C.veccat( [ dx*e11 + dy*e12 + dz*e13 + ddelta*e12*rA + ddelta*e12*x - ddelta*e11*y
                    , dx*e21 + dy*e22 + dz*e23 + ddelta*e22*rA + ddelta*e22*x - ddelta*e21*y
                    , dx*e31 + dy*e32 + dz*e33 + ddelta*e32*rA + ddelta*e32*x - ddelta*e31*y
                    ])
    dp_carousel_frame = C.veccat( [ dx - ddelta*y
                                  , dy + ddelta*rA + ddelta*x
                                  , dz
                                  ]) - C.veccat([C.cos(delta)*p['wind_x'],C.sin(delta)*p['wind_x'],0])
    
    ##### more model_integ ###########
    # EFFECTIVE WIND IN THE KITE`S SYSTEM :
    # ###############################
    
    #Airfoil speed in carousel frame
    we1 = dp_carousel_frame[0]
    we2 = dp_carousel_frame[1]
    we3 = dp_carousel_frame[2]
    
    vKite2 = C.mul(dp_carousel_frame.trans(), dp_carousel_frame) #Airfoil speed^2 
    vKite = C.sqrt(vKite2) #Airfoil speed
    outputs['airspeed'] = vKite
    
    # CALCULATION OF THE FORCES :
    # ###############################
    #
    #   FORCE ARE COMPUTED IN THE CAROUSEL FRAME @>@>@>
    
    #Aero coeff.
    
            
    # LIFT DIRECTION VECTOR
    # ############
    
    # Relative wind speed in Airfoil's referential 'E'
    
    wE1 = dpE[0]
    wE2 = dpE[1]
    wE3 = dpE[2]
    
    # Airfoil's transversal axis in carousel referential 'e'
    eTe1 = e21
    eTe2 = e22
    eTe3 = e23
    
    
    # Lift axis ** Normed to we @>@> **
    eLe1 = - eTe2*we3 + eTe3*we2
    eLe2 = - eTe3*we1 + eTe1*we3
    eLe3 = - eTe1*we2 + eTe2*we1
       
    
    # AERODYNAMIC COEEFICIENTS
    # #################
    vT1 =          wE1
    vT2 = -lT*w3 + wE2
    vT3 =  lT*w2 + wE3
    
    
    alpha = alpha0-wE3/wE1
    
    #NOTE: beta & alphaTail are compensated for the tail motion induced by
    #omega @>@>
    betaTail = vT2/C.sqrt(vT1*vT1 + vT3*vT3)
    beta = wE2/C.sqrt(wE1*wE1 + wE3*wE3)
    alphaTail = alpha0-vT3/vT1
    outputs['alpha(deg)']=alpha*180/C.pi
    outputs['alphaTail(deg)']=alphaTail*180/C.pi
    outputs['beta(deg)']=beta*180/C.pi
    outputs['betaTail(deg)']=betaTail*180/C.pi
    
    # cL = cLA*alpha + cLe*elevator   + cL0
    # cD = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cDe*elevator + cDr*aileron + cD0
    # cR = -rD*w1 + cRB*beta + cRAB*alphaTail*beta + cRr*aileron
    # cP = cPA*alphaTail + cPe*elevator  + cP0
    # cY = cYB*beta + cYAB*alphaTail*beta
    
    cL_ = cLA*alpha + cLe*elevator + cL0
    cD_ = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cD0
    cR = -rD*w1 + cRB*betaTail + cRr*aileron + cRAB*alphaTail*betaTail
    cP = -pD*w2 + cPA*alphaTail + cPe*elevator + cP0
    cY = -yD*w3 + cYB*betaTail + cYAB*alphaTail*betaTail + cYr*rudder
    
    
    # LIFT :
    # ###############################
    cL = 0.2*cL_
    cD = 0.5*cD_
    outputs['cL'] = cL
    outputs['cD'] = cD

    fL1 =  rho*cL*eLe1*vKite/2.0
    fL2 =  rho*cL*eLe2*vKite/2.0
    fL3 =  rho*cL*eLe3*vKite/2.0
    
    # DRAG :
    # #############################
    fD1 = -rho*vKite*cD*we1/2.0
    fD2 = -rho*vKite*cD*we2/2.0 
    fD3 = -rho*vKite*cD*we3/2.0 
    
    
    
    # FORCES (AERO)
    # ###############################
    f1 = fL1 + fD1
    f2 = fL2 + fD2
    f3 = fL3 + fD3
    
    #f = f-fT
       
    # TORQUES (AERO)
    # ###############################
     
    t1 =  0.5*rho*vKite2*span*cR
    t2 =  0.5*rho*vKite2*chord*cP
    t3 =  0.5*rho*vKite2*span*cY
    return (f1, f2, f3, t1, t2, t3)
Example #46
0
    def formulateMPC(self):
        X = ca.MX.sym('X')
        Y = ca.MX.sym('Y')
        th = ca.MX.sym('th')
        self.e_c = ca.Function('e_c', [X, Y, th], [
            ca.sin(self.Phi(th)) * (X - self.cs_x(th)) - ca.cos(self.Phi(th)) *
            (Y - self.cs_y(th))
        ])
        self.e_l = ca.Function('e_l', [X, Y, th], [
            -ca.cos(self.Phi(th)) *
            (X - self.cs_x(th)) - ca.sin(self.Phi(th)) * (Y - self.cs_y(th))
        ])

        gp_in = ca.SX.sym('gp_in', 4, 1)
        mean = self.gp_dx.predict(gp_in)[0]
        f_mean_dx = ca.Function('f_mean_dx', [gp_in], [mean])
        mean = self.gp_dy.predict(gp_in)[0]
        f_mean_dy = ca.Function('f_mean_dy', [gp_in], [mean])
        gp_in2 = ca.SX.sym('gp_in2', 2, 1)
        mean = self.gp_dth.predict(gp_in2)[0]
        f_mean_dth = ca.Function('f_mean_dth', [gp_in2], [mean])

        x = ca.SX.sym('x')
        y = ca.SX.sym('y')
        th = ca.SX.sym('th')
        v = ca.SX.sym('v')
        delta = ca.SX.sym('delta')
        state = np.array([x, y, th])
        control = np.array([v, delta])
        rhs = np.array([
            f_mean_dx(np.array([np.cos(th), np.sin(th), v, delta])),
            f_mean_dy(np.array([np.cos(th), np.sin(th), v, delta])),
            f_mean_dth(np.array([v, delta]))
        ]) + state

        self.f_dyn = ca.Function('f_dyn', [state, control], [rhs])

        self.mpc_opti = ca.casadi.Opti()
        self.U = self.mpc_opti.variable(2, self.H)
        self.X = self.mpc_opti.variable(3, self.H + 1)
        self.TH = self.mpc_opti.variable(self.H + 1)
        self.NU = self.mpc_opti.variable(self.H)
        self.P_1 = self.mpc_opti.parameter(3)
        self.P_2 = self.mpc_opti.parameter(2)

        J = 0
        for k in range(self.H + 1):
            J += self.qc * self.e_c(self.X[0, k], self.X[
                1, k], self.TH[k])**2 + self.ql * self.e_l(
                    self.X[0, k], self.X[1, k], self.TH[k])**2
        for k in range(self.H - 1):
            J += self.Ru[0] * (self.U[0, k + 1] - self.U[0, k])**2 + self.Ru[
                1] * (self.U[1, k + 1] - self.U[1, k])**2 + self.Rv * (
                    self.NU[k + 1] - self.NU[k])**2

        J += -self.gamma * self.TH[-1]
        self.mpc_opti.minimize(J)

        for k in range(self.H):
            self.mpc_opti.subject_to(
                self.X[:, k + 1] == self.f_dyn(self.X[:, k], self.U[:, k]))
            self.mpc_opti.subject_to(self.TH[k + 1] == self.TH[k] +
                                     self.T * self.NU[k])

        self.mpc_opti.subject_to(0 <= self.TH)
        self.mpc_opti.subject_to(self.TH <= self.L)

        self.mpc_opti.subject_to(self.nu_min <= self.NU)
        self.mpc_opti.subject_to(self.NU <= self.nu_max)

        self.mpc_opti.subject_to(self.v_min <= self.U[0, :])
        self.mpc_opti.subject_to(self.U[0, :] <= self.v_max)
        self.mpc_opti.subject_to(self.delta_min <= self.U[1, :])
        self.mpc_opti.subject_to(self.U[1, :] <= self.delta_max)

        self.mpc_opti.subject_to(self.ramp_v_min <= self.U[0, 0] - self.P_2[0])
        self.mpc_opti.subject_to(self.U[0, 0] - self.P_2[0] <= self.ramp_v_max)
        self.mpc_opti.subject_to(
            self.ramp_delta_min <= self.U[1, 0] - self.P_2[1])
        self.mpc_opti.subject_to(
            self.U[1, 0] - self.P_2[1] <= self.ramp_delta_max)

        self.mpc_opti.subject_to(
            self.ramp_v_min <= self.U[0, 1:] - self.U[0, 0:-1])
        self.mpc_opti.subject_to(
            self.U[0, 1:] - self.U[0, 0:-1] <= self.ramp_v_max)
        self.mpc_opti.subject_to(
            self.ramp_delta_min <= self.U[1, 1:] - self.U[1, 0:-1])
        self.mpc_opti.subject_to(
            self.U[1, 1:] - self.U[1, 0:-1] <= self.ramp_delta_max)

        self.mpc_opti.subject_to(self.X[:, 0] == self.P_1[0:3])

        p_opts = {'verbose_init': False}
        s_opts = {'tol': 0.01, 'print_level': 0, 'max_iter': 100}
        self.mpc_opti.solver('ipopt', p_opts, s_opts)

        # Warm up
        self.mpc_opti.set_value(self.P_1, self.state)
        self.mpc_opti.set_value(self.P_2, self.input)

        sol = self.mpc_opti.solve()
        self.mpc_opti.set_initial(self.U, sol.value(self.U))
        self.mpc_opti.set_initial(self.X, sol.value(self.X))
        self.mpc_opti.set_initial(self.NU, sol.value(self.NU))
        self.mpc_opti.set_initial(self.TH, sol.value(self.TH))
def fc(u,t):
    assert u.numel()==1
    return u*sin(.3*t) - cos(.4*t)
Example #48
0
l     = 1.
mip   = 1.
mic   = 5.

# Declare variables (use scalar graph)
t  = ca.SX.sym( 't' )      # time
u  = ca.SX.sym( 'u' )      # control
x  = ca.SX.sym( 'x' , 4 )  # state

# ODE rhs function
#   x[0] = dtheta
#   x[1] = theta
#   x[2] = dx
#   x[3] = x

sint = ca.sin( x[1] )
cost = ca.cos( x[1] )

fric = mic * ca.sign( x[2] )
num  = g * sint + cost * ( -u - mp * l * x[0] * x[0] * sint + fric ) / ( mc + mp ) - mip * x[0] / ( mp * l )
denom = l * ( 4. / 3. - mp * cost*cost / (mc + mp) )
ddtheta = num / denom
ddx = (-u + mp * l * ( x[0] * x[0] * sint - ddtheta * cost ) - fric) / (mc + mp)

x_err = x-x_target
cost_mat = np.diag( [1,1,1,1] )

ode = ca.vertcat([	ddtheta, \
					x[0], \
					ddx, \
					x[2]
Example #49
0
def Max_velocity(pts, config, show=False):
    mu = config['car']['mu']
    m = config['car']['m']
    g = config['car']['g']
    l_f = config['car']['l_f']
    l_r = config['car']['l_r']
    safety_f = config['pp']['force_f']
    f_max = mu * m * g * safety_f
    f_long_max = l_f / (l_r + l_f) * f_max
    max_v = config['lims'][
        'max_v']  # parameter to be adapted so that optimiser isnt too fast
    max_a = config['lims']['max_a']

    s_i, th_i = convert_pts_s_th(pts)
    th_i_1 = th_i[:-1]
    s_i_1 = s_i[:-1]
    N = len(s_i)
    N1 = len(s_i) - 1

    # setup possible casadi functions
    d_x = ca.MX.sym('d_x', N - 1)
    d_y = ca.MX.sym('d_y', N - 1)
    vel = ca.Function('vel', [d_x, d_y],
                      [ca.sqrt(ca.power(d_x, 2) + ca.power(d_y, 2))])

    dx = ca.MX.sym('dx', N)
    dy = ca.MX.sym('dy', N)
    dt = ca.MX.sym('t', N - 1)
    f_long = ca.MX.sym('f_long', N - 1)
    f_lat = ca.MX.sym('f_lat', N - 1)

    nlp = {\
        'x': ca.vertcat(dx, dy, dt, f_long, f_lat),
        'f': ca.sum1(dt),
        'g': ca.vertcat(
                    # dynamic constraints
                    dt - s_i_1 / ((vel(dx[:-1], dy[:-1]) + vel(dx[1:], dy[1:])) / 2 ),
                    # ca.arctan2(dy, dx) - th_i,
                    dx/dy - ca.tan(th_i),
                    dx[1:] - (dx[:-1] + (ca.sin(th_i_1) * f_long + ca.cos(th_i_1) * f_lat) * dt  / m),
                    dy[1:] - (dy[:-1] + (ca.cos(th_i_1) * f_long - ca.sin(th_i_1) * f_lat) * dt  / m),

                    # path constraints
                    ca.sqrt(ca.power(f_long, 2) + ca.power(f_lat, 2)),

                    # boundary constraints
                    # dx[0], dy[0]
                ) \
    }

    S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}})
    # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})

    # make init sol
    v0 = np.ones(N) * max_v / 2
    dx0 = v0 * np.sin(th_i)
    dy0 = v0 * np.cos(th_i)
    dt0 = s_i_1 / ca.sqrt(ca.power(dx0[:-1], 2) + ca.power(dy0[:-1], 2))
    f_long0 = np.zeros(N - 1)
    ddx0 = dx0[1:] - dx0[:-1]
    ddy0 = dy0[1:] - dy0[:-1]
    a0 = (ddx0**2 + ddy0**2)**0.5
    f_lat0 = a0 * m

    x0 = ca.vertcat(dx0, dy0, dt0, f_long0, f_lat0)

    # make lbx, ubx
    # lbx = [-max_v] * N + [-max_v] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1
    lbx = [-max_v] * N + [0] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max
                                                                    ] * N1
    ubx = [max_v] * N + [max_v] * N + [10] * N1 + [f_long_max] * N1 + [f_max
                                                                       ] * N1

    #make lbg, ubg
    lbg = [0] * N1 + [0] * N + [0] * 2 * N1 + [0] * N1  #+ [0] * 2
    ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [f_max] * N1  #+ [0] * 2

    r = S(x0=x0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)

    x_opt = r['x']

    dx = np.array(x_opt[:N])
    dy = np.array(x_opt[N:N * 2])
    dt = np.array(x_opt[2 * N:N * 2 + N1])
    f_long = np.array(x_opt[2 * N + N1:2 * N + N1 * 2])
    f_lat = np.array(x_opt[-N1:])

    f_t = (f_long**2 + f_lat**2)**0.5

    # print(f"Dt: {dt.T}")
    # print(f"DT0: {dt[0]}")
    t = np.cumsum(dt)
    t = np.insert(t, 0, 0)
    # print(f"Dt: {dt.T}")
    # print(f"Dx: {dx.T}")
    # print(f"Dy: {dy.T}")

    vs = (dx**2 + dy**2)**0.5

    if show:
        plt.figure(1)
        plt.title("Velocity vs dt")
        plt.plot(t, vs)
        plt.plot(t, th_i)
        plt.legend(['vs', 'ths'])
        # plt.plot(t, dx)
        # plt.plot(t, dy)
        # plt.legend(['v', 'dx', 'dy'])
        plt.plot(t, np.ones_like(t) * max_v, '--')

        plt.figure(2)
        plt.title("F_long, F_lat vs t")
        plt.plot(t[:-1], f_long)
        plt.plot(t[:-1], f_lat)
        plt.plot(t[:-1], f_t, linewidth=3)
        plt.plot(t, np.ones_like(t) * f_max, '--')
        plt.plot(t, np.ones_like(t) * -f_max, '--')
        plt.plot(t, np.ones_like(t) * f_long_max, '--')
        plt.plot(t, np.ones_like(t) * -f_long_max, '--')

        plt.legend(['Flong', "f_lat", "f_t"])

        # plt.figure(3)
        # plt.title("Theta vs t")
        # plt.plot(t, th_i)
        # plt.plot(t, np.abs(th_i))

        # plt.figure(5)
        # plt.title(f"t vs dt")
        # plt.plot(t[1:], dt)
        # plt.plot(t[1:], dt, '+')

    # plt.figure(9)
    # plt.clf()
    # plt.title("F_long, F_lat vs t")
    # plt.plot(t[:-1], f_long)
    # plt.plot(t[:-1], f_lat)
    # plt.plot(t[:-1], f_t, linewidth=3)
    # plt.plot(t, np.ones_like(t) * f_max, '--')
    # plt.plot(t, np.ones_like(t) * -f_max, '--')
    # plt.plot(t, np.ones_like(t) * f_long_max, '--')
    # plt.plot(t, np.ones_like(t) * -f_long_max, '--')
    # plt.legend(['Flong', "f_lat", "f_t"])

    return vs
Example #50
0
def forcesTorques(state, u, p, outputs):
    rho =    1.23      #  density of the air             #  [ kg/m^3]
    rA = 1.085 #(dixit Kurt)
    alpha0 = -0*C.pi/180

    #TAIL LENGTH
    lT = 0.4

    #ROLL DAMPING
    rD = 1e-2
    pD = 0*1e-3
    yD = 0*1e-3

    #WIND-TUNNEL PARAMETERS
    #Lift (report p. 67)
    cLA = 5.064

    cLe = -1.924

    cL0 = 0.239

    #Drag (report p. 70)
    cDA = -0.195
    cDA2 = 4.268
    cDB2 = 5

#    cDe = 0.044
#    cDr = 0.111

    cD0 = 0.026

    #Roll (report p. 72)
    cRB = -0.062
    cRAB = -0.271
    cRr = -5.637e-1

    #Pitch (report p. 74)
    cPA = 0.293
    cPe = -4.9766e-1

    cP0 = 0.03

    #Yaw (report p. 76)
    cYB = 0.05
    cYAB = 0.229

    # rudder (fudged by Greg)
    cYr = 0.02

    span = 0.96
    chord = 0.1

    ###########     model integ ###################
    x =   state['x']
    y =   state['y']

    e11 = state['e11']
    e12 = state['e12']
    e13 = state['e13']

    e21 = state['e21']
    e22 = state['e22']
    e23 = state['e23']

    e31 = state['e31']
    e32 = state['e32']
    e33 = state['e33']

    dx  =  state['dx']
    dy  =  state['dy']
    dz  =  state['dz']

    w1  =  state['w1']
    w2  =  state['w2']
    w3  =  state['w3']

    delta = 0
    ddelta = 0

    aileron = u['aileron']
    elevator = u['elevator']
    rudder = u['rudder']

    ####### kinfile ######
    dpE = C.veccat( [ dx*e11 + dy*e12 + dz*e13 + ddelta*e12*rA + ddelta*e12*x - ddelta*e11*y
                    , dx*e21 + dy*e22 + dz*e23 + ddelta*e22*rA + ddelta*e22*x - ddelta*e21*y
                    , dx*e31 + dy*e32 + dz*e33 + ddelta*e32*rA + ddelta*e32*x - ddelta*e31*y
                    ])
    dp_carousel_frame = C.veccat( [ dx - ddelta*y
                                  , dy + ddelta*rA + ddelta*x
                                  , dz
                                  ]) - C.veccat([C.cos(delta)*p['wind_x'],C.sin(delta)*p['wind_x'],0])

    ##### more model_integ ###########
    # EFFECTIVE WIND IN THE KITE`S SYSTEM :
    # ###############################

    #Airfoil speed in carousel frame
    we1 = dp_carousel_frame[0]
    we2 = dp_carousel_frame[1]
    we3 = dp_carousel_frame[2]

    vKite2 = C.mul(dp_carousel_frame.trans(), dp_carousel_frame) #Airfoil speed^2
    vKite = C.sqrt(vKite2) #Airfoil speed
    outputs['airspeed'] = vKite

    # CALCULATION OF THE FORCES :
    # ###############################
    #
    #   FORCE ARE COMPUTED IN THE CAROUSEL FRAME @>@>@>

    #Aero coeff.


    # LIFT DIRECTION VECTOR
    # ############

    # Relative wind speed in Airfoil's referential 'E'

    wE1 = dpE[0]
    wE2 = dpE[1]
    wE3 = dpE[2]

    # Airfoil's transversal axis in carousel referential 'e'
    eTe1 = e21
    eTe2 = e22
    eTe3 = e23


    # Lift axis ** Normed to we @>@> **
    eLe1 = - eTe2*we3 + eTe3*we2
    eLe2 = - eTe3*we1 + eTe1*we3
    eLe3 = - eTe1*we2 + eTe2*we1


    # AERODYNAMIC COEEFICIENTS
    # #################
    vT1 =          wE1
    vT2 = -lT*w3 + wE2
    vT3 =  lT*w2 + wE3


    alpha = alpha0-wE3/wE1

    #NOTE: beta & alphaTail are compensated for the tail motion induced by
    #omega @>@>
    betaTail = vT2/C.sqrt(vT1*vT1 + vT3*vT3)
    beta = wE2/C.sqrt(wE1*wE1 + wE3*wE3)
    alphaTail = alpha0-vT3/vT1
    outputs['alpha(deg)']=alpha*180/C.pi
    outputs['alphaTail(deg)']=alphaTail*180/C.pi
    outputs['beta(deg)']=beta*180/C.pi
    outputs['betaTail(deg)']=betaTail*180/C.pi

    # cL = cLA*alpha + cLe*elevator   + cL0
    # cD = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cDe*elevator + cDr*aileron + cD0
    # cR = -rD*w1 + cRB*beta + cRAB*alphaTail*beta + cRr*aileron
    # cP = cPA*alphaTail + cPe*elevator  + cP0
    # cY = cYB*beta + cYAB*alphaTail*beta

    cL_ = cLA*alpha + cLe*elevator + cL0
    cD_ = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cD0
    cR = -rD*w1 + cRB*betaTail + cRr*aileron + cRAB*alphaTail*betaTail
    cP = -pD*w2 + cPA*alphaTail + cPe*elevator + cP0
    cY = -yD*w3 + cYB*betaTail + cYAB*alphaTail*betaTail + cYr*rudder


    # LIFT :
    # ###############################
    cL = 0.2*cL_
    cD = 0.5*cD_
    outputs['cL'] = cL
    outputs['cD'] = cD

    fL1 =  rho*cL*eLe1*vKite/2.0
    fL2 =  rho*cL*eLe2*vKite/2.0
    fL3 =  rho*cL*eLe3*vKite/2.0

    # DRAG :
    # #############################
    fD1 = -rho*vKite*cD*we1/2.0
    fD2 = -rho*vKite*cD*we2/2.0
    fD3 = -rho*vKite*cD*we3/2.0



    # FORCES (AERO)
    # ###############################
    f1 = fL1 + fD1
    f2 = fL2 + fD2
    f3 = fL3 + fD3

    #f = f-fT

    # TORQUES (AERO)
    # ###############################

    t1 =  0.5*rho*vKite2*span*cR
    t2 =  0.5*rho*vKite2*chord*cP
    t3 =  0.5*rho*vKite2*span*cY
    return (f1, f2, f3, t1, t2, t3)
Example #51
0
def MinCurvatureTrajectory(track, obs_map=None):
    w_min = -track[:, 4] * 0.9
    w_max = track[:, 5] * 0.9
    nvecs = track[:, 2:4]
    th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]

    xgrid = np.arange(0, obs_map.shape[1])
    ygrid = np.arange(0, obs_map.shape[0])

    data_flat = np.array(obs_map).ravel(order='F')

    lut = ca.interpolant('lut', 'bspline', [xgrid, ygrid], data_flat)

    print(lut([10, 20]))

    n_max = 3
    N = len(track)

    n_f_a = ca.MX.sym('n_f', N)
    n_f = ca.MX.sym('n_f', N - 1)
    th_f = ca.MX.sym('n_f', N - 1)

    x0_f = ca.MX.sym('x0_f', N - 1)
    x1_f = ca.MX.sym('x1_f', N - 1)
    y0_f = ca.MX.sym('y0_f', N - 1)
    y1_f = ca.MX.sym('y1_f', N - 1)
    th1_f = ca.MX.sym('y1_f', N - 1)
    th2_f = ca.MX.sym('y1_f', N - 1)
    th1_f1 = ca.MX.sym('y1_f', N - 2)
    th2_f1 = ca.MX.sym('y1_f', N - 2)

    o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
    o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
    o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
    o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])

    dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
                      [ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])

    track_length = ca.Function('length', [n_f_a], [
        dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
            o_y_e(n_f_a[1:]))
    ])

    real = ca.Function(
        'real', [th1_f, th2_f],
        [ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
    im = ca.Function(
        'im', [th1_f, th2_f],
        [-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])

    sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
                            [ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])

    get_th_n = ca.Function(
        'gth', [th_f],
        [sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
    d_n = ca.Function('d_n', [n_f_a, th_f],
                      [track_length(n_f_a) / ca.tan(get_th_n(th_f))])

    # objective
    real1 = ca.Function(
        'real1', [th1_f1, th2_f1],
        [ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
    im1 = ca.Function(
        'im1', [th1_f1, th2_f1],
        [-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])

    sub_cmplx1 = ca.Function(
        'a_cpx1', [th1_f1, th2_f1],
        [ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])

    # define symbols
    n = ca.MX.sym('n', N)
    th = ca.MX.sym('th', N - 1)

    nlp = {\
    'x': ca.vertcat(n, th),
    'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
    # 'f': ca.sumsqr(track_length(n)),
    'g': ca.vertcat(
                # dynamic constraints
                n[1:] - (n[:-1] + d_n(n, th)),
                # lut(ca.horzcat(o_x_s(n[:-1]), o_y_s(n[:-1])).T).T,

                # boundary constraints
                n[0], #th[0],
                n[-1], #th[-1],
            ) \

    }

    S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}})
    # S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})

    ones = np.ones(N)
    n0 = ones * 0

    th0 = []
    for i in range(N - 1):
        th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
        th0.append(th_00)

    th0 = np.array(th0)

    x0 = ca.vertcat(n0, th0)

    # lbx = [-n_max] * N + [-np.pi]*(N-1)
    # ubx = [n_max] * N + [np.pi]*(N-1)
    lbx = list(w_min) + [-np.pi] * (N - 1)
    ubx = list(w_max) + [np.pi] * (N - 1)

    r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)

    x_opt = r['x']

    n_set = np.array(x_opt[:N])
    thetas = np.array(x_opt[1 * N:2 * (N - 1)])

    # lib.plot_race_line(np.array(track), n_set, wait=True)

    return n_set