0
$\begingroup$

I'm working with Ipopt C++ library (https://github.com/coin-or/Ipopt).

Planning to work on optimization of trajectories soon, but for now I'm practicing on the simple tasks.

In particular I've been trying to solve this:

Find a set of N points on the 2D plane such that:
• the first and last point are positioned at (0, 0) and (10, 10), respectively;
• the distances between two consecutive points are equal;
• there is no point lying on a circle or radius 1 centered at (4, 4);
• the sum of the distances between consecutive points is minimized.

I wrote C++ code for finding the solution. It creates cvs file with optimized points.

#include "Ipopt/src/Interfaces/IpIpoptApplication.hpp"
#include "Ipopt/src/Interfaces/IpTNLP.hpp"
#include <cmath>
#include <vector>
#include <fstream> // Include for file operations

using namespace Ipopt;

class PointSetProblem : public TNLP {
public:
    // Constructor
    PointSetProblem(int N, double distance, const std::vector<double>& obstacle, const std::vector<double>& pinit, const std::vector<double>& pfin)
        : N_(N), distance_(distance), obstacle_(obstacle), pinit_(pinit), pfin_(pfin) {}

    int getN() const {
        return N_;
    }

    // Define problem dimensions
    virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g, Index& nnz_h_lag, IndexStyleEnum& index_style) {
        // Number of variables (2D coordinates for N points)
        n = 2 * N_;

        // Number of equality constraints (N-1 distances between points)
        m = N_ - 1;

        // Number of non-zero elements in the Jacobian of the constraints
        nnz_jac_g = 2 * (N_ - 1);

        // Number of non-zero elements in the Hessian of the Lagrangian (0 if not available)
        nnz_h_lag = 0;

        // Use C-style indexing
        index_style = TNLP::C_STYLE;
        return true;
    }

    // Define bounds for variables and constraints
    virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u, Index m, Number* g_l, Number* g_u) {
    // Variable bounds (all points are in [0, 10] x [0, 10])
        for (int i = 0; i < N_; i++) {
            x_l[2 * i] = 0.0;   // x coordinate
            x_l[2 * i + 1] = 0.0; // y coordinate
            x_u[2 * i] = 10.0;   // x coordinate
            x_u[2 * i + 1] = 10.0; // y coordinate
        }

        // Constraint bounds (distances between points)
        for (int i = 0; i < N_ - 1; i++) {
            g_l[i] = 0.0; // Lower bound for distances
            g_u[i] = distance_; // Upper bound for distances
        }

        // Add constraints to avoid the circle of radius 1 centered at (4, 4)
        for (int i = 0; i < N_; i++) {
            double dx = x_l[2 * i] - obstacle_[0];
            double dy = x_l[2 * i + 1] - obstacle_[1];
            double distance_squared = dx * dx + dy * dy;
            g_l[N_ - 1 + i] = -1e20; // Ensure points are outside the circle
            g_u[N_ - 1 + i] = 1e20; // A large upper bound to ensure the circle constraint is not violated
        }

        return true;
    }

    // Define initial guess for the variables
    virtual bool get_starting_point(Index n, bool init_x, Number* x, bool init_z, Number* z_L, Number* z_U, Index m, bool init_lambda, Number* lambda) {
        // Initialize points with equally spaced distances
        for (int i = 0; i < N_; i++) {
            x[2 * i] = pinit_[0] + (pfin_[0] - pinit_[0]) * i / (N_ - 1);
            x[2 * i + 1] = pinit_[1] + (pfin_[1] - pinit_[1]) * i / (N_ - 1);
        }
        return true;
    }

    // Define the objective function
    virtual bool eval_f(Index n, const Number* x, bool new_x, Number& obj_value) {
        // Minimize the sum of distances between consecutive points
        obj_value = 0.0;
        for (int i = 0; i < N_ - 1; i++) {
            double dx = x[2 * (i + 1)] - x[2 * i];
            double dy = x[2 * (i + 1) + 1] - x[2 * i + 1];
            obj_value += std::sqrt(dx * dx + dy * dy);
        }
        return true;
    }

    // Define the constraints
    virtual bool eval_g(Index n, const Number* x, bool new_x, Index m, Number* g) {
        // Calculate distances between consecutive points and use them as constraints
        for (int i = 0; i < N_ - 1; i++) {
            double dx = x[2 * (i + 1)] - x[2 * i];
            double dy = x[2 * (i + 1) + 1] - x[2 * i + 1];
            g[i] = std::sqrt(dx * dx + dy * dy);
        }
        return true;
    }

    // Define the Jacobian of the constraints
    virtual bool eval_jac_g(Index n, const Number* x, bool new_x, Index m, Index nele_jac, Index* iRow, Index* jCol, Number* values) {
        // Structure of the Jacobian matrix
        if (values == NULL) {
            for (int i = 0; i < N_ - 1; i++) {
                iRow[2 * i] = i;
                jCol[2 * i] = 2 * i;
                iRow[2 * i + 1] = i;
                jCol[2 * i + 1] = 2 * i + 1;
            }
        } else {
            // Calculate the values of the Jacobian matrix
            for (int i = 0; i < N_ - 1; i++) {
                double dx = x[2 * (i + 1)] - x[2 * i];
                double dy = x[2 * (i + 1) + 1] - x[2 * i + 1];
                double dist = std::sqrt(dx * dx + dy * dy);
                values[2 * i] = dx / dist;
                values[2 * i + 1] = dy / dist;
            }
        }
        return true;
    }

    // Define the gradient of the objective function
    virtual bool eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f) {
        // Implement the gradient evaluation
        for (int i = 0; i < N_; i++) {
            grad_f[2 * i] = 0.0; // Gradient of the x-coordinate
            grad_f[2 * i + 1] = 0.0; // Gradient of the y-coordinate
        }
        return true;
    }

    // Method to finalize the solution
    virtual void finalize_solution(
        Ipopt::SolverReturn status, // Change this type to Ipopt::SolverReturn
        Ipopt::Index n,
        const Ipopt::Number* x,
        const Ipopt::Number* z_L,
        const Ipopt::Number* z_U,
        Ipopt::Index m,
        const Ipopt::Number* g,
        const Ipopt::Number* lambda,
        Ipopt::Number obj_value,
        const Ipopt::IpoptData* ip_data,
        Ipopt::IpoptCalculatedQuantities* ip_cq
    ) {
        if (status == Ipopt::SUCCESS) { // Change this comparison to Ipopt::SUCCESS
            // Write the optimal points to a CSV file
            std::ofstream outFile("optimal_points.csv");
            for (int i = 0; i < N_; i++) {
                outFile << x[2 * i] << "," << x[2 * i + 1] << std::endl;
            }
            outFile.close();
            std::cout << "Optimal points saved to 'optimal_points.csv'." << std::endl;
        }
    }

