Solve does not propogate states and gives zero control output

User Questions
1

Hi all

I am working on implementing acados in C++, and are utilizing the c generated code. To implement this, I have followed the implementation in the main_.c file which is generated along with the ocp_solver_.so library and the .c and .h files related. However, when implementing it, I struggle with my implementation not propogating the x-tradjectory (state trajectory) during optimization, and from this, does not give an control output. The output from the state tradjectory over the horizon can be seen in the following picture:

myXtraj
myXtraj1022×587 72.1 KB

I have verified that the c-code in main_*.c works as intended by building and running it, where it gives the following x-tradjectory, where I know the states in the second row is correct, as they coincide with just running the OCP in python:

main_xtraj
main_xtraj992×582 79.2 KB

It should be mentioned that I use vectors to input the array pointers to the acados functions instead of arrays, which I have seen has caused problems in the following topic:

However, I have used vector::data() method instead of “&”, which returns the equivalent to an array pointer.

At the same time, the difference from the main_.c is that I set a changing “yref” using the ocp_nlp_cost_model_set() method, and also want to alter the cost matrix online, which I do using the ocp_nlp_cost_model_set() method, with a pointer to an cost vector with NYNY elements. I have verified that these vectors, along with the parameters are correctly updated continuously while simulating.

It is also worth mentioning, and which can be seen in the pictures added above, that I have tried setting the initial states to non-zero values, such that the x-tradjectory changes over the horizon, independently of the optimization. Despite this, the solver does not propogate the x-tradjectory over the optimization horizon.

Lastly it should be mentioned that when running the *_acados_solve() method, it also returns status “0”, indicating no error.

I hope someone have some insight into what the might be the problem.

2

I realize that it might be hard to add some input when there is no code included in the questions. I will try to provide some color, but the project is a part of a much larger project, so a compilable code cannot be included.

It is desired to include the acados generated code with a ROS2 framework. To do this the cmake code below is used, where src_files are the .cpp files for the ROS2 wrapper around the c generated code, acados_lib_path is the path to acados/lib. Mark that files acados_solver_*.h has other names correlating to the project name, but I don’t want to include the full name.

add_compile_definitions(ACADOS_WITH_OSQP)

find_package(blasfeo REQUIRED)
find_package(hpipm REQUIRED)
if(hpipm_FOUND)
message("HPIPM package found")
else()
message("HPIPM was NOT found")
endif()
find_package(osqp REQUIRED)
if(osqp_FOUND)
message("OSQP package found")
else()
message("OSQP was NOT found")
endif()

set(DEPENDENCIES
  "blasfeo"
  "hpipm"
  "osqp")

add_library(${MK2_LIBRARY} ${SRC_FILES})

target_link_libraries(${MK2_LIBRARY}
  ${MK2_MPC_CODE_PATH}/libacados_ocp_solver_*.so
  ${acados_lib_path}/libacados.so
  ${acados_lib_path}/libosqp.so
  ${acados_lib_path}/libblasfeo.so
  ${acados_lib_path}/libhpipm.so
)

ament_target_dependencies(${MK2_LIBRARY} ${DEPENDENCIES})
target_include_directories(${MK2_LIBRARY} PUBLIC $<TARGET_PROPERTY:blasfeo,INTERFACE_INCLUDE_DIRECTORIES>)
target_include_directories(${MK2_LIBRARY} PUBLIC $<TARGET_PROPERTY:hpipm,INTERFACE_INCLUDE_DIRECTORIES>)

ament_export_targets(${MK2_LIBRARY} HAS_LIBRARY_TARGET)
ament_export_dependencies(${DEPENDENCIES})

install(
  TARGETS ${MK2_LIBRARY}
  EXPORT ${MK2_LIBRARY}
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

install(
  DIRECTORY 
  include/
  DESTINATION include
)

install(
  FILES 
  lib/mk2_c_generated_code/acados_solver_*.h
  DESTINATION include/${PROJECT_NAME}
)

install(
  FILES
  ${MK2_MPC_CODE_PATH}/libacados_ocp_solver_*so
  ${acados_lib_path}/libacados.so
  DESTINATION lib
)

install(
    DIRECTORY
    params
    launch
    DESTINATION share/${PROJECT_NAME}
)

Using the cmake above, the code gives no errors during building of the code, and all the packages blasfeo, hpipm and osqp returns true for checks if the package is found. As you may also understand from the cmake, the code is generated with OSQP as solver.

The wrapper code is in short setup in the following way:

  1. Initialize OCP
  • Get parameters value
  • Setup state, input, reference, constraint and costs according to the c generated dimensions and set values for all vectors.
  • Generate capsule using *_acados_create_capsule();
  • Create discretization using *_acados_create_with_discretization();
  • Set costs and parameters using ocp_nlp_cost_model_set() with "W" as field and *_acados_update_params(), both for full horizon+1
  • Initialize states and inputs in ocp using ocp_nlp_out_set() with respective fields "x" for full horizon+1 and "u" for full horizon
  • Set intial state box constraint vector using ocp_nlp_constraints_model_set() using "idxbx" as field at stage 0
  1. Solve OCP on event callback
  • Generate reference and set it using ocp_nlp_cost_model_set() with "y_ref" as field
  • Set initial condition based on observer
  • Set online parameters in loop, then update acados params using *_acados_update_params(), both for full horizon+1
  • Set box constraint using ocp_nlp_constraints_model_set() with "lbx" and "ubx" as field and stage 0
  • Solve using *_acados_solve();
  • Update control inputs and full horizon states, and set current control input
  • Print states over full horizon, control parameters over full horizon, last reference in horizon, current state from observer, parameters and solver status.

The outputs are as follows:

out_1
out_11033×816 48.7 KB

out2
out2354×806 9.45 KB

I reiterate that all functions use std::vector::data() as input instead of arrays.

Hope this helps in finding the problem. If I there is anything else which is needed of information, feel free to ask for it.

3

It seems like you did a lot of changes to the main file and I do not know which of them broke it.
I suggest to apply the changes a bit more sequentially and see check why it breaks.
Using git also helps.
Sorry it is very hard to tell in general.

Set intial state box constraint vector using ocp_nlp_constraints_model_set() using "idxbx" as field at stage 0

Maybe you did something wrong here and your initial state is not fixed anymore.