Пример #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']))
Пример #2
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)]
Пример #3
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))))
Пример #4
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)]
Пример #5
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)]
Пример #6
0
def quaternion_rpy(roll, pitch, yaw):
    """Returns a quaternion ([x,y,z,w], w scasadi.ar) from roll pitch yaw ZYX
    convention."""
    cr = cs.cos(roll/2.0)
    sr = cs.sin(roll/2.0)
    cp = cs.cos(pitch/2.0)
    sp = cs.sin(pitch/2.0)
    cy = cs.cos(yaw/2.0)
    sy = cs.sin(yaw/2.0)
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy
    w = cr * cp * cy + sr * sp * sy
    # Remember to normalize:
    nq = cs.sqrt(x*x + y*y + z*z + w*w)
    return cs.vertcat(w/nq, x/nq, y/nq, z/nq)
Пример #7
0
def axis_angle_to_rotation(axis, angle):
    # Source:
    # https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
    res = cs.MX.eye(3)
    res += cs.sin(angle) * cs.skew(axis)
    res += (1 - cs.cos(angle)) * cs.mtimes(cs.skew(axis), cs.skew(axis))
    return res
Пример #8
0
def robust_control_stages(ocp, delta, t0):
    """
  Returns
  -------
  stage: :obj: `~rockit.stage.Stage
  x : :obj: `~casadi.MX

  """
    stage = ocp.stage(t0=t0, T=1)
    x = stage.state(2)
    u = stage.state()
    der_state = vertcat(x[1],
                        -0.1 * (1 - x[0]**2 + delta) * x[1] - x[0] + u + delta)
    stage.set_der(x, der_state)
    stage.set_der(u, 0)
    stage.subject_to(u <= 40)
    stage.subject_to(u >= -40)
    stage.subject_to(x[0] >= -0.25)
    L = stage.variable()
    stage.add_objective(L)
    stage.subject_to(L >= sumsqr(x[0] - 3))
    bound = lambda t: 2 + 0.1 * cos(10 * t)
    stage.subject_to(x[0] <= bound(stage.t))
    stage.method(MultipleShooting(N=20, M=1, intg='rk'))

    return stage, x, stage.at_t0(u)
    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
Пример #10
0
def rotation_matrix_from_axis_angle(axis, angle):
    """
    Conversion of unit axis and angle to 4x4 rotation matrix according to:
    http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
    :param axis: 3x1 Matrix
    :type axis: Matrix
    :type angle: Union[float, Symbol]
    :return: 4x4 Matrix
    :rtype: Matrix
    """
    ct = ca.cos(angle)
    st = ca.sin(angle)
    vt = 1 - ct
    m_vt_0 = vt * axis[0]
    m_vt_1 = vt * axis[1]
    m_vt_2 = vt * axis[2]
    m_st_0 = axis[0] * st
    m_st_1 = axis[1] * st
    m_st_2 = axis[2] * st
    m_vt_0_1 = m_vt_0 * axis[1]
    m_vt_0_2 = m_vt_0 * axis[2]
    m_vt_1_2 = m_vt_1 * axis[2]
    return Matrix(
        [[ct + m_vt_0 * axis[0], -m_st_2 + m_vt_0_1, m_st_1 + m_vt_0_2, 0],
         [m_st_2 + m_vt_0_1, ct + m_vt_1 * axis[1], -m_st_0 + m_vt_1_2, 0],
         [-m_st_1 + m_vt_0_2, m_st_0 + m_vt_1_2, ct + m_vt_2 * axis[2], 0],
         [0, 0, 0, 1]])
Пример #11
0
def inverted_pendulum_nonlinear_ode(x, u):
    M = 2.4
    m = 0.23
    l = 0.36
    g = 9.81

    dx1_dt = x[1]
    dx2_dt = (u * ca.cos(x[0]) -
              (M + m) * g * ca.sin(x[0]) + m * l * ca.cos(x[0]) *
              ca.sin(x[0]) * x[1]**2) / (m * l * ca.cos(x[0])**2 - (M + m) * l)
    dx3_dt = x[3]
    dx4_dt = (u + m * l * ca.sin(x[0]) * x[1]**2 - m * g * ca.cos(x[0]) *
              ca.sin(x[0])) / (M + m - m * ca.cos(x[0])**2)
    rhs = [dx1_dt, dx2_dt, dx3_dt, dx4_dt]

    return ca.vertcat(*rhs)
Пример #12
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)
Пример #13
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))
Пример #14
0
def generate_griewank(n=2, a=1., b=4000., func_opts={}, data_type=cs.SX):
    x = data_type.sym("x", n)
    product_part = 1.
    for i in range(n):
        product_part = product_part * cs.cos(x[i] / cs.sqrt(i + 1))
    f = a + (1. / b) * cs.sumsqr(x) - product_part
    func = cs.Function("griewank", [x], [f], ["x"], ["f"], func_opts)
    return func, [[-600., 600.]] * n, [[0.] * n]
Пример #15
0
def rotation_z(angle):
    """Returns a rotation matrix for a left-handed rotation around the z-axis by the given angle."""
    c, s = cs.cos(angle), cs.sin(angle)
    R = cs.SX.zeros(3, 3)
    R[0,0],R[0,1],R[0,2] =  c, s, 0
    R[1,0],R[1,1],R[1,2] = -s, c, 0
    R[2,0],R[2,1],R[2,2] =  0, 0, 1
    return R
Пример #16
0
def Cd_wave_Korn(Cl, t_over_c, mach, sweep=0, kappa_A=0.95):
    """
    Wave drag_force coefficient prediction using the (very) low-fidelity Korn Equation method; derived in "Configuration Aerodynamics" by W.H. Mason, Sect. 7.5.2, pg. 7-18
    :param Cl: (2D) lift_force coefficient
    :param t_over_c: thickness-to-chord ratio
    :param sweep: sweep angle, in degrees
    :param kappa_A: Airfoil technology factor (0.95 for supercritical section, 0.87 for NACA 6-series)
    :return: Wave drag coefficient
    """
    mach = cas.fmax(mach, 0)
    sweep_rad = np.pi / 180 * sweep
    Mdd = kappa_A / cas.cos(sweep_rad) - t_over_c / cas.cos(sweep_rad) ** 2 - Cl / (10 * cas.cos(sweep_rad) ** 3)
    Mcrit = Mdd - (0.1 / 80) ** (1 / 3)
    # Cd_wave = 20 * cas.ramp(mach - Mcrit) ** 4
    Cd_wave = 20 * cas.if_else(mach > Mcrit, mach - Mcrit, 0) ** 4

    return Cd_wave
Пример #17
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])
Пример #18
0
 def step_sym(self, t, h, x, u, w):
     thetadot = u[1]
     v = u[0]
     theta_out = x[2] + thetadot * h + w[2]
     theta_average = 0.5 * (theta_out + x[2])
     x_out = x[0] + h * v * cs.cos(theta_average) + w[0]
     y_out = x[1] + h * v * cs.sin(theta_average) + w[1]
     return cs.vertcat(x_out, y_out, theta_out)
