def configure_model(self):
        """
        Create state and input variables, as well as describe the vehicle's physical properties by using approximative \
        motion's equations.
        """
        self._model = Model("continuous")

        # Create state and input variables
        self._position_x = self._model.set_variable(var_type="_x",
                                                    var_name="position_x",
                                                    shape=(1, 1))
        self._position_y = self._model.set_variable(var_type="_x",
                                                    var_name="position_y",
                                                    shape=(1, 1))
        self._heading_angle = self._model.set_variable(
            var_type="_x", var_name="heading_angle", shape=(1, 1))
        self._velocity = self._model.set_variable(var_type="_u",
                                                  var_name="velocity",
                                                  shape=(1, 1))
        self._steering_angle = self._model.set_variable(
            var_type="_u", var_name="steering_angle", shape=(1, 1))

        # Next state equations - these mathematical formulas are the core of the model
        slip_factor = casadi.arctan(LR * casadi.tan(self._steering_angle) /
                                    WHEELBASE_LENGTH)
        self._model.set_rhs(
            "position_x",
            self._velocity * casadi.cos(self._heading_angle + slip_factor))
        self._model.set_rhs(
            "position_y",
            self._velocity * casadi.sin(self._heading_angle + slip_factor))
        self._model.set_rhs(
            "heading_angle",
            self._velocity * casadi.tan(self._steering_angle) *
            casadi.cos(slip_factor) / WHEELBASE_LENGTH)
Ejemplo n.º 2
0
def bicycle_robot_model(q, u, L=0.3, dt=0.01):
    """
    Implements the discrete time dynamics of your robot.
    i.e. this function implements F in
    q_{t+1} = F(q_{t}, u_{t})
    dt is the discretization timestep.
    L is the axel-to-axel length of the car.
    q = array of casadi MX.sym symbolic variables [x, y, theta, phi].
    u = array of casadi MX.sym symbolic variables [u1, u2] (velocity and steering inputs).
    Use the casadi sin, cos, tan functions.
    The casadi vertcat or vcat functions may also be useful. Note that to turn a list of
    casadi expressions into a column vector of those expressions, you should use vertcat or
    vcat. vertcat takes as input a variable number of arguments, and vcat takes as input a list.
    Example:
        x = MX.sym('x')
        y = MX.sym('y')
        z = MX.sym('z')
        q = vertcat(x + y, y, z) # makes a 3x1 matrix with entries x + y, y, and z.
        # OR
        q = vcat([x + y, y, z]) # makes a 3x1 matrix with entries x + y, y, and z.
    """
    x, y, theta, sigma = q[0], q[1], q[2], q[3]
    v, w = u[0], u[1]
    x_dot = v * cos(theta)
    y_dot = v * sin(theta)
    theta_dot = v * tan(sigma) / L
    sigma_dot = w

    result = vertcat(x + x_dot * dt, y + y_dot * dt, theta + theta_dot * dt,
                     sigma + sigma_dot * dt)
    return result
        def proc_model(self):

                g1 = c.MX.zeros(self.nx,self.nx)
                #assigning zeros
                # for i in range(self.nx):
                #         for j in range(self.nx):
                #                 g1[i,j] = 0

                g1[0,3] = 1; g1[1,4] = 1; g1[2,5] = 1;
                g1[6,9] = 1; g1[6,10] = c.sin(self.x[6])*c.tan(self.x[7]); g1[6,11] = c.cos(self.x[6])*c.tan(self.x[7]);
                g1[7,10] = c.cos(self.x[6]); g1[7,11] = -c.sin(self.x[6]);
                g1[8,10] = c.sin(self.x[6])/c.cos(self.x[7]); g1[8,11] = c.cos(self.x[6])/c.cos(self.x[7]);

                g2 = c.MX.zeros(self.nx,self.nu)

                g2[3,0] = (c.cos(self.x[8])*c.sin(self.x[7])*c.cos(self.x[6]) + c.sin(self.x[8])*c.sin(self.x[6]))/self.m
                g2[4,0] = (c.sin(self.x[8])*c.sin(self.x[7])*c.cos(self.x[6]) - c.cos(self.x[8])*c.sin(self.x[6]))/self.m
                g2[5,0] = c.cos(self.x[7])*c.cos(self.x[6])/self.m
                g2[9:,1:] = c.inv(self.Ic)

                g3 = c.MX.zeros(self.nx,1)

                f =  c.Function('f',[self.x,self.u],[self.x + c.mtimes(g1,self.x)*self.dt + c.mtimes(g2,self.u)*self.dt - g3*self.g*self.dt])

                A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.x)])

                B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.u)])

                return f,A, B
Ejemplo n.º 4
0
 def exp(cls, v):
     assert v.shape == (3, 1) or v.shape == (3, )
     angle = ca.norm_2(v)
     res = ca.SX(4, 1)
     res[:3] = ca.tan(angle / 4) * v / angle
     res[3] = 0
     return ca.if_else(angle > eps, res, ca.SX([0, 0, 0, 0]))
    def proc_model(self):

        # f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
        #                                         self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
        #                                         self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
        #                                         self.x[3] + self.u[1]*self.dt])

        g = c.MX(self.nx,self.nu)
        g[0,0] = c.cos(self.x[2]); g[0,1] = 0;
        g[1,0] = c.sin(self.x[2]); g[1,1] = 0;
        g[2,0] = c.tan(self.x[3])/self.length; g[2,1] = 0
        g[3,0] = 0; g[3,1] = 1;

        # f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
        #                                         self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
        #                                         self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
        #                                         self.x[3] + self.u[1]*self.dt])

        f =  c.Function('f',[self.x,self.u],[self.x + c.mtimes(g,self.u)*self.dt])

        # A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.x),
        #                                         c.jacobian(f(self.x,self.u)[1],self.x),
        #                                         c.jacobian(f(self.x,self.u)[2],self.x),
        #                                         c.jacobian(f(self.x,self.u)[3],self.x)]) #linearization

        # B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.u),
        #                                         c.jacobian(f(self.x,self.u)[1],self.u),
        #                                         c.jacobian(f(self.x,self.u)[2],self.u),
        #                                         c.jacobian(f(self.x,self.u)[3],self.u)])

        A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.x)])

        B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.u)])

        return f,A, B
