# The Problem: Path optimization

The exact origin of this problem is somewhat blurry but since I already have the solution I won’t put much effort into clarifying it.

We have a particle that moves along a spatial curve, starting from point *r _{1}* with a defined velocity of

*v*and ending its path at

_{1}*r*with also well-defined velocity

_{2}*v*. Starting time is 0 and ending time is

_{2}*t*. The mission is to find the optimal trajectory given the constraints.

_{2}## Another Problem

But what does *optimal trajectory* even mean? Well, in my case, I decided to go for the least amount of force that will act on the particle to make it move as required. In this case that means the least acceleration but since acceleration is a vector I took its magnitude. So we’re searching for a path where magnitudes of acceleration vectors are as little as possible.

### Another Another Problem

One has to be very careful when choosing what *as little as possible means*. You need a criterion that describes all accelerations with a single number on which you can base optimization. And of course a simple *average* won’t do. It will likely find many points with zero acceleration and a single huge outlier, but the average will still be very low. So I decided to go with variance or standard deviation (both give the same results). Or, one could also sum n-th powers of each number and then make a n-th root of the sum – then as n goes towards infinity you keep minimizing the greatest outlier.

## The Recipe

This is how we roll:

- Take a number of unknown points between
*r*and_{1}*r*. Every point makes 3 unknown coordinates, so don’t take too many, say 3 or 4._{2} - Act as if you already know exactly where the points are.
- For each dimension, fit a polynomial to all or your points. The
*x*-value of the polynomial is time that goes from 0 to*t*, the_{2}*y*-values are point coordinates in specified dimension. Order of the polynomial is one less than the number of points. This way trajectory will surely go through all of the points. - Once you have the polynomial, you can differentiate it to get velocity and acceleration.
- Repeat points 3 and 4 for each dimension.
- Once you have all polynomials you can calculate accelerations and magnitudes and combine them in a single optimization criterion (variance in this exact case).
- Wrap the whole procedure above in another function then feed it to
`scipy.optimize.minimize()`

. The initial guess is, say, linear movement from*r*to_{1}*r*._{2}

The stability and speed of calculation might or might not be an issue depending on the specific problem but it can be tweaked to yield satisfiable results. Also some optimization would definitely not cause much harm, but in my case it’s pointless to say the least. Since this is a DIY project I can’t say don’t try this at home, but be very careful, wear safety goggles and do this in a well-ventilated area.

## The Code

Here it is.

