Ejemplo n.º 1
0
def makeOrthonormal(ocp_, R):
    ocp_.constrain(C.mul(R[0, :], R[0, :].T),
                   '==',
                   1,
                   tag=('R1[0]: e1^T * e1 == 1', None))
    ocp_.constrain(C.mul(R[1, :], R[0, :].T),
                   '==',
                   0,
                   tag=('R1[0]: e2^T * e1 == 0', None))
    ocp_.constrain(C.mul(R[1, :], R[1, :].T),
                   '==',
                   1,
                   tag=('R1[0]: e2^T * e2 == 1', None))
    rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
    ocp_.constrain(rhon[0],
                   '==',
                   0,
                   tag=('R1[0]: ( e1^T X e2 - e3 )[0] == 0', None))
    ocp_.constrain(rhon[2],
                   '==',
                   0,
                   tag=('R1[0]: ( e1^T X e2 - e3 )[1] == 0', None))
    ocp_.constrain(rhon[1],
                   '==',
                   0,
                   tag=('R1[0]: ( e1^T X e2 - e3 )[2] == 0', None))
Ejemplo n.º 2
0
def belief_dynamics(belief, control,     # current (b, u)
                    F, dt, A_fcn, C_fcn, # dynamics
                    Q, obs_cov_fcn):     # covariances Q and R(x)
    belief_next = cat.struct_SX(belief)
    
    # Predict the mean
    [mu_bar] = F([ belief['m'], control, dt ])
    
    # Compute linearization
    [A,_] = A_fcn([ belief['m'], control, dt ])
    [C,_] = C_fcn([ mu_bar ])
    
    # Predict the covariance
    S_bar = ca.mul([ A, belief['S'], A.T ]) + Q
    
    # Get the observations noise covariance
    [R] = obs_cov_fcn([ mu_bar ])
    
    # Compute the inverse
    P = ca.mul([ C, S_bar, C.T ]) + R
    P_inv = ca.inv(P)
    
    # Kalman gain
    K = ca.mul([ S_bar, C.T, P_inv ])
    
    # Update equations
    nx = belief['m'].size()
    belief_next['m'] = mu_bar
    belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar)
    
    # (belief, control) -> next_belief
    return ca.SXFunction('Belief dynamics',[belief,control],[belief_next])
Ejemplo n.º 3
0
    def _create_BF(self):
        """Belief dynamics"""
        b_next = cat.struct_SX(self.b)

        # Compute the mean
        [mu_bar] = self.F([self.b['m'], self.u])

        # Compute linearization
        [A, _] = self.Fj_x([self.b['m'], self.u])
        [C, _] = self.hj_x([mu_bar])

        # Get system and observation noises, as if the state was mu_bar
        [M] = self.M([self.b['m'], self.u])
        [N] = self.N([mu_bar])

        # Predict the covariance
        S_bar = ca.mul([A, self.b['S'], A.T]) + M

        # Compute the inverse
        P = ca.mul([C, S_bar, C.T]) + N
        P_inv = ca.inv(P)

        # Kalman gain
        K = ca.mul([S_bar, C.T, P_inv])

        # Update equations
        b_next['m'] = mu_bar
        b_next['S'] = ca.mul(ca.DMatrix.eye(self.nx) - ca.mul(K, C), S_bar)

        # (b, u) -> b_next
        op = {'input_scheme': ['b', 'u'],
              'output_scheme': ['b_next']}
        return ca.SXFunction('Belief dynamics',
                             [self.b, self.u], [b_next], op)
Ejemplo n.º 4
0
 def makeOrthonormal(self, g, R):
     g.add(C.mul(R[0, :], R[0, :].T), '==', 1, tag = ('R1[0]: e1^T * e1 == 1', None))
     g.add(C.mul(R[1, :], R[0, :].T), '==', 0, tag = ('R1[0]: e2^T * e1 == 0', None))
     g.add(C.mul(R[1, :], R[1, :].T), '==', 1, tag = ('R1[0]: e2^T * e2 == 1', None))
     rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
     g.add(rhon[0], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[0] == 0', None))
     g.add(rhon[2], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[1] == 0', None))
     g.add(rhon[1], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[2] == 0', None))
Ejemplo n.º 5
0
def T2W(T, p, dp):
    """
   w_101 = T2W(T_10,p,dp)
   
  """
    R = T2R(T)
    dR = c.reshape(c.mul(c.jacobian(R, p), dp), (3, 3))
    return invskew(c.mul(R.T, dR))
Ejemplo n.º 6
0
def T2W(T,p,dp):
  """
   w_101 = T2W(T_10,p,dp)
   
  """
  R = T2R(T)
  dR = c.reshape(c.mul(c.jacobian(R,p),dp),(3,3))
  return invskew(c.mul(R.T,dR))
Ejemplo n.º 7
0
def makeOrthonormal(g,R):
         g.add(C.mul(R[0,:],R[0,:].T),'==',1,  tag=('R1[0]: e1^T * e1 == 1',None))
         g.add(C.mul(R[1,:],R[0,:].T),'==',0,  tag=('R1[0]: e2^T * e1 == 0',None))
         g.add(C.mul(R[1,:],R[1,:].T),'==',1,  tag=('R1[0]: e2^T * e2 == 1',None))
         rhon = C.cross(R[0,:],R[1,:]) - R[2,:]
         g.add(rhon[0],'==',0,  tag=('R1[0]: ( e1^T X e2 - e3 )[0] == 0',None))
         g.add(rhon[2],'==',0,  tag=('R1[0]: ( e1^T X e2 - e3 )[1] == 0',None))
         g.add(rhon[1],'==',0,  tag=('R1[0]: ( e1^T X e2 - e3 )[2] == 0',None))
Ejemplo n.º 8
0
def get_orthonormal_constraints(R):
    ret = []
    ret.append( (C.mul(R[0,:],R[0,:].T) - 1,  'R1[0]: e1^T * e1 - 1 == 0') )
    ret.append( (C.mul(R[1,:],R[0,:].T),      'R1[0]: e2^T * e1     == 0') )
    ret.append( (C.mul(R[1,:],R[1,:].T) - 1,  'R1[0]: e2^T * e2 - 1 == 0') )
    rhon = C.cross(R[0,:],R[1,:]) - R[2,:]
    ret.append( (rhon[0,0], 'R1[0]: ( e1^T X e2 - e3 )[0] == 0') )
    ret.append( (rhon[0,2], 'R1[0]: ( e1^T X e2 - e3 )[1] == 0') )
    ret.append( (rhon[0,1], 'R1[0]: ( e1^T X e2 - e3 )[2] == 0') )
    return ret
Ejemplo n.º 9
0
def get_orthonormal_constraints(R):
    ret = []
    ret.append((C.mul(R[0, :], R[0, :].T) - 1, 'R1[0]: e1^T * e1 - 1 == 0'))
    ret.append((C.mul(R[1, :], R[0, :].T), 'R1[0]: e2^T * e1     == 0'))
    ret.append((C.mul(R[1, :], R[1, :].T) - 1, 'R1[0]: e2^T * e2 - 1 == 0'))
    rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
    ret.append((rhon[0], 'R1[0]: ( e1^T X e2 - e3 )[0] == 0'))
    ret.append((rhon[2], 'R1[0]: ( e1^T X e2 - e3 )[1] == 0'))
    ret.append((rhon[1], 'R1[0]: ( e1^T X e2 - e3 )[2] == 0'))
    return ret
Ejemplo n.º 10
0
    def setQuadratureDdt(self,quadratureStateName,quadratureStateDotName,lookup,lagrangePoly,h,symbolicDvs):
        if quadratureStateName in self._quadratures:
            raise ValueError(quadratureStateName+" is not unique")

        qStates = np.resize(np.array([None],dtype=C.MX),(self._nk+1,self._nicp,self._deg+1))
        # set the dimension of the initial quadrature state correctly, and make sure the derivative name is valid
        try:
            qStates[0,0,0] = 0*lookup(quadratureStateDotName,timestep=0,nicpIdx=0,degIdx=1)
        except ValueError:
            raise ValueError('quadrature derivative name \"'+quadratureStateDotName+
                             '\" is not a valid x/z/u/p/output')

        ldInv = np.linalg.inv(lagrangePoly.lDotAtTauRoot[1:,1:])
        ld0 = lagrangePoly.lDotAtTauRoot[1:,0]
        l1 = lagrangePoly.lAtOne
#        print -C.mul(C.mul(ldInv, ld0).T, l1[1:]) + l1[0]

        breakQuadratureIntervals = True

        if breakQuadratureIntervals:
            for k in range(self._nk):
                for i in range(self._nicp):
                    dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j)
                                      for j in range(1,self._deg+1)])
                    Qs = C.mul( ldInv, dQs)
                    for j in range(1,self._deg+1):
                        qStates[k,i,j] = qStates[k,i,0] + Qs[j-1]
                    
                    m = C.mul( Qs.T, l1[1:])
                    if i < self._nicp - 1:
                        qStates[k,i+1,0] = qStates[k,i,0] + m
                    else:
                        qStates[k+1,0,0] = qStates[k,i,0] + m
        else:
            for k in range(self._nk):
                for i in range(self._nicp):
                    dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j)
                                      for j in range(1,self._deg+1)])
                    Qs = C.mul( ldInv, dQs - ld0*qStates[k,i,0] )
                    for j in range(1,self._deg+1):
                        qStates[k,i,j] = Qs[j-1]
                    
                    m = C.veccat( [qStates[k,i,0], Qs] )
                    m = C.mul( m.T, l1)
                    if i < self._nicp - 1:
                        qStates[k,i+1,0] = m
                    else:
                        qStates[k+1,0,0] = m
                
        self._quadratures[quadratureStateName] = qStates
        self._setupQuadratureFunctions(symbolicDvs)