Ejemplo n.º 6
0
    def __init__(self, N=30, step=0.01):
        # We construct the model as a set of differential-algebraic equations (DAE)
        self.dae = casadi.DaeBuilder()

        # Parameters
        self.n = 4
        self.m = 2  # controls
        self.N = N
        self.step = step
        self.T = N * step  # Time horizon (seconds)
        self.u_upper = 2.5
        self.fixed_points = dict()  # None yet

        # Constants
        self.lr = 2.10  # distance from CG to back wheel in meters
        self.lf = 2.67
        # Source, page 40: https://www.diva-portal.org/smash/get/diva2:860675/FULLTEXT01.pdf

        # States
        z = self.dae.add_x('z', self.n)
        x = z[0]
        y = z[1]
        v = z[2]
        psi = z[3]

        self.STATE_NAMES = [
            'x',
            'y',
            'v',
            'psi',
        ]

        # Controls
        u = self.dae.add_u('u', self.m)  # acceleration
        a = u[0]
        delta_f = u[1]  # front steering angle

        # This is a weird "state".
        beta = casadi.arctan(self.lr / (self.lr + self.lf) *
                             casadi.tan(delta_f))

        self.CONTROL_NAMES = ['a', 'delta_f']

        # Define ODEs
        xdot = v * casadi.cos(psi + beta)
        ydot = v * casadi.sin(psi + beta)
        vdot = a
        psidot = v / self.lr * casadi.sin(beta)

        zdot = casadi.vertcat(xdot, ydot, vdot, psidot)
        self.dae.add_ode('zdot', zdot)

        # Customize Matplotlib:
        mpl.rcParams['font.size'] = 18
        mpl.rcParams['lines.linewidth'] = 3
        mpl.rcParams['axes.grid'] = True

        self.state_estimate = None
        self.control_estimate = np.zeros((self.m, self.N))
Ejemplo n.º 7
0
    def _add_constraints(self):
        # State Bound Constraints
        self.opti.subject_to(
            self.opti.bounded(self.V_MIN, self.v_dv, self.V_MAX))

        # Initial State Constraint
        self.opti.subject_to(self.x_dv[0] == self.z_curr[0])
        self.opti.subject_to(self.y_dv[0] == self.z_curr[1])
        self.opti.subject_to(self.psi_dv[0] == self.z_curr[2])
        self.opti.subject_to(self.v_dv[0] == self.z_curr[3])

        # State Dynamics Constraints
        for i in range(self.N):
            beta = casadi.atan(self.L_R / (self.L_F + self.L_R) *
                               casadi.tan(self.df_dv[i]))
            self.opti.subject_to(
                self.x_dv[i + 1] == self.x_dv[i] + self.DT *
                (self.v_dv[i] * casadi.cos(self.psi_dv[i] + beta)))
            self.opti.subject_to(
                self.y_dv[i + 1] == self.y_dv[i] + self.DT *
                (self.v_dv[i] * casadi.sin(self.psi_dv[i] + beta)))
            self.opti.subject_to(self.psi_dv[i +
                                             1] == self.psi_dv[i] + self.DT *
                                 (self.v_dv[i] / self.L_R * casadi.sin(beta)))
            self.opti.subject_to(self.v_dv[i + 1] == self.v_dv[i] + self.DT *
                                 (self.acc_dv[i]))

        # Input Bound Constraints
        self.opti.subject_to(
            self.opti.bounded(self.A_MIN, self.acc_dv, self.A_MAX))
        self.opti.subject_to(
            self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX))

        # Input Rate Bound Constraints
        self.opti.subject_to(
            self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[0],
                              self.acc_dv[0] - self.u_prev[0],
                              self.A_DOT_MAX + self.sl_acc_dv[0]))

        self.opti.subject_to(
            self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[0],
                              self.df_dv[0] - self.u_prev[1],
                              self.DF_DOT_MAX + self.sl_df_dv[0]))

        for i in range(self.N - 1):
            self.opti.subject_to(
                self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[i + 1],
                                  self.acc_dv[i + 1] - self.acc_dv[i],
                                  self.A_DOT_MAX + self.sl_acc_dv[i + 1]))
            self.opti.subject_to(
                self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[i + 1],
                                  self.df_dv[i + 1] - self.df_dv[i],
                                  self.DF_DOT_MAX + self.sl_df_dv[i + 1]))
        # Other Constraints
        self.opti.subject_to(0 <= self.sl_df_dv)
        self.opti.subject_to(0 <= self.sl_acc_dv)
Ejemplo n.º 8
0
def incidence_angle_function(latitude, day_of_year, time, scattering=False):
    """
    What is the fraction of insolation that a horizontal surface will receive as a function of sun position in the sky?
    :param latitude: Latitude [degrees]
    :param day_of_year: Julian day (1 == Jan. 1, 365 == Dec. 31)
    :param time: Time since (local) solar noon [seconds]
    :param scattering: Boolean: include scattering effects at very low angles?
    """
    # Old description:
    # To first-order, this is true. In class, Kevin Uleck claimed that you have higher-than-cosine losses at extreme angles,
    # since you get reflection losses. However, an experiment by Sharma appears to not reproduce this finding, showing only a
    # 0.4-percentage-point drop in cell efficiency from 0 to 60 degrees. So, for now, we'll just say it's a cosine loss.
    # Sharma: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6611928/

    elevation_angle = solar_elevation_angle(latitude, day_of_year, time)

    theta = 90 - elevation_angle  # Angle between panel normal and the sun, in degrees
    cosine_factor = cosd(theta)

    if not scattering:
        return cosine_factor
    else:
        # Calculate scattering knockdown (See C:\Users\User\Google Drive\School\Grad School\2020 Spring\16-885\Solar Panel Scattering Rough Fit)
        theta_rad = theta * cas.pi / 180
        # Model 1
        c = (0.27891510500505767300438719757949,
             -0.015994330894744987481281839336589,
             -19.707332432605799255043166340329,
             -0.66260979582573353852126274432521)
        scattering_factor = c[0] + c[3] * theta_rad + cas.exp(
            c[1] * (cas.tan(theta_rad - 1e-8) + c[2] * theta_rad))
        # Model 2
        # c = (
        #     -0.04636,
        #     -0.3171
        # )
        # scattering_factor = cas.exp(
        #     c[0] * (
        #         cas.tan(theta_rad-1e-8) + c[1] * theta_rad
        #     )
        # )
        # Model 3
        # p1 = -21.74
        # p2 = 282.6
        # p3 = -1538
        # p4 = 1786
        # q1 = -923.2
        # q2 = 1456
        # x = theta_rad
        # scattering_factor = ((p1*x**3 + p2*x**2 + p3*x + p4) /
        #            (x**2 + q1*x + q2))

        # scattering_factor = cas.fmin(cas.fmax(scattering_factor, 0), 1)

        return cosine_factor * scattering_factor
