We Keep Coding

Error in formulation of a Derivative(PD) controller

I'm applying Proportional-Derivative controller to control my model in ROS. I'm limited to python 2.7.17 version.
There are two types of errors in this script; position error(ep) and heading error(eth).
I've given last_error=0 and trying to get the updation in (ep_dot) and (eth_dot) as a method to find derivative of error.
I wonder whether my given formula for derivative(ep_dot) and (eth_dot) is correct or not.Is this correct way of finding derivative ? Is there any other relevant approach to find the same?
Kindly let me know the solution.
#!/usr/bin/env python
import rospy
import math
import csv
import time
from time import gmtime,strftime
from datetime import datetime
import numpy as np # for converting radians to degrees
from geometry_msgs.msg import Twist #to obtain command velocities
from nav_msgs.msg import Odometry #to obtain position and orientation
from tf.transformations import euler_from_quaternion, quaternion_from_euler
roll = pitch = yaw = 0.0
t_start = time.time()
**last_error =0**
def get_rotation(msg):
velo_msg = Twist()
global roll,pitch,yaw
orientation_q = msg.pose.pose.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
kpp = 0.74
kpth = 0.5
kd = 0.1
Tmax = 60
t_milli = (time.time() - t_start)*1000
t = t_milli/1000 # to get the values in seconds
print("t=",t)
if t > Tmax:
rospy.signal_shutdown("Simulation ends here!")
x_now = msg.pose.pose.position.x
y_now = msg.pose.pose.position.y
th = yaw
xT = math.cos(2*math.pi*t/Tmax)*0.8
yT = math.sin(2*math.pi*t/Tmax)*0.8
#Trasformation matrix # Finding INVERSE
I = np.array([[math.cos(th), -math.sin(th), x_now],[math.sin(th), math.cos(th), y_now],[0, 0, 1]])
B = np.array([[xT],[yT],[1]]) # print('B=',B)
C = np.dot(np.linalg.inv(I),B) # np.dot: for matrix multiplication
xTB = C[0][0] # [] indexing to extract the values of an array/matrix
yTB = C[1][0]
ep = math.sqrt(xTB*xTB + yTB*yTB) # error calc.
ep_dot = (ep-last_error)/t
eth = math.atan2(yTB,xTB)
eth_dot = (eth-last_error)/t
print('ep =',ep,'eth(deg)=',eth*180/math.pi,'radius=',math.sqrt(x_now*x_now + y_now*y_now),'t=',t)
PID_lin = ep*kpp + ep_dot*kd
PID_ang = eth*kpth - eth_dot*kd
# publishing the cmd_vel in linear and angular motion both
velo_msg.linear.x = PID_lin
velo_msg.angular.z = PID_ang
pub.publish(velo_msg)
rospy.init_node('shadan')
sub = rospy.Subscriber("/odom", Odometry, get_rotation)
pub = rospy.Publisher('/cmd_vel',Twist,queue_size=10)
r = rospy.Rate(10)
while not rospy.is_shutdown():
r.sleep()

I think there is an issue with your calculation of the derivative terms.
You get t_start=time.time() at the very beginning of your code and then every time you get in your callback you update t_milli = (time.time() - t_start)*1000 and t = t_milli/1000 with t_start being a constant.
Then you calculate ep_dot as being equals to ep_dot = (ep-last_error)/t.
But ep_dot should be equal to something like ep_dot = (actual_value - last_value)/(t_actual - t_last_value)
The derivative term should be equal to the difference of two consecutive values divided by the difference of time between those two values so you need to store the time of the previous data acquired in the callback in a new variable.
Also, the calculation of the proportional error should be equal to your desired value (goal) minus the actual value. Then you multiply that by your P coefficient.
Hope it fixes your problem !

GEKKO python, how to run a calculation until I get a desired value of a variable

Does anyone know how to run a dynamic simulation in GEKKO Python (IMODE 7) until a certain variable takes on the desired value? For example, I have a differential equation and I need to do the calculation until the pressure is 1 bar. I understand how to make a calculation for 100 seconds, 200 seconds, etc., but I do not understand how to let it run until I have a variable of a specific desired value.
The code that I have is kind of large, so in principle this code that I want to modify:
I want to have it without a while statement because every while statement creates a new model that takes time. And I have much bigger code and this time makes a negative impact.
#Start calculation
pressure_final = 0
t_guess = 10
while pressure_final < 1:
m = GEKKO(remote = False) # Create GEKKO model
k = 0.1
P = m.Var(0.1)
#Time discretization
tf = t_guess
nt = int(tf/1) + 1
m.time = np.linspace(0,tf,nt)
#Left boundary pressure
m.Equation(P.dt() == k*P)
print("Start Pressurization")
m.options.IMODE = 7
m.solve(disp = False)
print("Finished pressurization")
t_guess = t_guess + 1
pressure_final = P[-1]
I will be very grateful for the answer!