Ejemplo n.º 11
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])
Ejemplo n.º 12
0
def makeModel(conf, propertiesDir = '../properties'):
    print "\n#### File '%s' is old. It does not use the NED convention you should use carouselModel in offline_mhe_stuff instead.\n" \
           % inspect.getfile(inspect.currentframe())
    #input("Press Enter if you wish to continue...")
    dae = rawe.models.carousel(conf)
    (xDotSol, zSol) = dae.solveForXDotAndZ()
    ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
    ddt_w_bn_b  = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
    x =   dae['x']
    y =   dae['y']

    dx  =  dae['dx']
    dy  =  dae['dy']

    ddelta = dae['ddelta']
    dddelta = xDotSol['ddelta']

    R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
                                    'e21', 'e22', 'e23',
                                    'e31', 'e32', 'e33']]
                      ).reshape((3,3))

    rA = conf['rArm']
    g = conf['g']
    pIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat')))
    RIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat')))
    ddpIMU = C.mul(R.T,ddp) - ddelta**2*C.mul(R.T,C.vertcat([x+rA,y,0])) + 2*ddelta*C.mul(R.T,C.vertcat([-dy,dx,0])) + dddelta*C.mul(R.T,C.vertcat([-y,x+rA,0])) + C.mul(R.T,C.vertcat([0,0,g]))
    aBridle = C.cross(ddt_w_bn_b,pIMU)
    dae['IMU_acceleration'] = C.mul(RIMU,ddpIMU+aBridle)
    dae['IMU_angular_velocity'] = C.mul(RIMU,dae['w_bn_b'])

    camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC1.dat'))),
               'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC2.dat'))),
               'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC1.dat'))),
               'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC2.dat'))),
               'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body1.dat'))),
               'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body2.dat'))),
               'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body3.dat')))}
    dae['marker_positions'] = camModel.fullCamModel(dae,camConf)

    dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1
    dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23']
    dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33']
    dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1
    dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33']
    dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1
    #dae['ConstDelta'] = dae['cos_delta']*dae['cos_delta'] + dae['sin_delta']*dae['sin_delta'] - 1
    dae['ConstDelta'] = ( dae['cos_delta']**2 + dae['sin_delta']**2 - 1 )

    return dae
Ejemplo n.º 13
0
Archivo: cstr.py Proyecto: thj2009/AbCD
    def prior_construct(self, prior_info):
        Pnlp = self._Pnlp
        if prior_info['type'] == 'Ridge':
            L2 = prior_info['L2']
            prior = cas.mul(Pnlp.T, Pnlp) * L2
        elif prior_info['type'] == 'normal':
            mean = prior_info['mean']
            cov = prior_info['cov']
            dev = Pnlp - mean
            prior = cas.mul(cas.mul(dev.T, np.linalg.inv(cov)), dev)
        elif prior_info['type'] == 'normal_std':
            mean = prior_info['mean']
            std = prior_info['std']
            dev = Pnlp - mean
            prior = cas.sum_square(dev) / (2 * std**2)
        elif prior_info['type'] == 'uniform':
            prior = 0
        elif prior_info['type'] == 'GP':
            BEmean = prior_info['BEmean']
            BEcov = prior_info['BEcov']
            linear_BE2Ea = prior_info['BE2Ea']
            Eacov = prior_info['Eacov']

            _BE = Pnlp[self._NEa:]
            _Ea = Pnlp[:self._NEa]
            Eamean = cas.mul(linear_BE2Ea, _BE)
            dev_BE = _BE - BEmean
            dev_Ea = _Ea - Eamean
            prior = cas.mul(cas.mul(dev_BE.T, np.linalg.inv(BEcov)), dev_BE) + \
                    cas.mul(cas.mul(dev_Ea.T, np.linalg.inv(Eacov)), dev_Ea)
        self._prior_ = prior
        return prior
Ejemplo n.º 14
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])
Ejemplo n.º 15
0
    def prior_construct(self, prior_info):
        Pnlp = self._Pnlp
        if prior_info['type'] == 'Ridge':
            L2 = prior_info['L2']
            prior = cas.mul(Pnlp.T, Pnlp) * L2
        elif prior_info['type'] == 'Gaussian':
            mean = prior_info['mean']
            cov = prior_info['cov']
            dev = Pnlp - mean
            prior = cas.mul(cas.mul(dev.T, np.linalg.inv(cov)), dev)
        elif prior_info['type'] == 'GP':
            BEmean = prior_info['BEmean']
            BEcov = prior_info['BEcov']
            linear_BE2Ea = prior_info['BE2Ea']
            Eacov = prior_info['Eacov']

            _BE = Pnlp[self._NEa:]
            _Ea = Pnlp[:self._NEa]
            Eamean = cas.mul(linear_BE2Ea, _BE)
            dev_BE = _BE - BEmean
            dev_Ea = _Ea - Eamean
            prior = cas.mul(cas.mul(dev_BE.T, np.linalg.inv(BEcov)), dev_BE) + \
                    cas.mul(cas.mul(dev_Ea.T, np.linalg.inv(Eacov)), dev_Ea)
        self._prior_ = prior
        self.prob_func = cas.MXFunction('prob_func', [Pnlp], [prior])
        return prior
Ejemplo n.º 16
0
    def set_path(self, path, r2r=False):
        """Define an analytic expression of the geometric path.

        Note: The path must be defined as a function of self.s[0].

        Args:
            path (list of SXMatrix): An expression of the geometric path as
                a function of self.s[0]. Its dimension must equal self.sys.ny
            r2r (boolean): Reparameterize path such that a rest to rest
                transition is performed

        Example:
            >>> S = FlatSystem(2, 4)
            >>> P = PathFollowing(S)
            >>> P.set_path([P.s[0], P.s[0]])
        """
        if isinstance(path, list):
            path = cas.vertcat(path)
        if r2r:
            path = cas.substitute(path, self.s[0], self._r2r())
        self.path[:, 0] = path
        dot_s = cas.vertcat([self.s[1:], 0])
        for i in range(1, self.sys.order + 1):
            self.path[:,
                      i] = cas.mul(cas.jacobian(self.path[:, i - 1], self.s),
                                   dot_s)
Ejemplo n.º 17
0
def create_plan_fc(b0, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=belief),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])
    
    # Objective function
    m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
    m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
    dm_bc = m_bN - m_cN
    J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
    J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
    # Regularize controls
    J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
    
    # Multiple shooting constraints and running costs
    g = []
    for k in range(N_sim):
        # Multiple shooting
        [x_next] = BF([V['X',k], V['U',k]])
        g.append(x_next - V['X',k+1])
        
        # Penalize uncertainty
        J += 1e-1 * ca.trace(V['X',k,'S']) * dt
    g = ca.vertcat(g)
    
    # log-probability, doesn't work with full collocation
    #Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
    #J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
    #     ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
    
    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
    
    # Create solver
    opts = {}
    opts['linear_solver'] = 'ma97'
    #opts['hessian_approximation'] = 'limited-memory'
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # 0 <= v <= v_max
    lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    
    # m(t=0) = m0
    lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
    # S(t=0) = S0
    lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
    
    # Solve the NLP
    sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
    return V(sol['x'])
