def test_diff():
from sympy.abc import x, y, z
md = MutableDenseNDimArray([[x, y], [x*z, x*y*z]])
assert md.diff(x) == MutableDenseNDimArray([[1, 0], [z, y*z]])
assert diff(md, x) == MutableDenseNDimArray([[1, 0], [z, y*z]])
sd = MutableSparseNDimArray(md)
assert sd == MutableSparseNDimArray([x, y, x*z, x*y*z], (2, 2))
assert sd.diff(x) == MutableSparseNDimArray([[1, 0], [z, y*z]])
assert diff(sd, x) == MutableSparseNDimArray([[1, 0], [z, y*z]])
def test_sparse():
sparse_array = MutableSparseNDimArray([0, 0, 0, 1], (2, 2))
assert len(sparse_array) == 2 * 2
# dictionary where all data is, only non-zero entries are actually stored:
assert len(sparse_array._sparse_array) == 1
assert list(sparse_array) == [0, 0, 0, 1]
for i, j in zip(sparse_array, [0, 0, 0, 1]):
assert i == j
sparse_array[0, 0] = 123
assert len(sparse_array._sparse_array) == 2
assert sparse_array[0, 0] == 123
# when element in sparse array become zero it will disappear from
# dictionary
sparse_array[0, 0] = 0
assert len(sparse_array._sparse_array) == 1
sparse_array[1, 1] = 0
assert len(sparse_array._sparse_array) == 0
assert sparse_array[0, 0] == 0
# test for large scale sparse array
a = MutableSparseNDimArray.zeros(100000, 200000)
b = MutableSparseNDimArray.zeros(100000, 200000)
assert a == b
a[1, 1] = 1
b[1, 1] = 2
assert a != b
def test_slices():
md = MutableDenseNDimArray(range(10, 34), (2, 3, 4))
assert md[:] == MutableDenseNDimArray(range(10, 34), (2, 3, 4))
assert md[:, :, 0].tomatrix() == Matrix([[10, 14, 18], [22, 26, 30]])
assert md[0, 1:2, :].tomatrix() == Matrix([[14, 15, 16, 17]])
assert md[0, 1:3, :].tomatrix() == Matrix([[14, 15, 16, 17], [18, 19, 20, 21]])
assert md[:, :, :] == md
sd = MutableSparseNDimArray(range(10, 34), (2, 3, 4))
assert sd == MutableSparseNDimArray(md)
assert sd[:] == MutableSparseNDimArray(range(10, 34), (2, 3, 4))
assert sd[:, :, 0].tomatrix() == Matrix([[10, 14, 18], [22, 26, 30]])
assert sd[0, 1:2, :].tomatrix() == Matrix([[14, 15, 16, 17]])
assert sd[0, 1:3, :].tomatrix() == Matrix([[14, 15, 16, 17], [18, 19, 20, 21]])
assert sd[:, :, :] == sd
def test_calculation():
a = MutableDenseNDimArray([1]*9, (3, 3))
b = MutableDenseNDimArray([9]*9, (3, 3))
c = a + b
for i in c:
assert i == 10
assert c == MutableDenseNDimArray([10]*9, (3, 3))
assert c == MutableSparseNDimArray([10]*9, (3, 3))
c = b - a
for i in c:
assert i == 8
assert c == MutableDenseNDimArray([8]*9, (3, 3))
assert c == MutableSparseNDimArray([8]*9, (3, 3))
def test_sparse():
sparse_array = MutableSparseNDimArray([0, 0, 0, 1], (2, 2))
assert len(sparse_array) == 2 * 2
# dictionary where all data is, only non-zero entries are actually stored:
assert len(sparse_array._sparse_array) == 1
assert sparse_array.tolist() == [[0, 0], [0, 1]]
for i, j in zip(sparse_array, [[0, 0], [0, 1]]):
assert i == MutableSparseNDimArray(j)
sparse_array[0, 0] = 123
assert len(sparse_array._sparse_array) == 2
assert sparse_array[0, 0] == 123
assert sparse_array / 0 == MutableSparseNDimArray(
[[S.ComplexInfinity, S.NaN], [S.NaN, S.ComplexInfinity]], (2, 2))
# when element in sparse array become zero it will disappear from
# dictionary
sparse_array[0, 0] = 0
assert len(sparse_array._sparse_array) == 1
sparse_array[1, 1] = 0
assert len(sparse_array._sparse_array) == 0
assert sparse_array[0, 0] == 0
# test for large scale sparse array
# equality test
a = MutableSparseNDimArray.zeros(100000, 200000)
b = MutableSparseNDimArray.zeros(100000, 200000)
assert a == b
a[1, 1] = 1
b[1, 1] = 2
assert a != b
# __mul__ and __rmul__
assert a * 3 == MutableSparseNDimArray({200001: 3}, (100000, 200000))
assert 3 * a == MutableSparseNDimArray({200001: 3}, (100000, 200000))
assert a * 0 == MutableSparseNDimArray({}, (100000, 200000))
assert 0 * a == MutableSparseNDimArray({}, (100000, 200000))
# __div__
assert a / 3 == MutableSparseNDimArray({200001: Rational(1, 3)},
(100000, 200000))
# __neg__
assert -a == MutableSparseNDimArray({200001: -1}, (100000, 200000))
def test_slices_assign():
a = MutableDenseNDimArray(range(12), shape=(4, 3))
b = MutableSparseNDimArray(range(12), shape=(4, 3))
for i in [a, b]:
assert i.tolist() == [[0, 1, 2], [3, 4, 5], [6, 7, 8], [9, 10, 11]]
i[0, :] = [2, 2, 2]
assert i.tolist() == [[2, 2, 2], [3, 4, 5], [6, 7, 8], [9, 10, 11]]
i[0, 1:] = [8, 8]
assert i.tolist() == [[2, 8, 8], [3, 4, 5], [6, 7, 8], [9, 10, 11]]
i[1:3, 1] = [20, 44]
assert i.tolist() == [[2, 8, 8], [3, 20, 5], [6, 44, 8], [9, 10, 11]]
def get_costs(self):
costs = []
for n in range(self.time_points):
# minimize error at each time step
costs.append((self.state_sym[0, n] - self.set_point[0])**2)
costs.append((self.state_sym[1, n] - self.set_point[1])**2)
costs.append((self.state_sym[2, n] - self.set_point[2])**2)
costs.append((self.state_sym[3, n] - self.set_point[3])**2)
#costs.append((self.state_sym[4,n]-self.set_point[4])**2)
#costs.append((self.state_sym[5,n]-self.set_point[5])**2)
# minimize the control signal at each time set
if n != self.time_points:
costs.append(self.state_sym[4, n]**2)
costs = MutableSparseNDimArray(costs, (len(costs), 1))
self.costs = costs
def test_ndim_array_converting():
dense_array = MutableDenseNDimArray([1, 2, 3, 4], (2, 2))
alist = dense_array.tolist()
alist == [[1, 2], [3, 4]]
matrix = dense_array.tomatrix()
assert (isinstance(matrix, Matrix))
for i in range(len(dense_array)):
assert dense_array[i] == matrix[i]
assert matrix.shape == dense_array.shape
sparse_array = MutableSparseNDimArray([1, 2, 3, 4], (2, 2))
alist = sparse_array.tolist()
assert alist == [[1, 2], [3, 4]]
matrix = sparse_array.tomatrix()
assert(isinstance(matrix, SparseMatrix))
for i in range(len(sparse_array)):
assert sparse_array[i] == matrix[i]
assert matrix.shape == sparse_array.shape
def get_constraints(self):
(m1, m2, l1, l2, g) = self.plant_parameters
constraints = []
# Dynamics based constraints
for n in range(1, self.time_points):
# Each of these get a lambda in front of them.
# x0n
constraints.append(
self.lambdas[0, 4 * (n - 1) + 0] *
(self.state_sym[0, n] - self.state_sym[0, n - 1] -
self.state_sym[2, n - 1] * self.controller_resolution))
# x1n
constraints.append(
self.lambdas[0, 4 * (n - 1) + 1] *
(self.state_sym[1, n] - self.state_sym[1, n - 1] -
self.state_sym[3, n - 1] * self.controller_resolution))
# x2n
#constraints.append(self.lambdas[0,6*(n-1)+2] * (self.state_sym[2,n]-self.state_sym[2,n-1]- self.state_sym[4,n-1]*self.controller_resolution))
n_a = -1 * m2 * l1 * self.state_sym[2, n - 1] * sympy.sin(
self.state_sym[0, n - 1] - self.state_sym[1, n - 1])
n_b = m2 * g * sympy.sin(self.state_sym[1, n - 1])
n_1 = (n_a + n_b) * sympy.cos(self.state_sym[0, n - 1] -
self.state_sym[1, n - 1])
n_2 = -1 * m2 * l2 * self.state_sym[3, n - 1]**2 * sympy.sin(
self.state_sym[0, n - 1] - self.state_sym[1, n - 1])
n_3 = -1 * (m1 + m2) * g * sympy.sin(self.state_sym[0, n - 1])
d_1 = (m1 + m2) * l1
d_2 = -m2 * l1 * sympy.cos(self.state_sym[0, n - 1] -
self.state_sym[1, n - 1])**2
alpha_1 = (n_1 + n_2 + n_3) / (d_1 + d_2) + self.state_sym[4,
n - 1]
constraints.append(
self.lambdas[0, 4 * (n - 1) + 2] *
(self.state_sym[2, n] - self.state_sym[2, n - 1] -
(alpha_1) * self.controller_resolution))
# x3n
#constraints.append(self.lambdas[0,4*(n-1)+3] * (self.state_sym[3,n]-self.state_sym[3,n-1]- self.state_sym[5,n-1]*self.controller_resolution))
n_a = m2 * l2 * self.state_sym[3, n - 1]**2 * sympy.sin(
self.state_sym[0, n - 1] - self.state_sym[1, n - 1])
n_b = -(m1 + m2) * g * sympy.sin(self.state_sym[0, n - 1])
n_c = (m1 + m2) * l1
n_1 = -m2 * l1 * l2 * (n_a + n_b) / n_c * sympy.cos(
self.state_sym[0, n - 1] - self.state_sym[1, n - 1])
n_2 = m2 * l1 * l2 * self.state_sym[2, n - 1] * sympy.sin(
self.state_sym[0, n - 1] - self.state_sym[1, n - 1])
n_3 = -1 * m2 * g * l2 * sympy.sin(self.state_sym[1, n - 1])
d_1 = m2 * l2**2
d_2 = m2**2 * l1 * l2**2 * sympy.cos(
self.state_sym[0, n - 1] - self.state_sym[1, n - 1])**2 / n_c
alpha_2 = (n_1 + n_2 + n_3) / (d_1 + d_2)
constraints.append(
self.lambdas[0, 4 * (n - 1) + 3] *
(self.state_sym[3, n] - self.state_sym[3, n - 1] -
(alpha_2) * self.controller_resolution))
# x4n
#constraints.append(self.lambdas[0,6*(n-1)+4] * (self.state_sym[4,n] + m2*l2/((m1+m2)*l1)*self.state_sym[5,n-1] * sympy.cos(self.state_sym[0,n-1]-self.state_sym[1,n-1]) + m2*l2/((m1+m2)*l1)*self.state_sym[3,n-1]**2 *sympy.sin(self.state_sym[0,n-1]-self.state_sym[1,n-1]) +g/l1*sympy.sin(self.state_sym[0,n-1]) -self.state_sym[6,n-1] ) )
# x5n
#constraints.append(self.lambdas[0,6*(n-1)+5] * (self.state_sym[5,n] -l1/l2*self.state_sym[2,n-1]**2 * sympy.sin(self.state_sym[0,n-1]-self.state_sym[1,n-1]) +g/l2*sympy.sin(self.state_sym[1,n-1]) +l1/l2*self.state_sym[4,n-1]*sympy.cos(self.state_sym[0,n-1]-self.state_sym[1,n-1]) ) )
# Initial conditions boundary
constraints.append(
self.lambdas[0, 4 * (n) + 0] *
(self.state_sym[0, 0] - self.initial_conditions[0])**2)
constraints.append(
self.lambdas[0, 4 * (n) + 1] *
(self.state_sym[1, 0] - self.initial_conditions[1])**2)
constraints.append(
self.lambdas[0, 4 * (n) + 2] *
(self.state_sym[2, 0] - self.initial_conditions[2])**2)
constraints.append(
self.lambdas[0, 4 * (n) + 3] *
(self.state_sym[3, 0] - self.initial_conditions[3])**2)
#constraints.append(self.lambdas[0,6*(n)+4] * (self.state_sym[4,0] - self.initial_conditions[4])**2 )
#constraints.append(self.lambdas[0,6*(n)+5] * (self.state_sym[5,0] - self.initial_conditions[5])**2 )
# End condition boundary
constraints.append(
self.lambdas[0, 4 * (n) + 4] *
(self.state_sym[0, self.time_points - 1] - self.set_point[0])**2)
constraints.append(
self.lambdas[0, 4 * (n) + 5] *
(self.state_sym[1, self.time_points - 1] - self.set_point[1])**2)
constraints.append(
self.lambdas[0, 4 * (n) + 6] *
(self.state_sym[2, self.time_points - 1] - self.set_point[2])**2)
constraints.append(
self.lambdas[0, 4 * (n) + 7] *
(self.state_sym[3, self.time_points - 1] - self.set_point[3])**2)
#constraints.append(self.lambdas[0,6*(n)+10] * (self.state_sym[4,self.time_points-1] - self.set_point[4])**2 )
#constraints.append(self.lambdas[0,6*(n)+11] * (self.state_sym[5,self.time_points-1] - self.set_point[5])**2 )
constraints = MutableSparseNDimArray(constraints,
(len(constraints), 1))
self.constraints = constraints
