Constraint is not honoured in Scipy.minimize

Question

I am trying to find the minimum path between two points (A,B) while avoiding an obstacle. To get this, I am trying to find the least square distance that connect n points between A and B.

The way I designed my minimization function is to find the best positions of all n points between a and b that return the minimum squared distance and satisfies the constraints.

The code is shown below which uses Scipy.minimize but it does not seem that the routine satisfies the obstacle constraint. The code below shows that the minimization converges successfully but I can see that the results run through the obstacle.

Your help is much appreciated

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)

## Setting Input Data:

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

## 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]]
  pointCoordinates.append(point)
DOFCoordinates = np.array(pointCoordinates)


def initialGuess(DOFCoordinates): 
    numberOfDofCoordinates = len(DOFCoordinates)
    vecLength = 2 * numberOfDofCoordinates
    dofs = np.zeros(vecLength)
    dofs[:numberOfDofCoordinates] = DOFCoordinates[:,0]
    dofs[numberOfDofCoordinates:2*numberOfDofCoordinates] = DOFCoordinates[:,1]
    return dofs


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

## Get Straight Path Coordinates:
def straightPathCoordinates(DOF):
    allCoordinates = np.zeros((2+len(DOF),2))
    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):
    numberOfDofCoordinates = len(DOFCoordinates) 
    dofCoordinates = np.zeros((numberOfDofCoordinates,2))
    dofCoordinates[:,0] = DOF[:numberOfDofCoordinates]
    dofCoordinates[:,1] = DOF[numberOfDofCoordinates:2*numberOfDofCoordinates]
    return dofCoordinates

def GetNewCoordinates(DOF): 
    numberOfDofCoordinates = len(DOFCoordinates)
    allCoordinates = np.zeros((2+numberOfDofCoordinates,2))
    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 = GetNewCoordinates(setDOFCoordinates(DOF))
   sumDistance = 0.0
   for coordinate in range(len(pathPositions)):
        squaredDistance = distance(newCoordinates[coordinate],pathPositions[coordinate])

        sumDistance += squaredDistance
   return sumDistance 


minimumDistanceToObstacle = 2

## Constraints: all coordinates need to be away from an obstacle with a certain distance: 
constraint = []

for coordinate in range(len(DOFCoordinates)+2):
   cons = {'type': 'ineq', 'fun': lambda DOF:   np.sqrt((obstacle[0] - GetNewCoordinates(setDOFCoordinates(DOF))[coordinate][0])**2 +(obstacle[1] - GetNewCoordinates(setDOFCoordinates(DOF))[coordinate][1])**2) - minimumDistanceToObstacle}
   constraint.append(cons)

## Get Initial Guess:
starting_guess = initialGuess(DOFCoordinates)

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

newLineCoordinates = GetNewCoordinates(setDOFCoordinates(result.x))
print newLineCoordinates
print pathPositions
print result

ax.plot([startPoint[0],endPoint[0]],[startPoint[1],endPoint[1]],color='grey')
ax.scatter(obstacle[0],obstacle[1],color='red')

for coordinate in range(len(newLineCoordinates)-1):
  firstPoint = newLineCoordinates[coordinate]
  secondPoint = newLineCoordinates[coordinate+1]
  ax.plot([firstPoint[0],secondPoint[0]],[firstPoint[1],secondPoint[1]],color='black',linewidth=2)
  ax.scatter(firstPoint[0],firstPoint[1])
  ax.text(firstPoint[0],firstPoint[1],str(firstPoint[0])+','+str(firstPoint[1]))

plt.show()

The expected results is a path (note: it could be a curved path) that connects start and end point as well as finding the points between the start and end points that satisfy the obstacle constraint.

Answer 1

You can add the constrain directly to the objective function by penalising the squared distance if the new coordinate is near the obstacle.

def f(DOF):
    newCoordinates = GetNewCoordinates(setDOFCoordinates(DOF))
    sumDistance = 0.0
    for coordinate in range(len(pathPositions)):
        squaredDistance = distance(newCoordinates[coordinate],pathPositions[coordinate])

        des1=distance(newCoordinates[coordinate],obstacle)

        if des1<=minimumDistanceToObstacle:

            sumDistance += squaredDistance+1000000

        else:

            sumDistance += squaredDistance

   return sumDistance 

Also change the initial guess for minimize as follows

Hope it helps