Ejemplo n.º 18
0
def evalspline(s, x):
    # Evaluate spline with symbolic variable
    # This is possible not the best way to implement this. The conditional node
    # from casadi should be considered
    Bl = s.basis
    coeffs = s.coeffs
    k = Bl.knots
    basis = [[]]
    for i in range(len(k) - 1):
        if i < Bl.degree + 1 and Bl.knots[0] == Bl.knots[i]:
            basis[-1].append((x >= Bl.knots[i])*(x <= Bl.knots[i + 1]))
        else:
            basis[-1].append((x > Bl.knots[i])*(x <= Bl.knots[i + 1]))
    for d in range(1, Bl.degree + 1):
        basis.append([])
        for i in range(len(k) - d - 1):
            b = 0 * x
            bottom = k[i + d] - k[i]
            if bottom != 0:
                b = (x - k[i]) * basis[d - 1][i] / bottom
            bottom = k[i + d + 1] - k[i + 1]
            if bottom != 0:
                b += (k[i + d + 1] - x) * basis[d - 1][i + 1] / bottom
            basis[-1].append(b)
    result = 0.
    for l in range(len(Bl)):
        result += mul(coeffs[l], basis[-1][l])
    return result
Ejemplo n.º 19
0
def markers_from_pose_casadi(pose):
	T_body_to_anchor = unvectorize_se3(pose)
	markers_body = [m1_body, m2_body, m3_body]
	markers_body = map(append_one, markers_body)
	markers = SXMatrix.zeros(12,1) 
	RPCinvs = [RPC1inv, RPC2inv] # These map points in arm frames to camera frames
	fs = [f1,f2]
	cs = [c1,c2]
	for ci in xrange(2):
		for mi in xrange(3):
			m_anchor = mul(T_body_to_anchor, markers_body[mi])
			m_cam = mul(RPCinvs[ci],m_anchor)
			uv = project(fs[ci],cs[ci],m_cam)
			starti = ci*6+mi*2
			markers[starti:starti+2,0] = uv
	return markers
Ejemplo n.º 20
0
 def constrainInvariantErrs():
     dcm = dae['dcm']
     err = C.mul(dcm.T,dcm)
     g.add( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
                    ("initial dcm orthonormal",None))
     g.add(dae['c'], '==', 0, tag=('c(0)==0',None))
     g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0',None))
Ejemplo n.º 21
0
    def set_path(self, path, r2r=False):
        """Define an analytic expression of the geometric path.

        Note: The path must be defined as a function of self.s[0].

        Args:
            path (list of SXMatrix): An expression of the geometric path as
                a function of self.s[0]. Its dimension must equal self.sys.ny
            r2r (boolean): Reparameterize path such that a rest to rest
                transition is performed

        Example:
            >>> S = FlatSystem(2, 4)
            >>> P = PathFollowing(S)
            >>> P.set_path([P.s[0], P.s[0]])
        """
        if isinstance(path, list):
            path = cas.vertcat(path)
        if r2r:
            path = cas.substitute(path, self.s[0], self._r2r())
        self.path[:, 0] = path
        dot_s = cas.vertcat([self.s[1:], 0])
        for i in range(1, self.sys.order + 1):
            self.path[:, i] = cas.mul(
                cas.jacobian(self.path[:, i - 1], self.s), dot_s)
Ejemplo n.º 22
0
 def getSteadyStateNlpFunctions(self, dae, ref_dict = {}):
     # make steady state model
     g = Constraints()
     g.add(dae.getResidual(), '==', 0, tag = ('dae residual', None))
     def constrainInvariantErrs():
         R_c2b = dae['R_c2b']
         self.makeOrthonormal(g, R_c2b)
         g.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
         g.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
     constrainInvariantErrs()
 
     # Rotational velocity time derivative
     # NOTE: In the following constraint omega0 was hard-coded.
     g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
                        ("Rotational velocities", None))
 
     for name in ['alpha_deg', 'beta_deg', 'cL']:
         if name in ref_dict:
             g.addBnds(dae[name], ref_dict[name], tag = (name, None))
 
     dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
     obj = 0
 #     for name in ['aileron', 'elevator', 'rudder', 'flaps']:
     for name in dae.uNames():
         if name in dae:
             obj += dae[ name ] ** 2
     
     return dvs, obj, g.getG(), g.getLb(), g.getUb()
Ejemplo n.º 23
0
def setupImplicitFunction(operAlpha, An, geom):

    #setup function to obtain B and RHS for the Fourier series calc
    def getBandRHS(alphaiLoc):
        alphaLoc = geom.alphaGeometric(operAlpha) - alphaiLoc
        clLoc = geom.clLoc(alphaLoc)
        RHS = clLoc*geom.chordLoc
        B = numpy.sin(numpy.outer(geom.thetaLoc, geom.sumN))*(4.0*geom.bref)
        return (B,RHS)

    alphaiLoc = []
    for i in range(geom.n):
        alphaiLoc.append(
            C.inner_prod(geom.sumN*numpy.sin(geom.sumN*geom.thetaLoc[i])/numpy.sin(geom.thetaLoc[i]), An))
    alphaiLoc = C.veccat(alphaiLoc)

    (B,RHS) = getBandRHS(alphaiLoc)
    makeMeZero = RHS - C.mul(B, An)

    CL  = An[0]*numpy.pi*geom.AR
    CDi = 0.0
    for i in range(geom.n):
        k = geom.sumN[i]
        CDi += k * An[i]**2 / An[0]**2
    CDi *= numpy.pi*geom.AR*An[0]**2

    return (makeMeZero, alphaiLoc, CL, CDi)
Ejemplo n.º 24
0
 def constrainInvariantErrs():
     dcm = ocp.lookup('dcm',timestep=0)
     err = C.mul(dcm.T,dcm)
     ocp.constrain( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
                    ("initial dcm orthonormal",None))
     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))
Ejemplo n.º 25
0
def matchDcms(ocp,R0,Rf):
    err = C.mul(R0.T, Rf)
    ocp.constrain(err[0,1], '==', 0, tag=('dcm matching',"01"))
    ocp.constrain(err[0,2], '==', 0, tag=('dcm matching',"02"))
    ocp.constrain(err[1,2], '==', 0, tag=('dcm matching',"12"))

    ocp.constrain(err[0,0], '>=', 0.5, tag=('dcm matching',"00"))
    ocp.constrain(err[1,1], '>=', 0.5, tag=('dcm matching',"11"))
Ejemplo n.º 26
0
def ext_belief_dynamics(ext_belief, control,
                        F, dt, A_fcn, C_fcn,
                        Q, obs_cov_fcn):
    ext_belief_next = cat.struct_SX(ext_belief)
    
    # Predict the mean
    [mu_bar] = F([ ext_belief['m'], control, dt ])
    
    # Compute linearization
    [A,_] = A_fcn([ ext_belief['m'], control, dt ])
    [C,_] = C_fcn([ mu_bar ])
    
    # Predict the covariance
    S_bar = ca.mul([ A, ext_belief['S'], A.T ]) + Q
    
    # Get the observations noise covariance
    [R] = obs_cov_fcn([ mu_bar ])
    
    # Compute the inverse
    P = ca.mul([ C, S_bar, C.T ]) + R
    P_inv = ca.inv(P)
    
    # Kalman gain
    K = ca.mul([ S_bar, C.T, P_inv ])
    
    # Update equations
    nx = ext_belief['m'].size()
    ext_belief_next['m'] = mu_bar
    ext_belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar)
    ext_belief_next['L'] = ca.mul([ A, ext_belief['L'], A.T ]) + \
                           ca.mul([ K, C, S_bar ])
    
    # (ext_belief, control) -> next_ext_belief
    return ca.SXFunction('Extended-belief dynamics',
                       [ext_belief,control],[ext_belief_next])
