solution vector is empty

[hichamhendy]

I am trying the following on ROS

`Path modelPredictivePlanning(const Path& initial_path)
{
Path optimized_path = initial_path;
Eigen::Vector3d setting_point;
size_t N = 6;
double dt = 0.05;
const size_t x_start = 0;
const size_t y_start = x_start + N;
const size_t z_start = y_start + N;
const size_t x_dot_start = z_start + N;
const size_t y_dot_start = x_dot_start + N;
const size_t z_dot_start = y_dot_start + N;
const size_t roll_start = z_dot_start + N;
const size_t pitch_start = roll_start + N;
const size_t roll_command_start = pitch_start + N;
const size_t pitch_command_start = roll_command_start + N - 1;
const size_t thrust_command_start = pitch_command_start + N - 1;

ros::WallTime start_time = ros::WallTime::now();
for (int i = 0; i < optimized_path.points_.size() - 1; i++)
{
    typedef CPPAD_TESTVECTOR(double) Dvector;
    // number of independent variables 8 states and 3 inputs (domain dimension for f and g)
    const size_t n_vars = N * 8 + (N - 1) * 3;
    // Number of constraints
    const size_t n_constraints = N * 8;
        
        
    Dvector vars(n_vars);
    // Lower and upper limits for x
    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);

    // Lower and upper limits for constraints
    // All of these should be 0 except the initial
    // state indices.
    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);


        // options
    std::string options;
    options += "Integer print_level  0\n";
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // maximum number of iterations
    // options += "Integer max_iter     10\n";
    // options += "Numeric tol          1e-6\n";
    // options += "String  derivative_test            second-order\n";
    // options += "Numeric point_perturbation_radius  0.\n";

    SystemConstants sys_constants;
// 


    const Eigen::Vector3d p1 = optimized_path.points_[i];
    const Eigen::Vector3d p2 = optimized_path.points_[i + 1];

    double x_init = current_copter_position_.x();
    double y_init = current_copter_position_.y();
    double z_init = current_copter_position_.z();
    double x_dot_init = current_copter_velocity_.x();
    double y_dot_init = current_copter_velocity_.y();
    double z_dot_init = current_copter_velocity_.z();
    double roll_init = current_copter_euler_orientation_.x();
    double pitch_init = current_copter_euler_orientation_.y();
    // double yaw_init = current_copter_euler_orientation_.z(); // not needed

    // Initial value of the independent variables "vars".
    // Should be 0 except for the initial values.
    
    for (int i = 0; i < n_vars; ++i) 
    {
        vars[i] = 0.0;
    }

    vars[x_start] = x_init;
    vars[y_start] = y_init;
    vars[z_start] = z_init;
    vars[x_dot_start] = x_dot_init;
    vars[y_dot_start] = y_dot_init;
    vars[z_dot_start] = z_dot_init;
    vars[roll_command_start] = roll_init;
    vars[pitch_command_start] = pitch_init;



    // Set all non-actuators upper and lowerlimits
    // to the max negative and positive values.
    for (int i = 0; i < roll_command_start; ++i) 
    {
        vars_lowerbound[i] = -1.0e19;
        vars_upperbound[i] = 1.0e19;
    }

    for (int i = roll_command_start; i < thrust_command_start; ++i) 
    {
        vars_lowerbound[i] = - sys_constants.maxmin_angle;
        vars_upperbound[i] =   sys_constants.maxmin_angle;
    }

    for (int i = thrust_command_start; i < n_vars; i++)
    {
        vars_lowerbound[i] = sys_constants.min_thrust;
        vars_upperbound[i] = - sys_constants.max_thrust;
    }


    for (int i = 0; i < n_constraints; ++i) 
    {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }

    constraints_lowerbound[x_start] = x_init;
    constraints_lowerbound[y_start] = y_init;
    constraints_lowerbound[z_start] = z_init;
    constraints_lowerbound[x_dot_start] = x_dot_init;
    constraints_lowerbound[y_dot_start] = y_dot_init;
    constraints_lowerbound[z_dot_start] = z_dot_init;
    constraints_lowerbound[roll_start] = roll_init;
    constraints_lowerbound[pitch_start] = pitch_init;


    constraints_upperbound[x_start] = x_init;
    constraints_upperbound[y_start] = y_init;
    constraints_upperbound[z_start] = z_init;
    constraints_upperbound[x_dot_start] = x_dot_init;
    constraints_upperbound[y_dot_start] = y_dot_init;
    constraints_upperbound[z_dot_start] = z_dot_init;
    constraints_upperbound[roll_start] = roll_init;
    constraints_upperbound[pitch_start] = pitch_init;


    // pass
    FG_eval fg_eval(N, dt, p1,  p2);


    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;


        // solve the problem
        CppAD::ipopt::solve<Dvector, FG_eval>(
            options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
                constraints_upperbound, fg_eval, solution);

    bool ok = true;
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    if(CppAD::ipopt::solve_result<Dvector>::success)
        ROS_INFO_STREAM(kStreamPrefix <<"Solution found!!!");

    auto cost = solution.obj_value;
    ROS_INFO_STREAM(kStreamPrefix <<"Cost: "<< cost);

    std::cout << solution.x;//, solution.x[y_start + 1], solution.x[z_start + 1];

    while((ros::WallTime::now() - start_time).toSec() < ros::WallDuration(dt).toSec())
        ros::Duration(0.005).sleep();
}
return optimized_path;

}`

However the solution vector in x is empty.