Ejemplo n.º 9
0
 def omegaToRpyDot(self, rpy, omega):
     secant_pitch = (1.0 / cs.cos(rpy[1]))
     c_roll = cs.cos(rpy[0])
     s_roll = cs.sin(rpy[0])
     tan_pitch = cs.tan(rpy[1])
     c0 = cs.DM([1, 0, 0])
     c1 = cs.vertcat(s_roll * tan_pitch, c_roll, s_roll * secant_pitch)
     c2 = cs.vertcat(c_roll * tan_pitch, -s_roll, c_roll * secant_pitch)
     Momega_to_rpydot = cs.horzcat(c0, c1, c2)
     rpy_dot = cs.mtimes(Momega_to_rpydot, omega)
     return rpy_dot
Ejemplo n.º 10
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
Ejemplo n.º 11
0
def scattering_factor(elevation_angle):
    """
    Calculates a scattering factor (a factor that gives losses due to atmospheric scattering at low elevation angles).
    Source: AeroSandbox/studies/SolarPanelScattering
    :param elevation_angle: Angle between the horizon and the sun [degrees]
    :return: Fraction of the light that is not lost to scattering.
    """
    elevation_angle = cas.fmin(cas.fmax(elevation_angle, 0), 90)
    theta = 90 - elevation_angle  # Angle between panel normal and the sun, in degrees
    theta_rad = theta * cas.pi / 180

    # # Model 1
    # c = (
    #     0.27891510500505767300438719757949,
    #     -0.015994330894744987481281839336589,
    #     -19.707332432605799255043166340329,
    #     -0.66260979582573353852126274432521
    # )
    # scattering_factor = c[0] + c[3] * theta_rad + cas.exp(
    #     c[1] * (
    #             cas.tan(theta_rad) + c[2] * theta_rad
    #     )
    # )

    # Model 2
    c = (
        -0.04636,
        -0.3171
    )
    scattering_factor = cas.exp(
        c[0] * (
                cas.tan(theta_rad * 0.999) + c[1] * theta_rad
        )
    )

    # # Model 3
    # p1 = -21.74
    # p2 = 282.6
    # p3 = -1538
    # p4 = 1786
    # q1 = -923.2
    # q2 = 1456
    # x = theta_rad
    # scattering_factor = ((p1*x**3 + p2*x**2 + p3*x + p4) /
    #            (x**2 + q1*x + q2))

    # Keep this:
    # scattering_factor = cas.fmin(cas.fmax(scattering_factor, 0), 1)
    return scattering_factor
	def _add_constraints(self):
		## State Bound Constraints
		self.opti.subject_to( self.opti.bounded(self.EY_MIN, self.ey_dv, self.EY_MAX) )
		self.opti.subject_to( self.opti.bounded(self.EPSI_MIN, self.epsi_dv, self.EPSI_MAX) )

		## Initial State Constraint
		self.opti.subject_to( self.s_dv[0]    == self.z_curr[0] )   
		self.opti.subject_to( self.ey_dv[0]   == self.z_curr[1] )  
		self.opti.subject_to( self.epsi_dv[0] == self.z_curr[2] )  
		self.opti.subject_to( self.v_dv[0]    == self.z_curr[3] )   
		
		## State Dynamics Constraints
		for i in range(self.N):
			beta = casadi.atan( self.L_R / (self.L_F + self.L_R) * casadi.tan(self.df_dv[i]) )
			dyawdt = self.v_dv[i] / self.L_R * casadi.sin(beta)
			dsdt = self.v_dv[i] * casadi.cos(self.epsi_dv[i]+beta) / (1 - self.ey_dv[i] * self.curv_ref[i] )   
			ay   = self.v_dv[i] * dyawdt
			
			self.opti.subject_to( self.s_dv[i+1]    == self.s_dv[i]    + self.DT * (dsdt) )  
			self.opti.subject_to( self.ey_dv[i+1]   == self.ey_dv[i]   + self.DT * (self.v_dv[i] * casadi.sin(self.epsi_dv[i] + beta)) ) 
			self.opti.subject_to( self.epsi_dv[i+1] == self.epsi_dv[i] + self.DT * (dyawdt - dsdt * self.curv_ref[i]) )
			self.opti.subject_to( self.v_dv[i+1]    == self.v_dv[i]    + self.DT * (self.acc_dv[i]) )

			self.opti.subject_to( self.opti.bounded(self.AY_MIN - self.sl_ay_dv[i], 
				                                    ay,
				                                    self.AY_MAX + self.sl_ay_dv[i]) )		
            
		## Input Bound Constraints
		self.opti.subject_to( self.opti.bounded(self.AX_MIN, self.acc_dv, self.AX_MAX) )
		self.opti.subject_to( self.opti.bounded(self.DF_MIN, self.df_dv,  self.DF_MAX) )

		# Input Rate Bound Constraints
		self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT, 
			                                     self.acc_dv[0] - self.u_prev[0],
			                                     self.AX_DOT_MAX*self.DT) )

		self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT, 
			                                     self.df_dv[0] - self.u_prev[1],
			                                     self.DF_DOT_MAX*self.DT) )

		for i in range(self.N - 1):
			self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT, 
				                                     self.acc_dv[i+1] - self.acc_dv[i],
				                                     self.AX_DOT_MAX*self.DT) )
			self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT, 
				                                     self.df_dv[i+1]  - self.df_dv[i],
				                                     self.DF_DOT_MAX*self.DT) )
		# Other Constraints
		self.opti.subject_to( 0 <= self.sl_ay_dv ) # nonnegative lateral acceleration slack variable
Ejemplo n.º 13
0
def CL_over_Cl(AR, mach=0, sweep=0):
    """
    Returns the ratio of 3D lift_force coefficient (with compressibility) to 2D lift_force (incompressible) coefficient.
    :param AR: Aspect ratio
    :param mach: Mach number
    :param sweep: Sweep angle [deg]
    :return:
    """
    beta = cas.sqrt(1 - mach**2)
    sweep_rad = sweep * np.pi / 180
    # return AR / (AR + 2) # Equivalent to equation in Drela's FVA in incompressible, 2*pi*alpha limit.
    # return AR / (2 + cas.sqrt(4 + AR ** 2))  # more theoretically sound at low AR
    eta = 0.95
    return AR / (2 + cas.sqrt(4 + (AR * beta / eta)**2 *
                              (1 + (cas.tan(sweep_rad) / beta)**2))
                 )  # From Raymer, Sect. 12.4.1; citing DATCOM