There are two ways to accomplish this.
The first method is with step-by-step simulation with IMODE=4. There is no need to recreate the model each cycle. A solve command time shifts the final conditions to the initial conditions automatically because m.options.TIME_SHIFT=1. The pressure may exceed the final target value using this method.
import numpy as np
from gekko import GEKKO
m = GEKKO(remote = False) # Create GEKKO model once
k = 0.1
P = m.Var(0.1)
m.time = [0,1]
m.Equation(P.dt() == k*P)
m.options.IMODE = 4
m.options.NODES = 3
t=[0]; Pr=[0.1] # store time / Pressure
for i in range(100):
print(i)
m.solve(disp = False)
t.append(i+1)
Pr.append(P[-1])
if Pr[-1]>=1:
break
import matplotlib.pyplot as plt
plt.plot(t,Pr,'b-o')
plt.plot([0,t[-1]],[1,1],'r--')
plt.ylabel('Pressure'), plt.xlabel('Time')
plt.savefig('pressure.png',dpi=300)
plt.show()
The second method is to make the final time an optimization variable an FV with tf.STATUS=1.
tf = m.FV(1,lb=1,ub=100)
tf.STATUS=1
Divide all differential terms by the tf value and use m.time between 0 and 1.
m.time = np.linspace(0,1,20)
m.Equation(P.dt()/tf == k*P)
Use IMODE=6 to minimize the deviation of the final pressure from 1 with
final = np.zeros(20); final[-1]=1
f = m.Param(final)
m.Minimize(f*(P-1)**2)
This gives the exact time of Final Time: 23.041309678 when the pressure limit is reached.
import numpy as np
from gekko import GEKKO
m = GEKKO(remote = False) # Create GEKKO model once
k = 0.1
P = m.Var(0.1)
m.time = np.linspace(0,1,20)
tf = m.FV(1,lb=1,ub=100); tf.STATUS=1
m.Equation(P.dt()/tf == k*P)
final = np.zeros(20); final[-1]=1
f = m.Param(final)
m.Minimize(f*(P-1)**2)
m.options.IMODE = 6
m.options.NODES = 3
m.solve(disp = False)
print('Final Time: ', tf[-1])
import matplotlib.pyplot as plt
plt.plot(m.time*tf[-1],P,'b-o')
plt.plot([0,tf[-1]],[1,1],'r--')
plt.ylabel('Pressure'), plt.xlabel('Time')
plt.savefig('pressure.png',dpi=300)
plt.show()

Implementing uncertainty in dynamic optimization problem