By the way when I try to extract the error from that empty solution I get
cppad-20180000.0 error from a known source: vector: index greater than or equal vector size Error detected by false result for i < length_ at line 473 in the file /usr/include/cppad/utility/vector.hpp mav_swarm_commander_node: /usr/include/cppad/utility/error_handler.hpp:206: static void CppAD::ErrorHandler::Default(bool, int, const char*, const char*, const char*): Assertion false' failed.
Stack trace (most recent call last):
#19 Object "", at 0xffffffffffffffff, in
#18 Source "/usr/include/c++/11/bits/basic_string.h", line 187, in _start [0x562a223e27a9]
185: pointer
186: _M_data() const
> 187: { return _M_dataplus._M_p; }
188:
189: pointer
190: _M_local_data()
`
Any ideas?

[bradbell]

Do you get the report "Solution found!!! ?

[hichamhendy]

How could I do that?

[hichamhendy]

I can invite you to collaborate on my repo.
Already done.
You would need to publish a goal, so the callback works

[bradbell]

All I need is the clone command and instructions for how to reproduce the error .
I see the repo is private. I accepted the invite.
Please open an issue in
https://github.com/hichamhendy/mav_swarm_commander/issues
for this problem

[hichamhendy]

Done

[hichamhendy]

As you asked yesterday:
I added the following
bool memory_ok = CppAD::thread_alloc::free_all(); std::string group = "swarm/Node"; size_t width = 20; CppAD::test_boolofvoid Run(group, width); bool ok_sum = Run.summary(memory_ok); ROS_INFO_STREAM(kStreamPrefix <<"Summary (mem): "<< ok_sum);

and got that

Begin test group swarm/Node memory_leak Error 1 tests failed. [ INFO] [1678189848.577661006, 17.832000000]: [Swarm Commanding]: Summary (mem): 0 [ INFO] [1678189848.577679643, 17.832000000]: [Swarm Commanding]: Solution found!!! [ INFO] [1678189848.577699892, 17.832000000]: [Swarm Commanding]: Cost: 0 { }

any ideas???
why the leak?!!!

[bradbell]

I recently had a shoulder operation and it is still difficult for me to type. In addition, I am tying to solve a problem for work right now. I hope to be able to spend some time on your problem soon. Sorry for the delay.

[hichamhendy]

No problem. Get well soon. Thank you anyway

[hichamhendy]

After launching that in separate thread and activating the debug flag of CppAD i got this
Stack trace (most recent call last) in thread 24512: #15 Object "", at 0xffffffffffffffff, in #14 Source "../sysdeps/unix/sysv/linux/x86_64/clone.S", line 95, in __clone [0x7fa4a464861e] #13 Source "/build/glibc-CVJwZb/glibc-2.27/nptl/pthread_create.c", line 463, in start_thread [0x7fa4a61cd6da] #12 Object "/usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1", at 0x7fa4a303ebcc, in #11 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a56e5917, in boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf1<Path, SwarmCommander, Path const&>, boost::_bi::list2<boost::_bi::value<SwarmCommander*>, boost::_bi::value<Path> > > >::run() #10 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a56ec529, in boost::_bi::bind_t<void, boost::_mfi::mf1<Path, SwarmCommander, Path const&>, boost::_bi::list2<boost::_bi::value<SwarmCommander*>, boost::_bi::value<Path> > >::operator()() #9 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a56f259a, in void boost::_bi::list2<boost::_bi::value<SwarmCommander*>, boost::_bi::value<Path> >::operator()<boost::_mfi::mf1<Path, SwarmCommander, Path const&>, boost::_bi::list0>(boost::_bi::type<void>, boost::_mfi::mf1<Path, SwarmCommander, Path const&>&, boost::_bi::list0&, int) #8 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a56f8f42, in boost::_mfi::mf1<Path, SwarmCommander, Path const&>::operator()(SwarmCommander*, Path const&) const #7 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a563716a, in SwarmCommander::modelPredictivePlanning(Path const&) #6 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a5660b45, in CppAD::vector<double>::operator[](unsigned long) #5 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a5647658, in CppAD::ErrorHandler::Call(bool, int, char const*, char const*, char const*) #4 Object "/home/rnd/catkin_ws/devel/.private/mav_swarm_commander/lib/libmav_swarm_commander.so", at 0x7fa4a564783e, in CppAD::ErrorHandler::Default(bool, int, char const*, char const*, char const*) #3 Source "/build/glibc-CVJwZb/glibc-2.27/assert/assert.c", line 101, in __assert_fail [0x7fa4a4557471] #2 Source "/build/glibc-CVJwZb/glibc-2.27/assert/assert.c", line 92, in __assert_fail_base [0x7fa4a45573f9] #1 Source "/build/glibc-CVJwZb/glibc-2.27/stdlib/abort.c", line 79, in abort [0x7fa4a45677f0] #0 Source "../sysdeps/unix/sysv/linux/raise.c", line 51, in raise [0x7fa4a4565e87] Aborted (Signal sent by tkill() 24237 1000) Aborted (core dumped)

[hichamhendy]

I found the problem in the lower and upper boundaries of the variables exactly in
for (int i = thrust_command_start; i < n_vars; i++) { vars_lowerbound[i] = sys_constants.min_thrust; vars_upperbound[i] = - sys_constants.max_thrust; }

The solver kinda gave the impression that the formulation is well-defined but after analysis, especially by giving succeed in finding the solution even though the solution vector was empty.