Пример #19
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)]
Пример #20
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])
Пример #21
0
 def from_euler(cls, e):
     assert e.shape == (3, 1) or e.shape == (3, )
     q = ca.SX(4, 1)
     cosPhi_2 = ca.cos(e[0] / 2)
     cosTheta_2 = ca.cos(e[1] / 2)
     cosPsi_2 = ca.cos(e[2] / 2)
     sinPhi_2 = ca.sin(e[0] / 2)
     sinTheta_2 = ca.sin(e[1] / 2)
     sinPsi_2 = ca.sin(e[2] / 2)
     q[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + \
            sinPhi_2 * sinTheta_2 * sinPsi_2
     q[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - \
            cosPhi_2 * sinTheta_2 * sinPsi_2
     q[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + \
            sinPhi_2 * cosTheta_2 * sinPsi_2
     q[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - \
            sinPhi_2 * sinTheta_2 * cosPsi_2
     return q
        def kinematics(self, state, U, epsilon=0):

            f,_,_ = self.proc_model()

            w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0]))
            w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1]))
            w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2]))
            w3 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[3,3]))

            w = c.DM([[w0],[w1],[w2],[w3]])

            state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]],[U[2] + w[2]],[U[3] + w[3]]]))

            state_n[6] = c.atan2(c.sin(state_n[6]),c.cos(state_n[6]))
            state_n[7] = c.atan2(c.sin(state_n[7]),c.cos(state_n[7]))
            state_n[8] = c.atan2(c.sin(state_n[8]),c.cos(state_n[8]))

            return state_n
Пример #23
0
    def get_link_relative_transform_casadi(self, qi):
        """ Link transform according to the Denavit-Hartenberg convention.
        Casadi compatible function.
        """
        if self.joint_type == JointType.revolute:
            a, alpha, d, theta = self.dh.a, self.dh.alpha, self.dh.d, qi
        elif self.joint_type == JointType.prismatic:
            a, alpha, d, theta = self.dh.a, self.dh.alpha, qi, self.dh.theta

        c_t, s_t = ca.cos(theta), ca.sin(theta)
        c_a, s_a = ca.cos(alpha), ca.sin(alpha)

        row1 = ca.hcat([c_t, -s_t * c_a, s_t * s_a, a * c_t])
        row2 = ca.hcat([s_t, c_t * c_a, -c_t * s_a, a * s_t])
        row3 = ca.hcat([0, s_a, c_a, d])
        row4 = ca.hcat([0, 0, 0, 1])

        return ca.vcat([row1, row2, row3, row4])
Пример #24
0
def generate_bartelsconn(n=2, func_opts={}, data_type=cs.SX):
    if n != 2:
        raise ValueError("bartelsconn is only defined for n=2")
    x = data_type.sym("x", n)
    f = cs.norm_1(cs.sumsqr(x) + x[0] * x[1])
    f += cs.norm_1(cs.sin(x[0]))
    f += cs.norm_1(cs.cos(x[1]))
    func = cs.Function("bartelsconn", [x], [f], ["x"], ["f"], func_opts)
    return func, [[-500., 500.]] * n, [[0.] * n]
def prediction_state(x0, u, T, N):
    # define predition horizon function
    states_ = np.zeros((N + 1, 3))
    states_[0, :] = x0
    for i in range(N):
        states_[i + 1, 0] = states_[i, 0] + u[i, 0] * ca.cos(states_[i, 2]) * T
        states_[i + 1, 1] = states_[i, 1] + u[i, 0] * ca.sin(states_[i, 2]) * T
        states_[i + 1, 2] = states_[i, 2] + u[i, 1] * T
    return states_
Пример #26
0
    def __init__(self):
        self.opti = ca.Opti()
        self.N = 200
        self.d = 2.0
        self.dmax = 20.0
        self.umax = 20.0
        self.pi = -np.pi
        self.l = 0.5
        self.m1 = 0.5
        self.m2 = 0.1
        self.g = -9.81
        self.T = 2.0
        self.h = self.T / self.N

        goal1 = self.opti.parameter()
        goal2 = self.opti.parameter()
        goal3 = self.opti.parameter()
        goal4 = self.opti.parameter()
        self.opti.set_value(goal1, self.d)
        self.opti.set_value(goal2, self.pi)
        self.opti.set_value(goal3, 0)
        self.opti.set_value(goal4, 0)
        self.goal = [goal1, goal2, goal3, goal4]

        self.q1 = self.opti.variable(self.N)
        self.q2 = self.opti.variable(self.N)
        self.q1_dot = self.opti.variable(self.N)
        self.q2_dot = self.opti.variable(self.N)

        self.u = self.opti.variable(self.N)

        self.q1_ddot = (
            ((self.l * self.m2 * ca.sin(self.q2) * (self.q2**2)) + (self.u) +
             (self.m2 * self.g * ca.cos(self.q2) * ca.sin(self.q2))) /
            (self.m1 + self.m2 * (ca.sin(self.q2)**2)))
        self.q2_ddot = (
            ((self.l * self.m2 * ca.cos(self.q2) * ca.sin(self.q2) *
              (self.q2_dot**2)) + (self.u * ca.cos(self.q2)) +
             ((self.m1 + self.m2) * self.g * ca.sin(self.q2))) /
            (self.m1 * self.l + self.l * self.m2 * (ca.sin(self.q2)**2)))

        self.state = ca.horzcat(self.q1, self.q2, self.q1_dot, self.q2_dot)
        self.model = ca.horzcat(self.q1_dot, self.q2_dot, self.q1_ddot,
                                self.q2_ddot)
    def buildDynamicalSystem(self):
        g = 9.8

        x = ca.MX.sym('x')
        dx = ca.MX.sym('dx')
        theta = ca.MX.sym('theta')
        dtheta = ca.MX.sym('dtheta')
        u = ca.MX.sym('u')

        states = ca.vertcat(x, dx, theta, dtheta);
        controls = u;

        param1 = ca.MX.sym('param_1')
        param2 = ca.MX.sym('param_2')
        param3 = ca.MX.sym('param_3')

        params = ca.vertcat(param1, param2, param3)

        print("params_shape", params.shape)

        # build matrix for mass matrix
        phi_1_1 = ca.MX(2, 3)
        phi_1_1[0, 0] = ca.MX.ones(1)
        phi_1_1[1, 1] = ca.cos(theta)
        phi_1_2 = ca.MX(2, 3)
        phi_1_2[0, 1] = ca.cos(theta)
        phi_1_2[1, 2] = 4/3*ca.MX.ones(1)

        mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params), ca.mtimes(phi_1_2, params))

        phi_2 = ca.MX(2, 3)
        phi_2[0, 1] = -1 * ca.sin(theta) * dtheta * dtheta
        phi_2[1, 1] = -g * ca.sin(theta)

        forces = ca.mtimes(phi_2, params)

        actions = ca.vertcat(controls, ca.MX.zeros(1))

        b = actions - forces
        states_dd = ca.solve(mass_matrix, b)

        states_d = ca.vertcat(dx, states_dd[0], dtheta, states_dd[1])

        return states, states_d, controls, params
Пример #28
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)
Пример #29
0
def constrainInvariants(ocp):
    R_n2b = ocp.lookup('R_n2b',timestep=0)
    rawekite.kiteutils.makeOrthonormal(ocp, R_n2b)
    ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('c(0)==0',None))
    ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('cdot(0)==0',None))

    # constrain line angle
    for k in range(0,nk):
        for j in range(0,ocp.deg+1):
            ocp.constrain(ocp.lookup('cos_line_angle',timestep=k,degIdx=j),'>=',C.cos(65*pi/180), tag=('line angle',k))
Пример #30
0
    def dynamics(self, x, u):
        # state x = [x,y, vx, vy]
        x_next = x[0] + self.dt * cos(x[3]) * x[2]
        y_next = x[1] + self.dt * sin(x[3]) * x[2]
        v_next = x[2] + self.dt * u[0]
        theta_next = x[3] + self.dt * u[1]

        state_next = [x_next, y_next, v_next, theta_next]

        return state_next
