Constraint on Ability to Switch Input Values for Trajectory Optimization in Model Predictive Control Framework - optimization

I am trying to add a constraint which specifies that, in the optimization, the solver must pick a value of u for a set duration of time and can only switch after that set amount of time. For instance, say I have a mechanical device which can only switch its input value every 10 seconds. Then, I want the optimizer to account for that. I'll just attach the code here:
for it_i in range(0, N-1, equivalence_samples):
print("N: {}".format(N))
for it_j in range(0, equivalence_samples - 1):
if (it_i + it_j + 1) > N-1:
print("Breaking")
break
else:
constraint_u0 = prog.AddConstraint(u[0, it_i + it_j] == u[0, it_i + it_j + 1]) # add equivalence constraints
constraint_u1 = prog.AddConstraint(u[1, it_i + it_j] == u[1, it_i + it_j + 1]) # add equivalence constraints
print('Constraint_u_PE: {}'.format(constraint_u0))
print('Constraint_u_NI: {}'.format(constraint_u1))
I have implemented this in, what I expect to be a working solution. Sometimes it seems like it is working and other times, it does not.
I will show some photos of the output constraints from this and then a not working example.
Then, here are the plots that come out which clearly show there is some delineation between the switching times, but the values are not equivalent. I am attaching the code which generates this plot as well.
u_sol = result.GetSolution(u)
u_time = np.linspace(0, N-1, num = N)
# u_sol_trajectory = PiecewisePolynomial.ZeroOrderHold(u_time, u_sol)
plt.figure()
plt.plot(u_time, u_sol[0, :], 'o')
plt.plot(u_time, u_sol[1, :], 'o')
plt.xlabel('time steps')
plt.ylabel('u [mcg/min]')
plt.legend(['u_PE', 'u_NI'])

