Error in Path optimization while avoiding an obstacle

Question

I am trying to find the minimum path that avoids an obstacle between a start and an end point using squared distance method.

To do this, I define n points between the start and end - and calculate the squared distance between the optimized path and the straight path. The optimized path has to be at a minimum distance from the obstacle. The resulting optimized path is the least squared distance between the optimized and straight path.

I have implemented the code as follows but during the optimization, I receive the following error:

could not broadcast input array from shape (27) into shape (27,3)

It looks like Scipy.minimize change the shape of the array from 3-D array to 1D-array. Could you please suggest any recommendations to fix this issue?

import numpy as np
import matplotlib.pyplot as plt
import random 
from mpl_toolkits.mplot3d import Axes3D
from scipy.optimize import minimize

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

## Setting Input Data:

startPoint = np.array([1,1,0])
endPoint = np.array([0,8,0])
obstacle = np.array([5,5,0])

## Get degree of freedom coordinates based on specified number of segments:
numberOfPoints = 10 
pipelineStraightVector = endPoint - startPoint 
normVector = pipelineStraightVector/np.linalg.norm(pipelineStraightVector)
stepSize = np.linalg.norm(pipelineStraightVector)/numberOfPoints
pointCoordinates = []
for n in range(numberOfPoints-1): 
  point = [normVector[0]*(n+1)*stepSize+startPoint[0],normVector[1]*(n+1)*stepSize+startPoint[1],normVector[2]*(n+1)*stepSize+startPoint[2]]
  pointCoordinates.append(point)
DOFCoordinates = np.array(pointCoordinates)

## Assign a random z value for the DOF coordinates - change later: 
for coordinate in range(len(DOFCoordinates)):
    DOFCoordinates[coordinate][2] = random.uniform(-1.0, -0.0)
    ##ax.scatter(DOFCoordinates[coordinate][0],DOFCoordinates[coordinate][1],DOFCoordinates[coordinate][2])

## function to calculate the squared residual:
def distance(a,b): 
  dist = ((a[0]-b[0])**2 + (a[1]-b[1])**2 + (a[2]-b[2])**2)
  return dist

## Get Straight Path Coordinates:
def straightPathCoordinates(DOF):
    allCoordinates = np.zeros((2+len(DOF),3))
    allCoordinates[0] = startPoint
    allCoordinates[1:len(DOF)+1]=DOF
    allCoordinates[1+len(DOF)]=endPoint
    return allCoordinates

pathPositions = straightPathCoordinates(DOFCoordinates)

## Set Degree of FreeDom Coordinates during optimization:
def setDOFCoordinates(DOF): 
    print 'DOF',DOF
    allCoordinates = np.zeros((2+len(DOF),3))
    allCoordinates[0] = startPoint
    allCoordinates[1:len(DOF)+1]=DOF
    allCoordinates[1+len(DOF)]=endPoint
    return allCoordinates

## Objective Function: Set Degree of FreeDom Coordinates and Get Square Distance between optimized and straight path coordinates:
def f(DOF):
   newCoordinates = setDOFCoordinates(DOF)
   print DOF
   sumDistance = 0.0
   for coordinate in range(len(pathPositions)):
        squaredDistance = distance(newCoordinates[coordinate],pathPositions[coordinate])
        sumDistance += squaredDistance
   return sumDistance 

## Constraints: all coordinates need to be away from an obstacle with a certain distance: 
constraint = []
minimumDistanceToObstacle = 0
for coordinate in range(len(DOFCoordinates)+2):
   cons = {'type': 'ineq', 'fun': lambda DOF:  minimumDistanceToObstacle-((obstacle[0] - setDOFCoordinates(DOF)[coordinate][0])**2 +(obstacle[1] - setDOFCoordinates(DOF)[coordinate][1])**2+(obstacle[2] - setDOFCoordinates(DOF)[coordinate][2])**2)}
   constraint.append(cons)

## Get Initial Guess:
starting_guess = DOFCoordinates

## Run the minimization:
objectiveFunction = lambda DOF: f(DOF)
result = minimize(objectiveFunction,starting_guess,constraints=constraint, method='COBYLA')
print result.x
print DOFCoordinates


ax.plot([startPoint[0],endPoint[0]],[startPoint[1],endPoint[1]],[startPoint[2],endPoint[2]])
ax.scatter(obstacle[0],obstacle[1],obstacle[2])

The desired results is a set of points and their positions between point A and point B that avoid the obstacle and returns the minimum distance.

Answer 1

It's because the inputs to minimise has to work with 1D arrays,

From the scipy notes,

The objective function to be minimized.

 fun(x, *args) -> float

where x is an 1-D array with shape (n,) and args is a tuple of the fixed parameters needed to completely specify the function.

x0 : ndarray, shape (n,)

Initial guess. Array of real elements of size (n,), where ‘n’ is the number of independent variables.

This means you should use starting_guess.ravel() in the input and change the setDOFCoordinates to work with arrays which are 1 dimensional.