Пример #31
0
    def dynamics(self, x, u):
        # Get states
        xPos = x[0]
        yPos = x[1]
        xVel = x[2]
        yVel = x[3]
        ang = x[4]
        angVel = x[5]

        # Get controls
        thrust_percentage = u[0]
        thrust_angle = u[1]

        # Get disturbing wind force (only in horizontal direction)
        force_wind = u[2]

        # Compute some intermediate values
        thrust_magnitude = thrust_percentage * self.maxThrust
        sa = cas.sin(ang)
        ca = cas.cos(ang)
        st = cas.sin(thrust_angle)
        ct = cas.cos(thrust_angle)

        # == LINEAR FORCES ==
        force_gravity = -self.m * self.g
        force_thrust = thrust_magnitude * cas.vertcat(ct * sa - st * ca,
                                                      ct * ca + st * sa)

        # Compute the linear accelerations
        xAcc = (force_thrust[0] + force_wind) / self.m
        yAcc = (force_thrust[1] + force_gravity) / self.m

        # == TORQUES ==
        torque_wind = self.l * ca * force_wind
        torque_thrust = self.L * st * thrust_magnitude

        # Compute the angular acceleration
        angAcc = (torque_thrust + torque_wind) / self.I

        # Stack the derivatives
        xdot = cas.vertcat(xVel, yVel, xAcc, yAcc, angVel, angAcc)

        return xdot
Пример #32
0
def doit():

    # state vector is (x, y, theta)
    x = cs.SX.sym('x', 3)

    # control vector is (v, omega)
    u = cs.SX.sym('u', 2)

    # explicit variables for readability
    v = u[0]
    omega = u[1]
    theta = x[2]

    # unicycle kinematic model
    xdot = cs.vertcat(v*cs.cos(theta),
                      v*cs.sin(theta),
                      omega)

    # intermediate cost, final cost, final constraint
    l = cs.sumsqr(u)
    xf_des = np.array([0, 1, 0])
    lf = 1000.0 * cs.sumsqr(x - xf_des)
    hf = x - xf_des

    # create solver
    solver = ilqr.IterativeLQR(x=x,
                               u=u,
                               xdot=xdot,
                               dt=0.1,
                               N=100,
                               intermediate_cost=l,
                               final_cost=lf,
                               final_constraint=None)
    # solve
    solver.randomizeInitialGuess()
    solver.solve(50)

    plt.figure()
    plt.title('Total cost')
    plt.semilogy(solver._dcost, '--s')
    plt.grid()

    plt.figure()
    plt.title('State trajectory')
    plt.plot(solver._state_trj)
    plt.legend(['x', 'y', 'theta'])
    plt.grid()

    plt.figure()
    plt.title('Control trajectory')
    plt.plot(solver._ctrl_trj)
    plt.legend(['v', 'omega'])
    plt.grid()

    plt.show()
Пример #33
0
    def continuous_dynamics(x, u, p):
        #extract states and inputs
        posx = x[zvars.index('posx')-ninputs]
        posy = x[zvars.index('posy')-ninputs]
        phi = x[zvars.index('phi')-ninputs]
        vx = x[zvars.index('vx')-ninputs]
        vy = x[zvars.index('vy')-ninputs]
        omega = x[zvars.index('omega')-ninputs]
        d = x[zvars.index('d')-ninputs]
        delta = x[zvars.index('delta')-ninputs]
        theta = x[zvars.index('theta')-ninputs]

        ddot = u[zvars.index('ddot')]
        deltadot = u[zvars.index('deltadot')]
        thetadot = u[zvars.index('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

        #let z = [u, x] = [ddot, deltadot, thetadot, posx, posy, phi, vx, vy, omega, d, delta, theta]

        statedot = np.array([
                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
                ])
        return statedot
Пример #34
0
 def exp(cls, v):
     assert v.shape == (3, 1) or q.shape == (3, )
     q = ca.SX(4, 1)
     theta = ca.norm_2(v)
     q[0] = ca.cos(theta / 2)
     c = ca.sin(theta / 2)
     n = ca.norm_2(v)
     q[1] = c * v[0] / n
     q[2] = c * v[1] / n
     q[3] = c * v[2] / n
     return ca.if_else(n > eps, q, ca.SX([1, 0, 0, 0]))
Пример #35
0
def mobile_robot_ode(t, x, u, p, z):
    """
    Mobile robot
    """
    # Parameter configuratio

    dx_dt = u[0] * ca.cos(x[2])
    dy_dt = u[0] * ca.sin(x[2])
    dtheta_dt = u[1]
    rhs = [dx_dt, dy_dt, dtheta_dt]
    return ca.vertcat(*rhs)
Пример #36
0
def generate_salomon(n=2,
                     a=1.,
                     b=2 * cs.np.pi,
                     c=0.1,
                     func_opts={},
                     data_type=cs.SX):
    x = data_type.sym("x", n)
    sumsq = cs.sumsqr(x)
    f = a - cs.cos(b * cs.sqrt(sumsq)) + c * cs.sqrt(sumsq)
    func = cs.Function("salomon", [x], [f], ["x"], ["f"], func_opts)
    return func, [[-10., 10.]] * n, [[0.] * n]
Пример #37
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)
Пример #38
0
def generate_ackleyn4(n=2, a=0.2, b=3., c=2., func_opts={}, data_type=cs.SX):
    x = data_type.sym("x", n)
    f = 0.
    for i in range(n - 1):
        f += cs.exp(-a) * cs.sqrt(x[i]**2 + x[i + 1]**2)
        f += b * cs.cos(c * x[i]) + cs.sin(c * x[i + 1])
    func = cs.Function("ackleyn4", [x], [f], ["x"], ["f"], func_opts)
    if n == 2:
        glob_min = [[-1.51, -0.755]]
    else:
        glob_min = []
    return func, [[-35., 35.]] * n, glob_min
Пример #39
0
def axis_rotation(axis, qi):
    """Returns the dual quaternion for a rotation along an axis.
    AXIS MUST BE NORMALIZED!
    """
    res = cs.SX.zeros(8)
    cqi = cs.cos(qi / 2.0)
    sqi = cs.sin(qi / 2.0)
    res[0] = axis[0] * sqi
    res[1] = axis[1] * sqi
    res[2] = axis[2] * sqi
    res[3] = cqi
    return res
Пример #40
0
 def euler2mat(self, rpy):
     c_rpy = cs.cos(rpy)
     s_rpy = cs.sin(rpy)
     c0 = cs.vertcat(c_rpy[2] * c_rpy[1], s_rpy[2] * c_rpy[1], -s_rpy[1])
     c1 = cs.vertcat(c_rpy[2] * s_rpy[1] * s_rpy[0] - s_rpy[2] * c_rpy[0],
                     s_rpy[2] * s_rpy[1] * s_rpy[0] + c_rpy[2] * c_rpy[0],
                     c_rpy[1] * s_rpy[0])
     c2 = cs.vertcat(c_rpy[2] * s_rpy[1] * c_rpy[0] + s_rpy[2] * s_rpy[0],
                     s_rpy[2] * s_rpy[1] * c_rpy[0] - c_rpy[2] * s_rpy[0],
                     c_rpy[1] * c_rpy[0])
     M = cs.horzcat(c0, c1, c2)
     return M
Пример #41
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
Пример #42
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
Пример #43
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)
Пример #44
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
Пример #45
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])
Пример #46
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
Пример #47
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])
Пример #48
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']]
Пример #49
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)
Пример #50
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
Пример #51
0
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]
				] )
