Пример #1
0
 def test_0a(self):
   self.message("Typemap array -> IM")
   arrays = [array([[1,2,3],[4,5,6]],dtype=int32),array([[1,2,3],[4,5,6]]),array([[1,2,3],[4,5,6]],dtype=int)]
   for i in range(len(arrays)):
     m = arrays[i]
     zt=c.transpose(c.transpose(m))
     self.assertTrue(isinstance(zt,IM),"IM expected")
     self.checkarray(m,zt,"IM(numpy.ndarray)")
     self.checkarray(m,zt.full(),"IM(numpy.ndarray).full()")
Пример #2
0
 def test_1(self):
   self.message("DM -> DM")
   arrays = [DM(Sparsity(4,3,[0,2,2,3],[1,2,1]),[3,2.3,8])]
   for i in range(len(arrays)):
     m = arrays[i]
     zt=c.transpose(c.transpose(m))
     self.assertTrue(isinstance(zt,DM),"DM expected")
     self.checkarray(m,zt,"DM(DM)")
     self.checkarray(m,zt.full(),"DM(DM).full()")
     if scipy_available:
       self.checkarray(m,zt.sparse(),"DM(DM).sparse()")
Пример #3
0
 def test_0(self):
   self.message("Typemap array -> DM")
   arrays = [array([[1,2],[3,4],[5,6]],dtype=double),array([[3.2,4.6,9.9]])]
   for i in range(len(arrays)):
     m = arrays[i]
     zt=c.transpose(c.transpose(m))
     self.assertTrue(isinstance(zt,DM),"DM expected")
     self.checkarray(m,zt,"DM(numpy.ndarray)")
     self.checkarray(m,zt.full(),"DM(numpy.ndarray).full()")
     if scipy_available:
       self.checkarray(m,zt.sparse(),"DM(numpy.ndarray).sparse()")
Пример #4
0
 def test_1(self):
   self.message("DMatrix -> DMatrix")
   arrays = [DMatrix(Sparsity(4,3,[0,2,2,3],[1,2,1]),[3,2.3,8])]
   for i in range(len(arrays)):
     m = arrays[i]
     zt=c.transpose(c.transpose(m))
     self.assertTrue(isinstance(zt,DMatrix),"DMatrix expected")
     self.checkarray(m,zt,"DMatrix(DMatrix)")
     self.checkarray(m,zt.toArray(),"DMatrix(DMatrix).toArray()")
     if scipy_available:
       self.checkarray(m,zt.toCsc_matrix(),"DMatrix(DMatrix).toCsc_matrix()")
Пример #5
0
 def test_0(self):
   self.message("Typemap array -> DMatrix")
   arrays = [array([[1,2,3],[4,5,6]]),array([[1,2],[3,4],[5,6]],dtype=double),array([[3.2,4.6,9.9]])]
   for i in range(len(arrays)):
     m = arrays[i]
     zt=c.transpose(c.transpose(m))
     self.assertTrue(isinstance(zt,DMatrix),"DMatrix expected")
     self.checkarray(m,zt,"DMatrix(numpy.ndarray)")
     self.checkarray(m,zt.toArray(),"DMatrix(numpy.ndarray).toArray()")
     if scipy_available:
       self.checkarray(m,zt.toCsc_matrix(),"DMatrix(numpy.ndarray).toCsc_matrix()")
Пример #6
0
 def test_SX_func2(self):
   self.message("SXmatrix typemaps constructors")
   simplify(SX.sym("x"))
   list = [    ("number",2.3, (1,1)),
               ("SX", SX.sym("x"), (1,1))
   ];
   for name, arg,shape in list:
     self.message(":" + name)
     i=c.transpose(c.transpose(arg))
     self.assertEqual(i.shape[0],shape[0],"shape mismatch")
     self.assertEqual(i.shape[1],shape[1],"shape mismatch")
     SX(arg).is_empty()
Пример #7
0
 def test_2(self):
   self.message("crs_matrix -> DM")
   if not(scipy_available):
     return
   arrays = [csr_matrix( ([3,2.3,8],([0,2,0],[1,1,2])), shape = (3,4), dtype=double ),
             csr_matrix( ([3,2.3,8],([0,2,0],[1,1,2])), shape = (3,4), dtype=int )
             ]
   for i in range(len(arrays)):
     m = arrays[i]
     zt=c.transpose(c.transpose(m))
     self.assertTrue(isinstance(zt,DM),"DM expected")
     self.checkarray(m,zt,"DM(crs_matrix)")
     self.checkarray(m,zt.full(),"DM(crs_matrix).full()")
     if scipy_available:
       self.checkarray(m,zt.sparse(),"DM(crs_matrix).sparse()")