def update(X, U):
    """
    X contains  [x,y,theta] in a column
    U contains  [v,s] in a column
    """
    x = X[0]
    y = X[1]
    theta = X[2]
    v = U[0]
    s = U[1]

    dt = 0.1  #update time
    L, _ = robot_params()
    #kinematics
    x_new = x + v * casadi.cos(theta) * dt
    y_new = y + v * casadi.sin(theta) * dt
    theta_new = theta + ((v * casadi.tan(s)) / L) * dt

    #lets make it into a casadi thing
    X_new = casadi.blockcat([[x_new], [y_new], [theta_new]])
    return X_new
Ejemplo n.º 15
0
def T_rpy(roll, pitch, yaw):
    cr = cs.cos(roll)
    sr = cs.sin(roll)
    cp = cs.cos(pitch)
    tp = cs.tan(pitch)
    sp = cs.sin(pitch)
    cy = cs.cos(yaw)
    sy = cs.sin(yaw)
    T = cs.SX.zeros(3, 3)
    T[0, 0], T[0, 1], T[0, 2] = 1, sr * tp, cr * tp
    T[1, 0], T[1, 1], T[1, 2] = 0, cr, -sr
    T[2, 0], T[2, 1], T[2, 2] = 0, sr / cp, cr / cp
    # T[0,1] = sr*tp
    # T[0,2] = cr*tp
    # T[1,0] = 0
    # T[1,1] = cr
    # T[1,2] = -sr
    # T[2,0] = 0
    # T[2,1] = sr/cp
    # T[2,2] = cr/cp

    return T
    def f_ct(self, x, u, type='casadi'):
        if type == 'casadi':
            beta = lambda d: ca.atan2(self.L_r * ca.tan(d), self.L_r + self.L_f
                                      )

            x_dot = ca.vcat([
                x[3] * ca.cos(x[2] + beta(u[0])),
                x[3] * ca.sin(x[2] + beta(u[0])),
                x[3] * ca.sin(beta(u[0])) / self.L_r, u[1]
            ])
        elif type == 'numpy':
            beta = lambda d: np.arctan2(self.L_r * np.tan(d), self.L_r + self.
                                        L_f)

            x_dot = np.array([
                x[3] * np.cos(x[2] + beta(u[0])),
                x[3] * np.sin(x[2] + beta(u[0])),
                x[3] * np.sin(beta(u[0])) / self.L_r, u[1]
            ])
        else:
            raise RuntimeError('Dynamics type %s not recognized' % type)

        return x_dot
Ejemplo n.º 17
0
    def __init__(self, ntrailers):
        nstates = 3 + ntrailers
        x = ca.SX.sym('x')
        y = ca.SX.sym('y')
        phi = ca.SX.sym('phi')
        thetas = ca.SX.sym('theta', ntrailers)
        u = ca.SX.sym('u', 2)

        self.ntrailers = ntrailers
        self.state = ca.vertcat(x, y, phi, thetas)
        self.x = x
        self.y = y
        self.phi = phi
        self.thetas = thetas
        self.u = u

        g1 = ca.SX.zeros((nstates, 1))
        g1[0] = ca.cos(thetas[0])
        g1[1] = ca.sin(thetas[0])

        if ntrailers > 0:
            g1[3] = 1 * ca.tan(phi)

        for i in range(1, ntrailers):
            fun = lambda j: ca.cos(thetas[j - 1] - thetas[j])
            g1[3 +
               i] = product(fun, 1, i - 1) * ca.sin(thetas[i - 1] - thetas[i])

        g2 = ca.SX.zeros((nstates, 1))
        g2[2] = 1

        self.g = ca.horzcat(g1, g2)
        self.f = ca.SX.zeros((nstates, 1))
        self.rhs = ca.Function('RHS',
                               [x, y, phi] + thetas.elements() + u.elements(),
                               [self.g @ u])