The particular solver that was being used in this case is the OSQP solver. Although I, in an ideal solver world, specified the correct constraints in the above code (that input 1 == input 2, input 2 == input 3, etc.), I did not account for the fact that solvers have an accuracy with which they try to uphold the constraints.
I can fix this problem by either updating the accuracy of the solver (as recommended by https://osqp.discourse.group/t/constraints-violated/143) or inputing more explicit constraints. I solved this with the second option. Now, I am specifying not just constraints like the following pattern:
input 1 == input 2, input 2 == input 3, etc.
but I am also including constraints like the following pattern:
input 1 == input 3, input 1 == input 4, input 1 == input 5
input 2 == input 4, input 2 == input 5 etc.
By being more explicit, my solver is now doing what I asked with small deviations from the constraint. The small deviations are acceptable for my application, however! It is a bit slower, but this isn't a problem for what I am using this for at the moment. Here is my updated code:
for it_i in range(0, N-1, equivalence_samples):
for it_j in range(0, equivalence_samples - 1):
for it_f in range(1, equivalence_samples - it_j):
if (it_i + it_j + it_f) > N-1:
print("Breaking")
break
else:
con_0 = eq(u[:, it_i + it_j], u[:, it_i + it_j + it_f])
constraint_u = prog.AddConstraint(con_0) # add equivalence constraints
print('Constraint_u: {}'.format(constraint_u))
Not the prettiest code in the world, but it works.

Related

Applying normal noise to column, if in range. Pandas / Python

I want to add noise to a column of values in the range of 0-1.
But the noise shouldn't exceed these ranges, so my thought process was to check if adding the noise would be outside of the range, if it did, don't add the noise.
I tried:
df['val_x'].apply(lambda x: (x + np.random.normal(0, 0.2)) if (0 < x + np.random.normal(0, 0.2) < 1) else x)
at first, but I'm assuming it creates two separate random values, so some of the values pass the check with one and apply to the data frame with the other.
I feel like I need something like:
df['val_x'].apply(lambda x, withNoise = x + np.random.normal(0, 0.2): withNoise if (0 < withNoise < 1) else x)
defining the argument beforehand, but lambda doesn't support defining arguments with other arguments.
I want to do this without creating another function, but if it is the only way, I can.
Thanks in advance.
What about clipping?
df['val_x'] = df['val_x'].add(np.random.normal(0, 0.2, size=len(df))).clip(0, 1)
Or, adding your noise and only update the valid values:
s = df['val_x'].add(np.random.normal(0, 0.2, size=len(df)))
df['val_x'] = s.where(s.between(0, 1), df['val_x'])
# or
df.loc[s.between(0, 1), 'val_x'] = s

Constraint Programming, how to add x[i] <= (max(x[:i]) + 1)

I'm building a model using or-tools CP tools. The values I want to find are placed in a vector X, and I want to add a constraint that says up to each position of X, the next position cannot have as a value something bigger than the maximum found until X[:i] + 1
It would be something like this:
X[i] <= (max(X[:i]) + 1)
Of course, I cannot add this as a linear constraint with a max(), and creating one extra feature for each value of X upper bound seems excessive and also I would need to minimize each one to make it the "max", otherwise those are just upper bounds that could be huge and not prune my search space (and I already have an objective function).
I already have an objective function.
I know that one trick to add for instance a min-max (min(max(x[i])) problem is to create another variable that is an upper bound of each x and minimize that one. It would be sth like this:
model = cp_model.CpModel()
lb =0; ub=0
model.NewIntVar(z, lb, ub)
for i in domain(X):
model.NewIntVar(X[i], lb, up)
model.Add(X[i] <= z)
model.Minimize(z)
In case you don't want to program this you can use the method in or-tools:
model.AddMaxEquality(z, X)
Now I want to add a constraint that at each value of X sets an upper limit which is the maximum value found until the previous x. It would be something like this:
X[i] <= max(X[:i]) + 1
I was thinking of replicating the previous idea but that would require creating a "z" for each x... not sure if that is the best approach and how much it will reduce my space of solutions. At the same time couldn't find a method in or-tools to do this.
Any suggestions, please?
PS: I already have as an objective function min(z) like it is in the example presented.
Example:
For instance, you can have as a result of the model:
[0, 1, 2, 0, 2, 3]
But you shouldn't have:
[0, 1, 1, 2, 4]
Since the max until X[:3] is 2, so the ub of X[4] should be 2 + 1.
Thanks!
I have no specific hints except:
you need to experiment. One modeling trick may work on one kind of model and not on the other
make sure to use reuse the max variable at index i - 1. With X the array of variables and M the array of max, i.e. M[i] = max(X[0], .., X[i - 1])
M[i] = max(M[i - 1], X[i - 1])
X[i] <= M[i] + 1

How to accelerate my written python code: function containing nested functions for classification of points by polygons

I have written the following NumPy code by Python:
def inbox_(points, polygon):
""" Finding points in a region """
ll = np.amin(polygon, axis=0) # lower limit
ur = np.amax(polygon, axis=0) # upper limit
in_idx = np.all(np.logical_and(ll <= points, points < ur), axis=1) # points in the range [boolean]
return in_idx
def operation_(r, gap, ends_ind):
""" calculation formula which is applied on the points specified by inbox_ function """
r_active = np.take(r, ends_ind) # taking values from "r" based on indices and shape (paired_values) of "ends_ind"
r_sub = np.subtract.reduce(r_active, axis=1) # subtracting each paired "r" determined by "ends_ind" [this line will be used in the final return formula]
r_add = np.add.reduce(r_active, axis=1) # adding each paired "r" determined by "ends_ind" [this line will be used in the final return formula]
paired_cent_dis = np.sum((r_add, gap), axis=0) # distance of the each two paired points
return (np.power(gap, 2) * (np.power(paired_cent_dis, 2) + 5 * paired_cent_dis * r_add - 7 * np.power(r_sub, 2))) / (3 * paired_cent_dis) # Formula
def elapses(r, pos, gap, ends_ind, elem_vert, contact_poss):
if len(gap) > 0:
elaps = np.empty([len(elem_vert), ], dtype=object)
operate_ = operation_(r, gap, ends_ind)
#elbav = np.empty([len(elem_vert), ], dtype=object)
#con_num = 0
for i, j in enumerate(elem_vert): # loop for each section (cell or region) of a mesh
in_bool = inbox_(contact_poss, j) # getting boolean array for points within that section
elaps[i] = np.sum(operate_[in_bool]) # performing some calculations on that points and get the sum of them for each section
operate_ = operate_[np.invert(in_bool)] # slicing the arrays by deleting the points on which the calculations were performed to speed-up the code in next loops
contact_poss = contact_poss[np.invert(in_bool)] # as above
#con_num += sum(inbox_(contact_poss, j))
#inba_bool = inbox_(pos, j)
#elbav[i] = 4 * np.pi * np.sum(np.power(r[inba_bool], 3)) / 3
#pos = pos[np.invert(inba_bool)]
#r = r[np.invert(inba_bool)]
return elaps
r = np.load('a.npy')
pos = np.load('b.npy')
gap = np.load('c.npy')
ends_ind = np.load('d.npy')
elem_vert = np.load('e.npy')
contact_poss = np.load('f.npy')
elapses(r, pos, gap, ends_ind, elem_vert, contact_poss)
# a --------r-------> parameter corresponding to each coordinate (point); here radius (23605,) <class 'numpy.ndarray'> <class 'numpy.float64'>
# b -------pos------> coordinates of the points (23605, 3) <class 'numpy.ndarray'> <class 'numpy.ndarray'> <class 'numpy.float64'>
# c -------gap------> if we consider points as spheres by that radii [r], it is maximum length for spheres' over-lap (103832,) <class 'numpy.ndarray'> <class 'numpy.float64'>
# d ----ends_ind----> indices for each over-laped spheres (103832, 2) <class 'numpy.ndarray'> <class 'numpy.ndarray'> <class 'numpy.int64'>
# e ---elem_vert----> vertices of the mesh's sections or cells (2000, 8, 3) <class 'numpy.ndarray'> <class 'numpy.ndarray'> <class 'numpy.ndarray'> <class 'numpy.float64'>
# f --contact_poss--> a coordinate between the paired spheres (103832, 3) <class 'numpy.ndarray'> <class 'numpy.ndarray'> <class 'numpy.float64'>
This code will be called frequently from another code with big-data inputs. So, speeding up this code is essential. I have tried to use jit decorator from JAX and Numba libraries to accelerate the code, but I could not work with that properly to make the code better. I have tested the code on Colab (for 3 data sets with loops number of 20, 250, and 2000) for speed and the results were:
11 (ms), 47 (ms), 6.62 (s) (per loop) <-- without the commented code lines in the code
137 (ms), 1.66 (s) , 4 (m) (per loop) <-- with activating the commented code lines in the code
What this code does is finding some coordinates in a range and then performing some calculations on them.
I will be very appreciated for any answers that can speed up this code significantly (I believe it could). Also, I will be grateful for any experienced recommendations about speeding up the code by changing (substituting) the used NumPy methods and … or writing method for the math operations.
Notes:
The proposed answers must be executable by python version 2 (being applicable in both versions 2 and 3 is very excellent)
The commented code lines in the code are unnecessary for the main aim and are written just for further evaluations. Any recommendations to handle these lines with the proposed answers is appreciated (is not needed).
Data sets for test:
small data set: https://drive.google.com/file/d/1CswjyoqS8ogLmLQa_oNTOj221chDcbK8/view?usp=sharing
medium data set: https://drive.google.com/file/d/14RJ0Ackx88NzQWloops5FagzuNQYDSrh/view?usp=sharing
large data set: https://drive.google.com/file/d/1dJnXpb3HiAGcRC9PPTwui9joNcij4E_E/view?usp=sharing
First of all, the algorithm can be improved to be much more efficient. Indeed, a polygon can be directly assigned to each point. This is like a classification of points by polygons. Once the classification is done, you can perform one/many reductions by key where the key is the polygon ID.
This new algorithm consists in:
computing all the bounding boxes of the polygons;
classifying the points by polygons;
performing the reduction by key (where the key is the polygon ID).
This approach is much more efficient than iterating over all the points for each polygons and filtering the attributes arrays (eg. operate_ and contact_poss). Indeed, a filtering is an expensive operation since it requires the target array (that may not fit in the CPU caches) to be fully read and then written back. Not to mention this operation requires a temporary array to be allocated/deleted if it is not performed in-place and the operation cannot benefit from being implemented with SIMD instructions on most x86/x86-64 platforms (as it requires the new AVX-512 instruction set). It is also harder to parallelize since the filtering steps are too fast for threads to be useful but steps need to be done sequentially.
Regarding the implementation of the algorithm, Numba can be used to speed up a lot the overall computation. The main benefit of using Numba is to drastically reduce the number of expensive temporary arrays created by Numpy in your current implementation. Note that you can specify the function types to Numba so it can compile functions when it is defined. Assertions can be used to make the code more robust and help the compiler to know the size of a given dimension so to generate a significantly faster code (the JIT compiler of Numba can unroll the loops). Ternaries operators can help a bit the JIT compiler to generate a faster branch-less program.
Note the classification can be easily parallelized using multiple threads. However, one needs to be very careful about constant propagation since some critical constants (like the shape of the working arrays and assertions) tends not to be propagated to the code executed by threads while the propagation is critical to optimize the hot loops (eg. vectorization, unrolling). Note also that creating of many threads can be expensive on machines with many cores (from 10 ms to 0.1 ms). Thus, this is often better to use a parallel implementation only on big input data.
Here is the resulting implementation (working with both Python2 and Python3):
#nb.njit('float64[::1](float64[::1], float64[::1], int64[:,::1])')
def operation_(r, gap, ends_ind):
""" calculation formula which is applied on the points specified by findMatchingPolygons_ function """
nPoints = ends_ind.shape[0]
assert ends_ind.shape[1] == 2
assert gap.size == nPoints
formula = np.empty(nPoints, dtype=np.float64)
for i in range(nPoints):
ind0, ind1 = ends_ind[i]
r0, r1 = r[ind0], r[ind1]
r_sub = r0 - r1
r_add = r0 + r1
cur_gap = gap[i]
paired_cent_dis = r_add + cur_gap
formula[i] = (cur_gap**2 * (paired_cent_dis**2 + 5 * paired_cent_dis * r_add - 7 * r_sub**2)) / (3 * paired_cent_dis)
return formula
# Use `parallel=True` for a parallel implementation
#nb.njit('int32[::1](float64[:,::1], float64[:,:,::1])')
def findMatchingPolygons_(points, polygons):
""" Attribute to all point a region """
nPolygons = polygons.shape[0]
nPolygonPoints = polygons.shape[1]
nPoints = points.shape[0]
assert points.shape[1] == 3
assert polygons.shape[2] == 3
# Compute the bounding boxes of all polygons
ll = np.empty((nPolygons, 3), dtype=np.float64)
ur = np.empty((nPolygons, 3), dtype=np.float64)
for i in range(nPolygons):
ll_x, ll_y, ll_z = polygons[i, 0]
ur_x, ur_y, ur_z = polygons[i, 0]
for j in range(1, nPolygonPoints):
x, y, z = polygons[i, j]
ll_x = x if x<ll_x else ll_x
ll_y = y if y<ll_y else ll_y
ll_z = z if z<ll_z else ll_z
ur_x = x if x>ur_x else ur_x
ur_y = y if y>ur_y else ur_y
ur_z = z if z>ur_z else ur_z
ll[i] = ll_x, ll_y, ll_z
ur[i] = ur_x, ur_y, ur_z
# Find for each point its corresponding polygon
pointPolygonId = np.empty(nPoints, dtype=np.int32)
# Use `nb.prange(nPoints)` for a parallel implementation
for i in range(nPoints):
x, y, z = points[i, 0], points[i, 1], points[i, 2]
pointPolygonId[i] = -1
for j in range(polygons.shape[0]):
if ll[j, 0] <= x < ur[j, 0] and ll[j, 1] <= y < ur[j, 1] and ll[j, 2] <= z < ur[j, 2]:
pointPolygonId[i] = j
break
return pointPolygonId
#nb.njit('float64[::1](float64[:,:,::1], float64[:,::1], float64[::1])')
def computeSections_(elem_vert, contact_poss, operate_):
nPolygons = elem_vert.shape[0]
elaps = np.zeros(nPolygons, dtype=np.float64)
pointPolygonId = findMatchingPolygons_(contact_poss, elem_vert)
for i, polygonId in enumerate(pointPolygonId):
if polygonId >= 0:
elaps[polygonId] += operate_[i]
return elaps
def elapses(r, pos, gap, ends_ind, elem_vert, contact_poss):
if len(gap) > 0:
operate_ = operation_(r, gap, ends_ind)
return computeSections_(elem_vert, contact_poss, operate_)
r = np.load('a.npy')
pos = np.load('b.npy')
gap = np.load('c.npy')
ends_ind = np.load('d.npy')
elem_vert = np.load('e.npy')
contact_poss = np.load('f.npy')
elapses(r, pos, gap, ends_ind, elem_vert, contact_poss)
Here are the results on a old 2-core machine (i7-3520M):
Small dataset:
- Original version: 5.53 ms
- Proposed version (sequential): 0.22 ms (x25)
- Proposed version (parallel): 0.20 ms (x27)
Medium dataset:
- Original version: 53.40 ms
- Proposed version (sequential): 1.24 ms (x43)
- Proposed version (parallel): 0.62 ms (x86)
Big dataset:
- Original version: 5742 ms
- Proposed version (sequential): 144 ms (x40)
- Proposed version (parallel): 67 ms (x86)
Thus, the proposed implementation is up to 86 times faster than the original one.

How to handle log(0) when using cross entropy

In order to make the case simple and intuitive, I will using binary (0 and 1) classification for illustration.
Loss function
loss = np.multiply(np.log(predY), Y) + np.multiply((1 - Y), np.log(1 - predY)) #cross entropy
cost = -np.sum(loss)/m #num of examples in batch is m
Probability of Y
predY is computed using sigmoid and logits can be thought as the outcome of from a neural network before reaching the classification step
predY = sigmoid(logits) #binary case
def sigmoid(X):
return 1/(1 + np.exp(-X))
Problem
Suppose we are running a feed-forward net.
Inputs: [3, 5]: 3 is number of examples and 5 is feature size (fabricated data)
Num of hidden units: 100 (only 1 hidden layer)
Iterations: 10000
Such arrangement is set to overfit. When it's overfitting, we can perfectly predict the probability for the training examples; in other words, sigmoid outputs either 1 or 0, exact number because the exponential gets exploded. If this is the case, we would have np.log(0) undefined. How do you usually handle this issue?
If you don't mind the dependency on scipy, you can use scipy.special.xlogy. You would replace the expression
np.multiply(np.log(predY), Y) + np.multiply((1 - Y), np.log(1 - predY))
with
xlogy(Y, predY) + xlogy(1 - Y, 1 - predY)
If you expect predY to contain very small values, you might get better numerical results using scipy.special.xlog1py in the second term:
xlogy(Y, predY) + xlog1py(1 - Y, -predY)
Alternatively, knowing that the values in Y are either 0 or 1, you can compute the cost in an entirely different way:
Yis1 = Y == 1
cost = -(np.log(predY[Yis1]).sum() + np.log(1 - predY[~Yis1]).sum())/m
How do you usually handle this issue?
Add small number (something like 1e-15) to predY - this number doesn't make predictions much off, and it solves log(0) issue.
BTW if your algorithm outputs zeros and ones it might be useful to check the histogram of returned probabilities - when algorithm is so sure that something's happening it can be a sign of overfitting.
One common way to deal with log(x) and y / x where x is always non-negative but can become 0 is to add a small constant (as written by Jakub).
You can also clip the value (e.g. tf.clip_by_value or np.clip).

Incorrect result of sum int-casted BitVec using Z3, Z3py

I am using the following python code to find two binary numbers that:
sum to a certain number
their highest bits cast to integers must sum up to 2
The second constraint is more important to me; and in my case, it will scale: let's say it might become that highest bits of [N] number must sum up to [M].
I am not sure why z3 does not give the correct result. Any hints? Thanks a lot.
def BV2Int(var):
return ArithRef(Z3_mk_bv2int(ctx.ref(), var.as_ast(), 0), var.ctx)
def main():
s = Solver()
s.set(':models', True)
s.set(':auto-cfgig', False)
s.set(':smt.bv.enable_int2bv',True)
x = BitVec('x',4)
y = BitVec('y',4)
s = Solver()
s.add(x+y == 16, Extract(3,3,x) + Extract(3,3,y) == 2)
s.check()
print s.model()
# result: [y = 0, x = 0], fail both constraint
s = Solver()
s.add(x+y == 16, BV2Int(Extract(3,3,x)) + BV2Int(Extract(3,3,y)) == 2)
s.check()
print s.model()
# result: [y = 15, x = 1], fail the second constraint
Update: Thanks the answer from Christoph. Here is a quick fix:
Extract(3,3,x) -> ZeroExt(SZ, Extract(3,3,x)) where SZ is the bit width of RHS minus 1.
(Aside: auto-cfgig should be auto-config.)
Note that bv2int and int2bv are essentially treated as uninterpreted, so if this part is crucial to your problem, then don't use them (see documentation and previous questions).
The problem with this example are the widths of the bit-vectors. Both x and y are 4-bit variables, and the numeral 16 as a 4-bit vector is 0 (modulo 2^4), so, indeed x + y is equal to 16 when x=0 and y=0.
Further, the Extract(...) terms extract 1-bit vectors, which means that the sum Ex.. + Ex.. is again a 1-bit value and the numeral 2 as a 1-bit vector is 0 (modulo 2^1), i.e., it is indeed the case that Ex... + Ex... = 2.