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()")
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()")
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()")
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()")
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()")
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()
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()")
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
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()
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] = []
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]]
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()
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)