Ejemplo n.º 18
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))]
    # sls = np.sqrt(np.sum(np.power(np.diff(track[:, :2], axis=0), 2), axis=1))

    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_a = ca.MX.sym('n_f', N)
    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.atan(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]))])

    real1 = ca.Function(
        'real', [th1_f1, th2_f1],
        [ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
    im1 = ca.Function(
        'im', [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_cpx', [th1_f1, th2_f1],
        [ca.atan(im1(th1_f1, th2_f1) / real1(th1_f1, th2_f1))])

    d_n = ca.Function('d_n', [n_f_a, th_f],
                      [track_length(n_f_a) / ca.tan(get_th_n(th_f))])
    # curvature = ca.Function('curv', [th_f_a], [th_f_a[1:] - th_f_a[:-1]])
    grad = ca.Function(
        'grad', [n_f_a],
        [(o_y_e(n_f_a[1:]) - o_y_s(n_f_a[:-1])) /
         ca.fmax(o_x_e(n_f_a[1:]) - o_x_s(n_f_a[:-1]), 0.1 * np.ones(N - 1))])
    curvature = ca.Function(
        'curv', [n_f_a],
        [grad(n_f_a)[1:] - grad(n_f_a)[:-1]])  # changes in grad

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


    nlp = {\
    'x': ca.vertcat(n, th),
    # 'f': ca.sumsqr(curvature(n)),
    # 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
    'f': ca.sumsqr(sub_cmplx1(sub_cmplx1(th[2:], th[1:-1]), sub_cmplx1(th[1:-1], th[:-2]))),
    # 'f': ca.sumsqr(track_length(n)),
    '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

    print(curvature(n0))

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

    print(f"Solution found")
    x_opt = r['x']

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

    # print(sub_cmplx(thetas[1:], thetas[:-1]))

    plt.figure(2)
    th_data = sub_cmplx(thetas[1:], thetas[:-1])
    plt.plot(th_data)
    plt.plot(ca.fabs(th_data), '--')
    plt.pause(0.001)

    plt.figure(3)
    plt.plot(abs(thetas), '--')
    plt.plot(thetas)
    plt.pause(0.001)

    plt.figure(4)
    plt.plot(sub_cmplx(th0[1:], th0[:-1]))
    plt.pause(0.001)

    plot_race_line(np.array(track), n_set, width=3, wait=True)
    def compute_mprim(state_i, lattice, L, v, u_max, print_level):
        N = 75
        nx = 3
        Nc = 3
        mprim = []
        for state_f in lattice:
            x = ca.MX.sym('x', nx)
            u = ca.MX.sym('u')

            F = ca.Function('f', [x, u],
                            [v*ca.cos(x[2]), v*ca.sin(x[2]), v*ca.tan(u)/L])

            opti = ca.Opti()
            X = opti.variable(nx, N+1)
            pos_x = X[0, :]
            pos_y = X[1, :]
            ang_th = X[2, :]
            U = opti.variable(N, 1)
            T = opti.variable(1)
            dt = T/N

            opti.set_initial(T, 0.1)
            opti.set_initial(U, 0.0*np.ones(N))
            opti.set_initial(pos_x, np.linspace(state_i[0], state_f[0], N+1))
            opti.set_initial(pos_y, np.linspace(state_i[1], state_f[1], N+1))

            tau = ca.collocation_points(Nc, 'radau')
            C, _ = ca.collocation_interpolators(tau)

            Xc_vec = []
            for k in range(N):
                Xc = opti.variable(nx, Nc)
                Xc_vec.append(Xc)
                X_kc = ca.horzcat(X[:, k], Xc)
                for j in range(Nc):
                    fo = F(Xc[:, j], U[k])
                    opti.subject_to(X_kc@C[j+1] == dt*ca.vertcat(fo[0], fo[1], fo[2]))

                opti.subject_to(X_kc[:, Nc] == X[:, k+1]) 

            for k in range(N):
                opti.subject_to(U[k] <= u_max)
                opti.subject_to(-u_max <= U[k])

            opti.subject_to(T >= 0.001)
            opti.subject_to(X[:, 0] == state_i)
            opti.subject_to(X[:, -1] == state_f)

            alpha = 1e-2
            opti.minimize(T + alpha*ca.sumsqr(U))

            opti.solver('ipopt', {'expand': True},
                        {'tol': 10**-8, 'print_level': print_level})
            sol = opti.solve()

            pos_x_opt = sol.value(pos_x)
            pos_y_opt = sol.value(pos_y)
            ang_th_opt = sol.value(ang_th)
            u_opt = sol.value(U)
            T_opt = sol.value(T)
            mprim.append({'x': pos_x_opt, 'y': pos_y_opt, 'th': ang_th_opt,
                          'u': u_opt, 'T': T_opt, 'ds': T_opt*v})
        return np.array(mprim)
Ejemplo n.º 20
0
    def quad_nl_euler_dynamics(self, x, u, *_):
        """
        Pendulum nonlinear dynamics based on euler angle description.

        :param x: state
        :type x: casadi.DM or casadi.MX
        :param u: control input
        :type u: casadi.DM or casadi.MX
        :return: state time derivative
        :rtype: casadi.DM or casadi.MX, depending on inputs
        """

        x1 = x[X]  # x
        x2 = x[Y]  # y
        x3 = x[Z]  # z

        x4 = x[VX]  # Vx
        x5 = x[VY]  # Vy
        x6 = x[VZ]  # Vz

        x7 = x[WX]  # Wroll
        x8 = x[WY]  # Wpitch
        x9 = x[WZ]  # Wyaw

        x10 = x[Roll]  # Roll
        x11 = x[Pitch]  # Pitch
        x12 = x[Yaw]  # Yaw

        u1 = u[0]
        u2 = u[1]
        u3 = u[2]
        u4 = u[3]

        Ix = self.Ix
        Iy = self.Iy
        Iz = self.Iz

        xdot1 = x4
        xdot2 = x5
        xdot3 = x6

        xdot4 = (ca.cos(x10) * ca.sin(x11) * ca.cos(x12) +
                 ca.sin(x10) * ca.sin(x12)) * u1 / self.m
        xdot5 = (ca.cos(x10) * ca.sin(x11) * ca.sin(x12) -
                 ca.sin(x10) * ca.cos(x12)) * u1 / self.m
        xdot6 = (ca.cos(x10) * ca.cos(x11)) * u1 / self.m - self.g

        xdot7 = ((Iy - Iz) * x8 * x9 + u2) / Ix
        xdot8 = ((Iz - Ix) * x9 * x7 + u3) / Iy
        xdot9 = ((Ix - Iy) * x7 * x8 + u4) / Iz

        xdot10 = x7 + x8 * ca.sin(x10) * ca.tan(x11) + x9 * ca.cos(
            x10) * ca.tan(x11)
        xdot11 = x8 * ca.cos(x10) - x9 * ca.sin(x10)
        xdot12 = x8 * ca.sin(x10) / ca.cos(x11) + x9 * ca.cos(x10) / ca.cos(
            x11)

        dxdt = [
            xdot1,
            xdot2,
            xdot3,
            xdot4,
            xdot5,
            xdot6,
            xdot7,
            xdot8,
            xdot9,
            xdot10,
            xdot11,
            xdot12,
        ]

        return ca.vertcat(*dxdt)
Ejemplo n.º 21
0
    def quad_nl_euler_reduced_dynamics(self, x, u, *_):
        """
        Pendulum nonlinear dynamics based on euler angle description.

        :param x: state
        :type x: casadi.DM or casadi.MX
        :param u: control input
        :type u: casadi.DM or casadi.MX
        :return: state time derivative
        :rtype: casadi.DM or casadi.MX, depending on inputs
        """

        x1 = x[0]  # x
        x2 = x[1]  # y
        x3 = x[2]  # z

        x4 = x[3]  # Vx
        x5 = x[4]  # Vy
        x6 = x[5]  # Vz

        x10 = x[6]  # Roll
        x11 = x[7]  # Pitch
        x12 = x[8]  # Yaw

        T = u[0]
        w1 = u[1]
        w2 = u[2]
        w3 = u[3]

        Ix = self.Ix
        Iy = self.Iy
        Iz = self.Iz

        xdot1 = x4
        xdot2 = x5
        xdot3 = x6

        xdot4 = (ca.cos(x10) * ca.sin(x11) * ca.cos(x12) +
                 ca.sin(x10) * ca.sin(x12)) * T / self.m
        xdot5 = (ca.cos(x10) * ca.sin(x11) * ca.sin(x12) -
                 ca.sin(x10) * ca.cos(x12)) * T / self.m
        xdot6 = (ca.cos(x10) * ca.cos(x11)) * T / self.m - self.g

        xdot10 = w1 + w2 * ca.sin(x10) * ca.tan(x11) + w3 * ca.cos(
            x10) * ca.tan(x11)
        xdot11 = w2 * ca.cos(x10) - w3 * ca.sin(x10)
        xdot12 = w2 * ca.sin(x10) / ca.cos(x11) + w3 * ca.cos(x10) / ca.cos(
            x11)

        dxdt = [
            xdot1,
            xdot2,
            xdot3,
            xdot4,
            xdot5,
            xdot6,
            xdot10,
            xdot11,
            xdot12,
        ]

        return ca.vertcat(*dxdt)
Ejemplo n.º 22
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
Ejemplo n.º 23
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)))
Ejemplo n.º 24
0
 def skewY(self, theta):
     tan = cas.tan(theta / 180 * cas.pi)
     self.matrix(1, tan, 0, 1, 0, 0)
Ejemplo n.º 25
0
    # Load the dual
    if load_dual:
        with open(dual_location, 'r') as infile:
            dual_vals = json.load(infile)
        if len(dual_vals) != opti.ng:
            raise Exception(
                "Couldn't load the dual, since your problem has %i cons and the cached problem has %i cons."
                % (opti.ng, len(dual_vals)))
        for i in tqdm(range(opti.ng), desc="Loading dual variables:"):
            opti.set_initial(opti.lam_g[i], dual_vals[i])


sind = lambda theta: cas.sin(theta * cas.pi / 180)
cosd = lambda theta: cas.cos(theta * cas.pi / 180)
tand = lambda theta: cas.tan(theta * cas.pi / 180)
atan2d = lambda y_val, x_val: cas.atan2(y_val, x_val) * 180 / np.pi
clip = lambda x, min, max: cas.fmin(cas.fmax(min, x), max)


def smoothmax(value1, value2, hardness):
    """
    A smooth maximum between two functions.
    Useful because it's differentiable and convex!
    Great writeup by John D Cook here:
        https://www.johndcook.com/soft_maximum.pdf
    :param value1: Value of function 1.
    :param value2: Value of function 2.
    :param hardness: Hardness parameter. Higher values make this closer to max(x1, x2).
    :return: Soft maximum of the two supplied values.
    """
Ejemplo n.º 26
0
    def solve_opti(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False):
        opti = ca.Opti()

        x = opti.variable(self.n_x*self.n_a, N+1)
        u = opti.variable(self.n_u*self.n_a, N)
        slack = opti.variable(self.n_x*self.n_a)

        if x_guess is not None:
            opti.set_initial(x, x_guess)
        if u_guess is not None:
            opti.set_initial(u, u_guess)
        opti.set_initial(slack, np.zeros(self.n_x*self.n_a))

        da_lim = self.da_lim
        ddf_lim = self.ddf_lim

        pairs = list(itertools.combinations(range(self.n_a), 2))

        opti.subject_to(x[:,0] == np.squeeze(x_0))
        for j in range(self.n_a):
            opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[j*self.n_u+0,0]-last_u[j*self.n_u+0], ddf_lim[1]*self.dt))
            opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[j*self.n_u+1,0]-last_u[j*self.n_u+1], da_lim[1]*self.dt))

        stage_cost = 0
        for i in range(N):
            stage_cost = stage_cost + 1

            for j in range(self.n_a):
                opti.subject_to(x[j*self.n_x+0,i+1] == x[j*self.n_x+0,i] + self.dt*x[j*self.n_x+3,i]*ca.cos(x[j*self.n_x+2,i] + ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r)))
                opti.subject_to(x[j*self.n_x+1,i+1] == x[j*self.n_x+1,i] + self.dt*x[j*self.n_x+3,i]*ca.sin(x[j*self.n_x+2,i] + ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r)))
                opti.subject_to(x[j*self.n_x+2,i+1] == x[j*self.n_x+2,i] + self.dt*x[j*self.n_x+3,i]*ca.sin(ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r)))
                opti.subject_to(x[j*self.n_x+3,i+1] == x[j*self.n_x+3,i] + self.dt*u[j*self.n_u+1,i])

                if i < N-1:
                    opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[j*self.n_u+0,i+1]-u[j*self.n_u+0,i], ddf_lim[1]*self.dt))
                    opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[j*self.n_u+1,i+1]-u[j*self.n_u+1,i], da_lim[1]*self.dt))

            if self.F is not None:
                opti.subject_to(ca.mtimes(self.F, x[:,i]) <= self.b)
            if self.H is not None:
                opti.subject_to(ca.mtimes(self.H, u[:,i]) <= self.g)

            if expl_constraints is not None:
                V = expl_constraints[i][0]
                w = expl_constraints[i][1]
                opti.subject_to(ca.mtimes(V, x[:2,i]) + w <= np.zeros(len(w)))

            # Collision avoidance constraint
            for p in pairs:
                opti.subject_to(ca.norm_2(x[p[0]*self.n_x:p[0]*self.n_x+2,i] - x[p[1]*self.n_x:p[1]*self.n_x+2,i]) >= 2*self.r)

        opti.subject_to(x[:,N] - x_ss == slack)

        slack_cost = 1e6*ca.sumsqr(slack)
        total_cost = stage_cost + slack_cost
        opti.minimize(total_cost)

        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}
        opti.solver('ipopt', plugin_opts, solver_opts)

        sol = opti.solve()

        slack_val = sol.value(slack)
        slack_valid = True
        for j in range(self.n_a):
            if la.norm(slack_val[j*self.n_x:(j+1)*self.n_x]) > 2e-8:
                slack_valid = False
                print('Warning! Solved, but with agent %i slack norm of %g is greater than 2e-8!' % (j+1, la.norm(slack_val[j*self.n_x:(j+1)*self.n_x])))
                break

        if sol.stats()['success'] and slack_valid:
            feasible = True

            x_pred = sol.value(x)
            u_pred = sol.value(u)
            sol_cost = sol.value(stage_cost)
        else:
            # print(sol.stats()['return_status'])
            # print(opti.debug.show_infeasibilities())
            feasible = False
            x_pred = None
            u_pred = None
            sol_cost = None

            # pdb.set_trace()

        return x_pred, u_pred, sol_cost
