Constraints concatenation error

[braultp]

Hello,

I am currently a Ph.D. student working in trajectory planning for aerial robots. Concretely, the goal of the optimisation is to pass one (the main one is related to my topic of research) or several objectives (other could be curve regularisation, time minimisation, energy minimisation, and so forth), to design a trajectory that is to be followed by a quadcopter UAV.

The reference trajectories are defined by piecewise Béziers curves, where each piece is defined by the start and end waypoints, and the time of each waypoint. Each waypoint defines a 4D output (x, y, z, yaw) in position, speed, and acceleration. From this, two successive waypoints are enough to define a piece.

The optimisation is also subject to constraints that are specific to the system:

• since the actuators have limits, we define inequality constraints to define the saturations (min/max for each of the four motors, thus 8);

• we also define one more inequality constraint for the minimum radius of the curve, thus we add one more for a total of 9 inequality constraints for this specific system;

• we want the system to reach a specific target, contained in the last waypoint: however for the controller used, having the last waypoint as the target is not enough, since the system does not follow the reference trajectory perfectly (and since this is known, we can add specific equality constraints such that the system reaches the actual target at the end of the motion, by tweaking all the waypoints through the optimisation). Therefore, I define equality constraints for this 4D output target reach in position, velocity and acceleration, meaning that I have 12 equality constraints for the reference shape optimisation.

Before, I was using the algorithm COBYLA from NLOPT, but it seems that the optimizer fails to do what I want, thus I am now trying to switch to pymoo which seems very promising for the problem I try to solve. Currently, the function I use for the optimisation is the following, where the constraints (ieq and eq) are each defined by a wrapper function that outputs the flattened constraints:

'''
def pymoo_sens_opt(model: GenerateFromSystem, trajectory: PiecewiseSplineTrajectory, opt_options):

target, lb, ub, PI_mask, use_yaw, cost_mode, pymoo_settings = opt_options
pop_size, n_gen = pymoo_settings

trajopt = trajectory.deepcopy()  # local copy
x_ini = trajopt.get_flat_params()  # initial vector for the optimization (waypoints + times)
# print(f'x_ini = {x_ini}')  # debug

N = 500
ODE: JitParam = model.ODE
s = model.s
if trajectory.N_dim * (trajectory.degree + 1) != np.size(ODE["a"]):
    raise RuntimeError()

def get_cost_from_flat_params(x):
    ntrajopt = trajopt.deepcopy()
    ntrajopt.update_from_flat_params(x)  # get current trajectory
    costval = cost(ntrajopt, ODE, num=N, PI_mask=PI_mask, W_range=s.W_range, use_PI_yaw=use_yaw, cost_mode=cost_mode)
    return costval

def constraints_wrap(x):
    trajcon = trajopt.deepcopy()
    trajcon.update_from_flat_params(x)  # get current trajectory
    model.integrate_along_trajectory(trajcon, N)  # do the integration
    states_vec = ODE.last_result
    time_vec = ODE.time_points
    ineqconres = model.ineqcon(states_vec, time_vec, target, None)  # input saturations and rmin
    eqconres = model.pymoo_eqcon(states_vec, time_vec, target)  # target reach
    constraints = np.block([ineqconres, eqconres])
    return constraints.flatten()

def ieq_wrap(x):
    trajcon = trajopt.deepcopy()
    trajcon.update_from_flat_params(x)  # get current trajectory
    model.integrate_along_trajectory(trajcon, N)  # do the integration
    states_vec = ODE.last_result
    time_vec = ODE.time_points
    ineqconres = model.ineqcon(states_vec, time_vec, target, None)  # input saturations and rmin
    return ineqconres.flatten()

def eq_wrap(x):
    trajcon = trajopt.deepcopy()
    trajcon.update_from_flat_params(x)  # get current trajectory
    model.integrate_along_trajectory(trajcon, N)  # do the integration
    states_vec = ODE.last_result
    time_vec = ODE.time_points
    eqconres = model.eqcon(states_vec, time_vec, target)  # target reach
    return eqconres.flatten()

def get_init_pop(x, target, pop_size):
    X = np.zeros((pop_size, len(x)))
    init_traj = trajectory.deepcopy()
    lastvec = init_traj.waypoints[-2][0, :3] - target[0, :3]
    ldist = norm(lastvec)
    b1 = lastvec / ldist
    thr, psir, rr = [0, 2 * np.pi], [-np.pi / 2, 0.9 * np.pi / 2], [0.2 * ldist, 3 * ldist]
    e = np.array([0, 0, 1])
    b2 = np.cross(e, b1) / norm(np.cross(e, b1))
    b3 = np.cross(b1, b2)
    for k in range(pop_size):
        vec = np.dot(rotation_matrix(b1, np.random.uniform(thr[0], thr[1])), b3)
        ovec = np.cross(vec, b1)
        newvec = np.dot(rotation_matrix(ovec, np.random.uniform(psir[0], psir[1])), vec)
        gen_traj = init_traj.deepcopy()
        gen_traj.waypoints[-2][0, :3] = gen_traj.waypoints[-2][0, :3] + np.random.uniform(rr[0], rr[1]) * norm(lastvec) * newvec
        x = gen_traj.get_flat_params()
        X[k, :] = x
    return X

# needed for pymoo algorithm:
n_vars = len(x_ini)  # number of variables of the problem
objectives = [
    get_cost_from_flat_params
]
ieq = [ieq_wrap]
eq = [eq_wrap]
xl, xu = trajectory.gen_pymoo_bounds(target, [lb, ub])

problem = FunctionalProblem(n_var=n_vars,
                            objs=objectives,
                            constr_ieq=ieq,
                            constr_eq=eq,
                            xl=xl, xu=xu)

# create initial data for the algorithm (sample trajectories):

X = get_init_pop(x_ini, target, pop_size)
pop = Population.new('X', X)
Evaluator().eval(problem, pop)

reference_directions = np.array([[1.0]])
res = minimize(problem=problem,
               algorithm=UNSGA3(reference_directions, pop_size=pop_size, sampling=pop),
               termination=('n_gen', n_gen),
               save_history=True,
               seed=1)

print(f'UNSGA3: Best solution found: \nX = {res.X}\nF = {res.F}')
costevol = [np.min(e.pop.get("F")) for e in res.history]

trajopt = trajectory.deepcopy()
trajopt.update_from_flat_params(res.X)  # get optimal trajectory

return trajopt, costevol 

'''

For now, my problem is that I have an error when trying to start the optimisation, with the following error message:

Does someone have an hint on what is going on ? I can't seem to understand why the concatenation is not correct, but I probably misunderstand how to correctly write the constraints for pymoo.

Thank you for your time, best,

Pascal

[braultp]

Hello,

To whoever would be reading this post, just in case you come across the same problem, I have been able to find a solution, by redefining the evaluate function of the functional problem, as follows:

'''
class SensitivityProblem(FunctionalProblem):

    def _evaluate(self, x, out, *args, **kwargs):
        # calculate violation from the inequality constraints
        ieq = np.array([constr(x) for constr in self.constr_ieq])
        ieq[ieq < 0] = 0
        ieq = ieq.flatten()

        # calculate violation from the quality constraints
        eq = np.array([constr(x) for constr in self.constr_eq])
        eq = np.abs(eq)
        eq = eq - self.constr_eq_eps
        eq = eq.flatten()

        # calculate the objective function
        f = np.array([obj(x) for obj in self.objs])

        out["F"] = f
        out["G"] = np.concatenate([ieq, eq])

'''

Now, both the inequality and equality constraints are changed to a flat vector before concatenation, and are therefore properly handled by the algorithm.

Pascal