I have a question about implementing uncertainty terms into the Gekko optimization problem. I am a beginner in coding and starting by adding parts from the "fish management" example. I have two main questions.
I would like to add an uncertainty term in the model (e.g. fluctuating future prices) but it seems like I am not understanding how the module works. I am trying to draw a random value from a certain distribution and put it into m.Var, 'ss', hoping the module will take each value at each time as t moves on. But it seems like the module does not work that way. I am wondering if there is any way I can implement uncertainty terms into the process.
Assuming the optimization problem to allocate initial land, A(0), between use A and E, is solved for a single agent by controlling land to convert, e, I am planning to expand this to a multiple agents problem. For example, if land quality, h, and land quantity A differ among agents, n, I am planning to solve multiple optimization problems using for algorithm by calling the initial m.Var value and some parameters from a loaded dataframe. If possible, may I have a brief comment on this plan?
# -*- coding: utf-8 -*-
from gekko import GEKKO
from scipy.stats import norm
from scipy.stats import truncnorm
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
import operator
import math
import random
# create GEKKO model
m = GEKKO()
# Below, an agent is given initial land A(0) and makes a decision to convert this land to E
# Objective of an agent is to get maximum present utility(=log(income)) from both land use, income each period=(A+E(1-y))*Pa-C*u+E*Pe
# Uncertainty in future price lies for Pe, which I want to include with ss below
# After solving for a single agent, I want to solve this for all agents with different land quality h, risk aversion, mu, and land size A
# Then lastly collect data for total land use over time
# time points
n=51
year=10
k=50
m.time = np.linspace(0,year,n)
t=m.time
tt=t*(n-1)/year
tt = tt.astype(int)
ttt = np.exp(-t/(n-1))
# constants
Pa = 1
Pe = 1
C = 0.1
r = 0.05
y = 0.1
# distribution
# I am trying to generate a distribution, and use it as uncertainty term later in objective function
mu, sigma = 1, 0.1 # mean and standard deviation
#mu, sigman = df.loc[tt][2]
sn = np.random.normal(mu, sigma, n)
s= pd.DataFrame(sn)
ss=s.loc[tt][0]
# Control
# What is the difference between MV and CV? They give completely different solution
# MV seems to give correct answer
u = m.MV(value=0,lb=0,ub=10)
u.STATUS = 1
u.DCOST = 0
#u = m.CV(value=0,lb=0,ub=10)
# Variables
# m.Var and m.SV does not seem to lead to different results
# Can I call initial value from a dataset? for example, df.loc[tt][0] instead of 10 below?
# A = m.Var(value=df.loc[tt][0])
# h = m.Var(value=df.loc[tt][1])
A = m.SV(value=10)
E = m.SV(value=0)
#A = m.Var(value=10)
#E = m.Var(value=0)
t = m.Param(value=m.time)
Pe = m.Var(value=Pe)
d = m.Var(value=1)
# Equation
# It seems necessary to include restriction on u
m.Equation(A.dt()==-u)
m.Equation(E.dt()==u)
m.Equation(Pe.dt()==-1/k*Pe)
m.Equation(d==m.exp(-t*r))
m.Equation(A>=0)
# Objective (Utility)
J = m.Var(value=0)
# Final objective
# I want to include ss, uncertainty term in objective function
Jf = m.FV()
Jf.STATUS = 1
m.Connection(Jf,J,pos2='end')
#m.Equation(J.dt() == m.log(A*Pa-C*u+E*Pe))
m.Equation(J.dt() == m.log((A+E*(1-y))*Pa-C*u+E*Pe)*d)
#m.Equation(J.dt() == m.log(A*Pa-C*u+E*Pe*ss)*d)
# maximize profit
m.Maximize(Jf)
#m.Obj(-Jf)
# options
m.options.IMODE = 6 # optimal control
m.options.NODES = 3 # collocation nodes
m.options.SOLVER = 3 # solver (IPOPT)
# solve optimization problem
m.solve()
# print profit
print('Optimal Profit: ' + str(Jf.value[0]))
# collect data
# et=u.value
# print(et)
# At=A.value
# print(At)
# index = range(1, 2)
# columns = range(1, n+1)
# Ato=pd.DataFrame(index=index,columns=columns)
# Ato=At
# plot results
plt.figure(1)
plt.subplot(4,1,1)
plt.plot(m.time,J.value,'r--',label='profit')
plt.plot(m.time[-1],Jf.value[0],'ro',markersize=10,\
label='final profit = '+str(Jf.value[0]))
plt.plot(m.time,A.value,'b-',label='Agricultural Land')
plt.ylabel('Value')
plt.legend()
plt.subplot(4,1,2)
plt.plot(m.time,u.value,'k.-',label='adoption')
plt.ylabel('conversion')
plt.xlabel('Time (yr)')
plt.legend()
plt.subplot(4,1,3)
plt.plot(m.time,Pe.value,'k.-',label='Pe')
plt.ylabel('price')
plt.xlabel('Time (yr)')
plt.legend()
plt.subplot(4,1,4)
plt.plot(m.time,d.value,'k.-',label='d')
plt.ylabel('Discount factor')
plt.xlabel('Time (yr)')
plt.legend()
plt.show()