Ejemplo n.º 27
0
    def CorrThermoConsis(self, dE_start, fix_Ea=[], fix_BE=[], print_screen=False):
        if self._thermo_constraint_expression is None:
            self.build_thermo_constraint(thermoTem=298.15)
        Pnlp = self._Pnlp
        ini_p = np.hstack([dE_start])
        dev = Pnlp - ini_p
        if py == 2:
            object_fxn = cas.mul(dev.T, dev)
        elif py == 3:
            object_fxn = cas.dot(dev, dev)
        
        nlp = dict(f=object_fxn, x=Pnlp, g=self._thermo_constraint_expression)

        nlpopts = dict()
        if py == 2:
            nlpopts['max_iter'] = 500
            nlpopts['tol'] = 1e-8
            nlpopts['expect_infeasible_problem'] = 'yes'
            nlpopts['hessian_approximation'] = 'exact'
            nlpopts['jac_d_constant'] = 'yes'
        elif py == 3:
            nlpopts['ipopt.max_iter'] = 500
            nlpopts['ipopt.tol'] = 1e-8
            nlpopts['ipopt.expect_infeasible_problem'] = 'yes'
            nlpopts['ipopt.hessian_approximation'] = 'exact'
            nlpopts['ipopt.jac_d_constant'] = 'yes'

        if py == 2:
            solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts)
        elif py == 3:
            solver = cas.nlpsol('solver', 'ipopt', nlp, nlpopts)
        # Bounds and initial guess
        lbP = -np.inf * np.ones(self._Np)
        ubP = np.inf * np.ones(self._Np)

        for i in range(len(fix_Ea)):
            if check_index(fix_Ea[i], self.dEa_index):
                lbP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
                ubP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
        for i in range(len(fix_BE)):
            if check_index(fix_BE[i], self.dBE_index):
                lbP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
                ubP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]

        lbG = 0 * np.ones(2 * self.nrxn)
        ubG = np.inf * np.ones(2 * self.nrxn)

        if not print_screen:
            old_stdout = sys.stdout
            sys.stdout = tempfile.TemporaryFile()

        solution = solver(x0=ini_p, lbg=lbG, ubg=ubG, lbx=lbP, ubx=ubP)
        dE_corr = solution['x'].full().T[0].tolist()

        if not print_screen:
            sys.stdout = old_stdout
        return(dE_corr)
Ejemplo n.º 28
0
def matchDcms(ocp, R0, Rf):
    err = C.mul(R0.T, Rf)
    ocp.constrain(err[0, 1], '==', 0, tag=('dcm matching', "01"))
    ocp.constrain(err[0, 2], '==', 0, tag=('dcm matching', "02"))
    ocp.constrain(err[1, 2], '==', 0, tag=('dcm matching', "12"))

    ocp.constrain(err[0, 0], '>=', 0.5, tag=('dcm matching', "00"))
    ocp.constrain(err[1, 1], '>=', 0.5, tag=('dcm matching', "11"))
    ocp.constrain(err[2, 2], '>=', 0.5, tag=('dcm matching', "22"))
Ejemplo n.º 29
0
    def setup_oed(self, outputs, parameters, sigma, time_points, design="A"):
        """
        Transforms an Optimization Problem into an Optimal Experimental Design problem.

        Parameters::

            outputs --
                List of names for outputs.

                Type: [string]

            parameters --
                List of names for parameters to estimate.

                Type: [string]

            sigma --
                Experiment variance matrix.

                Type: [[float]]

            time_points --
                List of measurement time points.

                Type: [float]

            design --
                Design criterion.

                Possible values: "A", "T"
        """
        # Augment sensitivities and add timed variables
        self.augment_sensitivities(parameters)
        timed_sens = self.create_timed_sensitivities(outputs, parameters, time_points)
        
        # Create sensitivity and Fisher matrices
        Q = []
        for j in xrange(len(outputs)):
            Q.append(casadi.vertcat([casadi.horzcat([s.getVar() for s in timed_sens[i][j]])
                                     for i in xrange(len(time_points))]))
        Fisher = sum([sigma[i, j] * casadi.mul(Q[i].T, Q[j]) for i in xrange(len(outputs))
                      for j in xrange(len(outputs))])

        # Define the objective
        if design == "A":
            b = casadi.MX.sym("b", Fisher.shape[1], 1)
            Fisher_inv = casadi.jacobian(casadi.solve(Fisher, b), b)
            obj = casadi.trace(Fisher_inv)
        elif design == "T":
            obj = -casadi.trace(Fisher)
        elif design == "D":
            obj = -casadi.det(Fisher)
            raise NotImplementedError("D-optimal design is not supported.")
        else:
            raise ValueError("Invalid design %s." % design)
        old_obj = self.getObjective()
        self.setObjective(old_obj + obj)
Ejemplo n.º 30
0
 def constrainInvariantErrs():
     dcm = mhe.lookup('dcm', timestep=0)
     err = C.mul(dcm.T, dcm)
     mhe.constrain(
         C.veccat([
             err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1],
             err[0, 2], err[1, 2]
         ]), '==', 0)
     mhe.constrain(mhe.lookup('c', timestep=0), '==', 0)
     mhe.constrain(mhe.lookup('cdot', timestep=0), '==', 0)
Ejemplo n.º 31
0
 def constrainInvariantErrs():
     R_c2b = dae['R_c2b']
     self.makeOrthonormal(ginv, R_c2b)
     ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
     ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
     di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
     ginv.add(di, '==', 0, tag = ('delta invariant', None))
     
     ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
                    ("Rotational velocities", None))
Ejemplo n.º 32
0
 def construct_upd_x(self, build):
     # define parameters
     z_i = self.define_parameter('z_i', self.q_i_struct.shape[0])
     z_ji = self.define_parameter('z_ji', self.q_ji_struct.shape[0])
     l_i = self.define_parameter('l_i', self.q_i_struct.shape[0])
     l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
     rho = self.define_parameter('rho')
     # put them in the struct format
     z_i = self.q_i_struct(z_i)
     z_ji = self.q_ji_struct(z_ji)
     l_i = self.q_i_struct(l_i)
     l_ji = self.q_ji_struct(l_ji)
     # get time info
     t = self.define_symbol('t')
     T = self.define_symbol('T')
     t0 = t/T
     # get (part of) variables
     x_i = self._get_x_variables()
     # transform spline variables: only consider future piece of spline
     tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
     self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
     self._transform_spline([z_ji, l_ji], tf, self.q_ji)
     # construct objective
     obj = 0.
     for child, q_i in self.q_i.items():
         for name in q_i.keys():
             x = x_i[child.label][name]
             z = z_i[child.label, name]
             l = l_i[child.label, name]
             obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
             for nghb in self.q_ji.keys():
                 z = z_ji[str(nghb), child.label, name]
                 l = l_ji[str(nghb), child.label, name]
                 obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
     self.define_objective(obj)
     # construct problem
     self.father = OptiFather(self.group.values())
     prob, compile_time = self.father.construct_problem(self.options, build)
     self.problem_upd_x = prob
     self.father.init_transformations(self.problem.init_primal_transform,
                                      self.problem.init_dual_transform)
     self.init_var_admm()
Ejemplo n.º 33
0
 def constrainInvariantErrs():
     dcm = ocp.lookup("dcm", timestep=0)
     err = C.mul(dcm.T, dcm)
     ocp.constrain(
         C.veccat([err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1], err[0, 2], err[1, 2]]),
         "==",
         0,
         tag=("initial dcm", None),
     )
     ocp.constrain(ocp.lookup("c", timestep=0), "==", 0, tag=("initial c", None))
     ocp.constrain(ocp.lookup("cdot", timestep=0), "==", 0, tag=("initial cdot", None))
Ejemplo n.º 34
0
    def _create_final_cost(self, w_cl):
        # Final position
        x_b = self.x[ca.veccat, ['x_b', 'y_b']]
        x_c = self.x[ca.veccat, ['x_c', 'y_c']]
        dx_bc = x_b - x_c

        final_cost = 0.5 * ca.mul(dx_bc.T, dx_bc)
        op = {'input_scheme': ['x'],
              'output_scheme': ['cl']}
        return ca.SXFunction('Final cost', [self.x],
                             [w_cl * final_cost], op)
Ejemplo n.º 35
0
    def set_path(self, paths):
        """The path is defined as the convex combination of the paths in paths.

        Args:
            paths (list of lists of SXMatrix): The path is taken as the
            convex combination of the paths in paths.

        Example:
        The path is defined as the convex combination of
        (s, 0.5*s) and (2, 2*s):
            >>> P.set_path([(P.s[0], 0.5 * P.s[0]), [P.s[0], 2 * P.s[0]]])
        """
        l = len(paths)
        self.h = cas.ssym("h", l, self.sys.order + 1)
        self.path[:, 0] = np.sum(cas.SXMatrix(paths) * cas.horzcat([self.h[:, 0]] * len(paths[0])), axis=0)
        dot_s = cas.vertcat([self.s[1:], 0])
        dot_h = cas.horzcat([self.h[:, 1:], cas.SXMatrix.zeros(l, 1)])
        for i in range(1, self.sys.order + 1):  # Chainrule
            self.path[:, i] = (cas.mul(cas.jacobian(self.path[:, i - 1], self.s), dot_s) +
                sum([cas.mul(cas.jacobian(self.path[:, i - 1], self.h[j, :]), dot_h[j, :].trans()) for j in range(l)]) * self.s[1])
