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 !
Related
I am attempting to make an animation of the motion of the piano string
using the facilities provided by the vpython package. There are
various ways you could do this, but my goal is to do this with using
the curve object within the vpython package. Below is my code for
solution of the initial problem of solving the complete sets of
simultaneous 1st-order equation. Thanks in advance, I am really
uncertain as to where to start with the vpython animation.
# Key Module and Function Import(s):
import numpy as np
import math as m
import pylab as py
import matplotlib
from time import time
import scipy
# Variable(s) and Constant(s):
L = 1.0 # Length on string in m
C = 1.0 # velocity of the hammer strike in ms^-1
d = 0.1 # Hammer distance from 0 to point of impact with string
N = 100 # Number of divisions in grid
sigma = 0.3 # sigma value in meters
a = L/N # Grid spacing
v = 100.0 # Initial velocity of wave on the string
h = 1e-6 # Time-step
epsilon = h/1000
# Computation(s):
def initialpsi(x):
return (C*x*(L-x)/(L**2))*m.exp((-(x-d)**2)/(2*sigma**2)) # Definition of the function
phibeg = 0.0 # Beginning - fixed point
phimiddle = 0.0 # Initial x
phiend = 0.0 # End fixed point
psibeg = 0.0 # Initial v at beg
psiend = 0.0 # Initial v at end
t2 = 2e-3 # string at 2ms
t50 = 50e-3 # string at 50ms
t100 = 100e-3 # string at 100ms
tend = t100 + epsilon
# Creation of empty array(s)
phi = np.empty(N+1,float)
phi[0] = phibeg
phi[N] = phiend
phi[1:N] = phimiddle
phip = np.empty(N+1,float)
phip[0] = phibeg
phip[N] = phiend
psi = np.empty(N+1,float)
psi[0] = psibeg
psi[N] = psiend
for i in range(1,N):
psi[i] = initialpsi(i*a)
psip = np.empty(N+1,float)
psip[0] = psibeg
psip[N] = psiend
# Main loop
t = 0.0
D = h*v**2 / (a*a)
timestart = time()
while t<tend:
# Calculation the new values of T
for i in range(1,N):
phip[i] = phi[i] + h*psi[i]
psip[i] = psi[i] + D*(phi[i+1]+phi[i-1]-2*phi[i])
phip[1:N] = phi[1:N] + h*psi[1:N]
psip[1:N] = psi[1:N] + D*(phi[0:N-1] + phi[2:N+1] -2*phi[1:N])
phi= np.copy(phip)
psi= np.copy(psip)
#phi,phip = phip,phi
#psi,psip = psip,psi
t += h
# Plot creation in step(s)
if abs(t-t2)<epsilon:
t2array = np.copy(phi)
py.plot(phi, label = "2 ms")
if abs(t-t50)<epsilon:
t50array = np.copy(phi)
py.plot(phi, label = "50 ms")
if abs(t-t100)<epsilon:
t100array = np.copy(phi)
py.plot(phi, label = "100 ms")
See the curve documentation at
https://www.glowscript.org/docs/VPythonDocs/curve.html
Use the "modify" method to change the individual points along the curve object, inside a loop that contains a rate statement:
https://www.glowscript.org/docs/VPythonDocs/rate.html
I'm working on a large scale MILP. So I have to set the time limit to a reasonable value or I have to set the MIPGap to a reasonable level. I already know the documentation from gurobi.
MIPGap: https://www.gurobi.com/documentation/6.5/refman/mipgap.html
TimeLimit: https://www.gurobi.com/documentation/8.0/refman/timelimit.html#parameter:TimeLimit
MIPGap Gurobi will stop when it finds a solution within a percentage of optimal
TimeLimit Gurobi will stop after a certain amount of time.
But can you send me an example with setting for example the time limit to 5 minutes
or setting the MIPGap to 5 % ?
I don't know how to exactly implement those character?
Please help me I am quite new to python
I tried this but this doesn't work
model.Params.TimeLimit = 5
model.setParam("MIPGap", mipgap)
Here is a short version of my model
from gurobipy import *
import csv
import geopandas as gpd
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
from pandas.core.common import flatten
import math
################################# SOLVE function START ###################################################################
def solve(
vpmaint, wpunit, wuunit, vumaint,
kfuel, koil, kbio,
hb, ht,
cj, ci,
zinvestp, zinvestu,
DEMAND, DEMANDM,
LOCATION, SOURCE, BTYPE, SOURCEM,
osi, oij, ojm
):
model = Model("Biomass to liquid supply chain network design")
################################# SOLVE function END ###################################################################
####################################################### variable section START ####################################################################################################
#binary variables ############################# Binary 1-2 ####################################################
#binary 1: Pyrolyse i with capacity p open?
fpopen = {}
for i in LOCATION:
for p in R:
fpopen[i,p] = model.addVar(vtype = GRB.BINARY,name = "fpopen_%s_%s" % (i,p))
#binary 2: Upgrading j with capacity r and technology t open?
fuopen = {}
for j in LOCATION:
for r in R:
for t in TECHNOLOGY:
fuopen[j,r,t] = model.addVar(vtype = GRB.BINARY,name = "fuopen_%s_%s_%s" % (j,r,t))
################################################ continous variables Integer 1-9 #############################################################
#integer 1: Mass of Biomass type b from Source s to Pyrolyse i
xsi = {}
for s in SOURCE:
for i in LOCATION:
for b in BTYPE:
xsi[s,i,b] = model.addVar(vtype = GRB.INTEGER,name = "xsi_%s_%s_%s" % (s,i,b))
#integer 2:Mass of Biomass type b from Source s to Pyrolyse i
xjm = {}
for j in LOCATION:
for m in DEMAND:
xjm[j,m] = model.addVar(vtype = GRB.INTEGER,name = "xjm_%s_%s" % (j,m))
model.update()
model.modelSense = GRB.MAXIMIZE
####################################################### Objective Function START
model.setObjective(
#quicksum(DEMANDM[m] * l for m in DEMANDM )
quicksum(xjm[j,m] * l for j in LOCATION for m in DEMAND)
- quicksum(ainvest[i] + aoperation[i] + aprod[i] for i in LOCATION)
- quicksum(einvest[j] + eoperation[j] + eprod[j] for j in LOCATION)
## ......
####################################################### Constraints
############################## Satisfy Demand Constraint 1-3
# Constraint 1: Always Satisfy Demand at marketplace m
for m in DEMAND:
model.addConstr(quicksum(xjm[j,m] for j in LOCATION) <= int(DEMANDM[m]))
# for m in DEMAND:
# model.addConstr(quicksum(x[j,m] for j in LOCATION) >= DEMANDM[m])
# Constraint 2: The amount of bio-oil sent from pyrolyse station i to Upgrading
###...Here are more constraints
model.optimize()
model.getVars()
model.MIPGap = 5
model.Params.TimeLimit = 1.0
model.setParam("MIPGap", mipgap)
Alternatively, you could call the setParam() method of the model:
model.setParam('MIPGap', 0.05)
model.setParam('Timelimit', 300)
You need to set the parameters before you call Model.optimize(). Also, the units for MIPGap and TimeLimit are fractions and seconds, respectively. So your code should be:
model.Params.MIPGap = 0.05 # 5%
model.Params.TimeLimit = 300 # 5 minutes
model.optimize()
I haven't run this code in a couple years... I don't have a Gurobi license anymore. But, something like this should work. You didn't mention how you encoded your model. The below is from a pyomo script. I would think something similar would work however you get a handle on the solver instance.
solver = SolverFactory("gurobi")
solver.options['timeLimit'] = 1200 # seconds
solver.options['mipgap'] = 0.01
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])
I am trying to propagate a gaussian wave packet using the crank nicolson method in imaginary time (multiply the time step by the unit imaginary). The code that I have written in attempt to achieve this is shown here:
import matplotlib.pyplot as plt #this allows you to plot, and changes the name to plt
import numpy as np #this allows you to do math, and changes the name to np
import math
import scipy.linalg as la
def V(x):
# k = 1
# v = k*x**4
v = 0.25*(x-3)**2+0.15*(x-3)**4
return v
def Psi(x):
psi = np.exp(-2*(x-3)**2)
return psi
#Function for computing integral using trapezoid method
def TrapInt(y, h):
trap = [(float(y[ii]) + float(y[ii+1])) for ii in range(0, len(y)-1)]
return float(h)/2*sum(trap)
N = 1000
L = 3;
h = 0.01
x = np.arange(0,6,h);
t = np.linspace(0,L,300);
t = 1j*t;
dt = t[1] - t[0]
dx = x[1] - x[0]
A = 1j*dt/(2*dx**2)
pot = V(x)
Q = np.zeros([len(x),len(x)],dtype = complex)
P = np.zeros([len(x),len(x)],dtype = complex)
wave = np.zeros([len(x),len(t)],dtype = complex)
wave[:,0] = Psi(x)
B = (1- 2*A - 1j*dt*pot)
for ii in range(0,len(x)-1):
Q[ii][ii] = -(B[ii])
P[ii][ii] = (B[ii])
Q[ii][ii+1] = (2-A)
P[ii][ii+1] = A
if ii >= 1:
Q[ii][ii-1] = -A
P[ii][ii-1] = A
plt.plot(wave[:,0])
for ii in range(0,len(t)-1):
one = np.matmul(P,wave[:,ii])
wave[:,ii+1] = np.matmul(la.inv(Q),one)
I can't seem to find any mathematical errors in my implementation of the crank nicolson method; however, whenever I try to run this it gives an error saying that Q is singular (has no inverse). I'm not sure why this is occurring. Any help is appreciated. Thanks
You never assign to Q[-1]. Zero rows have been known to produce singular matrices in some cases.
Also, don’t repeatedly invert the matrix. Probably don’t invert it at all, but rather store some decomposition of it to allow efficient calculation of Q-1x.
I'm plotting in a simple 2D plane the path taken by a body in motion past another gravitationally attractive body.
Every loop, the time is incremented by a second and the body's new position is calculated and printed out.
I then paste the results into a spreadsheet and graph it.
Things look ok until the body's x component becomes negative - and then the trajectory goes linear, and scoots off to the top left.
This is all for my own solo entertainment, I'm no student. So after scratching my head for a bit I've finally lumped for asking someone for help.
I've probably missed something obvious. I suspect my trigonometry is lacking something.
I'm using Python 2.7.10
import sys
import os
import math
mu = 4.0*(10**14)
massAst = 1
earthRadius = 6371000.
alt = 100000.
r = earthRadius+ alt
rTheta = 270.
rAngtoX = math.radians(rTheta)
tInc = 1 ## increment time by 1 seconds - one update of pos&velocity per second of travel
calcTime = 1100 ## simulation runtime (86400 seconds = 1 day) 10 mins
t = 1 ## integral of time t to use in the calcs in the loop.
printbell = 120 ## print results now
printclock = 0
hourCount = 0
## Initialise velocity vectors for Asteroid:
uAstX = 1500.
uAstY = 0.
vAstX = 0.
vAstY = 0.
## Displacement
dAstX = r*math.cos(rAngtoX)
dAstY = r*math.sin(rAngtoX)
for i in range(0, calcTime):
acc = -1*(mu/r**2)
accX = acc*math.cos(rAngtoX)
accY = acc*math.sin(rAngtoX)
vAstX = uAstX + accX*t ## new value for velocity in X direction
vAstY = uAstY + accY*t ## and in Y
deltaDAstX = uAstX*t + 0.5*accX*(t**2) ## change in position over this time interval
deltaDAstY = uAstY*t + 0.5*accY*(t**2)
dAstX = dAstX + deltaDAstX
dAstY = dAstY + deltaDAstY
uAstX = vAstX
uAstY = vAstY
## Now calculate new angle and range
## tan(theta) = dAstY/dAstX, so:
rAngtoX = math.atan(dAstY/dAstX) ##+(2*3.141592654)
##print 'theta:', math.degrees(rAngtoX)
r = dAstY/math.sin(rAngtoX)
## if i == print
print dAstX, ' ', dAstY
As dAstX approaches zero, dAstY/dAstX will approach a division-by-zero... Which will cause all sorts of problems (roundoff issues at the very least).
I'd recommend keeping the x/y components of distance/velocity/acceleration separate. The distance between the objects is important, of course, but that can be calculated using r=sqrt(dAstX**2 + dAstY**2).
I'm not familiar with the math, but I took your code, modified it to run on my machine, plottet the data using the seaborn library and I came up with this:
This is the code I used:
import math
import pandas as pd
import seaborn as sns
import matplotlib.pyplot as plt
def calc(calcTime):
mu = 4.0*(10**14)
earthRadius = 6371000.0
alt = 100000.0
r = earthRadius+ alt
rTheta = 270.0
rAngtoX = math.radians(rTheta)
t = 1 # integral of time t to use in the calcs in the loop.
# Initialise velocity vectors for Asteroid:
uAstX = 1500.0
uAstY = 0.0
# Displacement
dAstX = r*math.cos(rAngtoX)
dAstY = r*math.sin(rAngtoX)
point_list = []
for i in range(0, calcTime):
acc = -1*(mu/r**2)
accX = acc*math.cos(rAngtoX)
accY = acc*math.sin(rAngtoX)
vAstX = uAstX + accX*t # new value for velocity in X direction
vAstY = uAstY + accY*t # and in Y
deltaDAstX = uAstX*t + 0.5*accX*(t**2) # change in pos over time interval
deltaDAstY = uAstY*t + 0.5*accY*(t**2)
dAstX = dAstX + deltaDAstX
dAstY = dAstY + deltaDAstY
uAstX = vAstX
uAstY = vAstY
# Now calculate new angle and range
# tan(theta) = dAstY/dAstX, so:
rAngtoX = math.atan(dAstY/dAstX) #+(2*3.141592654)
# print 'theta:', math.degrees(rAngtoX)
r = dAstY/math.sin(rAngtoX)
# if i == print
if i % 5 == 0:
print('{:05d} | {:15.2f} | {:15.2f}'.format(i, dAstX, dAstY))
point_list.append((i, dAstX, dAstY))
df = pd.DataFrame(data=point_list, columns=['i', 'x', 'y'])
return df
if __name__ == '__main__':
df = calc(950)
sns.scatterplot(data=df, x='x', y='y')
plt.show()
My analysis: the spaces between dots gets bigger each time (note: I only plottet every 5th point to make the image more readabl,e in my opinion). From a physics perspective, that would indicate that the object is gaining speed.
Isn't it possible that your calculations are correct and that the object is leaving the orbit because it gained enough speed (aka energy) to leave the gravitational field of the center mass (aka the earth) ?
As I said, I'm not familiar with the specific math, but to me it makes sense that the object could break free from the orbit with the speed it is gaining in the half turn.