Пример #52
0
def setupOcp(dae,conf,nk=50,nicp=1,deg=4):
    ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
    ocp.setupCollocation(ocp.lookup('endTime'))

    # constrain invariants
    def constrainInvariantErrs():
        dcm = ocp.lookup('dcm',timestep=0)
        rawekite.kiteutils.makeOrthonormal(ocp, dcm)
        ocp.constrain(ocp.lookup('c',timestep=0), '==', 0)
        ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0)
    constrainInvariantErrs()

    # constraint line angle
    for k in range(0,nk):
        ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(55*pi/180), tag=('line angle',k))
        
    # constrain airspeed
    def constrainAirspeedAlphaBeta():
        for k in range(0,nk):
            ocp.constrain(ocp.lookup('airspeed',timestep=k), '>=', 5)
            ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k), (-5,10))
            ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k), (-10,10))
    constrainAirspeedAlphaBeta()

    # constrain tether force
    for k in range(nk):
        ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=1), '>=', 0)
        ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=ocp.deg), '>=', 0)

    # make it periodic
    for name in [ "y","z",
                  "dy","dz",
                  "w1","w2","w3",
                  "ddelta",
                  "aileron","elevator",
                  "r","dr"]:
        ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1))

    # periodic attitude
#    rawekite.kiteutils.periodicEulers(ocp)
#    rawekite.kiteutils.periodicOrthonormalizedDcm(ocp)
    rawekite.kiteutils.periodicDcm(ocp)

    # bounds
    ocp.bound('aileron',(-0.04,0.04))
    ocp.bound('elevator',(-0.1,0.1))
    ocp.bound('daileron',(-2,2))
    ocp.bound('delevator',(-2,2))

    ocp.bound('x',(0.1,1000))
    ocp.bound('y',(-100,100))
    ocp.bound('z',(-0.5,7))
    ocp.bound('r',(4,4))
    ocp.bound('dr',(-10,10))
    ocp.bound('ddr',(-2.5,2.5))

    for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
        ocp.bound(e,(-1.1,1.1))

    for d in ['dx','dy','dz']:
        ocp.bound(d,(-70,70))

    for w in ['w1','w2','w3']:
        ocp.bound(w,(-4*pi,4*pi))

    ocp.bound('delta',(-0.01,1.01*2*pi))
    ocp.bound('ddelta',(-pi/8,8*pi))
    ocp.bound('motor_torque',(-1000,1000))
    ocp.bound('endTime',(0.5,7.0))
    ocp.bound('w0',(10,10))

    # boundary conditions
    ocp.bound('delta',(0,0),timestep=0)
    ocp.bound('delta',(2*pi,2*pi),timestep=-1)
    
    # objective function
    obj = 0
    for k in range(nk):
        # control regularization
        ddr = ocp.lookup('ddr',timestep=k)
        tc = ocp.lookup('motor_torque',timestep=k)
        daileron = ocp.lookup('daileron',timestep=k)
        delevator = ocp.lookup('delevator',timestep=k)
        
        daileronSigma = 0.1
        delevatorSigma = 0.1
        ddrSigma = 5.0
        torqueSigma = 1.0
        
#        tc = tc - 390

        ailObj = daileron*daileron / (daileronSigma*daileronSigma)
        eleObj = delevator*delevator / (delevatorSigma*delevatorSigma)
        winchObj = ddr*ddr / (ddrSigma*ddrSigma)
        torqueObj = tc*tc / (torqueSigma*torqueSigma)
        
        obj += ailObj + eleObj + winchObj + torqueObj
    ocp.setObjective( obj/nk )

    ocp.setQuadratureDdt('quadrature_energy', 'winch_power')

    # spawn telemetry thread
    callback = rawe.telemetry.startTelemetry(ocp, conf, callbacks=[(rawekite.kiteTelemetry.showAllPoints,'multi-carousel')])

    # solver
    solverOptions = [ ("expand",True)
#                     ,("qp_solver",C.NLPQPSolver)
#                     ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}})
                    , ("linear_solver","ma57")
                    , ("max_iter",1000)
                    , ("tol",1e-9)
#                    , ("Timeout", 1e6)
#                    , ("UserHM", True)
#                    , ("ScaleConIter",True)
#                    , ("ScaledFD",True)
#                    , ("ScaledKKT",True)
#                    , ("ScaledObj",True)
#                    , ("ScaledQP",True)
                    ]
    
    # initial guess
    ocp.guessX(x0)
    for k in range(0,nk+1):
        val = 2.0*pi*k/nk
        ocp.guess('delta',val,timestep=k,quiet=True)

    ocp.guess('motor_torque',0)
    ocp.guess('endTime',1.5)
    ocp.guess('daileron',0)
    ocp.guess('delevator',0)

    ocp.guess('ddr',0)
    ocp.guess('w0',10)

    ocp.setupSolver( solverOpts=solverOptions,
                     callback=callback )

    return ocp
Пример #53
0
def setupOcp(dae,conf,nk=50,nicp=1,deg=4):
    ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
    
    print "setting up collocation..."
    ocp.setupCollocation(ocp.lookup('endTime'))
    
    ocp.setQuadratureDdt('quadrature energy', 'winch power')
    
    # constrain invariants
    def constrainInvariantErrs():
        dcm = ocp.lookup('dcm',timestep=0)
        rawekite.kiteutils.makeOrthonormal(ocp, dcm)
        ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c',None))
        ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot',None))
    constrainInvariantErrs()

    # constrain line angle
    for k in range(0,nk):
        ocp.constrain(ocp.lookup('cos(line angle)',timestep=k),'>=',C.cos(55*pi/180), tag=('line angle',k))

    # constrain airspeed
    def constrainAirspeedAlphaBeta():
        for k in range(0,nk):
            ocp.constrain(ocp.lookup('airspeed',timestep=k), '>=', 5, tag=('airspeed',k))
            ocp.constrainBnds(ocp.lookup('alpha(deg)',timestep=k), (-5,20), tag=('alpha(deg)',k))
            ocp.constrainBnds(ocp.lookup('beta(deg)', timestep=k), (-10,10), tag=('beta(deg)',k))
    constrainAirspeedAlphaBeta()

    # constrain tether force
    for k in range(nk):
        ocp.constrain( ocp.lookup('tether tension',timestep=k,degIdx=1), '>=', 0, tag=('tether tension',(k,1)))
        ocp.constrain( ocp.lookup('tether tension',timestep=k,degIdx=ocp.deg), '>=', 0, tag=('tether tension',(k,ocp.deg)))

    # bounds
    ocp.bound('aileron',(-0.04,0.04))
    ocp.bound('elevator',(-0.1,0.1))
    ocp.bound('daileron',(-2.0,2.0))
    ocp.bound('delevator',(-2.0,2.0))
    ocp.bound('motor_torque',(-10000,10000))

    ocp.bound('x',(-200,200))
    ocp.bound('y',(-200,200))
    ocp.bound('z',(-0.5,200))
    ocp.bound('r',(1,31))
    ocp.bound('dr',(-30,30))
    ocp.bound('ddr',(-500,500))

    for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
        ocp.bound(e,(-1.1,1.1))

    for d in ['dx','dy','dz']:
        ocp.bound(d,(-70,70))

    for w in ['w1','w2','w3']:
        ocp.bound(w,(-4*pi,4*pi))

    ocp.bound('delta',(-12*pi,12*pi))
    ocp.bound('ddelta',(-12*pi,12*pi))
    ocp.bound('endTime',(1.5,15))
    ocp.bound('w0',(10,10))

    ocp.bound('phase0',(-12*pi,12*pi))
    ocp.bound('phaseF',(-12*pi,12*pi))

    # boundary conditions
    ocp.bound('delta',(0,0),timestep=-1,quiet=True)
    ocp.bound('ddelta',(0,0),timestep=-1,quiet=True)

    def getFourierFit(filename,phase):
        # load the fourier fit
        f=open(filename,'r')
        trajFits = pickle.load(f)
        f.close()
        trajFits.setPhase(phase)
        return trajFits
    startup   = getFourierFit("data/carousel_opt_fourier.dat",ocp.lookup('phase0'))
    crosswind = getFourierFit("data/crosswind_opt_fourier.dat",ocp.lookup('phaseF'))

    def getFourierDcm(vars):
        return C.vertcat([C.horzcat([vars['e11'], vars['e12'], vars['e13']]),
                          C.horzcat([vars['e21'], vars['e22'], vars['e23']]),
                          C.horzcat([vars['e31'], vars['e32'], vars['e33']])])

    