Пример #8
0
    def exitEquation(self, tree):
        logger.debug('exitEquation')

        if isinstance(tree.left, list):
            src_left = ca.vertcat(*[self.get_mx(c) for c in tree.left])
        else:
            src_left = self.get_mx(tree.left)

        if isinstance(tree.right, list):
            src_right = ca.vertcat(*[self.get_mx(c) for c in tree.right])
        else:
            src_right = self.get_mx(tree.right)

        src_left = ca.MX(src_left)
        src_right = ca.MX(src_right)

        # According to the Modelica spec,
        # "It is possible to omit left hand side component references and/or truncate the left hand side list in order to discard outputs from a function call."
        if isinstance(tree.right, ast.Expression) and tree.right.operator in self.root.classes:
            if src_left.size1() < src_right.size1():
                src_right = src_right[0:src_left.size1()]
        if isinstance(tree.left, ast.Expression) and tree.left.operator in self.root.classes:
            if src_left.size1() > src_right.size1():
                src_left = src_left[0:src_right.size1()]

        # If dimensions between the lhs and rhs do not match, but the dimensions of lhs
        # and transposed rhs do match, transpose the rhs.
        if src_left.shape != src_right.shape and src_left.shape == src_right.shape[::-1]:
            src_right = ca.transpose(src_right)

        self.src[tree] = src_left - src_right
Пример #9
0
 def test_SXFunctionc2(self):
   self.message("SXmatrix typemaps constructors")
   simplify(SXElement.sym("x"))                 
   SX(array([[SXElement.sym("x")]])).isEmpty()
   list = [ ("SXElement" ,SXElement.sym("x"),(1,1)),
               ("number",2.3, (1,1)),
               ("SX", SX.sym("x"), (1,1)),
               ("numpy.ndarray1D(SXElement)", array([SXElement.sym("x"),SXElement.sym("y")]), (2,1)),
               ("numpy.ndarray(SXElement)", array([[SXElement.sym("x"),SXElement.sym("y")],[SXElement.sym("w"),SXElement.sym("z")]]), (2,2)),
               ("numpy.ndarray(SX,number)", array([[SXElement.sym("x"),2.3]]), (1,2))
   ];
   for name, arg,shape in list:
     self.message(":" + name)
     i=c.transpose(c.transpose(arg))
     self.assertEqual(i.shape[0],shape[0],"shape mismatch")
     self.assertEqual(i.shape[1],shape[1],"shape mismatch")
     SX(arg).isEmpty()
Пример #10
0
    def exitForStatement(self, tree):
        logger.debug('exitForStatement')

        f = self.for_loops.pop()
        if len(f.values) > 0:
            indexed_symbols = list(f.indexed_symbols.keys())
            args = [f.index_variable] + indexed_symbols
            expr = ca.vcat([ca.vec(self.get_mx(e.right)) for e in tree.statements])
            free_vars = ca.symvar(expr)

            arg_names = [arg.name() for arg in args]
            free_vars = [e for e in free_vars if e.name() not in arg_names]
            all_args = args + free_vars
            F = ca.Function('loop_body', all_args, [expr])

            indexed_symbols_full = []
            for k in indexed_symbols:
                s = f.indexed_symbols[k]
                orig_symbol = self.nodes[self.current_class][s.tree.name]
                indexed_symbol = orig_symbol[s.indices]
                if s.transpose:
                    indexed_symbol = ca.transpose(indexed_symbol)
                indexed_symbols_full.append(indexed_symbol)

            Fmap = F.map("map", self.map_mode, len(f.values), list(
                range(len(args), len(all_args))), [])
            res = Fmap.call([f.values] + indexed_symbols_full + free_vars)

            # Split into a list of statements
            variables = [assignment.left for statement in tree.statements for assignment in self.get_mx(statement)]
            all_assignments = []
            for i in range(len(f.values)):
                for j, variable in enumerate(variables):
                    all_assignments.append(Assignment(variable, res[0][j, i].T))

            self.src[tree] = all_assignments
        else:
            self.src[tree] = []
