I am currently trying to write a Python ROS program which can be executed as a ROS node (using rosrun) that implements the defs declared in a separate Python file arm.py (available at: https://github.com/nortega1/dvrk-ros/blob/44c8604b6c120e91f5357e7fd3649a8f7936c504/dvrk_python/src/dvrk/arm.py). The program initially examines the current cartesian position of the arm. Subsequently, when provided with a series of points that the arm must pass through, the program calculates a polynomial equation and given a range of x values the program evaluates the equation to find the corresponding y values.
Within the arm.py file there is a publisher set_position_cartesian_pub that sets the Cartesian position of the arm as follows:
self.__set_position_cartesian_pub = rospy.Publisher(self.__full_ros_namespace + '/set_position_cartesian', Pose, latch = True, queue_size = 1)
However, I am unsure how to pass the x and y values (I'll calculate the z values at a later date) to the publisher in the python program that I am creating. This is what I have written thus far:
#!/usr/bin/env python
import rospy
from tf import transformations
from tf_conversions import posemath
from std_msgs.msg import String, Bool, Float32, Empty, Float64MultiArray
from geometry_msgs.msg import Pose, PoseStamped, Vector3, Quaternion, Wrench, WrenchStamped, TwistStamped
def callback(data):
rospy.loginfo(data.pose)
def currentPositionListener():
rospy.init_node('currentPositionListener', anonymous=True)
rospy.Subscriber('/dvrk/PSM1/position_cartesian_current', PoseStamped, callback)
rospy.spin()
def lagrange(f, x):
total = 0
n = len(f)
for i in range(n):
xi, yi = f[i]
def g(i, n):
g_tot = 1
for j in range(n):
if i == j:
continue
xj, yj = f[j]
g_tot *= (x - xj) / float(xi - xj)
return g_tot
total += yi * g(i, n)
return total
def newPositionListener():
rospy.Subscriber('/dvrk/PSM1/set_position_cartesian', PoseStamped, trajectoryMover)
rospy.spin()
def trajectoryMover(data):
points =[(0,0),(45,30),(23,10), (48,0)]
xlist = [i for i in range(100)]
ylist = [lagrange(points, xlist[i]) for i in range(100)]
for x, y in zip(xlist, ylist):
data.pose.x = x
data.pose.y = y
data.pose.z = 0
if __name__ == '__main__':
currentPositionListener()
newPositionListener()
Any help would be greatly appreciated!
Problem is in .trajectoryMover():
If you want to publish a Pose message you should create a Pose message and fill in its positions like below code snippet:
from geometry_msgs.msg import Pose
def trajectoryMover(data):
pose = Pose()
self.__set_position_cartesian_pub = rospy.Publisher(
self.__full_ros_namespace + '/set_position_cartesian',
Pose, latch = True, queue_size = 1
)
points =[(0,0),(45,30),(23,10), (48,0)]
xlist = [i for i in range(100)]
ylist = [lagrange(points, xlist[i]) for i in range(100)]
for x, y in zip(xlist, ylist):
pose.x = x
pose.y = y
pose.z = 0
self.__set_position_cartesian_pub.publish(pose)
[NOTE]:
A .spin() is enough for your code.
I couldn't realize what is data you need for.
Related
I'm doing some intense computation and I'd like to speed up the process using all the computational power available (8 cores on my PC).
I'm doing some calculations on an electrical field and propagation using some functions. I'm calculating an entire XY slice (assuming propagation goes along Z) and retrieving the X and Y band (but I need to calculate the entire slice because of the fft2 implementation). And adding those bands to an array to create complete XZ and YZ slices.
So I have the function propagate that I want to execute in parallel and I thought about using a callback function that takes care of adding my band to the XZ and YZ arrays.
Since I don't know how bad it could be to pass an instance reference to the function that executes in parallel I preferred giving it all the parameters as simple as possible so I used pool.apply_async and iterated for all the values of z that I needed to propagate it to.
So the my class look like this :
class Calculator:
def __init__(self, s:Simulation):
# self.q = Queue()
# self.manager = Manager()
self.sim = s
def start(self, U:Field, z_min, z_max, nbr_pnt):
l = np.linspace(z_min, z_max, nbr_pnt)
self.tot = nbr_pnt
self.state = 0
self.XZ_slice = np.zeros((self.sim.Res(), nbr_pnt))
self.YZ_slice = np.zeros((self.sim.Res(), nbr_pnt))
field = U.field()
xx, yy = self.sim.xx_yy()
k = self.sim.k()
wavelength = self.sim.Wavelength()
rho_sqr = xx*xx+yy*yy
with Pool() as pool:
for i, z in enumerate(l):
pool.apply_async(self.propa_A, (i, z, field, rho_sqr, k, wavelength), callback=self.add_slice)
def propa_A(self, i, z, F, rho_sqr, k, wavelength):
prop_term = np.exp(1j*k/(2*z)*(rho_sqr))
return (i,np.abs(1/(wavelength**2*z**2)*fft.fft2(F*prop_term))**2)
def add_slice(self, result):
i, F = result
shape = F.shape
self.XZ_slice[:, i] = F[:, shape//2]
self.YZ_slice[:, i] = F[shape//2, :]
self.state += 1
def error(self, e):
print(e)
def get_XZslice(self):
return self.XZ_slice
def get_YZslice(self):
return self.YZ_slice
And in my main function, I made some kind of progress bar in the terminal to check on the progression
plt.figure()
print("[ ]0%", end='\r')
c = Calculator(w)
z_min = 0.2
z_max = 1
nbr_pnt = 10000
c.start(U_0, z_min, z_max, nbr_pnt)
counter = 0
while(counter < nbr_pnt):
counter = c.state
progression = counter/nbr_pnt
s = "[" + "="*int(progression*20) +">"+" "*int((1-progression)*20) + "]" + str(int(progression*100)) + "%"
print(s, end="\r")
sleep(1)
print("")
plt.subplot(211)
plt.imshow(c.get_XZslice())
plt.subplot(212)
plt.imshow(c.get_YZslice())
plt.show()
Assume U_0 represents an N*N (Res * Res) complex array
When I execute this script there is nothing that happens. No progression, no load on the CPU. Maybe apply_async is not the right choice. Do you guys have any idea on how to implement this better? It's the first time that I get into multiprocessing so excuse my mistakes.
Thanks in advance.
EDIT
As suggested I added a callback_error but it's never called
I want to write a program which turns a 2nd order differential equation into two ordinary differential equations but I don't know how I can do that in Python.
I am getting lots of errors, please help in writing the code correctly.
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
import numpy as np
N = 30 # Number of coupled oscillators.
alpha=0.25
A = 1.0
# Initial positions.
y[0] = 0 # Fix the left-hand side at zero.
y[N+1] = 0 # Fix the right-hand side at zero.
# The range(1,N+1) command only prints out [1,2,3, ... N].
for p in range(1, N+1): # p is particle number.
y[p] = A * np.sin(3 * p * np.pi /(N+1.0))
####################################################
# Initial velocities.
####################################################
v[0] = 0 # The left and right boundaries are
v[N+1] = 0 # clamped and don't move.
# This version sets them all the particle velocities to zero.
for p in range(1, N+1):
v[p] = 0
w0 = [v[p], y[p]]
def accel(t,w):
v[p], y[p] = w
global a
a[0] = 0.0
a[N+1] = 0.0
# This version loops explicitly over all the particles.
for p in range(1,N+1):
a[p] = [v[p], y(p+1)+y(p-1)-2*y(p)+ alpha * ((y[p+1] - y[p])**2 - (y[p] - y[p-1])**2)]
return a
duration = 50
t = np.linspace(0, duration, 800)
abserr = 1.0e-8
relerr = 1.0e-6
solution = solve_ivp(accel, [0, duration], w0, method='RK45', t_eval=t,
vectorized=False, dense_output=True, args=(), atol=abserr, rtol=relerr)
Most general-purpose solvers do not do structured state objects. They just work with a flat array as representation of the state space points. From the construction of the initial point you seem to favor the state space ordering
[ v[0], v[1], ... v[N+1], y[0], y[1], ..., y[N+1] ]
This allows to simply split both and to assemble the derivatives vector from the velocity and acceleration arrays.
Let's keep things simple and separate functionality in small functions
a = np.zeros(N+2)
def accel(y):
global a ## initialized to the correct length with zero, avoids repeated allocation
a[1:-1] = y[2:]+y[:-2]-2*y[1:-1] + alpha*((y[2:]-y[1:-1])**2-(y[1:-1]-y[:-2])**2)
return a
def derivs(t,w):
v,y = w[:N+2], w[N+2:]
return np.concatenate([accel(y), v])
or keeping the theme of avoiding allocations
dwdt = np.zeros(2*N+4)
def derivs(t,w):
global dwdt
v,y = w[:N+2], w[N+2:]
dwdt[:N+2] = accel(y)
dwdt[N+2:] = v
return dwdt
Now you only need to set
w0=np.concatenate([v,y])
to rapidly get to a more interesting class of errors.
Im trying to write a Python class that creates a matrix of zeros, then uses a random number generator to pick spots on the matrix. It changes the zero in that spot to a one, until the matrix is all ones. Can someone critique/correct my code? (I also want the generator to check its proximity on the matrix, and try 3 times to find a spot that is 2 spots away from any ones.)
import random
import numpy as np
#agents is amount of agents available to fill grid
class Gridmodel():
def __init__(self, gridsize, agents):
self.gridsize = gridsize
self.agents = agents
self.gridmodel = np.zeros([self.gridsize, self.gridsize],dtype=int)
def foundspot(self):
foundspot = False
tries = 0
while foundspot == False and tries <= 3:
x = random.randint(0, self.gridsize)
y = random.randint(0, self.gridsize)
if self.gridmodel[x][y] < 0:
foundspot = True
else:
tries += 1
def goodspot(self, x, y):
goodspot = self.gridmodel[x][y]
for i in range(-1,2):
for j in range(-1,2):
print i, j, self.gridmodel[i][j]
I have a function calcField which, when given a numpy array with two elements representing a position, returns an array representing the electric field at that position. What is the most pythonic way to ask matplotlib to draw a vector field for this function? Currently I have this code working, but it feels against the spirit of numpy and is relatively unreadable.
Y, X = np.mgrid[-3:3:100j, -3:3:100j]
vectors = np.array([[field.calcField(r) for r in row]
for row in [zip(a, b) for a, b in zip(X, Y)]])
U = np.array([[vector[0] for vector in row] for row in vectors])
V = np.array([[vector[1] for vector in row] for row in vectors])
plt.streamplot(X, Y, U, V, color=U, linewidth=2, cmap=plt.cm.autumn)
Edit: As requested, the code of calcField:
import constants
import numpy as np
import numpy.linalg as l
class Field:
def __init__(self, charges = []):
self.charges = charges
def addCharge(self, charge):
self.charges = self.charges + [charge]
def calcField(self, point):
point = np.array(point)
return sum([charge.calcField(point) for charge in self.charges])
class PointCharge:
def __init__(self, q, position):
self.q = q
self.position = np.array(position)
def calcField(self, point):
return constants.k * self.q * (point - self.position) / l.norm (point - self.position)**3
A vectorized form of your code that plots the electric field of a set of point charges using streamlines might look like this:
num_charges = 4
charges = np.random.random_integers(-5,5,num_charges)
charges[charges==0] = 5
charges_positions = np.random.random((num_charges, 2))
y,x = np.mgrid[0:1:40j, 0:1:40j]
xdist = x - charges_positions[:,0].reshape(-1,1,1)
ydist = y - charges_positions[:,1].reshape(-1,1,1)
denom = ((xdist**2 + ydist**2)**1.5)
# Ignoring Coulomb's constant here...
Ex = (charges.reshape(-1,1,1) * xdist / denom).sum(axis=0)
Ey = (charges.reshape(-1,1,1) * ydist / denom).sum(axis=0)
Which I find more comprehensible than this alternative, which you might find more readable (and that was your question):
num_charges = 4
charges = np.random.random_integers(-5,5,(num_charges,1,1))
charges[charges==0] = 5 # only for clarity
positions = np.random.random((2, num_charges,1,1))
y,x = np.mgrid[0:1:40j, 0:1:40j]
M,N = y.shape
xy = np.array([x,y]).reshape(2,1, M,N)
rad_dist = xy - positions
denom = np.linalg.norm(rad_dist, axis=(0))**3
elec_fields = charges * rad_dist / denom
Ex, Ey = elec_fields.sum(axis=1)
You could plot either easily. I'll continue with the format from the last code block (you'll need to swap a few indices if you're using the first form):
pos_charges = charges > 0
neg_charges = charges < 0
f,(ax,ax1) = plt.subplots(1,2)
ax.plot(positions[0, pos_charges], positions[1, pos_charges], 'ro ')
ax.plot(positions[0, neg_charges], positions[1, neg_charges], 'bo ')
ax.streamplot(x,y, Ex, Ey, color='k')
ax.set_aspect('equal', adjustable='box')
ax.set_title('Electric field')
ax.set_xticks([])
ax.set_yticks([])
However, at this point I'm not using classes anymore. Sometimes it's worth it to enjoy easier access to vectorization. In the 2nd form of the code, you basically have the result of each PointCharge.calcField() in the 2nd axis of elec_fields, the first being merely the x and y components of these fields.
I have currently made quite a large code in python and when I run it, it takes about 3 minutes for it to make the full calculation. Eventually, I want to increase my N to about 400 and change my m in the for loop to an even larger number - This would probably take hours to calculate which I want to cut down.
It's steps 1-6 that take a long time.
When attempting to run this with cython (I.E. importing pyximport then importing my file)
I get the following error FDC.pyx:49:19: 'range' not a valid cython language construct and
FDC.pyx:49:19: 'range' not a valid cython attribute or is being used incorrectly
from physics import *
from operator import add, sub
import pylab
################ PRODUCING CHARGES AT RANDOM IN r #############
N=11 #Number of point charges
x = zeros(N,float) #grid
y = zeros(N,float)
i=0
while i < N: #code to produce values of x and y within r
x[i] = random.uniform(0,1)
y[i] = random.uniform(0,1)
if x[i] ** 2 + y[i] ** 2 <= 1:
i+=1
print x, y
def r(x,y): #distance between particles
return sqrt(x**2 + y**2)
o = 0; k = 0; W=0 #sum of energy for initial charges
for o in range(0, N):
for k in range(0, N):
if o==k:
continue
xdist=x[o]-x[k]
ydist=y[o]-y[k]
W+= 0.5/(r(xdist,ydist))
print "Initial Energy:", W
##################### STEPS 1-6 ######################
d=0.01 #fixed change in length
charge=(x,y)
l=0; m=0; n=0
prevsW = 0.
T=100
for q in range(0,100):
T=0.9*T
for m in range(0, 4000): #steps 1 - 6 in notes looped over
xRef = random.randint(0,1) #Choosing x or y
yRef = random.randint(0,N-1) #choosing the element of xRef
j = charge[xRef][yRef] #Chooses specific axis of a charge and stores it as 'j'
prevops = None #assigns prevops as having no variable
while True: #code to randomly change charge positions and ensure they do not leave the disc
ops =(add, sub); op=random.choice(ops)
tempJ = op(j, d)
#print xRef, yRef, n, tempJ
charge[xRef][yRef] = tempJ
ret = r(charge[0][yRef],charge[1][yRef])
if ret<=1.0:
j=tempJ
#print "working", n
break
elif prevops != ops and prevops != None: #!= is 'not equal to' so that if both addition and subtraction operations dont work the code breaks
break
prevops = ops #####
o = 0; k = 0; sW=0 #New energy with altered x coordinate
for o in range(0, N):
for k in range(0, N):
if o==k:
continue
xdist = x[o] - x[k]
ydist = y[o] - y[k]
sW+=0.5/(r( xdist , ydist ))
difference = sW - prevsW
prevsW = sW
#Conditions:
p=0
if difference < 0: #accept change
charge[xRef][yRef] = j
#print 'step 5'
randomnum = random.uniform(0,1) #r
if difference > 0: #acceptance with a probability
p = exp( -difference / T )
#print 'step 6', p
if randomnum >= p:
charge[xRef][yRef] = op(tempJ, -d) #revert coordinate to original if r>p
#print charge[xRef][yRef], 'r>p'
#print m, charge, difference
o = 0; k = 0; DW=0 #sum of energy for initial charges
for o in range(0, N):
for k in range(0, N):
if o==k:
continue
xdist=x[o]-x[k]
ydist=y[o]-y[k]
DW+= 0.5/(r(xdist,ydist))
print charge
print 'Final Energy:', DW
################### plotting circle ###################
# use radians instead of degrees
list_radians = [0]
for i in range(0,360):
float_div = 180.0/(i+1)
list_radians.append(pi/float_div)
# list of coordinates for each point
list_x2_axis = []
list_y2_axis = []
# calculate coordinates
# and append to above list
for a in list_radians:
list_x2_axis.append(cos(a))
list_y2_axis.append(sin(a))
# plot the coordinates
pylab.plot(list_x2_axis,list_y2_axis,c='r')
########################################################
pylab.title('Distribution of Charges on a Disc')
pylab.scatter(x,y)
pylab.show()
What is taking time seems to be this:
for q in range(0,100):
...
for m in range(0, 4000): #steps 1 - 6 in notes looped over
while True: #code to randomly change charge positions and ensure they do not leave the disc
....
for o in range(0, N): # <----- N will be brought up to 400
for k in range(0, N):
....
....
....
....
100 x 4000 x (while loop) + 100 x 4000 x 400 x 400 = [400,000 x while loop] + [64,000,000,000]
Before looking into a faster language, maybe there is a better way to build your simulation?
Other than that, you will likely have immediate performance gains if you:
- shift to numpy arrays i/o python lists.
- Use xrange i/o range
[edit to try to answer question in the comments]:
import numpy as np, random
N=11 #Number of point charges
x = np.random.uniform(0,1,N)
y = np.random.uniform(0,1,N)
z = np.zeros(N)
z = np.sqrt(x**2 + y**2) # <--- this could maybe replace r(x,y) (called quite often in your code)
print x, y, z
You also could look into all the variables that are assigned or recalculated many many times inside your main loop (the one described above), and pull all that outside the loop(s) so it is not repeatedly assigned or recalculated.
for instance,
ops =(add, sub); op=random.choice(ops)
maybe could be replaced by
ops = random.choice(add, sub)
Lastly, and here I am out on a limb because I've never used it myself, but it might be a little bit simpler for you to use a package like Numba or Jit as opposed to cython; they allow you to decorate a critical part of your code and have it precompiled prior to execution, with none or very minor changes.