DIY Constrained Path Optimization

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:

  1. 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.
  2. Act as if you already know exactly where the points are.
  3. 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.
  4. Once you have the polynomial, you can differentiate it to get velocity and acceleration.
  5. Repeat points 3 and 4 for each dimension.
  6. Once you have all polynomials you can calculate accelerations and magnitudes and combine them in a single optimization criterion (variance in this exact case).
  7. 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()