#    for name in ['y','z','dy','dz','r','dr','w1','w2','w3','delta','ddelta']:
    for name in ['y','z','dy','dz','r','dr','delta','ddelta']:
        ocp.constrain(ocp.lookup(name,timestep=0), '==', startup[name])
    
#    for name in ['y','z','dy','dz','r','dr','w1','w2','w3']:
    for name in ['y','z','dy','dz','r','dr']:
        ocp.constrain(ocp.lookup(name,timestep=-1), '==', crosswind[name])

    # match DCMs
    rawe.kiteutils.matchDcms(ocp, rawe.kiteutils.getDcm(ocp,0),  getFourierDcm(startup))
    rawe.kiteutils.matchDcms(ocp, rawe.kiteutils.getDcm(ocp,-1), getFourierDcm(crosswind))

    # initial guess
    phase0Guess = -4.0*pi
    phaseFGuess = -0.4*2*pi
    
    namesF = ['x','y','z','dx','dy','dz','r','dr','w1','w2','w3','e11','e12','e13','e21','e22','e23','e31','e32','e33']
    names0 = namesF+['delta','ddelta']

    initfun = C.MXFunction([ocp.lookup('phase0')],[startup[name] for name in names0])
    initfun.init()
    for j in range(nk+1):
        alpha = float(j)/nk
        phase = phase0Guess*(1-alpha) + phaseFGuess*alpha
        initfun.setInput([phase])
        initfun.evaluate()
        initvals = dict([(name,float(initfun.output(k)[0,0])) for k,name in enumerate(names0)])
        for name in initvals:
            ocp.guess(name,float(initvals[name]),timestep=j)

#    finalfun = C.MXFunction([ocp.lookup('phaseF')],[crosswind[name] for name in namesF])
#    finalfun.init()
#    finalfun.setInput([phaseFGuess])
#    finalfun.evaluate()
#    finalvals = dict([(name,float(finalfun.output(k)[0,0])) for k,name in enumerate(namesF)])
#    initvals['delta'] = 0
#    initvals['ddelta'] = 0
#    finalvals['delta'] = 0
#    finalvals['ddelta'] = 0

#    for name in namesF:
#        for k in range(nk+1):
#            alpha = float(k)/nk
#            ocp.guess(name,(1-alpha)*initvals[name] + alpha*finalvals[name], timestep=k)
#    
#    for j,name in enumerate(namesF):
#        for k in range(nk+1):
#            alpha = float(k)/nk
#            finalfun.setInput([phaseFGuess])
#            finalval = float(finalfun.output(k)[0,0])) for k,name in enumerate(namesF)])
#            ocp.guess(name,(1-alpha)*initvals[name] + alpha*finalvals[name], timestep=k)
    
    ocp.guess('aileron',0)
    ocp.guess('elevator',0)
    ocp.guess('ddr',0)
    ocp.guess('motor_torque',0)
    ocp.guess('phase0',phase0Guess)
    ocp.guess('phaseF',phaseFGuess)
    ocp.guess('endTime',8)
    ocp.guess('w0',10)
    
    # objective function
    obj = 0
    for k in range(nk):
        u = ocp.uVec(k)
        ddr = ocp.lookup('ddr',timestep=k)
        tc = ocp.lookup('motor_torque',timestep=k)
        torqueSigma = 1000.0
        aileron = ocp.lookup('aileron',timestep=k)
        elevator = ocp.lookup('elevator',timestep=k)
        
        aileronSigma = 0.1
        elevatorSigma = 0.1
        torqueSigma = 1000.0
        ddrSigma = 5.0
        
        ailObj = aileron*aileron / (aileronSigma*aileronSigma)
        eleObj = elevator*elevator / (elevatorSigma*elevatorSigma)
        winchObj = ddr*ddr / (ddrSigma*ddrSigma)
        torqueObj = tc*tc / (torqueSigma*torqueSigma)
        
        obj += ailObj + eleObj + winchObj + torqueObj
        
    ocp.setObjective( 1e1*obj/nk + ocp.lookup('endTime') )

    oldKiteProtos = []
    mcc = rawe.kite_pb2.MultiCarousel().FromString(startup.multiCarousel)
    oldKiteProtos.extend(mcc.horizon)
    mcc = rawe.kite_pb2.MultiCarousel().FromString(crosswind.multiCarousel)
    oldKiteProtos.extend(mcc.horizon)

    for okp in oldKiteProtos:
        okp.zt = conf['zt']
        okp.rArm = conf['rArm']
        okp.lineTransparency = 0.0
        okp.kiteTransparency = 0.3
#    for k in range(0,startupfits['x'].Mx.size):
#        oldKiteProtos.append( toFourierKiteProto(startupfits,k,zt=conf['kite']['zt'], rArm=conf['carousel']['rArm'],kiteAlpha=0.3,lineAlpha=0,dz=0) )
#    for k in range(0,crosswindfits['x'].Mx.size):
#        oldKiteProtos.append( toFourierKiteProto(crosswindfits,k,zt=conf['kite']['zt'], rArm=conf['carousel']['rArm'],kiteAlpha=0.3,lineAlpha=0,dz=0) )

    # callback function
    def myCallback(traj,myiter,ocp_,conf_):
        kiteProtos = []
        for k in range(0,ocp.nk):
            for nicpIdx in range(0,ocp.nicp):
                for degIdx in [0]:
#                for degIdx in range(ocp.deg+1):
                    lookup = lambda name: traj.lookup(name,timestep=k,nicpIdx=nicpIdx,degIdx=degIdx)
                    kiteProtos.append( kiteproto.toKiteProto(lookup,
                                                             lineAlpha=0.2) )
        kiteProtos += oldKiteProtos
        mc = kite_pb2.MultiCarousel()
        mc.horizon.extend(list(kiteProtos))
        
        mc.messages.append("w0: "+str(traj.lookup('w0')))
        mc.messages.append("iter: "+str(myiter))
        mc.messages.append("endTime: "+str(traj.lookup('endTime')))
        mc.messages.append("average power: "+str(traj.lookup('quadrature energy',timestep=-1)/traj.lookup('endTime'))+" W")
        mc.messages.append("phase0: "+str(traj.lookup('phase0')/pi)+" * pi")
        mc.messages.append("phaseF: "+str(traj.lookup('phaseF')/pi)+" * pi")