Try using the m.Param() (or m.FV, m.MV) instead of m.Var() to implement an uncertain value that is specified for each optimization. With m.Var(), an initial value can be specified but then it is calculated by the optimizer and the initial guess is no longer preserved.
Another way to implement uncertainty is to create an array of models with different parameters among the instances. Here is a simple example with K as a randomly selected value. This leads to 10 instances of the model that is optimized to a value of 40.
import numpy as np
from gekko import GEKKO
import matplotlib.pyplot as plt
# uncertain parameter
n = 10
K = np.random.rand(n)+1.0
m = GEKKO()
m.time = np.linspace(0,20,41)
# manipulated variable
p = m.MV(value=0, lb=0, ub=100)
p.STATUS = 1
p.DCOST = 0.1
p.DMAX = 20
# controlled variable
v = m.Array(m.CV,n)
for i in range(n):
v[i].STATUS = 1
v[i].SP = 40
v[i].TAU = 5
m.Equation(10*v[i].dt() == -v[i] + K[i]*p)
# solve optimal control problem
m.options.IMODE = 6
m.options.CV_TYPE = 2
m.solve()
# plot results
plt.figure()
plt.subplot(2,1,1)
plt.plot(m.time,p.value,'b-',LineWidth=2)
plt.ylabel('MV')
plt.subplot(2,1,2)
plt.plot([0,m.time[-1]],[40,40],'k-',LineWidth=3)
for i in range(n):
plt.plot(m.time,v[i].value,':',LineWidth=2)
plt.ylabel('CV')
plt.xlabel('Time')
plt.show()

Gurobi - Solving Capacity Location Problem

I am trying to solve a Capacity location Problem from the OR database named: capa
OR files here
The answer should be:
capa 19240822.449 (capacity 8000)
But I found:
Best objective 3.145815023928e+08
For sure it is a problem on my code... Can someone help me?
Maybe I am doing something wrong on the Optmization formula ... or on the construction of my shipping demand. I really do not know, because it looks correct to me.
Any help would be appreciated... thanks in advance
from itertools import product
from math import sqrt
import gurobipy as gp
from gurobipy import GRB
import time
# Get Clients and Facilities
def getFacilities_Clients(file_list):
return int(file_list[0]), int(file_list[1])
# Get Facilities Fixed Costs
def getFacilities_STRCapacity_FixedCosts(file_list, num_facilities):
shift = 2
capacity = []
cost = []
#loop to get all i location costs
for i in range(0,num_facilities*2,2):
#capacity.append(file_list[i+shift])
capacity.append(8000)
cost.append(int(file_list[i+1+shift].replace(".","")))
return capacity, cost
# Get Demand and Allocation Costs for j(customer) to each i(client)
def getClient_Demand_AllocationCosts(file_list, num_facilities, num_customers):
shift = 2 + (num_facilities*2)
demand = []
allocation_cost = []
#loop to get all j Clients
j=0
for r in range(0,num_customers):
#get demand
demand.append(int(file_list[j+shift]))
#loop to get all i location costs
for i in range(0,num_facilities):
allocation_cost.append(float(file_list[j+1+i+shift]))
#fix j
j += num_facilities+1
return demand, allocation_cost
#Read File from OR datasets
fileName='datasets/ORcapa'
ORlist = []
with open(fileName, "r") as f:
ORlist = f.read().split()
##### Sets and Indices #####
num_facilities, num_customers = getFacilities_Clients(ORlist)
capacity, fixed_cost = getFacilities_STRCapacity_FixedCosts(ORlist, num_facilities)
cartesian_prod = list(product(range(num_customers), range(num_facilities)))
# shipping costs
demand, alloc_cost = getClient_Demand_AllocationCosts(ORlist, num_facilities, num_customers)
shipping_cost = dict(zip(cartesian_prod, alloc_cost))
shipping_demand={}
for k, v in shipping_cost.items():
shipping_demand[k] = v * demand[k[0]]
#setup cost
setup_cost = fixed_cost
#demand of customer
dc = demand
#max production
maxp= capacity
start = time.time()
# MIP model formulation
m = gp.Model('CFLP')
##### Decision Variable #####
x = m.addVars(num_facilities, vtype=GRB.BINARY, name='x')
y = m.addVars(cartesian_prod, ub=1, vtype=GRB.CONTINUOUS, name='y')
##### Constraints #####
m.addConstrs((y[(c,f)] <= x[f] for c,f in cartesian_prod), name='Shipping')
m.addConstrs((gp.quicksum(y[(c,f)] for f in range(num_facilities)) == 1 for c in range(num_customers)), name='Demand')
m.addConstrs((gp.quicksum(dc[c]*y[(c,f)] for c in range(num_customers)) <= maxp[f]*x[f] for f in range(num_facilities)), name='Capacity')
##### Objective Function #####
m.setObjective(x.prod(setup_cost)+y.prod(shipping_demand), GRB.MINIMIZE)
m.Params.Method = 1
# Options are:-1=automatic, 0=primal simplex, 1=dual simplex, 2=barrier, 3=concurrent, 4=deterministic concurrent, 5=deterministic concurrent simplex
m.optimize()
end = time.time()