Пример #11
0
    def get_indexed_symbol(self, tree, s):
        assert len([dim for shape in s._modelica_shape for dim in shape if dim is not None]) <= 2,\
            "Dimensions higher than two are not yet supported"

        assert len(s._modelica_shape) >= len(tree.indices)

        # For nested variables where an equation is defined at one of the nested models,
        # the modelica shape will contain the shape for the whole nested variable, but the indices
        # will only contain the indices for the symbol in the nested model. We only use the last
        # part of _modelica_shape in this case.
        assert tree.indices
        shapes = s._modelica_shape[-len(tree.indices):]

        # Check whether we loop over an index of this symbol
        indices = []
        for_loop = None
        for i, (index_array, shape) in enumerate(zip(tree.indices, shapes)):
            if len(index_array) > len(shape):
                symbol_name = s.name() if len(tree.indices) == 1 \
                    else s.name().split('.')[i] + ' in nested symbol ' + s.name()
                raise ValueError('Too many indices found for symbol {}, check if the symbol has '
                                 'the correct dimensions.'.format(symbol_name))

            for index, dim in zip(index_array, shape):
                if index is None and dim is None:
                    continue

                sl = None

                if isinstance(index, ast.ComponentRef):
                    for f in self.for_loops:
                        if index.name == f.name:
                            # TODO support nested loops
                            for_loop = f
                            sl = for_loop.index_variable

                if sl is None:
                    sl = self.get_integer(index) if index is not None else None

                    if sl is None and dim is not None:
                        sl = slice(None, None, 1)
                    if sl is not None and dim is None:
                        symbol_name = s.name() if len(tree.indices) == 1 \
                            else s.name().split('.')[i] + ' in nested symbol ' + s.name()
                        raise ValueError('Symbol {} was given an index of {} but this symbol '
                                         'is not an array.'.format(symbol_name, sl))
                    elif isinstance(sl, int):
                        # Modelica indexing starts from one;  Python from zero.
                        if sl <= 0 or sl > dim:
                            symbol_name = s.name() if len(tree.indices) == 1 \
                                else s.name().split('.')[i] + ' in nested symbol ' + s.name()
                            raise ValueError("Index {} of symbol {} is out of bounds. "
                                             "Index should be in range [1,{}] "
                                             "(Modelica uses 1-based indexing)."
                                             .format(sl, symbol_name, dim))
                        sl = sl - 1
                    elif isinstance(sl, slice):
                        # Modelica indexing starts from one;  Python from zero.
                        sl = slice(None if sl.start is None else sl.start - 1, sl.stop, sl.step)
                    else:
                        for_loop = self.for_loops[-1]

                indices.append(sl)

        if for_loop is not None:
            if isinstance(indices[0], ca.MX):
                if len(indices) > 1:
                    s = s[:, indices[1]]
                    indexed_symbol = _new_mx('{}[{},{}]'.format(tree.name, for_loop.name, indices[1]), s.size2())
                    index_function = lambda i : (i, indices[1])
                else:
                    indexed_symbol = _new_mx('{}[{}]'.format(tree.name, for_loop.name))
                    index_function = lambda i : i

                # If the indexed symbol is empty, we know we do not have to
                # map the for loop over it
                if np.prod(s.shape) != 0:
                    for_loop.register_indexed_symbol(indexed_symbol, index_function, True, tree, indices[0])
            else:
                s = ca.transpose(s[indices[0], :])
                indexed_symbol = _new_mx('{}[{},{}]'.format(tree.name, indices[0], for_loop.name), s.size2())
                index_function = lambda i: (indices[0], i)
                if np.prod(s.shape) != 0:
                    for_loop.register_indexed_symbol(indexed_symbol, index_function, False, tree, indices[1])
            return indexed_symbol
        else:
            if len(indices) == 1:
                return s[indices[0]]
            else:
                return s[indices[0], indices[1]]
