Related
I am used to make my discrete time control systems simulations in Matlab and now I'm trying python and numpy.
So, my code bellow is working, but I would like to iterate over the numpy vector instead appending values into a list. Is it possible?
In other words, instead of using
xl.append(xt)
ul.append(uc)
I would like to use some Matlab equivalent like x[:, k+1] = np.dot(Ad, x[:, k]) + Bd*uc, but it's not working on my code. If I do that, instead of obtaining a two line column vector that is the expected, I got a 2x2 matrix and an error.
Another question: Why it's neccessary to use plt.plot(tk, u[:, 0], label='u') instead plt.plot(tk, u, label='u') ?
from control.matlab import *
import math
import numpy as np
import matplotlib.pyplot as plt
Ts = 0.1
N = 50
#x = np.zeros((2, N+1))
tk = np.zeros(N)
u = np.zeros(N)
v = np.random.randn(N)/86.6 #% measurement noise
wn = 1.12
wn2 = pow(wn, 2)
A = [[0, 1], [-1.5, -1.4]]
B = [[0], [1.5]]
C = [[1, 0]]
D = 0
# Control gains
K = np.array([2.64, 3.41071429])
# Now build a feedback with control law u = -K*x
Ad = np.eye(2) + np.multiply(A, Ts)
Bd = np.multiply(B, Ts)
Cd = C
xt = [[1.0], [0.12]] # initial states
xl = []
ul = []
for k in range(0, N):
tk[k] = k*Ts
uc = -K.dot(xt)
xt = np.dot(Ad, xt) + Bd*uc
xt[1, 0] += v[k]
xl.append(xt)
ul.append(uc)
x = np.array(xl)
u = np.array(ul)
#x = np.delete(x, N, 1) # delete the last position of x
#s = TransferFunction.s
#Gs = wn2/(s**2 + 0*s + wn2) # This is the KF solution
#yout, T = step(Gs)
plt.rcParams["figure.figsize"] = (10, 7)
plt.figure()
#plt.plot(T, yout, label='Open loop')
plt.plot(tk, x[:, 0], label='x_0')
plt.plot(tk, x[:, 1], label='x_1')
plt.plot(tk, u[:, 0], label='u')
plt.legend()
plt.title('Pendulum ex. 7.14 Franklin book')
plt.xlabel('Time')
plt.ylabel('amp.')
plt.show()
what I want is the code like this:
from control.matlab import *
import math
import numpy as np
import matplotlib.pyplot as plt
Ts = 0.1
N = 50
x = np.zeros((2, N+1))
tk = np.zeros(N)
u = np.zeros(N)
v = np.random.randn(N)/86.6 #% measurement noise
wn = 1.12
wn2 = pow(wn, 2)
A = [[0, 1], [-1.5, -1.4]]
B = [[0], [1.5]]
C = [[1, 0]]
D = 0
# Control gains
K = np.array([2.64, 3.41071429])
# Now build a feedback with control law u = -K*x
Ad = np.eye(2) + np.multiply(A, Ts)
Bd = np.multiply(B, Ts)
Cd = C
for k in range(0, N):
tk[k] = k*Ts
u[k] = -K.dot(x[:, k])
x[1, k] += v[k]
x[:, k+1] = np.dot(Ad, x[:, k]) + Bd*u[k]
x = np.delete(x, N, 1) # delete the last position of x
#s = TransferFunction.s
#Gs = wn2/(s**2 + 0*s + wn2) # This is the KF solution
#yout, T = step(Gs)
plt.rcParams["figure.figsize"] = (10, 7)
plt.figure()
#plt.plot(T, yout, label='Open loop')
plt.plot(tk, x[:, 0], label='x_0')
plt.plot(tk, x[:, 1], label='x_1')
plt.plot(tk, u[:, 0], label='u')
plt.legend()
plt.title('Pendulum ex. 7.14 Franklin book')
plt.xlabel('Time')
plt.ylabel('amp.')
plt.show()
But it results in a following error:
Traceback (most recent call last):
File "C:\Users\ ... \np_matrices_v1.py", line 46, in <module>
x[:, k+1] = np.dot(Ad, x[:, k]) + Bd*u[k]
ValueError: could not broadcast input array from shape (2,2) into shape (2,)
I don't know why, but if you try:
A = np.array([[1, 2], [2, 3]])
x = np.array([[0.5], [2.0]])
y = A.dot(x)
print(y)
xa = np.zeros((2, 10))
xa[:, 2] = A.dot(x)
You'll get:
Traceback (most recent call last):
File "C:\Users\eletr\.spyder-py3\temp.py", line 19, in <module>
xa[:, 2] = A.dot(x)
ValueError: could not broadcast input array from shape (2,1) into shape (2,)
But if you do:
import numpy as np
A = np.array([[1, 2], [2, 3]])
x = np.array([[0.5], [2.0]])
y = A.dot(x)
print(y)
xa = np.zeros((2, 10))
# xa[:, 2] = A.dot(x)
xa[:, [2]] = A.dot(x)
print(xa)
You'll get the correct answer:
[[4.5]
[7. ]]
[[0. 0. 4.5 0. 0. 0. 0. 0. 0. 0. ]
[0. 0. 7. 0. 0. 0. 0. 0. 0. 0. ]]
Can anyone explain it?
In [248]: A = np.array([[1, 2], [2, 3]])
...: x = np.array([[0.5], [2.0]])
In [249]: A.shape, x.shape
Out[249]: ((2, 2), (2, 1))
In [250]: y = A.dot(x)
In [251]: y.shape
Out[251]: (2, 1)
Note the shapes. x is (2,1), and as a result y is too. y can be assigned to a (2,1) slot, but not a (2,) shape.
In [252]: xa = np.zeros((2,5),int)
In [253]: xa
Out[253]:
array([[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0]])
In [254]: xa[:,2]
Out[254]: array([0, 0]) # (2,) shape
In [255]: xa[:,[2]]
Out[255]:
array([[0], # (2,1) shape
[0]])
In contrast to MATLAB numpy arrays can be 1d, e.g. (2,). Also leading dimensions are the outermost, as opposed to trailing. MATLAB readily reduces a (2,3,1) shape to (2,3), but a (2,1,1) only becomes (2,1).
broadcasting the way numpy uses arrays that can differ in shape. The two basic rules are that
- leading size 1 dimensions can added automatically to match
- size 1 dimensions can be adjusted to match
Thus a (2,) can become a (1,2).
If you remove the inner [] from x, you get a 1d array:
In [256]: x = np.array([0.5, 2.0])
In [257]: x.shape
Out[257]: (2,)
In [258]: A.dot(x)
Out[258]: array([4.5, 7. ]) # (2,) shape
This can then be assigned to a row of xa: xa[:,2] = A.dot(x)
reshape and ravel can be used to remove dimensions. Also indexing A.dot(x)[:,0]
Suppose we have two numpy array x1 and x2 like below:
x1 = np.array([[0,2,9,1,0]])
x2 = np.array([[7,3,0,6,8]])
Is there any operation like:
x2(operation)x1 = array([[ 0, 3, 0, 6, 0]])
i.e. if x1 or x2 is 0 at any index then make the result array's index value as zero. Otherwise, keep x2 as it is.
Use numpy.where:
x3 = np.where(x1 == 0, x1, x2)
print(x3)
Output:
[[0 3 0 6 0]]
Given that you want to keep x2 but make it zero in the case x1 is zero, just multiply x2 by the boolean of x1.
>>> x2 * x1.astype(bool)
array([[0, 3, 0, 6, 0]])
Note that if x2 is zero, the result is zero as expected.
I have a problem where I have 4 variables x1, x2, x3 and x4. I need to find the values for x1, x2, x3, x4 with the following conditions:
1. 1995 < 2*x1 + 4*x2 + 3*x3 + x4 < 2000
2. x1 >= 1.2*x2
3. x2 >= 1.3*x3
4. x3 >= 1.1*x4
5. x4 > 0.0
I was able do this using python-constraint (https://labix.org/python-constraint) but it takes ~30 mins to solve this on my system, which is too long.
from constraint import *
problem = Problem()
problem.addVariable("x1", range(100,500))
problem.addVariable("x2", range(100,500))
problem.addVariable("x3", range(100,500))
problem.addVariable("x4", range(100,500))
problem.addConstraint(lambda a, b, c, d: 2*a + 3*b + 4*c + 5*d > 1995, ["x1", "x2", "x3", "x4"])
problem.addConstraint(lambda a, b, c, d: 2*a + 3*b + 4*c + 5*d < 2005, ["x1", "x2", "x3", "x4"])
problem.addConstraint(lambda a, b: a >= 1.2 * b, ["x1", "x2"])
problem.addConstraint(lambda b, c: b >= 1.3 * c, ["x2", "x3"])
problem.addConstraint(lambda c, d: c >= 1.1 * d, ["x3", "x4"])
problem.addConstraint(lambda d: d > 0, ["x4"])
problem.getSolutions()
I also looked at scipy.optimize.linprog but I could not find a way to pass conditions 2, 3 and 4 because it is dependent on the value of another variable from the same problem. I can pass boundaries for each individual variables using the bounds parameter, like:
x1_bounds = (100, 200)
x2_bounds = (200, 300)
But how do I pass values of other variables in bounds, like x1_bounds >= 1.2*x2? Or is there any other way I can do this?
This can be solved using GRG non-linear solver in excel but I'm not able to find an equivalent in python.
Your problem is, in fact, linear, and so it's ideally suited to a linear programming approach. However, you are giving it to the solver with no clues as to the linearity of the problem, so it's bound to find that tricky: it pretty much has to try every possibility which is going to take a long time. It might be possible to rewrite your constraints into different forms for the python-constraint solver (it has, for example, a MaxSumConstraint constraint form) which might work better but ideally I think you should be using a solver specialised for linear problems.
There is a solver called kiwisolver which will do what you want. Here's your example converted for that library:
import kiwisolver
x1 = kiwisolver.Variable('x1')
x2 = kiwisolver.Variable('x2')
x3 = kiwisolver.Variable('x3')
x4 = kiwisolver.Variable('x4')
constraints = [1995 <= 2*x1 + 4*x2 + 3*x3 + x4,
2*x1 + 4*x2 + 3*x3 + x4 <= 2000,
x1 >= 1.2*x2,
x2 >= 1.3*x3,
x3 >= 1.1*x4,
x4 >= 0]
solver = kiwisolver.Solver()
for cn in constraints:
solver.addConstraint(cn)
for x in [x1, x2, x3, x4]:
print(x.value())
which gives
254.49152542372883
212.07627118644066
163.13559322033896
148.30508474576254
But you can also use a standard linear program solver like the scipy one. You just need to reorganise your inequalities into the right form.
You want:
1. 1995 < 2*x1 + 4*x2 + 3*x3 + x4 < 2000
2. x1 >= 1.2*x2
3. x2 >= 1.3*x3
4. x3 >= 1.1*x4
5. x4 > 0.0
So we rewrite this into:
2*x1 + 4*x2 + 3*x3 + 1*x4 < 2000
-2*x1 + -4*x2 + -3*x3 + -1*x4 < -1995
-1*x1 + 1.2*x2 + 0*x3 + 0*x4 < 0
0*x1 + -1*x2 + 1.3*x3 + 0*x4 < 0
0*x1 + 0*x2 + -1*x3 + 1.1*x4 < 0
You can add bounds for x1 to x4 as you mentioned in the question but by default they will just be non-negative. So then, for an LP, we also need to choose where in the polytope of possible solutions we want to optimise: in this case I'll just go for the solution with the minimum sum. So that gives us this:
from scipy.optimize import linprog
output = linprog([1, 1, 1, 1],
[[ 2, 4, 3, 1],
[-2, -4, -3, -1],
[-1, 1.2, 0, 0],
[0, -1, 1.3, 0],
[0, 0, -1, 1.1]],
[2000, -1995, 0, 0, 0])
print(output.x)
This gives
[274.92932862 229.10777385 176.23674912 0. ]
which is the optimal LP solution. Note that it has made x4 = 0: LPs typically don't distinguish between > and >= and so we have a solution where x4 is zero rather than a tiny epsilon greater than zero.
Finally, note that the problem is strongly under-constrained: we can choose a quite different solution by changing the objective. Here's a solution where we ask linprog to maximise 2*x1 + 4*x2 + 3*x3 + x4:
from scipy.optimize import linprog
output = linprog([-2, -4, -3, -1],
[[ 2, 4, 3, 1],
[-2, -4, -3, -1],
[-1, 1.2, 0, 0],
[0, -1, 1.3, 0],
[0, 0, -1, 1.1]],
[2000, -1995, 0, 0, 0])
print(output.x)
giving
[255.1293488 212.60779066 163.54445436 148.67677669]
I'm trying to implement viewing matrix and projection, similar to gluLookAt to get the view position of each 3D coordinate. I have implemented something that seems close to working but is reversed.
For example - the following code gets the correct position (When I actually don't change the coordinates. But if I change the up-vector to point towards X instead of Y, I get reversed coordinates.
import numpy as np
def normalize_vector(vector):
return vector / (np.linalg.norm(vector))
def get_lookat_matrix(position_vector, front_vector, up_vector):
m1 = np.zeros([4, 4], dtype=np.float32)
m2 = np.zeros([4, 4], dtype=np.float32)
z = normalize_vector(-front_vector)
x = normalize_vector(np.cross(up_vector, z))
y = np.cross(z, x)
m1[:3, 0] = x
m1[:3, 1] = y
m1[:3, 2] = z
m1[3, 3] = 1.0
m2[0, 0] = m2[1, 1] = m2[2, 2] = 1.0
m2[:3, 3] = -position_vector
m2[3, 3] = 1.0
return np.matmul(m1, m2)
def get_projection_matrix(near, far):
aspect = 1.0
fov = 1.0 # 90 Degrees
m = np.zeros([4, 4], dtype=np.float32)
m[0, 0] = fov/aspect
m[1, 1] = fov
m[2, 2] = (-far)/(far-near)
m[2, 3] = (-near*far)/(far-near)
m[3, 2] = -1.0
return m
position_vector = np.array([0, 0, 0], dtype=np.float32)
front_vector = np.array([0, 0, -1], dtype=np.float32)
up_vector = np.array([0, 1, 0], dtype=np.float32)
viewing_matrix = get_lookat_matrix(position_vector=position_vector, front_vector=front_vector, up_vector=up_vector)
print("viewing_matrix\n", viewing_matrix, "\n\n")
projection_matrix = get_projection_matrix(near=0.1, far=100.0)
point = np.array([1, 0, -10, 1], dtype=np.float32)
projected_point = projection_matrix.dot(viewing_matrix.dot(point))
# Normalize
projected_point /= projected_point[3]
print(projected_point)
And it happens with many changes of the coordinates. I'm not sure where am I wrong.
gluLookAt defines a 4*4 viewing transformation matrix, for the use of OpenGL.
A "mathematical" 4*4 matrix looks like this:
c0 c1 c2 c3 c0 c1 c2 c3
[ Xx Yx Zx Tx ] [ 0 4 8 12 ]
[ Xy Yy Zy Ty ] [ 1 5 9 13 ]
[ Xz Yz Zz Tz ] [ 2 6 10 14 ]
[ 0 0 0 1 ] [ 3 7 11 15 ]
But the memory image of a 4*4 OpenGL matrix looks like this:
[ Xx, Xy, Xz, 0, Yx, Yy, Yz, 0, Zx, Zy, Zz, 0, Tx, Ty, Tz, 1 ]
See The OpenGL Shading Language 4.6, 5.4.2 Vector and Matrix Constructors, page 101
and OpenGL ES Shading Language 3.20 Specification, 5.4.2 Vector and Matrix Constructors, page 100:
To initialize a matrix by specifying vectors or scalars, the components are assigned to the matrix elements in column-major order.
mat4(float, float, float, float, // first column
float, float, float, float, // second column
float, float, float, float, // third column
float, float, float, float); // fourth column
Note, in compare to a mathematical matrix where the columns are written from top to bottom, which feels natural, at the initialization of an OpenGL matrix, the colums are written from the left to the right. This lead sto the benefit, that the x, y, z components of an axis or of the translation are in direct succession in the memory. This is a big advantage when accessing the axis vectors or the translation vector of the matrix.
See also Data Type (GLSL) - Matrix constructors.
This means you have to "swap" columns and rows (transpose) of the matrix:
def get_lookat_matrix(position_vector, front_vector, up_vector):
m1 = np.zeros([4, 4], dtype=np.float32)
m2 = np.zeros([4, 4], dtype=np.float32)
z = normalize_vector(-front_vector)
x = normalize_vector(np.cross(up_vector, z))
y = np.cross(z, x)
m1[0, :3] = x
m1[1, :3] = y
m1[2, :3] = z
m1[3, 3] = 1.0
m2[0, 0] = m2[1, 1] = m2[2, 2] = 1.0
m2[3, :3] = -position_vector
m2[3, 3] = 1.0
return np.matmul(m1, m2)
def get_projection_matrix(near, far):
aspect = 1.0
fov = 1.0 # 90 Degrees
m = np.zeros([4, 4], dtype=np.float32)
m[0, 0] = fov/aspect
m[1, 1] = fov
m[2, 2] = (-far+near)/(far-near)
m[3, 2] = (-2.0*near*far)/(far-near)
m[2, 3] = -1.0
return m
There's a minor change you must do:
m[2, 2] = -(far+near)/(far-near) //instead of m[2, 2] = (-far)/(far-near)
m[2, 3] = (-2.0*near*far)/(far-near) //instead of m[2, 3] = (-near*far)/(far-near)
The big thing is the row/column order of your matrices.
As #Rabbid76 pointed out, mayor column order is preferred. GLSL provides a function to transpose a matrix. You can also tell to transpose the matrix when it's passed to GPU with glUniformMatrix family commands.
Let's see how to work with row mayor order matrices, as your code does.
The goal, by now with CPU, is to get: finalPoint = matrixMultiply(C, P) with C the combined matrix and P the point coordinates. matrixMultiply is any function you use to do matrices multplication. Remember the order matters, A·B is not the same as B·A
Because C is a 4x4 matrix and P is 1x4, C·P is not possible, it must be P·C.
Notice that with column order P is 4x1 and then C·P is the right operation.
Let's call L the look-at matrix (proper name is view matrix). It's formed by an orientation matrix O and a translation matrix T. With column order is L= O·T.
A property of transposed matrix is (A·B)t = Bt · At
So, with row order you get O·T = Oct · Tct = (Tc · Oc)t where c is for column order. Hey! what we wish is (Oc · Tc)t Notice the change in order of multiplication?
So, if you work with row mayor order matrices, the order they are multiplied is swapped.
The view&projection combined matrix also must be swapped.
Thus replace:
return np.matmul(m2, m1) //was return np.matmul(m1, m2)
and
//was projected_point = projection_matrix.dot(viewing_matrix.dot(point))
projected_point = point.dot(viewing_matrix.dot(projection_matrix))
Despite of all of above, I recommend to work with column mayor order. That's best for OpenGL. And you'll understand better any maths and tutorials you find on OpenGL.
I'm looking for a pythonic (1-line) way to extract a range of values from an array
Here's some sample code that will extract the array elements that are >2 and <8 from x,y data, and put them into a new array. Is there a way to accomplish this on a single line? The code below works but seems kludgier than it needs to be. (Note I'm actually working with floats in my application)
import numpy as np
x0 = np.array([0,3,9,8,3,4,5])
y0 = np.array([2,3,5,7,8,1,0])
x1 = x0[x0>2]
y1 = y0[x0>2]
x2 = x1[x1<8]
y2 = y1[x1<8]
print x2, y2
This prints
[3 3 4 5] [3 8 1 0]
Part (b) of the problem would be to extract values say 1 < x < 3 and 7 < x < 9 as well as their corresponding y values.
You can chain together boolean arrays using & for element-wise logical and and | for element-wise logical or, so that the condition 2 < x0 and x0 < 8 becomes
mask = (2 < x0) & (x0 < 8)
For example,
import numpy as np
x0 = np.array([0,3,9,8,3,4,5])
y0 = np.array([2,3,5,7,8,1,0])
mask = (2 < x0) & (x0 < 8)
x2 = x0[mask]
y2 = y0[mask]
print(x2, y2)
# (array([3, 3, 4, 5]), array([3, 8, 1, 0]))
mask2 = ((1 < x0) & (x0 < 3)) | ((7 < x0) & (x0 < 9))
x3 = x0[mask2]
y3 = y0[mask2]
print(x3, y3)
# (array([8]), array([7]))
import numpy as np
x0 = np.array([0,3,9,8,3,4,5])
y0 = np.array([2,3,5,7,8,1,0])
list( zip( *[(x,y) for x, y in zip(x0, y0) if 1<=x<=3 or 7<=x<=9] ) )
# [(3, 9, 8, 3), (3, 5, 7, 8)]