Ejemplo n.º 27
0
    def solve_opti(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False):
        opti = ca.Opti()

        x = opti.variable(self.n_x, N+1)
        u = opti.variable(self.n_u, N)
        slack = opti.variable(self.n_x)

        if x_guess is not None:
            opti.set_initial(x, x_guess)
        if u_guess is not None:
            opti.set_initial(u, u_guess)
        opti.set_initial(slack, np.zeros(self.n_x))

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

        opti.subject_to(x[:,0] == np.squeeze(x_0))
        opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[0,0]-last_u[0], ddf_lim[1]*self.dt))
        opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[1,0]-last_u[1], da_lim[1]*self.dt))

        stage_cost = 0
        for i in range(N):
            stage_cost = stage_cost + 1

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

            if self.F is not None:
                opti.subject_to(ca.mtimes(self.F, x[:,i]) <= self.b)
            if self.H is not None:
                opti.subject_to(ca.mtimes(self.H, u[:,i]) <= self.g)

            if expl_constraints is not None:
                V = expl_constraints[i][0]
                w = expl_constraints[i][1]
                opti.subject_to(ca.mtimes(V, x[:2,i]) + w <= np.zeros(len(w)))

            if i < N-1:
                opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[0,i+1]-u[0,i], ddf_lim[1]*self.dt))
                opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[1,i+1]-u[1,i], da_lim[1]*self.dt))

        opti.subject_to(x[:,N] - x_ss == slack)

        slack_cost = 1e6*ca.sumsqr(slack)
        total_cost = stage_cost + slack_cost
        opti.minimize(total_cost)

        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}
        opti.solver('ipopt', plugin_opts, solver_opts)

        sol = opti.solve()

        slack_val = sol.value(slack)
        if la.norm(slack_val) > 1e-6:
            print('Warning! Solved, but with slack norm of %g is greater than 1e-8!' % la.norm(slack_val))

        if sol.stats()['success'] and la.norm(slack_val) <= 1e-6:
            print('Solve success, with slack norm of %g!' % la.norm(slack_val))
            feasible = True

            x_pred = sol.value(x)
            u_pred = sol.value(u)
            sol_cost = sol.value(stage_cost)
        else:
            # print(sol.stats()['return_status'])
            # print(opti.debug.show_infeasibilities())
            feasible = False
            x_pred = None
            u_pred = None
            sol_cost = None

            # pdb.set_trace()

        return x_pred, u_pred, sol_cost