Ejemplo n.º 36
0
def shift_knot1_bwd(cfs, basis, t_shift):
    if isinstance(cfs, (SX, MX)):
        cfs_sym = SX.sym('cfs', cfs.shape)
        t_shift_sym = SX.sym('t_shift')
        T, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
        cfs2_sym = mul(Tinv, cfs_sym)
        fun = SXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym])
        return fun([cfs, t_shift])[0]
    else:
        T, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
        return Tinv.dot(cfs)
Ejemplo n.º 37
0
def shift_knot1_fwd(cfs, basis, t_shift):
    if isinstance(cfs, (SX, MX)):
        cfs_sym = MX.sym('cfs', cfs.shape)
        t_shift_sym = MX.sym('t_shift')
        T = shiftfirstknot_T(basis, t_shift_sym)
        cfs2_sym = mul(T, cfs_sym)
        fun = MXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym])
        return fun([cfs, t_shift])[0]
    else:
        T = shiftfirstknot_T(basis, t_shift)
        return T.dot(cfs)
Ejemplo n.º 38
0
def invert_se3(T):
	R = T[:3,:3]
	t = T[:3,3]
	if type(T)==casadi.SXMatrix:
		Tinv = casadi.SXMatrix(4,4)
		Tinv[:3,3] = -casadi.mul(R.T,t)
	else:
		Tinv = numpy.zeros((4,4))
		Tinv[:3,3] = -numpy.dot(R.T,t)
	Tinv[:3,:3] = R.T
	Tinv[3,3] = 1
	return Tinv
Ejemplo n.º 39
0
def fullCamModel(dae,conf):
    PdatC1 = conf['PdatC1']
    PdatC2 = conf['PdatC2']
    RPC1 = conf['RPC1']
    RPC2 = conf['RPC2']
    pos_marker_body1 = conf['pos_marker_body1']
    pos_marker_body2 = conf['pos_marker_body2']
    pos_marker_body3 = conf['pos_marker_body3']
    
    RpC1 = C.DMatrix.eye(4)
    RpC1[0:3,0:3] = RPC1[0:3,0:3].T
    RpC1[0:3,3] = C.mul(-RPC1[0:3,0:3].T,RPC1[0:3,3])
    RpC2 = C.DMatrix.eye(4);
    RpC2[0:3,0:3] = RPC2[0:3,0:3].T
    RpC2[0:3,3] = C.mul(-RPC2[0:3,0:3].T,RPC2[0:3,3])
    
    PC1 = C.SXMatrix(3,3)
    PC1[0,0] = PdatC1[0]
    PC1[1,1] = PdatC1[1]
    PC1[0,2] = PdatC1[2]
    PC1[1,2] = PdatC1[3]
    PC1[2,2] = 1.0
    PC2 = C.SXMatrix(3,3)
    PC2[0,0] = PdatC2[0]
    PC2[1,1] = PdatC2[1]
    PC2[0,2] = PdatC2[2]
    PC2[1,2] = PdatC2[3]
    PC2[2,2] = 1.0
    p = C.vertcat([dae['x'],dae['y'],dae['z']])
    R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
                                    'e21', 'e22', 'e23',
                                    'e31', 'e32', 'e33']]
                      ).reshape((3,3))
    uv_all = C.vertcat([C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body1)) ,\
                        C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body2)) ,\
                        C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body3)) ,\
                        C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body1)) ,\
                        C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body2)) ,\
                        C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body3))])
    return uv_all
Ejemplo n.º 40
0
 def getRobustSteadyStateNlpFunctions(self, dae, ref_dict = {}):
     xDotSol, zSol = dae.solveForXDotAndZ()
 
     ginv = Constraints()
     def constrainInvariantErrs():
         R_c2b = dae['R_c2b']
         self.makeOrthonormal(ginv, R_c2b)
         ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
         ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
         di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
         ginv.add(di, '==', 0, tag = ('delta invariant', None))
         
         ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
                        ("Rotational velocities", None))
         
     constrainInvariantErrs()
     invariants = ginv.getG()
     
     J = C.jacobian(invariants,dae.xVec())
 
     # make steady state model
     g = Constraints()
     
     xds = C.vertcat([xDotSol[ name ] for name in dae.xNames()])
     jInv = C.mul(J.T,C.solve(C.mul(J,J.T),invariants))
     
     g.add(xds - jInv - dae.xDotVec(), '==', 0, tag = ('dae residual', None))            
 
     for name in ['alpha_deg', 'beta_deg', 'cL']:
         if name in ref_dict:
             g.addBnds(dae[name], ref_dict[name], tag = (name, None))
 
     dvs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
     
     obj = 0
     for name in dae.uNames() + ['aileron', 'elevator']:
         if name in dae:
             obj += dae[ name ] ** 2
     
     return dvs, obj, g.getG(), g.getLb(), g.getUb(), zSol
Ejemplo n.º 41
0
 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
Ejemplo n.º 42
0
def matchDcms(ocp, R0, Rf, tag=None):
    err = C.mul(R0.T, Rf)
    if tag is None:
        tag = ''
    else:
        tag = ' ' + tag
    ocp.constrain(err[0, 1], '==', 0, tag=('dcm matching 01' + tag, None))
    ocp.constrain(err[0, 2], '==', 0, tag=('dcm matching 02' + tag, None))
    ocp.constrain(err[1, 2], '==', 0, tag=('dcm matching 12' + tag, None))

    ocp.constrain(err[0, 0], '>=', 0.5, tag=('dcm matching 00' + tag, None))
    ocp.constrain(err[1, 1], '>=', 0.5, tag=('dcm matching 11' + tag, None))
    ocp.constrain(err[2, 2], '>=', 0.5, tag=('dcm matching 22' + tag, None))
Ejemplo n.º 43
0
 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
Ejemplo n.º 44
0
def T2WJ(T, p):
    """
   w_101 = T2WJ(T_10,p).diff(p,t)
   
  """
    R = T2R(T)
    RT = R.T

    temp = []
    for i, k in [(2, 1), (0, 2), (1, 0)]:
        #temp.append(c.mul(c.jacobian(R[:,k],p).T,R[:,i]).T)
        temp.append(c.mul(RT[i, :], c.jacobian(R[:, k], p)))

    return c.vertcat(temp)
Ejemplo n.º 45
0
def fwd(x_all, u_all, k_all, K_all, alpha, F, dt, state, control):
    n_sim = len(u_all[:])
    xk = x_all[0]
    x_new = [xk]
    u_new = []
    for k in range(n_sim):
        uk = u_all[k] + alpha * k_all[k] + ca.mul(K_all[k], xk - x_all[k])
        u_new.append(uk)
        [xk_next] = F([xk, uk, dt])
        x_new.append(xk_next)
        xk = xk_next
    x_new = state.repeated(ca.horzcat(x_new))
    u_new = control.repeated(ca.horzcat(u_new))
    return [x_new, u_new]
Ejemplo n.º 46
0
 def getWind():
     if 'wind_model' not in conf:
         return C.veccat([0, 0, 0])
     
     if conf['wind_model']['name'] == 'wind_shear':
         # use a logarithmic wind shear model
         # wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
         # where w0 is wind at z0 altitude
         # zt is surface roughness characteristic length
         # altitude is -z - altitude0, where altitude0 is a user parameter
         z0 = conf['wind_model']['z0']
         zt_roughness = conf['wind_model']['zt_roughness']
         altitude = -z - conf['wind_model']['altitude0']
         wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
         
         dae['wind_at_altitude'] = wind
         
         return C.mul([R_g2c, C.veccat([wind, 0, 0])])
         
     elif conf['wind_model']['name'] in ['constant','hardcoded']:
         # constant wind
         dae['wind_at_altitude'] = dae['w0']
         
         return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
         
     elif conf['wind_model']['name'] == 'random_walk':
         dae.addU('delta_wind_x')
         dae.addU('delta_wind_y')
         dae.addU('delta_wind_z')
         
         wind_x = dae.addX('wind_x')
         wind_y = dae.addX('wind_y')
         wind_z = dae.addX('wind_z')
         
         dae['wind_at_altitude'] = wind_x
         
         return C.veccat([wind_x, wind_y, wind_z])