# -*- coding: utf-8 -*- import numpy as np import scipy.optimize import scipy.integrate import scipy.interpolate def optimize_curve(r_1, r_2, v_1, v_2, t_2, n_output=50): """ Motion 'boundary conditions' are given by start and end position vectors and velocities at those points. This function returns optimized points so that there is minimum acceleration* in motion from r_1, v_1 to r_2, v_2. *see comment in motion_3d() r_1, r_2: position vectors v_1, v_2: velocity vectors t_2: end time (start time is 0) n_output: return a 'smoother' curve consisting of this many points; this is just a polyval() after fitting to optimized points. """ # number of points for linspace() etc. n_guess = 4 # freely adjustable points for optimization n_points = n_guess + 2 # + 2 fixed (beginning and end) n_poly = 5 # order of polynomials: must be lower than n_points # number of points used for calculation after polynomials have been defined n_calc = n_output/2 # polynomial coefficients P = { } # reassigned in each iteration def motion_1d(r_start, r_end, v_start, v_end, guess_coords): """ calculates acceleration and velocity along a curve defined with [r_1, guess_coords, r_2], in one dimension """ # these are the points that define the curve and are fed by optimization function t = np.linspace(0, t_2, n_points) # timesteps are equally spaced r = np.concatenate(([r_start], guess_coords, [r_end])) # positions at the above times # fit a polynomial to the points Pr = np.polyfit(t, r, n_poly) t_calc = np.linspace(0, t_2, n_calc) # positions, a little more points r = np.polyval(Pr, t_calc) # velocity: first derivative of the fitted polynomial Pv = np.polyder(Pr) v = np.polyval(Pv, t_calc) # acceleration: second derivative Pa = np.polyder(Pv) a = np.polyval(Pa, t_calc) # return calculated values return r, Pr, v, Pv, a, Pa def motion_3d(flat_guess): """ calculates acceleration and velocity along a 3d curve defined by flat_guess - N_points vectors, flattened into a 1d-array """ # r_guess is a flattened array of guess_points: vector_guess = flat_guess.reshape(n_guess, 3).T # get a list of x-, y- and z- coordinates. guess_x = vector_guess[0] guess_y = vector_guess[1] guess_z = vector_guess[2] r_x, P['r_x'], v_x, P['v_x'], a_x, P['a_x'] = \ motion_1d(r_1[0], r_2[0], v_1[0], v_2[0], guess_x) r_y, P['r_y'], v_y, P['v_y'], a_y, P['a_y'] = \ motion_1d(r_1[1], r_2[1], v_1[1], v_2[1], guess_y) r_z, P['r_z'], v_z, P['v_z'], a_z, P['a_z'] = \ motion_1d(r_1[2], r_2[2], v_1[2], v_2[2], guess_z) r = np.array([r_x, r_y, r_z]).T v = np.array([v_x, v_y, v_z]).T a = np.array([a_x, a_y, a_z]).T # acceleration magnitudes a_mag = np.array([np.linalg.norm(ai) for ai in a]) # criterion for acceleration: standard deviation of the accelerations # at every point; less deviation -> more even acceleration; # minimum acceleration between two points = constant acceleration #ac = np.var(a_mag) # alternative criterion: sum of n-th root of (a_mag**n); # higher n values means minimizing the most outlying value; # n must be an even number (d0h!) n = 10 ac_n = np.sum([ai**n for ai in a_mag]) ac = np.power(ac_n, 1.0/float(n)) return ac, r, v, a def acceleration(flat_guess): ac, _, _, _ = motion_3d(flat_guess) return ac def initial_velocity(flat_guess): _, _, v, _ = motion_3d(flat_guess) return v[0] - v_1 def final_velocity(flat_guess): _, _, v, _ = motion_3d(flat_guess) return v[-1] - v_2 constraints = [ # equality: set positions and velocities { 'type': 'eq', 'fun': initial_velocity }, { 'type': 'eq', 'fun': final_velocity }, ] # the initial guess is a line with equally spaced points between r_1 and r_2 def line_1d(dim): x_1 = r_1[dim] x_2 = r_2[dim] Px = np.polyfit([0, t_2], [x_1, x_2], 1) rx = np.polyval(Px, np.linspace(0, t_2, n_guess)) return rx ri = np.concatenate((line_1d(0), line_1d(1), line_1d(2))) # execute the optimization (leave) opt_results = scipy.optimize.minimize(acceleration, ri, constraints=constraints) if not opt_results.success: raise ArithmeticError(opt_results.message) # get the optimized results from polynomials in P{} dict t = np.linspace(0, t_2, n_output) # positions r_smooth = np.array([ np.polyval(P['r_x'], t), np.polyval(P['r_y'], t), np.polyval(P['r_z'], t), ]).T # velocities v_smooth = np.array([ np.polyval(P['v_x'], t), np.polyval(P['v_y'], t), np.polyval(P['v_z'], t), ]).T # accelerations a_smooth = np.array([ np.polyval(P['a_x'], t), np.polyval(P['a_y'], t), np.polyval(P['a_z'], t), ]).T return r_smooth, v_smooth, a_smooth def test_optimize_curve(): # just a quick test import matplotlib.pyplot as plt r_1 = np.array([0, 0, 0]) r_2 = np.array([0, 1, 1]) v_1 = np.array([1, 0, 0]) v_2 = np.array([0, 0, 1]) t_2 = 2 N = 50 r, v, a = optimize_curve(r_1, r_2, v_1, v_2, t_2, n_output=N) ts = np.linspace(0, t_2, N) # achtung, three curves are plotted, one for each dimension plt.plot(ts, r) #plt.plot(ts, v, color='green') #plt.plot(ts, a, color='blue') test_optimize_curve()