Ejemplo n.º 28
0
v_f = ca.MX.sym('v_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)

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

d_n = ca.Function('d_n', [n_f, th_f], [sls / ca.tan(th_ns[:-1] - th_f)])

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:]))
])

d_th = ca.Function('d_th', [v_f, d_f], [v_f / l * ca.tan(d_f)])

# d_t = ca.Function('d_t', [n_f_a, v_f], [])
d_t = ca.Function('d_t', [n_f_a, v_f], [
    dis(o_x_e(n_f_a[1:]), o_x_s(n_f_a[:-1]), o_y_e(n_f_a[1:]), o_y_s(
        n_f_a[:-1])) / v_f
])
# d_t = ca.Function('d_t', [n_f_a, v_f], [ca.sqrt(ca.sum1(ca.power(ca.diff(, axis=0), 2), axis=1))])
Ejemplo n.º 29
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)))
Ejemplo n.º 30
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
Ejemplo n.º 31
0
def example_loop_v_d():
    pathpoints = 30
    ref_path = {}
    ref_path['x'] = 5 * np.sin(np.linspace(0, 2 * np.pi, pathpoints + 1))
    ref_path['y'] = np.linspace(1, 2, pathpoints + 1)**2 * 10
    wp = ca.horzcat(ref_path['x'], ref_path['y']).T

    N = 10
    N1 = N - 1

    ocp = MPC(N)

    x = ocp.state('x')
    y = ocp.state('y')
    th = ocp.state('th')

    d = ocp.control('d')
    v = ocp.control('v')

    dt = ocp.get_time()

    ocp.set_der(x, v * ca.cos(th[:-1]))
    ocp.set_der(y, v * ca.sin(th[:-1]))
    ocp.set_der(th, v / 0.33 * ca.tan(d))

    ocp.set_objective(dt, ca.GenMX_zeros(N1), 0.01)

    # set limits
    max_speed = 10
    ocp.set_lims(x, -ca.inf, ca.inf)
    ocp.set_lims(y, -ca.inf, ca.inf)
    ocp.set_lims(v, 0, max_speed)
    ocp.set_lims(th, -ca.pi, ca.pi)
    ocp.set_lims(d, -0.4, 0.4)

    wpts = np.array(wp[:, 0:N])

    # find starting vals
    T = 5
    dt00 = np.array([T / N1] * N1)
    ocp.set_inital(dt, dt00)
    x00 = wpts[0, :]
    ocp.set_inital(x, x00)
    y00 = wpts[1, :]
    ocp.set_inital(y, y00)
    th00 = [lib.get_bearing(wpts[:, i], wpts[:, i + 1]) for i in range(N1)]
    th00.append(0)
    ocp.set_inital(th, th00.copy())
    v00 = np.array(
        [lib.get_distance(wpts[:, i], wpts[:, i + 1])
         for i in range(N1)]) / dt00
    ocp.set_inital(v, v00)
    th00.pop(-1)
    d00 = np.arctan(np.array(th00) * 0.33 / v00)
    ocp.set_inital(d, d00)

    ocp.set_up_solve()

    for i in range(20):
        wpts = np.array(wp[:, i:i + N])
        x0 = [wpts[0, 0], wpts[1, 0], lib.get_bearing(wpts[:, 0], wpts[:, 1])]

        ocp.set_objective(x, wpts[0, :])
        ocp.set_objective(y, wpts[1, :])

        states, controls, times = ocp.solve(x0)

        xs = states[:N]
        ys = states[N:2 * N]
        ths = states[2 * N:]
        vs = controls[:N1]
        ds = controls[N1:]
        total_time = np.sum(times)

        print(f"Times: {times}")
        print(f"Total Time: {total_time}")
        print(f"xs: {xs.T}")
        print(f"ys: {ys.T}")
        print(f"Thetas: {ths.T}")
        print(f"Velocities: {vs.T}")

        plt.figure(1)
        plt.clf()
        plt.plot(wpts[0, :], wpts[1, :], 'o', markersize=12)

        plt.plot(xs, ys, '+', markersize=20)

        plt.pause(0.5)