Ejemplo n.º 47
0
    def dt(self, expr):
        """Return time derivative of expr

        The time derivative is computed using the chainrule:
        df/dt = df/dy * dy/dt

        Args:
            expr (SXMatrix): casadi expression that is differentiated wrt time

        Returns:
            SXMatrix. The time derivative of expr

        Example:
            >>> S = FlatSystem(2, 4)
            >>> dy00 = S.dt(S.y[0, 0])
        """
        return cas.mul(cas.jacobian(expr, self.y), self._dy[:])
Ejemplo n.º 48
0
 def constrainInvariantErrs():
     dcm = ocp.lookup('dcm', timestep=0)
     err = C.mul(dcm.T, dcm)
     ocp.constrain(C.veccat([
         err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1], err[0, 2],
         err[1, 2]
     ]),
                   '==',
                   0,
                   tag=("initial dcm orthonormal", None))
     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))
Ejemplo n.º 49
0
    def _make_path(self):
        """Rewrite the path as a function of the optimization variables.

        Substitutes the time derivatives of s in the expression of the path by
        expressions that are function of b and its path derivatives by
        repeatedly applying the chainrule

        Returns:
            * SXMatrix. The substituted path
            * SXMatrix. b and the path derivatives
            * SXMatrix. The derivatives of s as a function of b
        """
        b = cas.ssym("b", self.sys.order)
        db = cas.vertcat((b[1:], 0))
        Ds = cas.SXMatrix.nan(self.sys.order)  # Time derivatives of s
        Ds[0] = cas.sqrt(b[0])
        Ds[1] = b[1] / 2
        # Apply chainrule for finding higher order derivatives
        for i in range(1, self.sys.order - 1):
            Ds[i + 1] = (cas.mul(cas.jacobian(Ds[i], b), db) * self.s[1] +
                         cas.jacobian(Ds[i], self.s[1]) * Ds[1])
        Ds = cas.substitute(Ds, self.s[1], cas.sqrt(b[0]))
        return cas.substitute(self.path, self.s[1:], Ds), b, Ds
Ejemplo n.º 50
0
    def setUp(self):

        # System

        self.u = ca.MX.sym("u", 2)
        self.p = ca.MX.sym("p", 2)

        self.phi = self.u[0] * self.p[0] + self.u[1] * self.p[1]**2
        self.g = (2 - ca.mul(self.p.T, self.p))

        self.bsys = pecas.systems.BasicSystem(u = self.u, p = self.p, \
            phi = self.phi, g = self.g)

        # Inputs

        self.tu = np.linspace(0, 3, 4)

        self.invalidpargs = [[0, 1, 2], [[2, 2, 3], [2, 2, 3]], \
            np.asarray([1, 2, 2]), np.asarray([[2, 3, 3], [2, 3, 3]])]
        self.validpargs = [None, [0, 1], np.asarray([1, 1]), \
            np.asarray([1, 2]).T, np.asarray([[2], [2]])]

        self.invaliduargs = [np.ones((self.u.size(), \
            self.tu.size - 1)), \
            np.ones((self.tu.size - 1, self.u.size()))]
        self.validuargs = [None, np.ones((self.u.size(), self.tu.size)), \
            np.ones((self.tu.size, self.u.size()))]

        self.yN = np.asarray([2.23947, 2.84568, 4.55041, 5.08583])
        self.wv = np.asarray([1.0 / (0.5**2)] * 4)

        self.uN = np.vstack([np.ones(4), np.linspace(1, 4, 4)])

        self.pinit = [1, 1]

        self.phat = np.atleast_2d([0.961943, 1.03666]).T
Ejemplo n.º 51
0

def aeroForcesTorques(dae, conf, v_bw_n, v_bw_b, w_bn_b, (eTe1, eTe2, eTe3)):
    rho = conf['rho']
    alpha0 = conf['alpha0deg'] * C.pi / 180

    sref = conf['sref']
    bref = conf['bref']
    cref = conf['cref']

    # airfoil speed in wind frame
    v_bw_n_x = v_bw_n[0]
    v_bw_n_y = v_bw_n[1]
    v_bw_n_z = v_bw_n[2]

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

    # Lift axis, normed to airspeed
    eLe_v = C.cross(C.veccat([eTe1, eTe2, eTe3]), v_bw_n)

    # sideforce axis, normalized to airspeed^2
    eYe_v2 = C.cross(eLe_v, -v_bw_n)

    # Relative wind speed in Airfoil's referential 'E'
    v_bw_b_x = v_bw_b[0]
    v_bw_b_y = v_bw_b[1]
    v_bw_b_z = v_bw_b[2]

    if conf['alpha_beta_computation'] == 'first_order':
Ejemplo n.º 52
0
def getSteadyState(dae, conf, omega0, r0, z0):
    # make steady state model
    g = Constraints()
    g.add(dae.getResidual(), '==', 0, tag=('dae residual', None))

    def constrainInvariantErrs():
        R_c2b = dae['R_c2b']
        makeOrthonormal(g, R_c2b)
        g.add(dae['c'], '==', 0, tag=('c(0)==0', None))
        g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0', None))

    constrainInvariantErrs()

    # Rotational velocity time derivative
    g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, omega0]),
          '==',
          0,
          tag=("Rotational velocities", None))
    g.addBnds(dae['alpha_deg'], (-4.0, 8.0), tag=("alpha deg", None))
    g.addBnds(dae['beta_deg'], (-7.0, 7.0), tag=("beta deg", None))

    dvs = C.veccat(
        [dae.xVec(),
         dae.zVec(),
         dae.uVec(),
         dae.pVec(),
         dae.xDotVec()])
    #    ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])])
    ffcn = C.SXFunction([dvs], [(dae['cL'] - 0.5)**2])
    gfcn = C.SXFunction([dvs], [g.getG()])
    ffcn.init()
    gfcn.init()

    guess = {
        'x': r0,
        'y': 0,
        'z': 0,
        'r': r0,
        'dr': 0,
        'e11': 0,
        'e12': 1,
        'e13': 0,
        'e21': 0,
        'e22': 0,
        'e23': -1,
        'e31': -1,
        'e32': 0,
        'e33': 0,
        'dx': 0,
        'dy': 0,
        'dz': 0,
        'w_bn_b_x': 0,
        'w_bn_b_y': -omega0,
        'w_bn_b_z': 0,
        'ddelta': omega0,
        'cos_delta': 1,
        'sin_delta': 0,
        'aileron': 0,
        'elevator': 0,
        'daileron': 0,
        'delevator': 0,
        'nu': 100,
        'motor_torque': 10,
        'dmotor_torque': 0,
        'ddr': 0,
        'dddr': 0.0,
        'w0': 0.0
    }
    dotGuess = {
        'x': 0,
        'y': 0,
        'z': 0,
        'dx': 0,
        'dy': 0,
        'dz': 0,
        'r': 0,
        'dr': 0,
        'e11': 0,
        'e12': 0,
        'e13': 0,
        'e21': 0,
        'e22': 0,
        'e23': 0,
        'e31': 0,
        'e32': 0,
        'e33': 0,
        'w_bn_b_x': 0,
        'w_bn_b_y': 0,
        'w_bn_b_z': 0,
        'ddelta': 0,
        'cos_delta': 0,
        'sin_delta': omega0,
        'aileron': 0,
        'elevator': 0,
        'motor_torque': 0,
        'ddr': 0
    }

    guessVec = C.DMatrix([
        guess[n]
        for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()
    ] + [dotGuess[n] for n in dae.xNames()])

    bounds = {
        'x': (0.01, r0 * 2),
        'y': (-r0, r0),
        'z': (z0, z0),
        'dx': (-50, 50),
        'dy': (0, 0),
        'dz': (0, 0),
        'r': (r0, r0),
        'dr': (0, 0),
        'e11': (-2, 2),
        'e12': (-2, 2),
        'e13': (-2, 2),
        'e21': (-2, 2),
        'e22': (-2, 2),
        'e23': (-2, 2),
        'e31': (-2, 0),
        'e32': (-2, 2),
        'e33': (-2, 2),
        'w_bn_b_x': (-50, 50),
        'w_bn_b_y': (-50, 50),
        'w_bn_b_z': (-50, 50),
        'ddelta': (omega0, omega0),
        'cos_delta': (1, 1),
        'sin_delta': (0, 0),
        'aileron': (-0.2, 0.2),
        'elevator': (-0.2, 0.2),
        'daileron': (0, 0),
        'delevator': (0, 0),
        'nu': (0, 3000),
        'motor_torque': (0, 1000),
        'ddr': (0, 0),
        'dmotor_torque': (0, 0),
        'dddr': (0, 0),
        'w0': (0, 0)
    }
    dotBounds = {
        'x': (-50, 50),
        'y': (-50, 50),
        'z': (-50, 50),
        'dx': (0, 0),
        'dy': (-50, 50),
        'dz': (0, 0),
        'r': (-1, 1),
        'dr': (-1, 1),
        'e11': (-50, 50),
        'e12': (-50, 50),
        'e13': (-50, 50),
        'e21': (-50, 50),
        'e22': (-50, 50),
        'e23': (-50, 50),
        'e31': (-50, 50),
        'e32': (-50, 50),
        'e33': (-50, 50),
        'w_bn_b_x': (0, 0),
        'w_bn_b_y': (0, 0),
        'w_bn_b_z': (0, 0),
        'ddelta': (0, 0),
        'cos_delta': (-1, 1),
        'sin_delta': (omega0 - 1, omega0 + 1),
        'aileron': (-1, 1),
        'elevator': (-1, 1),
        'motor_torque': (-1000, 1000),
        'ddr': (-100, 100)
    }
    boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \
                [dotBounds[n] for n in dae.xNames()]

    #    gfcn.setInput(guessVec)
    #    gfcn.evaluate()
    #    ret = gfcn.output()
    #    for k,v in enumerate(ret):
    #        if math.isnan(v):
    #            print 'index ',k,' is nan: ',g._tags[k]
    #    import sys; sys.exit()

    #    context   = zmq.Context(1)
    #    publisher = context.socket(zmq.PUB)
    #    publisher.bind("tcp://*:5563")
    #    class MyCallback:
    #        def __init__(self):
    #            import rawekite.kiteproto as kiteproto
    #            import rawekite.kite_pb2 as kite_pb2
    #            self.kiteproto = kiteproto
    #            self.kite_pb2 = kite_pb2
    #            self.iter = 0
    #        def __call__(self,f,*args):
    #            x = C.DMatrix(f.input('x'))
    #            sol = {}
    #            for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
    #                sol[name] = x[k].at(0)
    #            lookup = lambda name: sol[name]
    #            kp = self.kiteproto.toKiteProto(lookup,
    #                                            lineAlpha=0.2)
    #            mc = self.kite_pb2.MultiCarousel()
    #            mc.horizon.extend([kp])
    #            mc.messages.append("iter: "+str(self.iter))
    #            self.iter += 1
    #            publisher.send_multipart(["multi-carousel", mc.SerializeToString()])

    solver = C.IpoptSolver(ffcn, gfcn)

    def addCallback():
        nd = len(boundsVec)
        nc = g.getLb().size()
        c = C.PyFunction(
            MyCallback(),
            C.nlpsolverOut(x=C.sp_dense(nd, 1),
                           f=C.sp_dense(1, 1),
                           lam_x=C.sp_dense(nd, 1),
                           lam_p=C.sp_dense(0, 1),
                           lam_g=C.sp_dense(nc, 1),
                           g=C.sp_dense(nc, 1)), [C.sp_dense(1, 1)])
        c.init()
        solver.setOption("iteration_callback", c)