#         # bounds feedback
#        lbx = ocp.solver.input(C.NLP_LBX)
#        ubx = ocp.solver.input(C.NLP_UBX)
#        ocp._bounds.printBoundsFeedback(xOpt,lbx,ubx,reportThreshold=0)
#
#        # constraints feedback
#        lbg = ocp.solver.input(C.NLP_LBG)
#        ubg = ocp.solver.input(C.NLP_UBG)
#        g = numpy.array(f.input(C.NLP_G))
#        ocp._constraints.printViolations(g,lbg,ubg,reportThreshold=0)
        
        return mc.SerializeToString()
    callback = rawe.telemetry.startTelemetry(ocp, conf, callbacks=[(myCallback,'multi-carousel')])


    # solver
    solverOptions = [ ("expand_f",True)
                    , ("expand_g",True)
                    , ("generate_hessian",True)
#                     ,("qp_solver",C.NLPQPSolver)
#                     ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}})
                    , ("linear_solver","ma57")
                    , ("max_iter",1000)
                    , ("tol",1e-9)
#                    , ("Timeout", 1e6)
#                    , ("UserHM", True)
#                    , ("ScaleConIter",True)
#                    , ("ScaledFD",True)
#                    , ("ScaledKKT",True)
#                    , ("ScaledObj",True)
#                    , ("ScaledQP",True)
                    ]

    print "setting up solver..."
    ocp.setupSolver( solverOpts=solverOptions,
                     callback=callback )
    return ocp
Пример #54
0
def setupOcp(dae,conf,nk,nicp,deg,collPoly):
    def addCosts():
        dddr = dae['dddr']
        daileron = dae['daileron']
        delevator = dae['delevator']
        drudder = dae['drudder']
        dflaps = dae['dflaps']

        daileronSigma = 0.001
        delevatorSigma = 0.8
        dddrSigma = 10.0
        drudderSigma = 0.1
        dflapsSigma = 0.1

        fudgeFactor = 1e-1
        nkf = float(nk)
        dae['daileronCost'] =  fudgeFactor*daileron*daileron / (daileronSigma*daileronSigma*nkf)
        dae['delevatorCost'] = fudgeFactor*delevator*delevator / (delevatorSigma*delevatorSigma*nkf)
        dae['drudderCost'] =   fudgeFactor*drudder*drudder / (drudderSigma*drudderSigma)
        dae['dflapsCost'] =    fudgeFactor*dflaps*dflaps / (dflapsSigma*dflapsSigma)
        dae['dddrCost'] =      fudgeFactor*dddr*dddr / (dddrSigma*dddrSigma*nkf)

    addCosts()

    ocp = rawe.collocation.Coll(dae, nk=nk, nicp=nicp, deg=deg, collPoly=collPoly)

    print "setting up collocation..."
    ocp.setupCollocation(ocp.lookup('endTime'))

    print "moar setting up ocp..."

    # constrain invariants
    def constrainInvariantErrs():
        R_n2b = ocp.lookup('R_n2b',timestep=0)
        rawekite.kiteutils.makeOrthonormal(ocp, R_n2b)
        ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('c(0)==0',None))
        ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('cdot(0)==0',None))
    constrainInvariantErrs()

    # constrain line angle
    for k in range(0,nk):
        for j in range(0,ocp.deg+1):
            ocp.constrain(ocp.lookup('cos_line_angle',timestep=k,degIdx=j),'>=',C.cos(65*pi/180), tag=('line angle',k))

    # constrain airspeed
    def constrainAirspeedAlphaBeta():
        for k in range(0,nk):
            ocp.constrain(ocp.lookup('airspeed',timestep=k), '>=', 10, tag=('airspeed',k))
            for j in range(0,ocp.deg+1):
                ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,8.5), tag=('alpha(deg)',k))

            ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k), (-9,9), tag=('beta(deg)',k))
    constrainAirspeedAlphaBeta()
    #def constrainCl():
    #    for k in range(0,nk):
    #        ocp.constrain(ocp.lookup('cL',timestep=k), '<=', 2.0, tag=('cL',k))
    #constrainCl()

    # constrain tether force
    for k in range(nk):
#        ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=1), '>=', 0, tag=('tether tension positive',k))
#        ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=ocp.deg), '>=', 0, tag=('tether tension positive',k))
        for j in range(1,ocp.deg+1):
            ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=j), '>=', 0, tag=('tether tension positive',k))

#    # real motor constraints
#    for k in range(nk):
##        ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=1),       '<=', 150, tag=('motor torque',k))
##        ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=ocp.deg), '<=', 150, tag=('motor torque',k))
#        ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=1),       '<=', 78, tag=('motor torque',k))
#        ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=ocp.deg), '<=', 78, tag=('motor torque',k))
#
#        ocp.constrain( ocp.lookup('rpm',timestep=k),       '<=', 1500, tag=('rpm',k))
#        ocp.constrain( -1500, '<=', ocp.lookup('rpm',timestep=k),       tag=('rpm',k))

    # make it periodic
    for name in [ "y","z",
                  "dy","dz",
                  "w_bn_b_x","w_bn_b_y","w_bn_b_z",
                  "r","dr","ddr",
                  'aileron','elevator','rudder','flaps'
                  ]:
        ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1), tag=('periodic diff state \"'+name+'"',None))

    # periodic attitude
    rawekite.kiteutils.periodicDcm(ocp)

    # bounds
    ocp.bound('aileron', (numpy.radians(-10),numpy.radians(10)))
    ocp.bound('elevator',(numpy.radians(-10),numpy.radians(10)))
    ocp.bound('rudder',  (numpy.radians(-10),numpy.radians(10)))
    ocp.bound('flaps',  (numpy.radians(0),numpy.radians(0)))
    # can't bound flaps==0 AND have periodic flaps at the same time
    # bounding flaps (-1,1) at timestep 0 doesn't really free them, but satisfies LICQ
    ocp.bound('flaps', (-1,1),timestep=0,quiet=True)
    ocp.bound('daileron',(-2.0,2.0))
    ocp.bound('delevator',(-2.0,2.0))
    ocp.bound('drudder',(-2.0,2.0))
    ocp.bound('dflaps',(-2.0,2.0))

    ocp.bound('x',(-2000,2000))
    ocp.bound('y',(-2000,2000))
    if 'minAltitude' in conf:
        ocp.bound('z',(-2000, -conf['minAltitude']))
    else:
        ocp.bound('z',(-2000, -0.05))
    ocp.bound('r',(1,500))
    ocp.bound('dr',(-30,30))
    ocp.bound('ddr',(-500,500))
    ocp.bound('dddr',(-50000,50000))

    for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
        ocp.bound(e,(-1.1,1.1))

    for d in ['dx','dy','dz']:
        ocp.bound(d,(-200,200))

    for w in ['w_bn_b_x',
              'w_bn_b_y',
              'w_bn_b_z']:
        ocp.bound(w,(-6*pi,6*pi))

#    ocp.bound('endTime',(0.5,12))
    ocp.bound('endTime',(0.5,numLoops*7.5))
    ocp.bound('w0',(10,10))

    # boundary conditions
    ocp.bound('y',(0,0),timestep=0,quiet=True)

    # guesses
    ocp.guess('endTime',5.4)
    ocp.guess('w0',10)

    # objective function
    obj = 0
    for k in range(nk):
        # control regularization
        obj += ocp.lookup('daileronCost',timestep=k)
        obj += ocp.lookup('delevatorCost',timestep=k)
        obj += ocp.lookup('drudderCost',timestep=k)
        obj += ocp.lookup('dflapsCost',timestep=k)
        obj += ocp.lookup('dddrCost',timestep=k)

    ocp.setQuadratureDdt('mechanical_energy', 'mechanical_winch_power')
    ocp.setQuadratureDdt('electrical_energy', 'electrical_winch_power')

    ocp.setObjective( obj + ocp.lookup(powerType+'_energy',timestep=-1)/ocp.lookup('endTime') )

    return ocp