Ejemplo n.º 32
0
    def solve(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False):
        x = ca.SX.sym('x', self.n_x*(N+1))
        u = ca.SX.sym('y', self.n_u*N)
        slack = ca.SX.sym('slack', self.n_x)

        z = ca.vertcat(x, u, slack)

        # Flatten candidate solutions into 1-D array (- x_1 -, - x_2 -, ..., - x_N+1 -)
        if x_guess is not None:
            x_guess_flat = x_guess.flatten(order='F')
        else:
            x_guess_flat = np.zeros(self.n_x*(N+1))

        if u_guess is not None:
            u_guess_flat = u_guess.flatten(order='F')
        else:
            u_guess_flat = np.zeros(self.n_u*N)
        slack_guess = np.zeros(self.n_x)

        z_guess = np.concatenate((x_guess_flat, u_guess_flat, slack_guess))

        lb_slack = [-1000.0]*self.n_x
        ub_slack = [1000.0]*self.n_x

        # Box constraints on decision variables
        lb_x = x_0.tolist() + self.state_lb*(N) + self.input_lb*(N) + lb_slack
        ub_x = x_0.tolist() + self.state_ub*(N) + self.input_ub*(N) + ub_slack

        # Constraints on functions of decision variables
        lb_g = []
        ub_g = []

        stage_cost = 0
        constraint = []
        for i in range(N):
            stage_cost += 1

            # Formulate dynamics equality constraints as inequalities
            beta = ca.atan2(self.l_r*ca.tan(u[self.n_u*i+0]), self.l_f+self.l_r)
            constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+0] - (x[self.n_x*i+0] + self.dt*x[self.n_x*i+3]*ca.cos(x[self.n_x*i+2] + beta)))
            constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+1] - (x[self.n_x*i+1] + self.dt*x[self.n_x*i+3]*ca.sin(x[self.n_x*i+2] + beta)))
            constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+2] - (x[self.n_x*i+2] + self.dt*x[self.n_x*i+3]*ca.sin(beta)))
            constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+3] - (x[self.n_x*i+3] + self.dt*u[self.n_u*i+1]))

            lb_g += [0.0]*self.n_x
            ub_g += [0.0]*self.n_x

            # Steering rate constraints
            if self.ddf_lim is not None:
                if i == 0:
                    # Constrain steering rate with respect to previously applied input
                    constraint = ca.vertcat(constraint, u[self.n_u*(i)+0]-last_u[0])
                if i < N-1:
                    # Constrain steering rate along horizon
                    constraint = ca.vertcat(constraint, u[self.n_u*(i+1)+0]-u[self.n_u*(i)+0])
                lb_g += [self.ddf_lim[0]*self.dt]
                ub_g += [self.ddf_lim[1]*self.dt]

            # Throttle rate constraints
            if self.da_lim is not None:
                if i == 0:
                    # Constrain throttle rate
                    constraint = ca.vertcat(constraint, u[self.n_u*i+1]-last_u[1])
                if i < N-1:
                    constraint = ca.vertcat(constraint, u[self.n_u*(i+1)+1]-u[self.n_u*(i)+1])
                lb_g += [self.da_lim[0]*self.dt]
                ub_g += [self.da_lim[1]*self.dt]

            # Exploration constraints on predicted positions of agent
            if expl_constraints is not None:
                V = expl_constraints[i][0]
                w = expl_constraints[i][1]

                for j in range(V.shape[0]):
                    constraint = ca.vertcat(constraint, V[j,0]*x[self.n_x*i+0] + V[j,1]*x[self.n_x*i+1] + w[j])
                    lb_g += [-1e9]
                    ub_g += [0]

        # Formulate terminal soft equality constraint as inequalities
        constraint = ca.vertcat(constraint, x[self.n_x*N:] - x_ss + slack)
        lb_g += [0.0]*self.n_x
        ub_g += [0.0]*self.n_x

        slack_cost = 1e6*ca.sumsqr(slack)
        total_cost = stage_cost + slack_cost

        opts = {'verbose' : False, 'print_time' : 0,
            'ipopt.print_level' : 5,
            'ipopt.mu_strategy' : 'adaptive',
            'ipopt.mu_init' : 1e-5,
            'ipopt.mu_min' : 1e-15,
            'ipopt.barrier_tol_factor' : 1,
            'ipopt.linear_solver': 'ma27'}
        nlp = {'x' : z, 'f' : total_cost, 'g' : constraint}
        solver = ca.nlpsol('solver', 'ipopt', nlp, opts)

        sol = solver(lbx=lb_x, ubx=ub_x, lbg=lb_g, ubg=ub_g, x0=z_guess)

        pdb.set_trace()

        if solver.stats()['success']:
            if la.norm(slack_val) <= 1e-8:
                print('Solve success for safe set point', x_ss)
                feasible = True

                z_sol = np.array(sol['x'])
                x_pred = z_sol[:self.n_x*(N+1)].reshape((N+1,self.n_x)).T
                u_pred = z_sol[self.n_x*(N+1):self.n_x*(N+1)+self.n_u*N].reshape((N,self.n_u)).T
                slack_val = z_sol[self.n_x*(N+1)+self.n_u*N:]
                cost_val = sol['f']
            else:
                print('Warning! Solved, but with slack norm of %g is greater than 1e-8!' % la.norm(slack_val))
                feasible = False
                x_pred = None
                u_pred = None
                cost_val = None
        else:
            print(solver.stats()['return_status'])
            feasible = False
            x_pred = None
            u_pred = None
            cost_val = None

            # pdb.set_trace()

        # pdb.set_trace()

        return x_pred, u_pred, cost_val
Ejemplo n.º 33
0
def Max_velocity(pts, conf, show=False):
    mu = conf.mu
    m = conf.m
    g = conf.g
    l_f = conf.l_f
    l_r = conf.l_r
    safety_f = conf.force_f
    f_max = mu * m * g #* safety_f
    f_long_max = l_f / (l_r + l_f) * f_max
    max_v = conf.max_v  
    max_a = conf.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), 
        'f': ca.sumsqr(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 + [-ca.inf] * N1 + [-f_max] * N1
    ubx = [max_v] * N + [max_v] * N + [10] * N1 + [ca.inf] * 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 + [ca.inf] * 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"Total Time: {t[-1]}")
    # 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.clf()
        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(3)
        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.ylim([-25, 25])
        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.show()
        plt.pause(0.001)
    
    # 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