Пример #12
0
    def exitForEquation(self, tree):
        logger.debug('exitForEquation')

        f = self.for_loops.pop()
        if len(f.values) > 0:
            indexed_symbols = list(f.indexed_symbols.keys())
            args = [f.index_variable] + indexed_symbols
            expr = ca.vcat([ca.vec(self.get_mx(e)) for e in tree.equations])
            free_vars = ca.symvar(expr)

            arg_names = [arg.name() for arg in args]
            free_vars = [e for e in free_vars if e.name() not in arg_names]
            all_args = args + free_vars
            F = ca.Function('loop_body', all_args, [expr])

            indexed_symbols_full = []
            for k in indexed_symbols:
                s = f.indexed_symbols[k]
                indices = s.indices
                try:
                    i = self.model.delay_states.index(k.name())
                except ValueError:
                    orig_symbol = self.nodes[self.current_class][s.tree.name]
                else:
                    # We are missing a similarly shaped delayed symbol. Make a new one with the appropriate shape.
                    delay_symbol = self.model.delay_arguments[i]

                    # We need to figure out the shape of the expression that
                    # we are delaying. The symbols that can occur in the delay
                    # expression should have been encountered before this
                    # iteration of the loop. The assert statement below covers
                    # this.
                    delay_expr_args = free_vars + all_args[:len(indexed_symbols_full)+1]
                    assert set(ca.symvar(delay_symbol.expr)).issubset(delay_expr_args)

                    f_delay_expr = ca.Function('delay_expr', delay_expr_args, [delay_symbol.expr])
                    f_delay_map = f_delay_expr.map("map", self.map_mode, len(f.values), list(
                        range(len(free_vars))), [])
                    [res] = f_delay_map.call(free_vars + [f.values] + indexed_symbols_full)
                    res = res.T

                    # Make the symbol with the appropriate size, and replace the old symbol with the new one.
                    orig_symbol = _new_mx(k.name(), *res.size())
                    assert res.size1() == 1 or res.size2() == 1, "Slicing does not yet work with 2-D indices"
                    indices = slice(None, None)

                    model_input = next(x for x in self.model.inputs if x.symbol.name() == k.name())
                    model_input.symbol = orig_symbol
                    self.model.delay_arguments[i] = DelayArgument(res, delay_symbol.duration)

                indexed_symbol = orig_symbol[indices]
                if s.transpose:
                    indexed_symbol = ca.transpose(indexed_symbol)
                indexed_symbols_full.append(indexed_symbol)

            Fmap = F.map("map", self.map_mode, len(f.values), list(
                range(len(args), len(all_args))), [])
            res = Fmap.call([f.values] + indexed_symbols_full + free_vars)

            self.src[tree] = res[0].T
        else:
            self.src[tree] = ca.MX()
Пример #13
0
    def __init__(self, inertial_frame_id='world'):
        Vehicle.__init__(self, inertial_frame_id)
    
        # Declaring state variables
        ## Generalized position vector
        self.eta = casadi.SX.sym('eta', 6)
        ## Generalized velocity vector
        self.nu = casadi.SX.sym('nu', 6)

        # Build the Coriolis matrix
        self.CMatrix = casadi.SX.zeros(6, 6)

        S_12 = - cross_product_operator(
            casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
            casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
        S_22 = - cross_product_operator(
            casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
            casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))

        self.CMatrix[0:3, 3:6] = S_12
        self.CMatrix[3:6, 0:3] = S_12
        self.CMatrix[3:6, 3:6] = S_22

        # Build the damping matrix (linear and nonlinear elements)
        self.DMatrix = - casadi.diag(self._linear_damping)        
        self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
        self.DMatrix -= casadi.diag(self._quad_damping * self.nu)      

        # Build the restoring forces vectors wrt the BODY frame
        Rx = np.array([[1, 0, 0],
                       [0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
                       [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
        Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
                       [0, 1, 0],
                       [-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
        Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
                       [casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
                       [0, 0, 1]])

        R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))

        if inertial_frame_id == 'world_ned':
            Fg = casadi.SX([0, 0, -self.mass * self.gravity])
            Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
        else:
            Fg = casadi.SX([0, 0, self.mass * self.gravity])
            Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])

        self.gVec = casadi.SX.zeros(6)

        self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)  
        self.gVec[3:6] = -1 * casadi.mtimes(
            R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
        
        # Build Jacobian
        T = 1 / casadi.cos(self.eta[4]) * np.array(
            [[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])],
             [0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])],
             [0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])

        self.eta_dot = casadi.vertcat(
            casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
            casadi.mtimes(T, self.nu[3::]))

        self.u = casadi.SX.sym('u', 6)
        
        self.nu_dot = casadi.solve(
            self._Mtotal, 
            self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)