Пример #55
0
def setupOcp(dae,conf,nk,nicp=1,deg=4):
    ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)

    print "setting up collocation..."
    ocp.setupCollocation(ocp.lookup('endTime'))

    for k in range(ocp.nk):
        for j in range(1,ocp.deg+1): #[1]:
            ocp.constrain( ocp('x',timestep=k,degIdx=j), '>=', ocp('y',timestep=k,degIdx=j), tag=('x>y',k))
            ocp.constrain( ocp('x',timestep=k,degIdx=j), '>=', -ocp('y',timestep=k,degIdx=j), tag=('x>-y',k))

    # constrain invariants
    def constrainInvariantErrs():
        R_c2b = ocp.lookup('R_c2b',timestep=0)
        rawekite.kiteutils.makeOrthonormal(ocp, R_c2b)
        ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c 0',None))
        ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot 0',None))
        ocp.constrain(ocp('sin_delta',timestep=0)**2 + ocp('cos_delta',timestep=0)**2,
                      '==', 1, tag=('sin**2 + cos**2 == 1',None))
    constrainInvariantErrs()

    # constrain line angle
    for k in range(0,nk):
        ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(45*pi/180), tag=('line angle',k))

    # constrain airspeed
    def constrainAirspeedAlphaBeta():
        for k in range(0,nk):
            for j in range(0,deg+1):
                ocp.constrainBnds(ocp.lookup('airspeed',timestep=k,degIdx=j),
                                  (8,35), tag=('airspeed',(k,j)))
                ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,7.5), tag=('alpha(deg)',nk))
                ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k,degIdx=j), (-6,6), tag=('beta(deg)',nk))
                ocp.constrain(ocp.lookup('cL',timestep=k,degIdx=j), '>=', 0, tag=('CL positive',nk))
    constrainAirspeedAlphaBeta()

    # constrain tether force
    constrainTetherForce(ocp)
    realMotorConstraints(ocp)

    # make it periodic
    for name in [ "y","z",
                  "dy","dz",
                  "w_bn_b_x","w_bn_b_y","w_bn_b_z",
                  "r","dr",'ddr',
                  'ddelta',
                  'aileron','elevator','rudder','flaps',
                  'sin_delta','motor_torque'
                  ]:
        ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1), tag=('periodic '+name,None))

    # periodic attitude
    rawekite.kiteutils.periodicDcm(ocp)

    # bounds
    ocp.bound('aileron', (numpy.radians(-8),numpy.radians(8)))
    ocp.bound('elevator',(numpy.radians(-8),numpy.radians(8)))
    ocp.bound('rudder',  (numpy.radians(-8),numpy.radians(8)))
    ocp.bound('flaps',  (numpy.radians(0),numpy.radians(0)))
    # can't bound flaps==0 AND have periodic flaps at the same time
    # bounding flaps (-1,1) at timestep 0 doesn't really free them, but satisfies LICQ
    ocp.bound('flaps', (-1,1),timestep=0,quiet=True)
    ocp.bound('daileron',  (numpy.radians(-20), numpy.radians(20)))
    ocp.bound('delevator', (numpy.radians(-20), numpy.radians(20)))
    ocp.bound('drudder',   (numpy.radians(-20), numpy.radians(20)))
    ocp.bound('dflaps',    (numpy.radians(-20), numpy.radians(20)))
    ocp.bound('ddelta',(-0.98*2*pi, 0.98*2*pi))

    ocp.bound('x',(-2000,2000))
    ocp.bound('y',(-2000,2000))
    ocp.bound('z',(-3, -0.3))
    ocp.bound('r',(5,8))
    # can't bound r AND have periodic r at the same time
    ocp.bound('r',(0,100),timestep=-1)
    ocp.bound('dr',(-1,1))
    ocp.bound('ddr',(-15,15))
    ocp.bound('dddr',(-15,15))

    ocp.bound('motor_torque',(-50,50))
    ocp.bound('dmotor_torque',(-1,1))

    ocp.bound('cos_delta',(-1.5,1.5))
    ocp.bound('sin_delta',(-1.5,1.5))

    for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
        ocp.bound(e,(-1.1,1.1))

    for d in ['dx','dy','dz']:
        ocp.bound(d,(-70,70))

    for w in ['w_bn_b_x',
              'w_bn_b_y',
              'w_bn_b_z']:
        ocp.bound(w,(-3*pi,3*pi))

    ocp.bound('endTime',(0.5,4.0))
    ocp.guess('endTime',1)
    ocp.bound('w0',(10,10))

    # boundary conditions
    ocp.bound('sin_delta', (0,0), timestep=0, quiet=True)
    ocp.bound('cos_delta', (0.5,1.5), timestep=0, quiet=True)
    ocp.bound('cos_delta', (0.5,1.5), timestep=-1, quiet=True)

    return ocp
Пример #56
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)
Пример #57
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
Пример #58
0
def setupOcp(dae,conf,nk,nicp=1,deg=4):
    ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)

    print "setting up collocation..."
    ocp.setupCollocation(ocp.lookup('endTime'))

    # constrain invariants
    def constrainInvariantErrs():
        R_n2b = ocp.lookup('R_n2b',timestep=0)
        rawekite.kiteutils.makeOrthonormal(ocp, R_n2b)
        ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c 0',None))
        ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot 0',None))
    constrainInvariantErrs()

    # constrain line angle
    for k in range(0,nk):
        ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(55*pi/180), tag=('line angle',k))

    # constrain airspeed
    def constrainAirspeedAlphaBeta():
        for k in range(0,nk):
            for j in range(0,deg+1):
                ocp.constrain(ocp.lookup('airspeed',timestep=k,degIdx=j), '>=', 20, tag=('airspeed',nk))
                ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,8.5), tag=('alpha',nk))
                ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k,degIdx=j), (-10,10), tag=('beta',nk))
    constrainAirspeedAlphaBeta()

    # constrain tether force
    for k in range(nk):
        ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=1), '>=', 0, tag=('tether tension',(nk,0)))
        ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=ocp.deg), '>=', 0, tag=('tether tension',(nk,1)))

    # make it periodic
    for name in [ "r_n2b_n_y","r_n2b_n_z",
                  "v_bn_n_y","v_bn_n_z",
                  "w_bn_b_x","w_bn_b_y","w_bn_b_z",
                  "r","dr",'ddr',
                  'aileron','elevator','rudder','flaps',
                  ]:
        ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1), tag=('periodic '+name,None))

    # periodic attitude
    rawekite.kiteutils.periodicDcm(ocp)

    # bounds
    ocp.bound('aileron', (numpy.radians(-10),numpy.radians(10)))
    ocp.bound('elevator',(numpy.radians(-10),numpy.radians(10)))
    ocp.bound('rudder',  (numpy.radians(-10),numpy.radians(10)))
    ocp.bound('flaps',  (numpy.radians(0),numpy.radians(0)))
    # can't bound flaps==0 AND have periodic flaps at the same time
    # bounding flaps (-1,1) at timestep 0 doesn't really free them, but satisfies LICQ
    ocp.bound('flaps', (-1,1),timestep=0,quiet=True)
    ocp.bound('daileron',(-2.0,2.0))
    ocp.bound('delevator',(-2.0,2.0))
    ocp.bound('drudder',(-2.0,2.0))
    ocp.bound('dflaps',(-2.0,2.0))

    ocp.bound('r_n2b_n_x',(-2000,2000))
    ocp.bound('r_n2b_n_y',(-2000,2000))
    if 'minAltitude' in conf:
        ocp.bound('r_n2b_n_z',(-2000, -conf['minAltitude']))
    else:
        ocp.bound('r_n2b_n_z',(-2000, -0.5))
    ocp.bound('r',(1,2000))
    ocp.bound('dr',(-30,30))
    ocp.bound('ddr',(-15,15))
    ocp.bound('dddr',(-15,15))

    for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
        ocp.bound(e,(-1.1,1.1))

    for d in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']:
        ocp.bound(d,(-70,70))

    for w in ['w_bn_b_x',
              'w_bn_b_y',
              'w_bn_b_z']:
        ocp.bound(w,(-4*pi,4*pi))

    ocp.bound('endTime',(4.0,4.0))
    ocp.guess('endTime',4.0)
    ocp.bound('w0',(10,10))

    # boundary conditions
    ocp.bound('r_n2b_n_y',(0,0),timestep=0,quiet=True)

    return ocp
