1

I am working on the following QP Receding Horizon Path Planning Problem:

Problem statement

At each time step, a point (simulating a robot) computes its Voronoi Cell (VC) with respect to the others, plans a path within the VC, then executes the first step along that path. The robot position, VC, and planned path evolve together throughout the execution of the algorithm. Assuming that the planning horizon is T steps, and for robot i at each time instance, we denote the positions of the planned trajectory as p_{i,1} , p_{i,2} , ... , p_{i,T}, for all i in {1, 2, ... , n}. The algorithm requires each robot to solve a QP at each time step, e.g., for robot i, the path and input (or velocity) are given by solving the following QP Receding Horizon Path Planning Problem.

In Problem 1, the cost function J_{i} is a summation of intermediate state costs and a terminal cost, p_{i,f} is the goal position, {p}{i,0} to {p}{i,T} and u_{i,0} to u_{i,T-1} are the path, and inputs to be planned, respectively. The positive definite or semi-definite matrices Q,R, and Q_{f} are weight factors to balance among the three costs. The decision variables for this standard QP problem are {p}{i,1} to {p}{i,T}. The constraint (i) ensures that the path is feasible with the robots dynamics. The constraint (ii) restrains the planned path to be in the corresponding BVC of robot i. Constraint (ii) can be written explicitly in the form of a set of linear inequality constrain

where epsilon_{j} is a 2 x 1 vector, corresponding to an edge of the VC V_{i}. Constraint (iii) makes sure the path starts from the robot’s current position, and the lower and upper bounds for the input u_{i,t} are written component-wisely in constraints (iv) and (v).

Code Attempt

I have the following code:

# Import packages.

import cvxpy as cp
import numpy as np

def optimizer(p_i, p_f, vertices):

    np.random.seed(1)

    # Dimensions
    T = 1 # Steps
    m = 2 # u dimension
    n = 2 # x dimension

    # Mechanics
    alpha = 0.2
    A = np.eye(n) + alpha*np.random.randn(n,n)
    B = np.random.randn(n,m)

    # Parameters
    R = cp.Parameter((m,m), PSD = True)
    Q = cp.Parameter((n,n), PSD = True)
    Qf = cp.Parameter((n,n), PSD = True)
    R.value = np.identity(m)
    Q.value = np.identity(n)
    Qf.value = np.identity(n)

    # Variables
    p = cp.Variable((n,T+1))
    u = cp.Variable((m,T))
    u_max_x = 1
    u_max_y = 1

    # Cost and Constraints
    cost = 0
    constr = []
    constr += [p[:,0] == p_i]
    for t in range(T):
        cost += cp.quad_form(p[:,t]-p_f,Q) + cp.quad_form(u[:,t],R)
        constr += [p[:,t+1] == A@p[:,t] + B@u[:,t],
                   cp.abs(u[0,t]) <= u_max_x,
                   cp.abs(u[1,t]) <= u_max_y]
                                                                
        for j in range(len(vertices)-1):
            constr += [((vertices[j%3][0] - p[0,t]) * (vertices[(j+1)%3][1] - p[1,t])) <= ((vertices[j%3][1] - p[1,t]) * (vertices[(j+1)%3][0] - p[0,t]))]
       
    # Last case
    cost += cp.quad_form(p[:,T]-p_f,Qf)
    for j in range(len(vertices)):
        constr += [((vertices[j%3][0] - p[0,T]) * (vertices[(j+1)%3][1] - p[1,T])) <= ((vertices[j%3][1] - p[1,T]) * (vertices[(j+1)%3][0] - p[0,T]))]

    # Objective
    prob = cp.Problem(cp.Minimize(cost), constr)

    # Solve with no Disciplined Geometric Programming
    prob.solve(gp=False)

    # Check status
    print(prob.status)

    #print(cost.is_dcp(dpp=True))

    # Optimal solution
    optimal_value = prob.value

    # Return
    return optimal_value

# Testing to move a point c within a triangle to the target g
v1 = [-2,1]
v2 = [2,2]
v3 = [1,-3]
c = np.array([1,1])
g = np.array([3,-2])
inst_bvc = np.array([v1, v2, v3])

solution = optimizer(c, g, inst_bvc)
print(solution)

Running the code, I obtained the DCPError: Problem does not follow DCP rules. The constraints are not DCP.

Somebody can help me to develop a better version of my code? Thank you in advance

Pepe
  • 11
  • 1

0 Answers0