I need to transfer some Matlab code into P. I got stuck at a numpy.arange I use to set points continuously on the arc of circle with a given angle (in radians).
I got this far (example for points on x-axis):
def sensor_data_arc_x():
theta = np.arange(0, angle/2, 2*np.pi/360)
return np.multiply(radius, np.cos(np.transpose(theta)))
I know numpy.arange does not include the endpoint, although the Matlab equivalent does; the array is always one element short, which is messing up my further computations.
Is there an way to include the endpoint?
I recommend that you work through a tutorial on for loops -- the information you need is there, as well as other hints on using controlled iteration. To solve your immediate need, simply increase your upper bound by one loop increment:
inc = 2*np.pi/360
theta = np.arange(0, angle/2 + inc, inc)
Related
I'm attempting to fit a 2D point-cloud (x and y coordinates). So far, I've had limited success with scipy's interpolation packages, most notable UnivariateSpline, which produced the following (sub-optimal) fit (please ignore the colors):
However, this is obviously the wrong package, since I need a final curve that can bend back in on itself (so no longer a 1d function) near the edges of my parabolic point-cloud.
I then read up on interp2d, for example, but don't understand what my z array would be. Is there a better class of packages that I'm perhaps overlooking?
Update 1: as suggested in the comments, I've redone this using scipy.interpolate.splprep; my general setup is:
from scipy.interpolate import splprep, splev
pts = np.vstack((X.ravel(), Y.ravel)) #X and Y contain my points
(tck, u), fp, ier, msg = splprep(pts, u=None, per=0, k=3, full_output=True) #s = optional parameter (default used here)
print('Spline score:',fp) #goodness of fit flatlines after a given s value (and higher), which captures the default s-value as well
x_new, y_new = splec(u_new, tck, der=0)
plt.plot(x_new, y_new, 'k')
plt.show()
The plot is below. Can anyone suggest a method for automating the decision of s... possible a loop while assessing coefficient of determination of each s plot? Or is there something baked in?
Update 2:
I've since re-run this on two different point-clouds, and found that the ordering of the points significantly alters the outcome. When I re-order the points in the point-cloud to be along an initial fit to a parabola, I get much better results with my spline. As well, the results are still sub-optimal, as can be seen below.
Are there any further adjustments I can make with this method? Alternatively, does anyone have a suggestion for a competitive approach I can investigate?
Update 3: actually, setting knots = 5 helps out tremendously:
I faced a similar issue, and the solution to my problem was to use Principal Curves (https://hastie.su.domains/Papers/Principal_Curves.pdf).
This implementation available on GitHub could help you: https://github.com/zsteve/pcurvepy.
I used sympy to derive, via lagrange, the equations of motion of my 3 link robot. The resultant equation of motion in the form (theta_dot_dot = f(theta, theta_dot)) turned out very complicated with A LOT of cos and sin. I then lambdified the functions to use with drake, replacing all the sympy.sin and sympy.cos with drake.sin, drake.cos.
The final function can be evaluated numerically (i.e. given theta, theta_dot, find theta_dot_dot) somewhat efficiently in the milliseconds range.
I then tried to use direct transcription to do trajectory optimization. Note I did not use the DirectTranscription library, instead manually added the necessary constraints.
The constraints are added roughly as follows:
for i in range(NUM_TIME_STEPS-1):
print("Adding constraints for t = " + str(i))
tau = mp.NewContinuousVariables(3, "tau_%d" % i)
next_state = mp.NewContinuousVariables(8, "state_%d" % (i+1))
for j in range(8):
mp.AddConstraint(next_state[j] <= (state_over_time[i] + TIME_INTERVAL*derivs(state_over_time[i], tau))[j])
mp.AddConstraint(next_state[j] >= (state_over_time[i] + TIME_INTERVAL*derivs(state_over_time[i], tau))[j])
state_over_time[i+1] = next_state
tau_over_time[i] = tau
The problem I'm facing right now is that on each iteration of adding constraints, I observe that my memory usage increases by around 70-100MB. This means that my number of time steps cannot go more than around 50 before the program crashes due to out of memory.
I'm wondering what I can do to make trajectory optimization work for my robot. Obviously I can try to simplify (by hand or otherwise) the equations of motions... but is there anything else I can try? Is it even normal that the constraints are taking up so much memory? Am I doing something very wrong here?
You're pushing drake's symbolic through your complex equations. Making that better is a good goal, but probably you want to avoid it by using the other overload for AddConstraint:
AddConstraint(your_method, lb, ub, vars)
https://drake.mit.edu/pydrake/pydrake.solvers.mathematicalprogram.html?highlight=addconstraint#pydrake.solvers.mathematicalprogram.MathematicalProgram.AddConstraint
That will use your python code as a function pointer, and should use autodiff instead of symbolic.
The smooth.spline function in R allows a tradeoff between roughness (as defined by the integrated square of the second derivative) and fitting the points (as defined by summing the squares of the residuals). This tradeoff is accomplished by the spar or df parameter. At one extreme you get the least squares line, and the other you get a very wiggly curve which intersects all of the data points (or the mean if you have duplicated x values with different y values)
I have looked at scipy.interpolate.UnivariateSpline and other spline variants in Python, however, they seem to only tradeoff by increasing the number of knots, and setting a threshold (called s) for the allowed SS residuals. By contrast, the smooth.spline in R allows having knots at all the x values, without necessarily having a wiggly curve that hits all the points -- the penalty comes from the second derivative.
Does Python have a spline fitting mechanism that behaves in this way? Allowing all knots but penalizing the second derivative?
You can use R functions in Python with rpy2:
import rpy2.robjects as robjects
r_y = robjects.FloatVector(y_train)
r_x = robjects.FloatVector(x_train)
r_smooth_spline = robjects.r['smooth.spline'] #extract R function# run smoothing function
spline1 = r_smooth_spline(x=r_x, y=r_y, spar=0.7)
ySpline=np.array(robjects.r['predict'](spline1,robjects.FloatVector(x_smooth)).rx2('y'))
plt.plot(x_smooth,ySpline)
If you want to directly set lambda: spline1 = r_smooth_spline(x=r_x, y=r_y, lambda=42) doesn't work, because lambda has already another meaning in Python, but there is a solution: How to use the lambda argument of smooth.spline in RPy WITHOUT Python interprating it as lambda.
To get the code running you first need to define the data x_train and y_train and you can define x_smooth=np.array(np.linspace(-3,5,1920)). if you want to plot it between -3 and 5 in Full-HD-resolution.
Note that this code is not fully compatible with Jupyter-notebooks for the latest versions of rpy2. You can fix this by using !pip install -Iv rpy2==3.4.2 as described in NotImplementedError: Conversion 'rpy2py' not defined for objects of type '<class 'rpy2.rinterface.SexpClosure'>' only after I run the code twice
I've been looking for exactly the same thing, but would rather not have to translate the code to Python. The Splinter package seems like an option, however: https://github.com/bgrimstad/splinter
From research on google, I concluded that
By contrast, the smooth.spline in R allows having knots at all the x values, without necessarily having a wiggly curve that hits all the points -- the penalty comes from the second derivative.
I'm working on some python code to interpolate irregular data onto a 180° lat x 360° lon spherical grid. The code is currently hanging when I call the following:
def geo_interp(lats,lons,data,grid_size_deg):
deg2rad = pi/180.
new_lats = np.linspace(grid_size_deg, 180, 180) * deg2rad
new_lons = np.linspace(grid_size_deg, 360, 360) * deg2rad
knotst, knotsp = new_lats.copy(), new_lons.copy()
knotst[0] += .0001
knotst[-1] -= .0001
knotsp[0] += .0001
knotsp[-1] -= .0001
lut = LSQSphereBivariateSpline(lats.ravel(),lons.ravel(),data.T.ravel(),knotst,knotsp)
data_interp = lut(new_lats, new_lons)
return data_interp
The arrays I'm using as arguments when I call the above subroutine all fit the requirements of LSQSphereBivariateSpline as listed in the documentation. When I run it, it takes much longer than I feel like it should take to process a 180x360 dataset.
When I run the script with python -m trace --trace, the last line of output before nothing happens for a long time is
fitpack2.py(1025): w=w, eps=eps)
As far as I can tell, line 1025 of fitpack2.py is in a comment, which is even more confusing.
So my questions are:
1. Is there a way to tell if it's hanging or just very slow?
2. If it's hanging, how might I fix it?
The only thing I can think of is that I have no idea what I'm doing as far as choosing knots. Is there a good way to choose those? I just went with the grid I'll be interpolating to later, since the example in the doc seemed to be an arbitrary grid.
UPDATE: It finally finished after about 3 hours but the "interpolated data" looked like random noise. Also, if this is relevant, as far as I can tell LSQSphereBivariateSpline is the only function I can use for this because my lats and lons are not strictly increasing.
Also, I should add that when it finished it output the following warning: Warning (from warnings module):
File "/System/Library/Frameworks/Python.framework/Versions/2.7/Extras/lib/python/scipy/interpolate/fitpack2.py", line 1029
warnings.warn(message)
UserWarning:
WARNING. The coefficients of the spline returned have been computed as the
minimal norm least-squares solution of a (numerically) rank
deficient system (deficiency=16336, rank=48650). Especially if the rank
deficiency, which is computed by 6+(nt-8)*(np-7)+ier, is large,
the results may be inaccurate. They could also seriously depend on
the value of eps.
SOLUTION: I had far too many knots, causing both the glacial pace and the useless results.
I am working on translating a model from MATLAB to Python. The crux of the model lies in MATLAB's ode15s. In the MATLAB execution, the ode15s has standard options:
options = odeset()
[t P] = ode15s(#MODELfun, tspan, y0, options, params)
For reference, y0 is a vector (of size 98) as is MODELfun.
My Python attempt at an equivalent is as follows:
ode15s = scipy.integrate.ode(Model.fun)
ode15s.set_integrator('vode', method = 'bdf', order = 15)
ode15s.set_initial_value(y0).set_f_params(params)
dt = 1
while ode15s.successful() and ode15s.t < duration:
ode15s.integrate(ode15s.t+dt)
This though, does not seem to be working. Any suggestions, or an alternative?
Edit:
After looking at the output, the result I'm getting from the Python is either no change in some elements of y0 over time, or a constant change at each step for the rest of the y0. Any experience with something like this?
According to the SciPy wiki for Matlab Users, the right way for using the ode15s is
scipy.integrate.ode(f).set_integrator('vode', method='bdf', order=15)
One point to make clear is that, unlike Matlab's ode15s, the scipy integrator 'vode' does not support models with a mass matrix. So any recommendation should include this caveat.