Пример #59
0
def setupOcp(dae,conf,publisher,nk=50,nicp=1,deg=4):
    ocp = Coll(dae, nk=nk,nicp=nicp,deg=deg)
    
    # constrain invariants
    def invariantErrs():
        dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')])
                         , C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')])
                         , C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')])
                         ] ).trans()
        err = C.mul(dcm.trans(), dcm)
        dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ])
        f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
                        , [dae.output('c'),dae.output('cdot'),dcmErr]
                        )
        f.setOption('name','invariant errors')
        f.init()
        return f

    [c0,cdot0,dcmError0] = invariantErrs().call([ocp.xVec(0),ocp.uVec(0),ocp.pVec()])
    ocp.constrain(c0,'==',0)
    ocp.constrain(cdot0,'==',0)
    ocp.constrain(dcmError0,'==',0)

    # constrain line angle
    for k in range(0,nk+1):
        ocp.constrain(kiteutils.getCosLineAngle(ocp,k),'>=',C.cos(55*pi/180))

    # constrain airspeed
    def constrainAirspeedAlphaBeta():
        f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
                        , [dae.output('airspeed'),dae.output('alpha(deg)'),dae.output('beta(deg)')]
                        )
        f.setOption('name','airspeed/alpha/beta')
        f.init()

        for k in range(0,nk):
            [airspeed,alphaDeg,betaDeg] = f.call([ocp.xVec(k),ocp.uVec(k),ocp.pVec()])
            ocp.constrainBnds(airspeed,(10,50))
            ocp.constrainBnds(alphaDeg,(-5,10))
            ocp.constrainBnds(betaDeg,(-10,10))
    constrainAirspeedAlphaBeta()

    # make it periodic
    for name in [ #"x"   # state 0
                  "y"   # state 1
                , "z"   # state 2
#                , "e11" # state 3
#                , "e12" # state 4
#                , "e13" # state 5
#                , "e21" # state 6
#                , "e22" # state 7
#                , "e23" # state 8
#                , "e31" # state 9
#                , "e32" # state 10
#                , "e33" # state 11
#                , "dx"  # state 12
                , "dy"  # state 13
                , "dz"  # state 14
                , "w1"  # state 15
                , "w2"  # state 16
                , "w3"  # state 17
                , "r" # state 20
                , "dr" # state 21
                ]:
        ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1))

    # periodic attitude
    kiteutils.periodicDcm(ocp)

    # bounds
    ocp.bound('aileron',(-0.04,0.04))
    ocp.bound('elevator',(-0.1,0.1))

    ocp.bound('x',(-2,200))
    ocp.bound('y',(-200,200))
    ocp.bound('z',(0.5,200))
    ocp.bound('r',(1,30))
    ocp.bound('dr',(-10,10))
    ocp.bound('ddr',(-2.5,2.5))

    for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
        ocp.bound(e,(-1.1,1.1))

    for d in ['dx','dy','dz']:
        ocp.bound(d,(-70,70))

    for w in ['w1','w2','w3']:
        ocp.bound(w,(-4*pi,4*pi))

    ocp.bound('endTime',(0.5,5))
    ocp.bound('w0',(10,10))
    ocp.bound('energy',(-1e6,1e6))

    # boundary conditions
    ocp.bound('energy',(0,0),timestep=0,quiet=True)
    ocp.bound('y',(0,0),timestep=0,quiet=True)
    
    # objective function
    obj = 0
    for k in range(nk):
        u = ocp.uVec(k)
        ddr = ocp.lookup('ddr',timestep=k)
        aileron = ocp.lookup('aileron',timestep=k)
        elevator = ocp.lookup('elevator',timestep=k)
        
        aileronSigma = 0.1
        elevatorSigma = 0.1
        ddrSigma = 5.0
        
        ailObj = aileron*aileron / (aileronSigma*aileronSigma)
        eleObj = elevator*elevator / (elevatorSigma*elevatorSigma)
        winchObj = ddr*ddr / (ddrSigma*ddrSigma)
        
        obj += ailObj + eleObj + winchObj
    ocp.setObjective( 1e1*C.sumAll(obj)/float(nk) + ocp.lookup('energy',timestep=-1)/ocp.lookup('endTime') )

    # callback function
    class MyCallback:
        def __init__(self):
            self.iter = 0 
        def __call__(self,f,*args):
            self.iter = self.iter + 1
            xOpt = numpy.array(f.input(C.NLP_X_OPT))

            opt = ocp.devectorize(xOpt)
            xup = opt['vardict']
            
            kiteProtos = []
            for k in range(0,nk):
                j = nicp*(deg+1)*k
                kiteProtos.append( kiteproto.toKiteProto(C.DMatrix(opt['x'][:,j]),
                                                         C.DMatrix(opt['u'][:,j]),
                                                         C.DMatrix(opt['p']),
                                                         conf['kite']['zt'],
                                                         conf['carousel']['rArm'],
                                                         zeroDelta=True) )
#            kiteProtos = [kiteproto.toKiteProto(C.DMatrix(opt['x'][:,k]),C.DMatrix(opt['u'][:,k]),C.DMatrix(opt['p']), conf['kite']['zt'], conf['carousel']['rArm'], zeroDelta=True) for k in range(opt['x'].shape[1])]
            
            mc = kite_pb2.MultiCarousel()
            mc.css.extend(list(kiteProtos))
            
            mc.messages.append("endTime: "+str(xup['endTime']))
            mc.messages.append("w0: "+str(xup['w0']))
            mc.messages.append("iter: "+str(self.iter))

            # bounds feedback
#            lbx = ocp.solver.input(C.NLP_LBX)
#            ubx = ocp.solver.input(C.NLP_UBX)
#            violations = boundsFeedback(xOpt,lbx,ubx,ocp.bndtags,tolerance=1e-9)
#            for name in violations:
#                violmsg = "violation!: "+name+": "+str(violations[name])
#                mc.messages.append(violmsg)
            
            publisher.send_multipart(["multi-carousel", mc.SerializeToString()])


    # solver
    solverOptions = [ ("expand_f",True)
                    , ("expand_g",True)
                    , ("generate_hessian",True)
#                     ,("qp_solver",C.NLPQPSolver)
#                     ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}})
                    , ("linear_solver","ma57")
                    , ("max_iter",1000)
                    , ("tol",1e-9)
#                    , ("Timeout", 1e6)
#                    , ("UserHM", True)
#                    , ("ScaleConIter",True)
#                    , ("ScaledFD",True)
#                    , ("ScaledKKT",True)
#                    , ("ScaledObj",True)
#                    , ("ScaledQP",True)
                    ]
    
    # initial guess
#    ocp.guessX(x0)
#    for k in range(0,nk+1):
#        val = 2.0*pi*k/nk
#        ocp.guess('delta',val,timestep=k,quiet=True)
#
#    ocp.guess('aileron',0)
#    ocp.guess('elevator',0)
#    ocp.guess('tc',0)
    ocp.guess('endTime',5.4)
#
#    ocp.guess('ddr',0)
    ocp.guess('w0',10)

    print "setting up collocation..."
    ocp.setupCollocation(ocp.lookup('endTime'))
    print "setting up solver..."
    ocp.setupSolver( solverOpts=solverOptions,
                     callback=MyCallback() )
    return ocp