that solved my issue:
m.setObjective(x.prod(setup_cost)+y.prod(shipping_cost), GRB.MINIMIZE)
I was computing a different value for the shipping.

Is it possible to loop to a certain value and carry on further calculations with this value?

I am new here and new in programming, so excuse me if the question is not formulated clearly enough.
For a uni assignment, my labpartner and I are programming a predator-prey system.
In this predator-prey system, there is a certain load factor 'W0'.
We want to find a load factor W0, accurate to 5 significant digits, for which applies that there will never be less than 250 predators (wnum[1] in our code). We want to find this value of W0 and we need the code to carry on further calculations with this found value of W0. Here is what we've tried so far, but python does not seem to give any response:
# Import important stuff and settings
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
print ('Results of Group 4')
def W0():
W0 = 2.0
while any(wnum[1])<250:
W0 = W0-0.0001
return W0
def W(t):
if 0 <= t < 3/12:
Wt = 0
elif 3/12 <= t <= 8/12:
Wt = W0
elif 8/12 < t < 1:
Wt = 0
else:
Wt = W(t - 1)
return Wt
# Define the right-hand-side function
def rhsf(t,y):
y1 = y[0]
y2 = y[1]
f1 = (2-2*10**-3*y2)*y1-W(t)*y1
f2 = (-3.92+7*10**-3*y1)*y2
return np.array([f1,f2])
# Define one step of the RK4 method
def RK4Step(tn,wn,Dt,f):
# tn = current time
# wn = known approximation at time tn
# Dt = the time step to use
# f = the right-hand-side function to use
# wnplus1 = the new approximation at time tn+Dt
k1 = Dt*f(tn,wn)
k2 = Dt*f(tn+0.5*Dt,wn+0.5*k1)
k3 = Dt*f(tn+0.5*Dt,wn+0.5*k2)
k4 = Dt*f(tn+Dt,wn+k3)
wnplus1 = wn + 1/6*(k1 +2*k2 +2*k3 +k4)
return wnplus1
# Define the complete RK4 method
def RK4Method(t0,tend,Dt,f,y0):
# t0 = initial time of simulation
# tend = final time of simulation
# Dt = the time step to use
# f = the right-hand-side function to use
# y0 = the initial values
# calculate the number of time steps to take
N = int(np.round((tend-t0)/Dt))
# make the list of times t which we want the solution
time = np.linspace(t0,tend,num=N+1)
# make sure Dt matches with the number of time steps
Dt = (tend-t0)/N
# Allocate memory for the approximations
# row i represents all values of variable i at all times
# column j represents all values of all variables at time t_j
w = np.zeros((y0.size,N+1))
# store the (given) initial value
w[:,0] = y0
# Perform all time steps
for n,tn in enumerate(time[:-1]):
w[:,n+1] = RK4Step(tn,w[:,n],Dt,f)
return time, w
# Set all known values and settings
t0 = 0.0
tend = 10.0
y0 = np.array([600.0,1000.0])
Dt = 0.5/(2**7)
# Execute the method
tnum, wnum = RK4Method(t0,tend,Dt,rhsf,y0)
# Make a nice table
alldata = np.concatenate(([tnum],wnum),axis=0).transpose()
table = pd.DataFrame(alldata,columns=['t','y1(t)','y2(t)'])
print('\nA nice table of the simulation:\n')
print(table)
# Make a nice picture
plt.close('all')
plt.figure()
plt.plot(tnum,wnum[0,:],label='$y_1$',marker='o',linestyle='-')
plt.plot(tnum,wnum[1,:],label='$y_2$',marker='o',linestyle='-')
plt.xlabel('$t$')
plt.ylabel('$y(t)$')
plt.title('Simulation')
plt.legend()
# Do an error computation
# Execute the method again with a doubled time step
tnum2, wnum2 = RK4Method(t0,tend,2.0*Dt,rhsf,y0)
# Calculate the global truncation errors at the last simulated time
errors = (wnum[:,-1] - wnum2[:,-1])/(2**4-1)
print('\nThe errors are ',errors[0],' for y1 and ',errors[1],' for y2 at time t=',tnum[-1])