#    addCallback()

    solver.setOption('max_iter', 10000)
    #    solver.setOption('tol',1e-14)
    #    solver.setOption('suppress_all_output','yes')
    #    solver.setOption('print_time',False)
    solver.init()

    solver.setInput(g.getLb(), 'lbg')
    solver.setInput(g.getUb(), 'ubg')
    solver.setInput(guessVec, 'x0')
    lb, ub = zip(*boundsVec)
    solver.setInput(C.DMatrix(lb), 'lbx')
    solver.setInput(C.DMatrix(ub), 'ubx')

    solver.solve()
    ret = solver.getStat('return_status')
    assert ret in ['Solve_Succeeded',
                   'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret

    #    publisher.close()
    #    context.destroy()
    xOpt = solver.output('x')
    k = 0
    sol = {}
    for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames():
        sol[name] = xOpt[k].at(0)
        k += 1
#        print name+':\t',sol[name]
    dotSol = {}
    for name in dae.xNames():
        dotSol[name] = xOpt[k].at(0)
        k += 1
#        print 'DDT('+name+'):\t',dotSol[name]
    return sol, dotSol
Ejemplo n.º 53
0
    def setQuadratureDdt(self, quadratureStateName, quadratureStateDotName,
                         lookup, lagrangePoly, h, symbolicDvs):
        if quadratureStateName in self._quadratures:
            raise ValueError(quadratureStateName + " is not unique")

        qStates = np.resize(np.array([None], dtype=C.MX),
                            (self._nk + 1, self._nicp, self._deg + 1))
        # set the dimension of the initial quadrature state correctly, and make sure the derivative name is valid
        try:
            qStates[0, 0, 0] = 0 * lookup(
                quadratureStateDotName, timestep=0, nicpIdx=0, degIdx=1)
        except ValueError:
            raise ValueError('quadrature derivative name \"' +
                             quadratureStateDotName +
                             '\" is not a valid x/z/u/p/output')

        ldInv = np.linalg.inv(lagrangePoly.lDotAtTauRoot[1:, 1:])
        ld0 = lagrangePoly.lDotAtTauRoot[1:, 0]
        l1 = lagrangePoly.lAtOne
        #        print -C.mul(C.mul(ldInv, ld0).T, l1[1:]) + l1[0]

        breakQuadratureIntervals = True

        if breakQuadratureIntervals:
            for k in range(self._nk):
                for i in range(self._nicp):
                    dQs = h * C.veccat([
                        lookup(quadratureStateDotName,
                               timestep=k,
                               nicpIdx=i,
                               degIdx=j) for j in range(1, self._deg + 1)
                    ])
                    Qs = C.mul(ldInv, dQs)
                    for j in range(1, self._deg + 1):
                        qStates[k, i, j] = qStates[k, i, 0] + Qs[j - 1]

                    m = C.mul(Qs.T, l1[1:])
                    if i < self._nicp - 1:
                        qStates[k, i + 1, 0] = qStates[k, i, 0] + m
                    else:
                        qStates[k + 1, 0, 0] = qStates[k, i, 0] + m
        else:
            for k in range(self._nk):
                for i in range(self._nicp):
                    dQs = h * C.veccat([
                        lookup(quadratureStateDotName,
                               timestep=k,
                               nicpIdx=i,
                               degIdx=j) for j in range(1, self._deg + 1)
                    ])
                    Qs = C.mul(ldInv, dQs - ld0 * qStates[k, i, 0])
                    for j in range(1, self._deg + 1):
                        qStates[k, i, j] = Qs[j - 1]

                    m = C.veccat([qStates[k, i, 0], Qs])
                    m = C.mul(m.T, l1)
                    if i < self._nicp - 1:
                        qStates[k, i + 1, 0] = m
                    else:
                        qStates[k + 1, 0, 0] = m

        self._quadratures[quadratureStateName] = qStates
        self._setupQuadratureFunctions(symbolicDvs)
Ejemplo n.º 54
0
import casadi as c
import numpy as n

N = 5000

# We work with a sparse diaginal matrix with 5000 elements
x = DM(sp_diag(N), 5)

x_s = x.toCsr_matrix()

t = time()
dummy = n.dot(x_s.T, x_s)
print "Scipy pure = %.4f s" % (time() - t)

t = time()
dummy = c.mul(x.T, x)
print "CasADi pure = %.4f s" % (time() - t)

X = MX("X", x.sparsity())

t = time()
f = MXFunction([X], [c.mul(X.T, X)])
f.init()
print "CasADi MX wrapped init overhead = %.4f s" % (time() - t)
f.setInput(x)

t = time()
f.evaluate()
print "CasADi MX wrapped = %.4f s" % (time() - t)

t = time()
Ejemplo n.º 55
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']+'"')

    if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True:
        dae.addP( [ "daileron",
                    "delevator",
                    "dmotor_torque",
                    'dddr' ] )
    else:
        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
Ejemplo n.º 56
0
def setupModel(dae, conf):
    '''
    take the dae that has x/z/u/p added to it already and return
    the states added to it and return mass matrix and rhs of the dae residual

    mass matrix columns:
     ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu

    rhs:
      forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu

    '''

    # Parameters
    m =  conf['mass']
    g = conf['g']

    j1 =  conf['j1']
    j31 = conf['j31']
    j2 =  conf['j2']
    j3 =  conf['j3']

    jCarousel = conf['jCarousel']
    cfric = conf['cfric']

    zt = conf['zt']
    rA = conf['rArm']

    # Frames
    # ==========================================
    #   World frame  (n)  NED North-East-Down
    #
    #   Wind carrying frame (w)
    #      - Translates along the world frame's x axis with the wind speed
    #
    #   Carousel frame (c)
    #      - Origin coincides with the world origin
    #      - Makes and angle delta
    #
    #   Carousel tip frame (a)
    #      - Origin sits at tip of arm
    #      - e_x extends radially outwards
    #      - e_z points downwards
    #
    #   Body frame  (b)
    #      - attached to body of aircraft
    #      - e_x extends to the nose
    #      - e_z points downwards
    #
    # Vector naming convention
    # =========================
    #   v_ab_c
    #      Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy)
    #      Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology)

    # States

    # Components that make up the rotation matrix from carousel frame to body frame   R_c2b  [-]
    # e11 e12 e13
    # e21 e22 e23
    # e31 e32 e33

    e11 = dae['e11']
    e12 = dae['e12']
    e13 = dae['e13']

    e21 = dae['e21']
    e22 = dae['e22']
    e23 = dae['e23']

    e31 = dae['e31']
    e32 = dae['e32']
    e33 = dae['e33']

    x =   dae['x'] # Aircraft position in carousel tip frame coordinates [m]
    y =   dae['y']
    z =   dae['z']

    dx  =  dae['dx'] # Time derivatives of x [m/s]
    dy  =  dae['dy']
    dz  =  dae['dz']

    w1 =  dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2]
    w2 =  dae['w_bn_b_y']
    w3 =  dae['w_bn_b_z']

    ddelta = dae['ddelta'] # Carousel turning speed  [rad/s]

    r = dae['r']
    dr = dae['dr']

    ddr = dae['ddr']

    tc = dae['motor_torque'] #Carousel motor torque
    
    if 'xt' in conf:
        t_xt = dae['tether_tension'] * conf['xt']
    else:
        t_xt = 0.0
        
    R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0],
                        [dae['sin_delta'],  dae['cos_delta'], 0.0],
                        [0.0,               0.0,              1.0]])

    # wind model
    def getWind():
        if 'wind_model' not in conf:
            return C.veccat([0, 0, 0])
        
        if conf['wind_model']['name'] == 'wind_shear':
            # use a logarithmic wind shear model
            # wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
            # where w0 is wind at z0 altitude
            # zt is surface roughness characteristic length
            # altitude is -z - altitude0, where altitude0 is a user parameter
            z0 = conf['wind_model']['z0']
            zt_roughness = conf['wind_model']['zt_roughness']
            altitude = -z - conf['wind_model']['altitude0']
            wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
            
            dae['wind_at_altitude'] = wind
            
            return C.mul([R_g2c, C.veccat([wind, 0, 0])])
            
        elif conf['wind_model']['name'] in ['constant','hardcoded']:
            # constant wind
            dae['wind_at_altitude'] = dae['w0']
            
            return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
            
        elif conf['wind_model']['name'] == 'random_walk':
            dae.addU('delta_wind_x')
            dae.addU('delta_wind_y')
            dae.addU('delta_wind_z')
            
            wind_x = dae.addX('wind_x')
            wind_y = dae.addX('wind_y')
            wind_z = dae.addX('wind_z')
            
            dae['wind_at_altitude'] = wind_x
            
            return C.veccat([wind_x, wind_y, wind_z])
            
    wind = getWind()

    # Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame
    v_bw_c = C.veccat( [ dx - ddelta*y
                       , dy + ddelta*(rA + x)
                       , dz
                       ]) - wind
    # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame
    # (needed to compute the aero forces and torques !)
    v_bw_b = C.mul( dae['R_c2b'], v_bw_c )

    (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b,
                                                 dae['w_bn_b'],
                                                 (dae['e21'], dae['e22'], dae['e23'])  # y-axis of body frame in carousel coordinates
                                                 )
    # f1..f3 expressed in carrousel coordinates
    # t1..t3 expressed in body coordinates

    # if we are running a homotopy, add psudeo forces and moments as algebraic states
    if 'runHomotopy' in conf and conf['runHomotopy']:
        gamma_homotopy = dae.addP('gamma_homotopy')
        f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
        f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
        f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
        t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
        t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
        t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
        
    fx_dist = 0
    fy_dist = 0
    fz_dist = 0
    
    mx_dist = 0
    my_dist = 0
    mz_dist = 0
        
    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 == 'random_walk':
            if _which[ 0 ]:
                dae.addU('df1_disturbance')
                fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')

            if _which[ 1 ]:
                dae.addU('df2_disturbance')
                fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')

            if _which[ 2 ]:
                dae.addU('df3_disturbance')
                fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')

        elif _type == 'parameter':
            if _which[ 0 ]:
                fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
            if _which[ 1 ]:
                fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
            if _which[ 2 ]:
                fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
            
        elif _type == 'online_data':
            if _which[ 0 ]:
                fx_dist += dae['rho_sref_v2'] * dae.addP('f1_disturbance')
            if _which[ 1 ]:
                fy_dist += dae['rho_sref_v2'] * dae.addP('f2_disturbance')
            if _which[ 2 ]:
                fz_dist += dae['rho_sref_v2'] * dae.addP('f3_disturbance')
        else:
            raise ValueError("WTF?")
        
        
    f1 += fx_dist
    f2 += fy_dist
    f3 += fz_dist
    
    dae["aero_fx_dist"] = fx_dist
    dae["aero_fy_dist"] = fy_dist
    dae["aero_fz_dist"] = fz_dist
        
    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 ]:
                dae.addU('dt1_disturbance')
                mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
            if _which[ 1 ]:
                dae.addU('dt2_disturbance')
                my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
            if _which[ 2 ]:
                dae.addU('dt3_disturbance')
                mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
            
        elif _type == 'parameter':
            if _which[ 0 ]:
                mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
            if _which[ 1 ]:
                my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
            if _which[ 2 ]:
                mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')

        elif _type == 'online_data':
            if _which[ 0 ]:
                mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance')
            if _which[ 1 ]:
                my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance')
            if _which[ 2 ]:
                mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance')
        else:
            raise ValueError("WTF?")
        
    t1 += mx_dist
    t2 += my_dist
    t3 += mz_dist
    
    dae["aero_mx_dist"] = mx_dist
    dae["aero_my_dist"] = my_dist
    dae["aero_mz_dist"] = mz_dist

    # mass matrix
    mm = C.SXMatrix(8, 8)
    mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
    mm[0,1] = -m*y
    mm[0,2] = m*(rA + x)
    mm[0,3] = 0
    mm[0,4] = 0
    mm[0,5] = 0
    mm[0,6] = 0
    mm[0,7] = 0

    mm[1,0] = -m*y
    mm[1,1] = m
    mm[1,2] = 0
    mm[1,3] = 0
    mm[1,4] = 0
    mm[1,5] = 0
    mm[1,6] = 0
    mm[1,7] = x + zt*e31

    mm[2,0] = m*(rA + x)
    mm[2,1] = 0
    mm[2,2] = m
    mm[2,3] = 0
    mm[2,4] = 0
    mm[2,5] = 0
    mm[2,6] = 0
    mm[2,7] = y + zt*e32

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

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

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

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

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

    # right hand side
    zt2 = zt*zt
    rhs = C.veccat(
          [ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
          , 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) + t_xt
          , 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


    # The cable constraint
    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['ddx']
#    ddy = dae['ddy']
#    ddz = dae['ddz']
#    dw1 = dae['dw1']
#    dw2 = dae['dw2']
#    dddelta = dae['dddelta']
    ddx = dae.ddt('dx')
    ddy = dae.ddt('dy')
    ddz = dae.ddt('dz')
    dw1 = dae.ddt('w_bn_b_x')
    dw2 = dae.ddt('w_bn_b_y')
    dddelta = dae.ddt('ddelta')

    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['c'] =  c
    dae['cdot'] = cdot
    dae['cddot'] = cddot
    return (mm, rhs, dRexp)