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*): Assertionfalse' 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 > > >::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 > >::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 >::operator()<boost::_mfi::mf1<Path, SwarmCommander, Path const&>, boost::_bi::list0>(boost::_bi::type, 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::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.