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 r1 with a defined velocity of v1 and ending its path at r2 with also well-defined velocity v2. Starting time is 0 and ending time is t2. The mission is to find the optimal trajectory given the constraints.
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 r1 and r2. Every point makes 3 unknown coordinates, so don’t take too many, say 3 or 4.
- 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 t2, the 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 r1 to r2.
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()