private:
    int N_; // Number of points
    double distance_; // Constraint distance
    std::vector<double> obstacle_; // Obstacle position
    std::vector<double> pinit_; // Initial point
    std::vector<double> pfin_; // Final point
};

int main() {
    int N = 30; // Number of points
    double distance = 2; // Constraint distance
    std::vector<double> obstacle = {4.0, 4.0};
    std::vector<double> pinit = {0.0, 0.0};
    std::vector<double> pfin = {10.0, 10.0};

    SmartPtr<TNLP> pointSetProblem = new PointSetProblem(N, distance, obstacle, pinit, pfin);
    SmartPtr<IpoptApplication> app = IpoptApplicationFactory();

    // Set the Hessian approximation method to limited-memory
    app->Options()->SetStringValue("hessian_approximation", "limited-memory");

    app->RethrowNonIpoptException(true);
    ApplicationReturnStatus status = app->Initialize();
    if (status != Solve_Succeeded) {
        std::cout << "IPOPT initialization failed!" << std::endl;
        return -1;
    }

    status = app->OptimizeTNLP(pointSetProblem);

    if (status == Solve_Succeeded || status == Solved_To_Acceptable_Level) {
        Number obj_value;
        Number x[2 * N];
        pointSetProblem->eval_f(2 * N, x, true, obj_value);

        std::cout << "Optimal points:" << std::endl;
        for (int i = 0; i < N; i++) {
            std::cout << "Point " << i + 1 << ": (" << x[2 * i] << ", " << x[2 * i + 1] << ")" << std::endl;
        }
        std::cout << "Optimal Objective Value: " << obj_value << std::endl;
    } else {
        std::cout << "IPOPT optimization failed with status " << status << std::endl;
    }

    return 0;
}

The problem is that my code satisfies all the required tasks except for this one:

• there is no point lying on a circle or radius 1 centered at (4, 4);

That's why I added this part of the code for creating some obstacle:

// Add constraints to avoid the circle of radius 1 centered at (4, 4)
        for (int i = 0; i < N_; i++) {
            double dx = x_l[2 * i] - obstacle_[0];
            double dy = x_l[2 * i + 1] - obstacle_[1];
            double distance_squared = dx * dx + dy * dy;
            g_l[N_ - 1 + i] = -1e20; // Ensure points are outside the circle
            g_u[N_ - 1 + i] = 1e20; // A large upper bound to ensure the circle constraint is not violated
    }

And the thing is that when the last two lines are commented out :

g_l[N_ - 1 + i] = -1e20; 
g_u[N_ - 1 + i] = 1e20;

the code can be compiled but it doesn't affect the location of the points (because I didn't give the upper and lower bounds) But when they are not commented out it can be compiled again but when I run the program the Ipopt library gives the following error:

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit https://github.com/coin-or/Ipopt
******************************************************************************

This is Ipopt version 3.14.13, running with linear solver MUMPS 5.6.0.

malloc(): invalid size (unsorted)
Aborted (core dumped)

What can be the problem? Why should I do? Thank you for your time in advance!

$\endgroup$
1
  • $\begingroup$ Welcome to Robotics Stack Exchange! I looked at your code to find out where g_l and g_u variables are defined but couldn't find it. How are they defined? The malloc error indicates that you are accessing an invalid memory! $\endgroup$
    – ravi
    Oct 12, 2023 at 6:47

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.