diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 8158ab8b03cf..c5531ed878e7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -48,6 +48,12 @@ fi # End Estimator Group Selection # ############################################################################### +# +# Start Indi Controller +# +indi_control start + + # # Start Control Allocator # @@ -56,26 +62,29 @@ control_allocator start # # Start Multicopter Rate Controller. # -mc_rate_control start +#mc_rate_control start +#indi_rate_control start # # Start Multicopter Attitude Controller. # -mc_att_control start +#mc_att_control start +#indi_att_control start -if param greater -s MC_AT_EN 0 -then - mc_autotune_attitude_control start -fi +#if param greater -s MC_AT_EN 0 +#then +# mc_autotune_attitude_control start +#fi # # Start Multicopter Position Controller. # -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start +#mc_hover_thrust_estimator start +#flight_mode_manager start +#mc_pos_control start +#indi_pos_control start # # Start Multicopter Land Detector. # -land_detector start multicopter +#land_detector start multicopter diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 7b2b9b095d01..4710b16577f3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -21,3 +21,7 @@ param set-default RTL_RETURN_ALT 30 param set-default RTL_DESCEND_ALT 10 param set-default GPS_UBX_DYNMODEL 6 + + +# make sure commander doesnt disarm vehicle by thinking it hasnt taken off +param set-default COM_DISARM_PRFLT -1 diff --git a/boards/px4/sitl/indi.px4board b/boards/px4/sitl/indi.px4board new file mode 100644 index 000000000000..6521e1161067 --- /dev/null +++ b/boards/px4/sitl/indi.px4board @@ -0,0 +1,17 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_AIRSHIP_ATT_CONTROL=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL_L1=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_MICRORTPS_BRIDGE=y +CONFIG_MODULES_INDI_CONTROL=y diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 10f6ea8c8199..7d83c40490af 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -518,47 +518,48 @@ bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t perf_count(_rtcm_stream_pub_perf); msg.data.clear(); } - - return result; + + return result; } bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len) { using uavcan::equipment::gnss::RTCMStream; - perf_count(_rtcm_perf); + perf_count(_rtcm_perf); RTCMStream msg; msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3; - const size_t capacity = msg.data.capacity(); - size_t written = 0; - bool result = true; - while (result && written < data_len) { - size_t chunk_size = data_len - written; - - if (chunk_size > capacity) { - chunk_size = capacity; - } - - for (size_t i = 0; i < chunk_size; ++i){ - msg.data.push_back(data[written]); - written += 1; - } - - result = _pub_rtcm.broadcast(msg) >= 0; - msg.data.clear(); - } - - return result; + const size_t capacity = msg.data.capacity(); + size_t written = 0; + bool result = true; + + while (result && written < data_len) { + size_t chunk_size = data_len - written; + + if (chunk_size > capacity) { + chunk_size = capacity; + } + + for (size_t i = 0; i < chunk_size; ++i) { + msg.data.push_back(data[written]); + written += 1; + } + + result = _pub_rtcm.broadcast(msg) >= 0; + msg.data.clear(); + } + + return result; } bool UavcanGnssBridge::PublishMovingBaselineData(const uint8_t *data, size_t data_len) { ardupilot::gnss::MovingBaselineData msg; - - const size_t capacity = msg.data.capacity(); + + const size_t capacity = msg.data.capacity(); size_t written = 0; bool result = true; diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 3a4e1205e17a..9f64621f5a7b 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -91,7 +91,7 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase const float heading_accuracy); void handleInjectDataTopic(); - bool injectData(const uint8_t *const data, const size_t data_len); + bool injectData(const uint8_t *const data, const size_t data_len); bool PublishRTCMStream(const uint8_t *data, size_t data_len); bool PublishMovingBaselineData(const uint8_t *data, size_t data_len); diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1601c4d4e6e1..5f5fa7a4e032 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(airspeed) +add_subdirectory(allocator_qp) add_subdirectory(avoidance) add_subdirectory(battery) add_subdirectory(bezier) diff --git a/src/lib/allocator_qp/CMakeLists.txt b/src/lib/allocator_qp/CMakeLists.txt new file mode 100644 index 000000000000..39bb3972e9b3 --- /dev/null +++ b/src/lib/allocator_qp/CMakeLists.txt @@ -0,0 +1,114 @@ + + +message(STATUS "*****************") +message(STATUS "*****************") +message(STATUS "COMPILING OSQP") + +# Detect operating system +# ---------------------------------------------- +message(STATUS "We are on a ${CMAKE_SYSTEM_NAME} system") +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + set(IS_LINUX ON) +elseif(${CMAKE_SYSTEM_NAME} STREQUAL "Darwin") + set(IS_MAC ON) +elseif(${CMAKE_SYSTEM_NAME} STREQUAL "Windows") + set(IS_WINDOWS ON) +endif() + + +set(EMBEDDED 2) +set(PRINTING OFF) +set(PROFILING OFF) +option(DFLOAT "Use floats instead of doubles" OFF) +option(DLONG "Use long (64bit) for indexing" OFF) # by default its ON + + +# set types for QDLDL +if(DFLOAT) + set(QDLDL_FLOAT_TYPE "float") +else() + set(QDLDL_FLOAT_TYPE "double") +endif() + +if(DLONG) + set(QDLDL_INT_TYPE "long long") +else() + set(QDLDL_INT_TYPE "int") +endif() + + +set(QDLDL_BOOL_TYPE "unsigned char") + +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure/qdldl_types.h.in + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_types.h + NEWLINE_STYLE LF) + +# Include math library if EMBEDDED != 1 +if(NOT (EMBEDDED EQUAL 1)) + set(CMAKE_C_STANDARD_LIBRARIES "${CMAKE_C_STANDARD_LIBRARIES} -lm") +endif() +# Include real time library in linux +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + set(CMAKE_C_STANDARD_LIBRARIES "${CMAKE_C_STANDARD_LIBRARIES} -lrt") +endif() + +# Generate header file with the global options +# --------------------------------------------- +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure/osqp_configure.h.in + ${CMAKE_CURRENT_SOURCE_DIR}/include/osqp_configure.h + NEWLINE_STYLE LF) + +# Include header directory +# ---------------------------------------------- +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + + +# Set sources +# ---------------------------------------------- +add_subdirectory (src/osqp) +add_subdirectory (include) + +# Append the generated workspace files and qdldl files +list (APPEND + osqp_src + ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/workspace.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/qdldl.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/qdldl_interface.c +) + +list (APPEND + osqp_headers + ${CMAKE_CURRENT_SOURCE_DIR}/include/workspace.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_types.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_interface.h +) + + + +px4_add_library(allocator_qp + ${osqp_src} + ${osqp_headers}) + + +message(STATUS "DONE COMPILING OSQP") +message(STATUS "*****************") +message(STATUS "*****************") + +# set(SRCS +# conversions.c +# conversions.h +# crc.c +# crc.h +# mavlink_log.cpp +# mavlink_log.h +# ) +# +# if(${PX4_PLATFORM} STREQUAL "nuttx") +# list(APPEND SRCS +# otp.c +# ) +# endif() +# +# px4_add_library(systemlib ${SRCS}) +# target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/lib/allocator_qp/_CMakeLists.txt b/src/lib/allocator_qp/_CMakeLists.txt new file mode 100644 index 000000000000..1a75a90a36cb --- /dev/null +++ b/src/lib/allocator_qp/_CMakeLists.txt @@ -0,0 +1,143 @@ +# Minimum version required +cmake_minimum_required (VERSION 3.5) + +# Project name +project (osqp) + + +# Set the output folder where your program will be created +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/out) +set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/out) + + +# Detect operating system +# ---------------------------------------------- +message(STATUS "We are on a ${CMAKE_SYSTEM_NAME} system") +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + set(IS_LINUX ON) +elseif(${CMAKE_SYSTEM_NAME} STREQUAL "Darwin") + set(IS_MAC ON) +elseif(${CMAKE_SYSTEM_NAME} STREQUAL "Windows") + set(IS_WINDOWS ON) +endif() + + +# Set options +# ---------------------------------------------- + + +# Is the code generated for embedded platforms? +# 1 : Yes. Matrix update not allowed. +# 2 : Yes. Matrix update allowed. + +if (NOT DEFINED EMBEDDED) # enable embedded anyway + set (EMBEDDED 2) +endif() + +message(STATUS "Embedded is ${EMBEDDED}") +message(STATUS "Passing EMBEDDED flag to compiler") + +# Is printing enabled? +option (PRINTING "Enable solver printing" ON) +if (DEFINED EMBEDDED) + message(STATUS "Disabling printing for embedded") + set(PRINTING OFF) +endif() +message(STATUS "Printing is ${PRINTING}") + + +# Is profiling enabled? +option (PROFILING "Enable solver profiling (timing)" ON) +if (DEFINED EMBEDDED) + message(STATUS "Disabling profiling for embedded") + set(PROFILING OFF) +endif() +message(STATUS "Profiling is ${PROFILING}") + +# Use floats instead of integers +option (DFLOAT "Use float numbers instead of doubles" OFF) +message(STATUS "Floats are ${DFLOAT}") + +# Use long integers for indexing +option (DLONG "Use long integers (64bit) for indexing" ON) +if (NOT (CMAKE_SIZEOF_VOID_P EQUAL 8)) + message(STATUS "Disabling long integers (64bit) on 32bit machine") + set(DLONG OFF) +endif() +message(STATUS "Long integers (64bit) are ${DLONG}") + +# Types for QDLDL +# ---------------------------------------------- +if(DFLOAT) + set(QDLDL_FLOAT_TYPE "float") +else() + set(QDLDL_FLOAT_TYPE "double") +endif() + +if(DLONG) + set(QDLDL_INT_TYPE "long long") +else() + set(QDLDL_INT_TYPE "int") +endif() + +# boolean type is always unsigned char for +# now, since _Bool does not exist in C89 +set(QDLDL_BOOL_TYPE "unsigned char") + +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure/qdldl_types.h.in + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_types.h + NEWLINE_STYLE LF) + +# Set Compiler flags +# ---------------------------------------------- +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") +set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -O0 -g") +set(CMAKE_POSITION_INDEPENDENT_CODE ON) # -fPIC + +# Include math library if EMBEDDED != 1 +if(NOT (EMBEDDED EQUAL 1)) + set(CMAKE_C_STANDARD_LIBRARIES "${CMAKE_C_STANDARD_LIBRARIES} -lm") +endif() +# Include real time library in linux +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + set(CMAKE_C_STANDARD_LIBRARIES "${CMAKE_C_STANDARD_LIBRARIES} -lrt") +endif() + +# Generate header file with the global options +# --------------------------------------------- +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure/osqp_configure.h.in + ${CMAKE_CURRENT_SOURCE_DIR}/include/osqp_configure.h + NEWLINE_STYLE LF) + +# Include header directory +# ---------------------------------------------- +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + + +# Set sources +# ---------------------------------------------- +add_subdirectory (src/osqp) +add_subdirectory (include) + +# Append the generated workspace files and qdldl files +list (APPEND + osqp_src + ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/workspace.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/qdldl.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/qdldl_interface.c +) + +list (APPEND + osqp_headers + ${CMAKE_CURRENT_SOURCE_DIR}/include/workspace.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_types.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_interface.h +) + +# Create static library for embedded solver +add_library (emosqpstatic STATIC ${osqp_src} ${osqp_headers}) + +# Create example executable +add_executable (example ${PROJECT_SOURCE_DIR}/src/example.c) +target_link_libraries (example emosqpstatic) diff --git a/src/lib/allocator_qp/configure/osqp_configure.h.in b/src/lib/allocator_qp/configure/osqp_configure.h.in new file mode 100644 index 000000000000..6df0ed50f598 --- /dev/null +++ b/src/lib/allocator_qp/configure/osqp_configure.h.in @@ -0,0 +1,49 @@ +#ifndef OSQP_CONFIGURE_H +# define OSQP_CONFIGURE_H + +# ifdef __cplusplus +extern "C" { +# endif /* ifdef __cplusplus */ + +/* DEBUG */ +#cmakedefine DEBUG + +/* Operating system */ +#cmakedefine IS_LINUX +#cmakedefine IS_MAC +#cmakedefine IS_WINDOWS + +/* EMBEDDED */ +#cmakedefine EMBEDDED (@EMBEDDED@) + +/* PRINTING */ +#cmakedefine PRINTING + +/* PROFILING */ +#cmakedefine PROFILING + +/* CTRLC */ +#cmakedefine CTRLC + +/* DFLOAT */ +#cmakedefine DFLOAT + +/* DLONG */ +#cmakedefine DLONG + +/* ENABLE_MKL_PARDISO */ +#cmakedefine ENABLE_MKL_PARDISO + +/* MEMORY MANAGEMENT */ +#cmakedefine OSQP_CUSTOM_MEMORY +#ifdef OSQP_CUSTOM_MEMORY +#include "@OSQP_CUSTOM_MEMORY@" +#endif + + + +# ifdef __cplusplus +} +# endif /* ifdef __cplusplus */ + +#endif /* ifndef OSQP_CONFIGURE_H */ diff --git a/src/lib/allocator_qp/configure/qdldl_types.h.in b/src/lib/allocator_qp/configure/qdldl_types.h.in new file mode 100644 index 000000000000..ac6b2c69a353 --- /dev/null +++ b/src/lib/allocator_qp/configure/qdldl_types.h.in @@ -0,0 +1,23 @@ +#ifndef QDLDL_TYPES_H +# define QDLDL_TYPES_H + +# ifdef __cplusplus +extern "C" { +# endif /* ifdef __cplusplus */ + +#include //for the QDLDL_INT_TYPE_MAX + +// QDLDL integer and float types + +typedef @QDLDL_INT_TYPE@ QDLDL_int; /* for indices */ +typedef @QDLDL_FLOAT_TYPE@ QDLDL_float; /* for numerical values */ +typedef @QDLDL_BOOL_TYPE@ QDLDL_bool; /* for boolean values */ + +//Maximum value of the signed type QDLDL_int. +#define QDLDL_INT_MAX @QDLDL_INT_TYPE_MAX@ + +# ifdef __cplusplus +} +# endif /* ifdef __cplusplus */ + +#endif /* ifndef QDLDL_TYPES_H */ diff --git a/src/lib/allocator_qp/include/CMakeLists.txt b/src/lib/allocator_qp/include/CMakeLists.txt new file mode 100644 index 000000000000..b4ed773f47d7 --- /dev/null +++ b/src/lib/allocator_qp/include/CMakeLists.txt @@ -0,0 +1,51 @@ +# Add the OSQP headers +set( + osqp_headers + "${CMAKE_CURRENT_SOURCE_DIR}/auxil.h" + "${CMAKE_CURRENT_SOURCE_DIR}/constants.h" + "${CMAKE_CURRENT_SOURCE_DIR}/error.h" + "${CMAKE_CURRENT_SOURCE_DIR}/glob_opts.h" + "${CMAKE_CURRENT_SOURCE_DIR}/lin_alg.h" + "${CMAKE_CURRENT_SOURCE_DIR}/osqp.h" + "${CMAKE_CURRENT_SOURCE_DIR}/osqp_configure.h" + "${CMAKE_CURRENT_SOURCE_DIR}/proj.h" + "${CMAKE_CURRENT_SOURCE_DIR}/scaling.h" + "${CMAKE_CURRENT_SOURCE_DIR}/types.h" + "${CMAKE_CURRENT_SOURCE_DIR}/util.h" +) + +# Add the KKT update only in normal mode and matrix-updating embedded mode (not mode 1) +if (NOT (EMBEDDED EQUAL 1)) + list( + APPEND + osqp_src + "${CMAKE_CURRENT_SOURCE_DIR}/kkt.h" + ) +endif() + +# Add more files that should only be in non-embedded code +if (NOT DEFINED EMBEDDED) + list( + APPEND + osqp_headers + "${CMAKE_CURRENT_SOURCE_DIR}/cs.h" + "${CMAKE_CURRENT_SOURCE_DIR}/polish.h" + "${CMAKE_CURRENT_SOURCE_DIR}/lin_sys.h" + ) +endif() + +# Add the ctrl-c handler if enabled +if (CTRLC) + list( + APPEND + osqp_headers + "${CMAKE_CURRENT_SOURCE_DIR}/ctrlc.h" + ) +endif() + +# Pass the header list up to the main CMakeLists scope +set( + osqp_headers + "${osqp_headers}" + PARENT_SCOPE +) diff --git a/src/lib/allocator_qp/include/auxil.h b/src/lib/allocator_qp/include/auxil.h new file mode 100644 index 000000000000..d756c2a49fd4 --- /dev/null +++ b/src/lib/allocator_qp/include/auxil.h @@ -0,0 +1,181 @@ +#ifndef AUXIL_H +# define AUXIL_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "types.h" + + +/*********************************************************** +* Auxiliary functions needed to compute ADMM iterations * * +***********************************************************/ +# if EMBEDDED != 1 + +/** + * Compute rho estimate from residuals + * @param work Workspace + * @return rho estimate + */ +c_float compute_rho_estimate(OSQPWorkspace *work); + +/** + * Adapt rho value based on current unscaled primal/dual residuals + * @param work Workspace + * @return Exitflag + */ +c_int adapt_rho(OSQPWorkspace *work); + +/** + * Set values of rho vector based on constraint types + * @param work Workspace + */ +void set_rho_vec(OSQPWorkspace *work); + +/** + * Update values of rho vector based on updated constraints. + * If the constraints change, update the linear systems solver. + * + * @param work Workspace + * @return Exitflag + */ +c_int update_rho_vec(OSQPWorkspace *work); + +# endif // EMBEDDED + +/** + * Swap c_float vector pointers + * @param a first vector + * @param b second vector + */ +void swap_vectors(c_float **a, + c_float **b); + + +/** + * Cold start workspace variables xz and y + * @param work Workspace + */ +void cold_start(OSQPWorkspace *work); + + +/** + * Update x_tilde and z_tilde variable (first ADMM step) + * @param work [description] + */ +void update_xz_tilde(OSQPWorkspace *work); + + +/** + * Update x (second ADMM step) + * Update also delta_x (For for dual infeasibility) + * @param work Workspace + */ +void update_x(OSQPWorkspace *work); + + +/** + * Update z (third ADMM step) + * @param work Workspace + */ +void update_z(OSQPWorkspace *work); + + +/** + * Update y variable (fourth ADMM step) + * Update also delta_y to check for primal infeasibility + * @param work Workspace + */ +void update_y(OSQPWorkspace *work); + + +/** + * Compute objective function from data at value x + * @param work OSQPWorkspace structure + * @param x Value x + * @return Objective function value + */ +c_float compute_obj_val(OSQPWorkspace *work, + c_float *x); + +/** + * Check whether QP has solution + * @param info OSQPInfo + */ +c_int has_solution(OSQPInfo *info); + +/** + * Store the QP solution + * @param work Workspace + */ +void store_solution(OSQPWorkspace *work); + + +/** + * Update solver information + * @param work Workspace + * @param iter Iteration number + * @param compute_objective Boolean (if compute the objective or not) + * @param polish Boolean (if called from polish) + */ +void update_info(OSQPWorkspace *work, + c_int iter, + c_int compute_objective, + c_int polish); + + +/** + * Reset solver information (after problem updates) + * @param info Information structure + */ +void reset_info(OSQPInfo *info); + + +/** + * Update solver status (value and string) + * @param info OSQPInfo + * @param status_val new status value + */ +void update_status(OSQPInfo *info, + c_int status_val); + + +/** + * Check if termination conditions are satisfied + * If the boolean flag is ON, it checks for approximate conditions (10 x larger + * tolerances than the ones set) + * + * @param work Workspace + * @param approximate Boolean + * @return Residuals check + */ +c_int check_termination(OSQPWorkspace *work, + c_int approximate); + + +# ifndef EMBEDDED + +/** + * Validate problem data + * @param data OSQPData to be validated + * @return Exitflag to check + */ +c_int validate_data(const OSQPData *data); + + +/** + * Validate problem settings + * @param settings OSQPSettings to be validated + * @return Exitflag to check + */ +c_int validate_settings(const OSQPSettings *settings); + + +# endif // #ifndef EMBEDDED + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef AUXIL_H diff --git a/src/lib/allocator_qp/include/constants.h b/src/lib/allocator_qp/include/constants.h new file mode 100644 index 000000000000..872276479491 --- /dev/null +++ b/src/lib/allocator_qp/include/constants.h @@ -0,0 +1,129 @@ +#ifndef CONSTANTS_H +# define CONSTANTS_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + + +/******************* +* OSQP Versioning * +*******************/ +# define OSQP_VERSION ("0.6.2") /* string literals automatically null-terminated + */ + +/****************** +* Solver Status * +******************/ +# define OSQP_DUAL_INFEASIBLE_INACCURATE (4) +# define OSQP_PRIMAL_INFEASIBLE_INACCURATE (3) +# define OSQP_SOLVED_INACCURATE (2) +# define OSQP_SOLVED (1) +# define OSQP_MAX_ITER_REACHED (-2) +# define OSQP_PRIMAL_INFEASIBLE (-3) /* primal infeasible */ +# define OSQP_DUAL_INFEASIBLE (-4) /* dual infeasible */ +# define OSQP_SIGINT (-5) /* interrupted by user */ +# ifdef PROFILING +# define OSQP_TIME_LIMIT_REACHED (-6) +# endif // ifdef PROFILING +# define OSQP_NON_CVX (-7) /* problem non convex */ +# define OSQP_UNSOLVED (-10) /* Unsolved. Only setup function has been called */ + + +/************************* +* Linear System Solvers * +*************************/ +enum linsys_solver_type { QDLDL_SOLVER, MKL_PARDISO_SOLVER }; +extern const char * LINSYS_SOLVER_NAME[]; + + +/****************** +* Solver Errors * +******************/ +enum osqp_error_type { + OSQP_DATA_VALIDATION_ERROR = 1, /* Start errors from 1 */ + OSQP_SETTINGS_VALIDATION_ERROR, + OSQP_LINSYS_SOLVER_LOAD_ERROR, + OSQP_LINSYS_SOLVER_INIT_ERROR, + OSQP_NONCVX_ERROR, + OSQP_MEM_ALLOC_ERROR, + OSQP_WORKSPACE_NOT_INIT_ERROR, +}; +extern const char * OSQP_ERROR_MESSAGE[]; + + +/********************************** +* Solver Parameters and Settings * +**********************************/ + +# define RHO (0.1) +# define SIGMA (1E-06) +# define MAX_ITER (4000) +# define EPS_ABS (1E-3) +# define EPS_REL (1E-3) +# define EPS_PRIM_INF (1E-4) +# define EPS_DUAL_INF (1E-4) +# define ALPHA (1.6) +# define LINSYS_SOLVER (QDLDL_SOLVER) + +# define RHO_MIN (1e-06) +# define RHO_MAX (1e06) +# define RHO_EQ_OVER_RHO_INEQ (1e03) +# define RHO_TOL (1e-04) ///< tolerance for detecting if an inequality is set to equality + + +# ifndef EMBEDDED +# define DELTA (1E-6) +# define POLISH (0) +# define POLISH_REFINE_ITER (3) +# define VERBOSE (1) +# endif // ifndef EMBEDDED + +# define SCALED_TERMINATION (0) +# define CHECK_TERMINATION (25) +# define WARM_START (1) +# define SCALING (10) + +# define MIN_SCALING (1e-04) ///< minimum scaling value +# define MAX_SCALING (1e+04) ///< maximum scaling value + + +# ifndef OSQP_NULL +# define OSQP_NULL 0 +# endif /* ifndef OSQP_NULL */ + +# ifndef OSQP_NAN +# define OSQP_NAN ((c_float)0x7fc00000UL) // not a number +# endif /* ifndef OSQP_NAN */ + +# ifndef OSQP_INFTY +# define OSQP_INFTY ((c_float)1e30) // infinity +# endif /* ifndef OSQP_INFTY */ + +# ifndef OSQP_DIVISION_TOL +# define OSQP_DIVISION_TOL ((c_float)1.0 / OSQP_INFTY) +# endif /* ifndef OSQP_DIVISION_TOL */ + + +# if EMBEDDED != 1 +# define ADAPTIVE_RHO (1) +# define ADAPTIVE_RHO_INTERVAL (0) +# define ADAPTIVE_RHO_FRACTION (0.4) ///< fraction of setup time after which we update rho +# define ADAPTIVE_RHO_MULTIPLE_TERMINATION (4) ///< multiple of check_termination after which we update rho (if PROFILING disabled) +# define ADAPTIVE_RHO_FIXED (100) ///< number of iterations after which we update rho if termination_check and PROFILING are disabled +# define ADAPTIVE_RHO_TOLERANCE (5) ///< tolerance for adopting new rho; minimum ratio between new rho and the current one +# endif // if EMBEDDED != 1 + +# ifdef PROFILING +# define TIME_LIMIT (0) ///< Disable time limit as default +# endif // ifdef PROFILING + +/* Printing */ +# define PRINT_INTERVAL 200 + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef CONSTANTS_H diff --git a/src/lib/allocator_qp/include/error.h b/src/lib/allocator_qp/include/error.h new file mode 100644 index 000000000000..9d7879f03d49 --- /dev/null +++ b/src/lib/allocator_qp/include/error.h @@ -0,0 +1,38 @@ +#ifndef ERROR_H +# define ERROR_H + +/* OSQP error handling */ + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "types.h" + + +/* OSQP error macro */ +# if __STDC_VERSION__ >= 199901L +/* The C99 standard gives the __func__ macro, which is preferred over __FUNCTION__ */ +# define osqp_error(error_code) _osqp_error(error_code, __func__); +#else +# define osqp_error(error_code) _osqp_error(error_code, __FUNCTION__); +#endif + + + +/** + * Internal function to print error description and return error code. + * @param Error code + * @param Function name + * @return Error code + */ + c_int _osqp_error(enum osqp_error_type error_code, + const char * function_name); + + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef ERROR_H diff --git a/src/lib/allocator_qp/include/glob_opts.h b/src/lib/allocator_qp/include/glob_opts.h new file mode 100644 index 000000000000..e2b5b2474457 --- /dev/null +++ b/src/lib/allocator_qp/include/glob_opts.h @@ -0,0 +1,167 @@ +#ifndef GLOB_OPTS_H +# define GLOB_OPTS_H + +# ifdef __cplusplus +extern "C" { +# endif /* ifdef __cplusplus */ + +/* + Define OSQP compiler flags + */ + +// cmake generated compiler flags +#include "osqp_configure.h" + +/* DATA CUSTOMIZATIONS (depending on memory manager)----------------------- */ + +// We do not need memory allocation functions if EMBEDDED is enabled +# ifndef EMBEDDED + +/* define custom printfs and memory allocation (e.g. matlab/python) */ +# ifdef MATLAB + # include "mex.h" +static void* c_calloc(size_t num, size_t size) { + void *m = mxCalloc(num, size); + mexMakeMemoryPersistent(m); + return m; +} + +static void* c_malloc(size_t size) { + void *m = mxMalloc(size); + mexMakeMemoryPersistent(m); + return m; +} + +static void* c_realloc(void *ptr, size_t size) { + void *m = mxRealloc(ptr, size); + mexMakeMemoryPersistent(m); + return m; +} + # define c_free mxFree +# elif defined PYTHON +// Define memory allocation for python. Note that in Python 2 memory manager +// Calloc is not implemented +# include +# if PY_MAJOR_VERSION >= 3 +// https://docs.python.org/3/c-api/memory.html +// The following function sets are wrappers to the system allocator. These functions are thread-safe, the GIL does not need to be held. +// The default raw memory allocator uses the following functions: malloc(), calloc(), realloc() and free(); call malloc(1) (or calloc(1, 1)) when requesting zero bytes. +# define c_malloc PyMem_RawMalloc +# define c_calloc PyMem_RawCalloc +# define c_free PyMem_RawFree +# define c_realloc PyMem_RawRealloc +# else /* if PY_MAJOR_VERSION >= 3 */ +# define c_malloc PyMem_Malloc +# define c_free PyMem_Free +# define c_realloc PyMem_Realloc +static void* c_calloc(size_t num, size_t size) { + void *m = PyMem_Malloc(num * size); + memset(m, 0, num * size); + return m; +} +# endif /* if PY_MAJOR_VERSION >= 3 */ + +# elif !defined OSQP_CUSTOM_MEMORY +/* If no custom memory allocator defined, use + * standard linux functions. Custom memory allocator definitions + * appear in the osqp_configure.h generated file. */ + # include + # define c_malloc malloc + # define c_calloc calloc + # define c_free free + # define c_realloc realloc +# endif /* ifdef MATLAB */ + +# endif // end ifndef EMBEDDED + + +/* Use customized number representation ----------------------------------- */ +# ifdef DLONG // long integers +typedef long long c_int; /* for indices */ +# else // standard integers +typedef int c_int; /* for indices */ +# endif /* ifdef DLONG */ + + +# ifndef DFLOAT // Doubles +typedef double c_float; /* for numerical values */ +# else // Floats +typedef float c_float; /* for numerical values */ +# endif /* ifndef DFLOAT */ + + +/* Use customized operations */ + +# ifndef c_absval +# define c_absval(x) (((x) < 0) ? -(x) : (x)) +# endif /* ifndef c_absval */ + +# ifndef c_max +# define c_max(a, b) (((a) > (b)) ? (a) : (b)) +# endif /* ifndef c_max */ + +# ifndef c_min +# define c_min(a, b) (((a) < (b)) ? (a) : (b)) +# endif /* ifndef c_min */ + +// Round x to the nearest multiple of N +# ifndef c_roundmultiple +# define c_roundmultiple(x, N) ((x) + .5 * (N)-c_fmod((x) + .5 * (N), (N))) +# endif /* ifndef c_roundmultiple */ + + +/* Use customized functions ----------------------------------------------- */ + +# if EMBEDDED != 1 + +# include +# ifndef DFLOAT // Doubles +# define c_sqrt sqrt +# define c_fmod fmod +# else // Floats +# define c_sqrt sqrtf +# define c_fmod fmodf +# endif /* ifndef DFLOAT */ + +# endif // end EMBEDDED + +# ifdef PRINTING +# include +# include + +/* informational print function */ +# ifdef MATLAB +# define c_print mexPrintf +# elif defined PYTHON +# include +# define c_print(...) \ + { \ + PyGILState_STATE gilstate = PyGILState_Ensure(); \ + PySys_WriteStdout(__VA_ARGS__); \ + PyGILState_Release(gilstate); \ + } +# elif defined R_LANG +# include +# define c_print Rprintf +# else /* ifdef MATLAB */ +# define c_print printf +# endif /* c_print configuration */ + +/* error printing function */ +# ifdef R_LANG + /* Some CRAN builds complain about __VA_ARGS__, so just print */ + /* out the error messages on R without the __FUNCTION__ trace */ +# define c_eprint Rprintf +# else +# define c_eprint(...) c_print("ERROR in %s: ", __FUNCTION__); \ + c_print(__VA_ARGS__); c_print("\n"); +# endif /* c_eprint configuration */ + +# endif /* PRINTING */ + + +# ifdef __cplusplus +} +# endif /* ifdef __cplusplus */ + +#endif /* ifndef GLOB_OPTS_H */ diff --git a/src/lib/allocator_qp/include/kkt.h b/src/lib/allocator_qp/include/kkt.h new file mode 100644 index 000000000000..9560d5ecbf4e --- /dev/null +++ b/src/lib/allocator_qp/include/kkt.h @@ -0,0 +1,109 @@ +#ifndef KKT_H +# define KKT_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "types.h" + +# ifndef EMBEDDED + +# include "cs.h" + +/** + * Form square symmetric KKT matrix of the form + * + * [P + param1 I, A'; + * A -diag(param2)] + * + * NB: Only the upper triangular part is stuffed! + * + * + * If Pdiag_idx is not OSQP_NULL, it saves the index of the diagonal + * elements of P there and the number of diagonal elements in Pdiag_n. + * + * Similarly, if rhotoKKT is not null, + * it saves where the values of param2 go in the final KKT matrix + * + * NB: Pdiag_idx needs to be freed! + * + * @param P cost matrix (already just upper triangular part) + * @param A linear constraint matrix + * @param format CSC (0) or CSR (1) + * @param param1 regularization parameter + * @param param2 regularization parameter (vector) + * @param PtoKKT (modified) index mapping from elements of P to KKT matrix + * @param AtoKKT (modified) index mapping from elements of A to KKT matrix + * @param Pdiag_idx (modified) Address of the index of diagonal elements in P + * @param Pdiag_n (modified) Address to the number of diagonal elements in P + * @param param2toKKT (modified) index mapping from param2 to elements of + *KKT + * @return return status flag + */ +csc* form_KKT(const csc *P, + const csc *A, + c_int format, + c_float param1, + c_float *param2, + c_int *PtoKKT, + c_int *AtoKKT, + c_int **Pdiag_idx, + c_int *Pdiag_n, + c_int *param2toKKT); +# endif // ifndef EMBEDDED + + +# if EMBEDDED != 1 + +/** + * Update KKT matrix using the elements of P + * + * @param KKT KKT matrix in CSC form (upper-triangular) + * @param P P matrix in CSC form (upper-triangular) + * @param PtoKKT Vector of pointers from P->x to KKT->x + * @param param1 Parameter added to the diagonal elements of P + * @param Pdiag_idx Index of diagonal elements in P->x + * @param Pdiag_n Number of diagonal elements of P + */ +void update_KKT_P(csc *KKT, + const csc *P, + const c_int *PtoKKT, + const c_float param1, + const c_int *Pdiag_idx, + const c_int Pdiag_n); + + +/** + * Update KKT matrix using the elements of A + * + * @param KKT KKT matrix in CSC form (upper-triangular) + * @param A A matrix in CSC form (upper-triangular) + * @param AtoKKT Vector of pointers from A->x to KKT->x + */ +void update_KKT_A(csc *KKT, + const csc *A, + const c_int *AtoKKT); + + +/** + * Update KKT matrix with new param2 + * + * @param KKT KKT matrix + * @param param2 Parameter of the KKT matrix (vector) + * @param param2toKKT index where param2 enters in the KKT matrix + * @param m number of constraints + */ +void update_KKT_param2(csc *KKT, + const c_float *param2, + const c_int *param2toKKT, + const c_int m); + +# endif // EMBEDDED != 1 + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef KKT_H diff --git a/src/lib/allocator_qp/include/lin_alg.h b/src/lib/allocator_qp/include/lin_alg.h new file mode 100644 index 000000000000..e9589e9d7b71 --- /dev/null +++ b/src/lib/allocator_qp/include/lin_alg.h @@ -0,0 +1,216 @@ +#ifndef LIN_ALG_H +# define LIN_ALG_H + + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "types.h" + + +/* VECTOR FUNCTIONS ----------------------------------------------------------*/ + +# ifndef EMBEDDED + +/* copy vector a into output (Uses MALLOC)*/ +c_float* vec_copy(c_float *a, + c_int n); +# endif // ifndef EMBEDDED + +/* copy vector a into preallocated vector b */ +void prea_vec_copy(const c_float *a, + c_float *b, + c_int n); + +/* copy integer vector a into preallocated vector b */ +void prea_int_vec_copy(const c_int *a, + c_int *b, + c_int n); + +/* set float vector to scalar */ +void vec_set_scalar(c_float *a, + c_float sc, + c_int n); + +/* set integer vector to scalar */ +void int_vec_set_scalar(c_int *a, + c_int sc, + c_int n); + +/* add scalar to vector*/ +void vec_add_scalar(c_float *a, + c_float sc, + c_int n); + +/* multiply scalar to vector */ +void vec_mult_scalar(c_float *a, + c_float sc, + c_int n); + +/* c = a + sc*b */ +void vec_add_scaled(c_float *c, + const c_float *a, + const c_float *b, + c_int n, + c_float sc); + +/* ||v||_inf */ +c_float vec_norm_inf(const c_float *v, + c_int l); + +/* ||Sv||_inf */ +c_float vec_scaled_norm_inf(const c_float *S, + const c_float *v, + c_int l); + +/* ||a - b||_inf */ +c_float vec_norm_inf_diff(const c_float *a, + const c_float *b, + c_int l); + +/* mean of vector elements */ +c_float vec_mean(const c_float *a, + c_int n); + +# if EMBEDDED != 1 + +/* Vector elementwise reciprocal b = 1./a (needed for scaling)*/ +void vec_ew_recipr(const c_float *a, + c_float *b, + c_int n); +# endif // if EMBEDDED != 1 + +/* Inner product a'b */ +c_float vec_prod(const c_float *a, + const c_float *b, + c_int n); + +/* Elementwise product a.*b stored in c*/ +void vec_ew_prod(const c_float *a, + const c_float *b, + c_float *c, + c_int n); + +# if EMBEDDED != 1 + +/* Elementwise sqrt of the vector elements */ +void vec_ew_sqrt(c_float *a, + c_int n); + +/* Elementwise max between each vector component and max_val */ +void vec_ew_max(c_float *a, + c_int n, + c_float max_val); + +/* Elementwise min between each vector component and max_val */ +void vec_ew_min(c_float *a, + c_int n, + c_float min_val); + +/* Elementwise maximum between vectors c = max(a, b) */ +void vec_ew_max_vec(const c_float *a, + const c_float *b, + c_float *c, + c_int n); + +/* Elementwise minimum between vectors c = min(a, b) */ +void vec_ew_min_vec(const c_float *a, + const c_float *b, + c_float *c, + c_int n); + +# endif // if EMBEDDED != 1 + + +/* MATRIX FUNCTIONS ----------------------------------------------------------*/ + +/* multiply scalar to matrix */ +void mat_mult_scalar(csc *A, + c_float sc); + +/* Premultiply matrix A by diagonal matrix with diagonal d, + i.e. scale the rows of A by d + */ +void mat_premult_diag(csc *A, + const c_float *d); + +/* Premultiply matrix A by diagonal matrix with diagonal d, + i.e. scale the columns of A by d + */ +void mat_postmult_diag(csc *A, + const c_float *d); + + +/* Matrix-vector multiplication + * y = A*x (if plus_eq == 0) + * y += A*x (if plus_eq == 1) + * y -= A*x (if plus_eq == -1) + */ +void mat_vec(const csc *A, + const c_float *x, + c_float *y, + c_int plus_eq); + + +/* Matrix-transpose-vector multiplication + * y = A'*x (if plus_eq == 0) + * y += A'*x (if plus_eq == 1) + * y -= A'*x (if plus_eq == -1) + * If skip_diag == 1, then diagonal elements of A are assumed to be zero. + */ +void mat_tpose_vec(const csc *A, + const c_float *x, + c_float *y, + c_int plus_eq, + c_int skip_diag); + + +# if EMBEDDED != 1 + +/** + * Infinity norm of each matrix column + * @param M Input matrix + * @param E Vector of infinity norms + * + */ +void mat_inf_norm_cols(const csc *M, + c_float *E); + +/** + * Infinity norm of each matrix row + * @param M Input matrix + * @param E Vector of infinity norms + * + */ +void mat_inf_norm_rows(const csc *M, + c_float *E); + +/** + * Infinity norm of each matrix column + * Matrix M is symmetric upper-triangular + * + * @param M Input matrix (symmetric, upper-triangular) + * @param E Vector of infinity norms + * + */ +void mat_inf_norm_cols_sym_triu(const csc *M, + c_float *E); + +# endif // EMBEDDED != 1 + +/** + * Compute quadratic form f(x) = 1/2 x' P x + * @param P quadratic matrix in CSC form (only upper triangular) + * @param x argument float vector + * @return quadratic form value + */ +c_float quad_form(const csc *P, + const c_float *x); + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef LIN_ALG_H diff --git a/src/lib/allocator_qp/include/osqp.h b/src/lib/allocator_qp/include/osqp.h new file mode 100644 index 000000000000..296aff82deac --- /dev/null +++ b/src/lib/allocator_qp/include/osqp.h @@ -0,0 +1,430 @@ +#ifndef OSQP_H +# define OSQP_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +/* Includes */ +# include "types.h" +# include "util.h" // Needed for osqp_set_default_settings functions + + +// Library to deal with sparse matrices enabled only if embedded not defined +# ifndef EMBEDDED +# include "cs.h" +# endif // ifndef EMBEDDED + +/******************** +* Main Solver API * +********************/ + +/** + * @name Main solver API + * @{ + */ + +/** + * Set default settings from constants.h file + * assumes settings already allocated in memory + * @param settings settings structure + */ +void osqp_set_default_settings(OSQPSettings *settings); + + +# ifndef EMBEDDED + +/** + * Initialize OSQP solver allocating memory. + * + * All the inputs must be already allocated in memory before calling. + * + * It performs: + * - data and settings validation + * - problem data scaling + * - automatic parameters tuning (if enabled) + * - setup linear system solver: + * - direct solver: KKT matrix factorization is performed here + * - indirect solver: KKT matrix preconditioning is performed here + * + * NB: This is the only function that allocates dynamic memory and is not used + *during code generation + * + * @param workp Solver workspace pointer + * @param data Problem data + * @param settings Solver settings + * @return Exitflag for errors (0 if no errors) + */ +c_int osqp_setup(OSQPWorkspace** workp, const OSQPData* data, const OSQPSettings* settings); + +# endif // #ifndef EMBEDDED + +/** + * Solve quadratic program + * + * The final solver information is stored in the \a work->info structure + * + * The solution is stored in the \a work->solution structure + * + * If the problem is primal infeasible, the certificate is stored + * in \a work->delta_y + * + * If the problem is dual infeasible, the certificate is stored in \a + * work->delta_x + * + * @param work Workspace allocated + * @return Exitflag for errors + */ +c_int osqp_solve(OSQPWorkspace *work); + + +# ifndef EMBEDDED + +/** + * Cleanup workspace by deallocating memory + * + * This function is not used in code generation + * @param work Workspace + * @return Exitflag for errors + */ +c_int osqp_cleanup(OSQPWorkspace *work); + +# endif // ifndef EMBEDDED + +/** @} */ + + +/******************************************** +* Sublevel API * +* * +* Edit data without performing setup again * +********************************************/ + +/** + * @name Sublevel API + * @{ + */ + +/** + * Update linear cost in the problem + * @param work Workspace + * @param q_new New linear cost + * @return Exitflag for errors and warnings + */ +c_int osqp_update_lin_cost(OSQPWorkspace *work, + const c_float *q_new); + + +/** + * Update lower and upper bounds in the problem constraints + * @param work Workspace + * @param l_new New lower bound + * @param u_new New upper bound + * @return Exitflag: 1 if new lower bound is not <= than new upper bound + */ +c_int osqp_update_bounds(OSQPWorkspace *work, + const c_float *l_new, + const c_float *u_new); + + +/** + * Update lower bound in the problem constraints + * @param work Workspace + * @param l_new New lower bound + * @return Exitflag: 1 if new lower bound is not <= than upper bound + */ +c_int osqp_update_lower_bound(OSQPWorkspace *work, + const c_float *l_new); + + +/** + * Update upper bound in the problem constraints + * @param work Workspace + * @param u_new New upper bound + * @return Exitflag: 1 if new upper bound is not >= than lower bound + */ +c_int osqp_update_upper_bound(OSQPWorkspace *work, + const c_float *u_new); + + +/** + * Warm start primal and dual variables + * @param work Workspace structure + * @param x Primal variable + * @param y Dual variable + * @return Exitflag + */ +c_int osqp_warm_start(OSQPWorkspace *work, + const c_float *x, + const c_float *y); + + +/** + * Warm start primal variable + * @param work Workspace structure + * @param x Primal variable + * @return Exitflag + */ +c_int osqp_warm_start_x(OSQPWorkspace *work, + const c_float *x); + + +/** + * Warm start dual variable + * @param work Workspace structure + * @param y Dual variable + * @return Exitflag + */ +c_int osqp_warm_start_y(OSQPWorkspace *work, + const c_float *y); + + +# if EMBEDDED != 1 + +/** + * Update elements of matrix P (upper triangular) + * without changing sparsity structure. + * + * + * If Px_new_idx is OSQP_NULL, Px_new is assumed to be as long as P->x + * and the whole P->x is replaced. + * + * @param work Workspace structure + * @param Px_new Vector of new elements in P->x (upper triangular) + * @param Px_new_idx Index mapping new elements to positions in P->x + * @param P_new_n Number of new elements to be changed + * @return output flag: 0: OK + * 1: P_new_n > nnzP + * <0: error in the update + */ +c_int osqp_update_P(OSQPWorkspace *work, + const c_float *Px_new, + const c_int *Px_new_idx, + c_int P_new_n); + + +/** + * Update elements of matrix A without changing sparsity structure. + * + * + * If Ax_new_idx is OSQP_NULL, Ax_new is assumed to be as long as A->x + * and the whole A->x is replaced. + * + * @param work Workspace structure + * @param Ax_new Vector of new elements in A->x + * @param Ax_new_idx Index mapping new elements to positions in A->x + * @param A_new_n Number of new elements to be changed + * @return output flag: 0: OK + * 1: A_new_n > nnzA + * <0: error in the update + */ +c_int osqp_update_A(OSQPWorkspace *work, + const c_float *Ax_new, + const c_int *Ax_new_idx, + c_int A_new_n); + + +/** + * Update elements of matrix P (upper triangular) and elements of matrix A + * without changing sparsity structure. + * + * + * If Px_new_idx is OSQP_NULL, Px_new is assumed to be as long as P->x + * and the whole P->x is replaced. + * + * If Ax_new_idx is OSQP_NULL, Ax_new is assumed to be as long as A->x + * and the whole A->x is replaced. + * + * @param work Workspace structure + * @param Px_new Vector of new elements in P->x (upper triangular) + * @param Px_new_idx Index mapping new elements to positions in P->x + * @param P_new_n Number of new elements to be changed + * @param Ax_new Vector of new elements in A->x + * @param Ax_new_idx Index mapping new elements to positions in A->x + * @param A_new_n Number of new elements to be changed + * @return output flag: 0: OK + * 1: P_new_n > nnzP + * 2: A_new_n > nnzA + * <0: error in the update + */ +c_int osqp_update_P_A(OSQPWorkspace *work, + const c_float *Px_new, + const c_int *Px_new_idx, + c_int P_new_n, + const c_float *Ax_new, + const c_int *Ax_new_idx, + c_int A_new_n); + +/** + * Update rho. Limit it between RHO_MIN and RHO_MAX. + * @param work Workspace + * @param rho_new New rho setting + * @return Exitflag + */ +c_int osqp_update_rho(OSQPWorkspace *work, + c_float rho_new); + +# endif // if EMBEDDED != 1 + +/** @} */ + + +/** + * @name Update settings + * @{ + */ + + +/** + * Update max_iter setting + * @param work Workspace + * @param max_iter_new New max_iter setting + * @return Exitflag + */ +c_int osqp_update_max_iter(OSQPWorkspace *work, + c_int max_iter_new); + + +/** + * Update absolute tolernace value + * @param work Workspace + * @param eps_abs_new New absolute tolerance value + * @return Exitflag + */ +c_int osqp_update_eps_abs(OSQPWorkspace *work, + c_float eps_abs_new); + + +/** + * Update relative tolernace value + * @param work Workspace + * @param eps_rel_new New relative tolerance value + * @return Exitflag + */ +c_int osqp_update_eps_rel(OSQPWorkspace *work, + c_float eps_rel_new); + + +/** + * Update primal infeasibility tolerance + * @param work Workspace + * @param eps_prim_inf_new New primal infeasibility tolerance + * @return Exitflag + */ +c_int osqp_update_eps_prim_inf(OSQPWorkspace *work, + c_float eps_prim_inf_new); + + +/** + * Update dual infeasibility tolerance + * @param work Workspace + * @param eps_dual_inf_new New dual infeasibility tolerance + * @return Exitflag + */ +c_int osqp_update_eps_dual_inf(OSQPWorkspace *work, + c_float eps_dual_inf_new); + + +/** + * Update relaxation parameter alpha + * @param work Workspace + * @param alpha_new New relaxation parameter value + * @return Exitflag + */ +c_int osqp_update_alpha(OSQPWorkspace *work, + c_float alpha_new); + + +/** + * Update warm_start setting + * @param work Workspace + * @param warm_start_new New warm_start setting + * @return Exitflag + */ +c_int osqp_update_warm_start(OSQPWorkspace *work, + c_int warm_start_new); + + +/** + * Update scaled_termination setting + * @param work Workspace + * @param scaled_termination_new New scaled_termination setting + * @return Exitflag + */ +c_int osqp_update_scaled_termination(OSQPWorkspace *work, + c_int scaled_termination_new); + +/** + * Update check_termination setting + * @param work Workspace + * @param check_termination_new New check_termination setting + * @return Exitflag + */ +c_int osqp_update_check_termination(OSQPWorkspace *work, + c_int check_termination_new); + + +# ifndef EMBEDDED + +/** + * Update regularization parameter in polish + * @param work Workspace + * @param delta_new New regularization parameter + * @return Exitflag + */ +c_int osqp_update_delta(OSQPWorkspace *work, + c_float delta_new); + + +/** + * Update polish setting + * @param work Workspace + * @param polish_new New polish setting + * @return Exitflag + */ +c_int osqp_update_polish(OSQPWorkspace *work, + c_int polish_new); + + +/** + * Update number of iterative refinement steps in polish + * @param work Workspace + * @param polish_refine_iter_new New iterative reginement steps + * @return Exitflag + */ +c_int osqp_update_polish_refine_iter(OSQPWorkspace *work, + c_int polish_refine_iter_new); + + +/** + * Update verbose setting + * @param work Workspace + * @param verbose_new New verbose setting + * @return Exitflag + */ +c_int osqp_update_verbose(OSQPWorkspace *work, + c_int verbose_new); + + +# endif // #ifndef EMBEDDED + +# ifdef PROFILING + +/** + * Update time_limit setting + * @param work Workspace + * @param time_limit_new New time_limit setting + * @return Exitflag + */ +c_int osqp_update_time_limit(OSQPWorkspace *work, + c_float time_limit_new); +# endif // ifdef PROFILING + +/** @} */ + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef OSQP_H diff --git a/src/lib/allocator_qp/include/osqp_configure.h b/src/lib/allocator_qp/include/osqp_configure.h new file mode 100644 index 000000000000..a233a070bba4 --- /dev/null +++ b/src/lib/allocator_qp/include/osqp_configure.h @@ -0,0 +1,49 @@ +#ifndef OSQP_CONFIGURE_H +# define OSQP_CONFIGURE_H + +# ifdef __cplusplus +extern "C" { +# endif /* ifdef __cplusplus */ + +/* DEBUG */ +/* #undef DEBUG */ + +/* Operating system */ +#define IS_LINUX +/* #undef IS_MAC */ +/* #undef IS_WINDOWS */ + +/* EMBEDDED */ +#define EMBEDDED (2) + +/* PRINTING */ +/* #undef PRINTING */ + +/* PROFILING */ +/* #undef PROFILING */ + +/* CTRLC */ +/* #undef CTRLC */ + +/* DFLOAT */ +/* #undef DFLOAT */ + +/* DLONG */ +/* #undef DLONG */ + +/* ENABLE_MKL_PARDISO */ +/* #undef ENABLE_MKL_PARDISO */ + +/* MEMORY MANAGEMENT */ +/* #undef OSQP_CUSTOM_MEMORY */ +#ifdef OSQP_CUSTOM_MEMORY +#include "" +#endif + + + +# ifdef __cplusplus +} +# endif /* ifdef __cplusplus */ + +#endif /* ifndef OSQP_CONFIGURE_H */ diff --git a/src/lib/allocator_qp/include/proj.h b/src/lib/allocator_qp/include/proj.h new file mode 100644 index 000000000000..ec0066a3af8f --- /dev/null +++ b/src/lib/allocator_qp/include/proj.h @@ -0,0 +1,37 @@ +#ifndef PROJ_H +# define PROJ_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "types.h" + + +/* Define Projections onto set C involved in the ADMM algorithm */ + +/** + * Project z onto \f$C = [l, u]\f$ + * @param z Vector to project + * @param work Workspace + */ +void project(OSQPWorkspace *work, + c_float *z); + + +/** + * Ensure z satisfies box constraints and y is is normal cone of z + * @param work Workspace + * @param z Primal variable z + * @param y Dual variable y + */ +void project_normalcone(OSQPWorkspace *work, + c_float *z, + c_float *y); + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef PROJ_H diff --git a/src/lib/allocator_qp/include/qdldl.h b/src/lib/allocator_qp/include/qdldl.h new file mode 100644 index 000000000000..7b2626cf8c6f --- /dev/null +++ b/src/lib/allocator_qp/include/qdldl.h @@ -0,0 +1,169 @@ +#ifndef QDLDL_H +#define QDLDL_H + +// Include qdldl type options +#include "qdldl_types.h" + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +/** + * Compute the elimination tree for a quasidefinite matrix + * in compressed sparse column form, where the input matrix is + * assumed to contain data for the upper triangular part of A only, + * and there are no duplicate indices. + * + * Returns an elimination tree for the factorization A = LDL^T and a + * count of the nonzeros in each column of L that are strictly below the + * diagonal. + * + * Does not use MALLOC. It is assumed that the arrays work, Lnz, and + * etree will be allocated with a number of elements equal to n. + * + * The data in (n,Ap,Ai) are from a square matrix A in CSC format, and + * should include the upper triangular part of A only. + * + * This function is only intended for factorisation of QD matrices specified + * by their upper triangular part. An error is returned if any column has + * data below the diagonal or s completely empty. + * + * For matrices with a non-empty column but a zero on the corresponding diagonal, + * this function will *not* return an error, as it may still be possible to factor + * such a matrix in LDL form. No promises are made in this case though... + * + * @param n number of columns in CSC matrix A (assumed square) + * @param Ap column pointers (size n+1) for columns of A + * @param Ai row indices of A. Has Ap[n] elements + * @param work work vector (size n) (no meaning on return) + * @param Lnz count of nonzeros in each column of L (size n) below diagonal + * @param etree elimination tree (size n) + * @return total sum of Lnz (i.e. total nonzeros in L below diagonal). + * Returns -1 if the input is not triu or has an empty column. + * Returns -2 if the return value overflows QDLDL_int. + * +*/ + QDLDL_int QDLDL_etree(const QDLDL_int n, + const QDLDL_int* Ap, + const QDLDL_int* Ai, + QDLDL_int* work, + QDLDL_int* Lnz, + QDLDL_int* etree); + + +/** + * Compute an LDL decomposition for a quasidefinite matrix + * in compressed sparse column form, where the input matrix is + * assumed to contain data for the upper triangular part of A only, + * and there are no duplicate indices. + * + * Returns factors L, D and Dinv = 1./D. + * + * Does not use MALLOC. It is assumed that L will be a compressed + * sparse column matrix with data (n,Lp,Li,Lx) with sufficient space + * allocated, with a number of nonzeros equal to the count given + * as a return value by QDLDL_etree + * + * @param n number of columns in L and A (both square) + * @param Ap column pointers (size n+1) for columns of A (not modified) + * @param Ai row indices of A. Has Ap[n] elements (not modified) + * @param Ax data of A. Has Ap[n] elements (not modified) + * @param Lp column pointers (size n+1) for columns of L + * @param Li row indices of L. Has Lp[n] elements + * @param Lx data of L. Has Lp[n] elements + * @param D vectorized factor D. Length is n + * @param Dinv reciprocal of D. Length is n + * @param Lnz count of nonzeros in each column of L below diagonal, + * as given by QDLDL_etree (not modified) + * @param etree elimination tree as as given by QDLDL_etree (not modified) + * @param bwork working array of bools. Length is n + * @param iwork working array of integers. Length is 3*n + * @param fwork working array of floats. Length is n + * @return Returns a count of the number of positive elements + * in D. Returns -1 and exits immediately if any element + * of D evaluates exactly to zero (matrix is not quasidefinite + * or otherwise LDL factorisable) + * +*/ +QDLDL_int QDLDL_factor(const QDLDL_int n, + const QDLDL_int* Ap, + const QDLDL_int* Ai, + const QDLDL_float* Ax, + QDLDL_int* Lp, + QDLDL_int* Li, + QDLDL_float* Lx, + QDLDL_float* D, + QDLDL_float* Dinv, + const QDLDL_int* Lnz, + const QDLDL_int* etree, + QDLDL_bool* bwork, + QDLDL_int* iwork, + QDLDL_float* fwork); + + +/** + * Solves LDL'x = b + * + * It is assumed that L will be a compressed + * sparse column matrix with data (n,Lp,Li,Lx). + * + * @param n number of columns in L + * @param Lp column pointers (size n+1) for columns of L + * @param Li row indices of L. Has Lp[n] elements + * @param Lx data of L. Has Lp[n] elements + * @param Dinv reciprocal of D. Length is n + * @param x initialized to b. Equal to x on return + * +*/ +void QDLDL_solve(const QDLDL_int n, + const QDLDL_int* Lp, + const QDLDL_int* Li, + const QDLDL_float* Lx, + const QDLDL_float* Dinv, + QDLDL_float* x); + + +/** + * Solves (L+I)x = b + * + * It is assumed that L will be a compressed + * sparse column matrix with data (n,Lp,Li,Lx). + * + * @param n number of columns in L + * @param Lp column pointers (size n+1) for columns of L + * @param Li row indices of L. Has Lp[n] elements + * @param Lx data of L. Has Lp[n] elements + * @param x initialized to b. Equal to x on return + * +*/ +void QDLDL_Lsolve(const QDLDL_int n, + const QDLDL_int* Lp, + const QDLDL_int* Li, + const QDLDL_float* Lx, + QDLDL_float* x); + + +/** + * Solves (L+I)'x = b + * + * It is assumed that L will be a compressed + * sparse column matrix with data (n,Lp,Li,Lx). + * + * @param n number of columns in L + * @param Lp column pointers (size n+1) for columns of L + * @param Li row indices of L. Has Lp[n] elements + * @param Lx data of L. Has Lp[n] elements + * @param x initialized to b. Equal to x on return + * +*/ +void QDLDL_Ltsolve(const QDLDL_int n, + const QDLDL_int* Lp, + const QDLDL_int* Li, + const QDLDL_float* Lx, + QDLDL_float* x); + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef QDLDL_H diff --git a/src/lib/allocator_qp/include/qdldl_interface.h b/src/lib/allocator_qp/include/qdldl_interface.h new file mode 100644 index 000000000000..7995ef36e6ef --- /dev/null +++ b/src/lib/allocator_qp/include/qdldl_interface.h @@ -0,0 +1,135 @@ +#ifndef QDLDL_INTERFACE_H +#define QDLDL_INTERFACE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "types.h" +#include "qdldl_types.h" + +/** + * QDLDL solver structure + */ +typedef struct qdldl qdldl_solver; + +struct qdldl { + enum linsys_solver_type type; + + /** + * @name Functions + * @{ + */ + c_int (*solve)(struct qdldl * self, c_float * b); + +#ifndef EMBEDDED + void (*free)(struct qdldl * self); ///< Free workspace (only if desktop) +#endif + + // This used only in non embedded or embedded 2 version +#if EMBEDDED != 1 + c_int (*update_matrices)(struct qdldl * self, const csc *P, const csc *A); ///< Update solver matrices + c_int (*update_rho_vec)(struct qdldl * self, const c_float * rho_vec); ///< Update rho_vec parameter +#endif + +#ifndef EMBEDDED + c_int nthreads; +#endif + /** @} */ + + /** + * @name Attributes + * @{ + */ + csc *L; ///< lower triangular matrix in LDL factorization + c_float *Dinv; ///< inverse of diag matrix in LDL (as a vector) + c_int *P; ///< permutation of KKT matrix for factorization + c_float *bp; ///< workspace memory for solves + c_float *sol; ///< solution to the KKT system + c_float *rho_inv_vec; ///< parameter vector + c_float sigma; ///< scalar parameter +#ifndef EMBEDDED + c_int polish; ///< polishing flag +#endif + c_int n; ///< number of QP variables + c_int m; ///< number of QP constraints + + +#if EMBEDDED != 1 + // These are required for matrix updates + c_int * Pdiag_idx, Pdiag_n; ///< index and number of diagonal elements in P + csc * KKT; ///< Permuted KKT matrix in sparse form (used to update P and A matrices) + c_int * PtoKKT, * AtoKKT; ///< Index of elements from P and A to KKT matrix + c_int * rhotoKKT; ///< Index of rho places in KKT matrix + // QDLDL Numeric workspace + QDLDL_float *D; + QDLDL_int *etree; + QDLDL_int *Lnz; + QDLDL_int *iwork; + QDLDL_bool *bwork; + QDLDL_float *fwork; +#endif + + /** @} */ +}; + + + +/** + * Initialize QDLDL Solver + * + * @param s Pointer to a private structure + * @param P Cost function matrix (upper triangular form) + * @param A Constraints matrix + * @param sigma Algorithm parameter. If polish, then sigma = delta. + * @param rho_vec Algorithm parameter. If polish, then rho_vec = OSQP_NULL. + * @param polish Flag whether we are initializing for polish or not + * @return Exitflag for error (0 if no errors) + */ +c_int init_linsys_solver_qdldl(qdldl_solver ** sp, const csc * P, const csc * A, c_float sigma, const c_float * rho_vec, c_int polish); + +/** + * Solve linear system and store result in b + * @param s Linear system solver structure + * @param b Right-hand side + * @return Exitflag + */ +c_int solve_linsys_qdldl(qdldl_solver * s, c_float * b); + + +#if EMBEDDED != 1 +/** + * Update linear system solver matrices + * @param s Linear system solver structure + * @param P Matrix P + * @param A Matrix A + * @return Exitflag + */ +c_int update_linsys_solver_matrices_qdldl(qdldl_solver * s, const csc *P, const csc *A); + + + + +/** + * Update rho_vec parameter in linear system solver structure + * @param s Linear system solver structure + * @param rho_vec new rho_vec value + * @return exitflag + */ +c_int update_linsys_solver_rho_vec_qdldl(qdldl_solver * s, const c_float * rho_vec); + +#endif + +#ifndef EMBEDDED +/** + * Free linear system solver + * @param s linear system solver object + */ +void free_linsys_solver_qdldl(qdldl_solver * s); +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/lib/allocator_qp/include/qdldl_types.h b/src/lib/allocator_qp/include/qdldl_types.h new file mode 100644 index 000000000000..3734977cf52b --- /dev/null +++ b/src/lib/allocator_qp/include/qdldl_types.h @@ -0,0 +1,23 @@ +#ifndef QDLDL_TYPES_H +# define QDLDL_TYPES_H + +# ifdef __cplusplus +extern "C" { +# endif /* ifdef __cplusplus */ + +#include //for the QDLDL_INT_TYPE_MAX + +// QDLDL integer and float types + +typedef int QDLDL_int; /* for indices */ +typedef double QDLDL_float; /* for numerical values */ +typedef unsigned char QDLDL_bool; /* for boolean values */ + +//Maximum value of the signed type QDLDL_int. +#define QDLDL_INT_MAX + +# ifdef __cplusplus +} +# endif /* ifdef __cplusplus */ + +#endif /* ifndef QDLDL_TYPES_H */ diff --git a/src/lib/allocator_qp/include/scaling.h b/src/lib/allocator_qp/include/scaling.h new file mode 100644 index 000000000000..0df9c04d9c77 --- /dev/null +++ b/src/lib/allocator_qp/include/scaling.h @@ -0,0 +1,44 @@ +#ifndef SCALING_H +# define SCALING_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +// Functions to scale problem data +# include "types.h" +# include "lin_alg.h" +# include "constants.h" + +// Enable data scaling if EMBEDDED is disabled or if EMBEDDED == 2 +# if EMBEDDED != 1 + +/** + * Scale problem matrices + * @param work Workspace + * @return exitflag + */ +c_int scale_data(OSQPWorkspace *work); +# endif // if EMBEDDED != 1 + + +/** + * Unscale problem matrices + * @param work Workspace + * @return exitflag + */ +c_int unscale_data(OSQPWorkspace *work); + + +/** + * Unscale solution + * @param work Workspace + * @return exitflag + */ +c_int unscale_solution(OSQPWorkspace *work); + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef SCALING_H diff --git a/src/lib/allocator_qp/include/types.h b/src/lib/allocator_qp/include/types.h new file mode 100644 index 000000000000..c1954cabcaad --- /dev/null +++ b/src/lib/allocator_qp/include/types.h @@ -0,0 +1,326 @@ +#ifndef OSQP_TYPES_H +# define OSQP_TYPES_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "glob_opts.h" +# include "constants.h" + + +/****************** +* Internal types * +******************/ + +/** + * Matrix in compressed-column form. + * The structure is used internally to store matrices in the triplet form as well, + * but the API requires that the matrices are in the CSC format. + */ +typedef struct { + c_int nzmax; ///< maximum number of entries + c_int m; ///< number of rows + c_int n; ///< number of columns + c_int *p; ///< column pointers (size n+1); col indices (size nzmax) start from 0 when using triplet format (direct KKT matrix formation) + c_int *i; ///< row indices, size nzmax starting from 0 + c_float *x; ///< numerical values, size nzmax + c_int nz; ///< number of entries in triplet matrix, -1 for csc +} csc; + +/** + * Linear system solver structure (sublevel objects initialize it differently) + */ + +typedef struct linsys_solver LinSysSolver; + +/** + * OSQP Timer for statistics + */ +typedef struct OSQP_TIMER OSQPTimer; + +/** + * Problem scaling matrices stored as vectors + */ +typedef struct { + c_float c; ///< cost function scaling + c_float *D; ///< primal variable scaling + c_float *E; ///< dual variable scaling + c_float cinv; ///< cost function rescaling + c_float *Dinv; ///< primal variable rescaling + c_float *Einv; ///< dual variable rescaling +} OSQPScaling; + +/** + * Solution structure + */ +typedef struct { + c_float *x; ///< primal solution + c_float *y; ///< Lagrange multiplier associated to \f$l <= Ax <= u\f$ +} OSQPSolution; + + +/** + * Solver return information + */ +typedef struct { + c_int iter; ///< number of iterations taken + char status[32]; ///< status string, e.g. 'solved' + c_int status_val; ///< status as c_int, defined in constants.h + +# ifndef EMBEDDED + c_int status_polish; ///< polish status: successful (1), unperformed (0), (-1) unsuccessful +# endif // ifndef EMBEDDED + + c_float obj_val; ///< primal objective + c_float pri_res; ///< norm of primal residual + c_float dua_res; ///< norm of dual residual + +# ifdef PROFILING + c_float setup_time; ///< time taken for setup phase (seconds) + c_float solve_time; ///< time taken for solve phase (seconds) + c_float update_time; ///< time taken for update phase (seconds) + c_float polish_time; ///< time taken for polish phase (seconds) + c_float run_time; ///< total time (seconds) +# endif // ifdef PROFILING + +# if EMBEDDED != 1 + c_int rho_updates; ///< number of rho updates + c_float rho_estimate; ///< best rho estimate so far from residuals +# endif // if EMBEDDED != 1 +} OSQPInfo; + + +# ifndef EMBEDDED + +/** + * Polish structure + */ +typedef struct { + csc *Ared; ///< active rows of A + ///< Ared = vstack[Alow, Aupp] + c_int n_low; ///< number of lower-active rows + c_int n_upp; ///< number of upper-active rows + c_int *A_to_Alow; ///< Maps indices in A to indices in Alow + c_int *A_to_Aupp; ///< Maps indices in A to indices in Aupp + c_int *Alow_to_A; ///< Maps indices in Alow to indices in A + c_int *Aupp_to_A; ///< Maps indices in Aupp to indices in A + c_float *x; ///< optimal x-solution obtained by polish + c_float *z; ///< optimal z-solution obtained by polish + c_float *y; ///< optimal y-solution obtained by polish + c_float obj_val; ///< objective value at polished solution + c_float pri_res; ///< primal residual at polished solution + c_float dua_res; ///< dual residual at polished solution +} OSQPPolish; +# endif // ifndef EMBEDDED + + +/********************************** +* Main structures and Data Types * +**********************************/ + +/** + * Data structure + */ +typedef struct { + c_int n; ///< number of variables n + c_int m; ///< number of constraints m + csc *P; ///< the upper triangular part of the quadratic cost matrix P in csc format (size n x n). + csc *A; ///< linear constraints matrix A in csc format (size m x n) + c_float *q; ///< dense array for linear part of cost function (size n) + c_float *l; ///< dense array for lower bound (size m) + c_float *u; ///< dense array for upper bound (size m) +} OSQPData; + + +/** + * Settings struct + */ +typedef struct { + c_float rho; ///< ADMM step rho + c_float sigma; ///< ADMM step sigma + c_int scaling; ///< heuristic data scaling iterations; if 0, then disabled. + +# if EMBEDDED != 1 + c_int adaptive_rho; ///< boolean, is rho step size adaptive? + c_int adaptive_rho_interval; ///< number of iterations between rho adaptations; if 0, then it is automatic + c_float adaptive_rho_tolerance; ///< tolerance X for adapting rho. The new rho has to be X times larger or 1/X times smaller than the current one to trigger a new factorization. +# ifdef PROFILING + c_float adaptive_rho_fraction; ///< interval for adapting rho (fraction of the setup time) +# endif // Profiling +# endif // EMBEDDED != 1 + + c_int max_iter; ///< maximum number of iterations + c_float eps_abs; ///< absolute convergence tolerance + c_float eps_rel; ///< relative convergence tolerance + c_float eps_prim_inf; ///< primal infeasibility tolerance + c_float eps_dual_inf; ///< dual infeasibility tolerance + c_float alpha; ///< relaxation parameter + enum linsys_solver_type linsys_solver; ///< linear system solver to use + +# ifndef EMBEDDED + c_float delta; ///< regularization parameter for polishing + c_int polish; ///< boolean, polish ADMM solution + c_int polish_refine_iter; ///< number of iterative refinement steps in polishing + + c_int verbose; ///< boolean, write out progress +# endif // ifndef EMBEDDED + + c_int scaled_termination; ///< boolean, use scaled termination criteria + c_int check_termination; ///< integer, check termination interval; if 0, then termination checking is disabled + c_int warm_start; ///< boolean, warm start + +# ifdef PROFILING + c_float time_limit; ///< maximum number of seconds allowed to solve the problem; if 0, then disabled +# endif // ifdef PROFILING +} OSQPSettings; + + +/** + * OSQP Workspace + */ +typedef struct { + /// Problem data to work on (possibly scaled) + OSQPData *data; + + /// Linear System solver structure + LinSysSolver *linsys_solver; + +# ifndef EMBEDDED + /// Polish structure + OSQPPolish *pol; +# endif // ifndef EMBEDDED + + /** + * @name Vector used to store a vectorized rho parameter + * @{ + */ + c_float *rho_vec; ///< vector of rho values + c_float *rho_inv_vec; ///< vector of inv rho values + + /** @} */ + +# if EMBEDDED != 1 + c_int *constr_type; ///< Type of constraints: loose (-1), equality (1), inequality (0) +# endif // if EMBEDDED != 1 + + /** + * @name Iterates + * @{ + */ + c_float *x; ///< Iterate x + c_float *y; ///< Iterate y + c_float *z; ///< Iterate z + c_float *xz_tilde; ///< Iterate xz_tilde + + c_float *x_prev; ///< Previous x + + /**< NB: Used also as workspace vector for dual residual */ + c_float *z_prev; ///< Previous z + + /**< NB: Used also as workspace vector for primal residual */ + + /** + * @name Primal and dual residuals workspace variables + * + * Needed for residuals computation, tolerances computation, + * approximate tolerances computation and adapting rho + * @{ + */ + c_float *Ax; ///< scaled A * x + c_float *Px; ///< scaled P * x + c_float *Aty; ///< scaled A' * y + + /** @} */ + + /** + * @name Primal infeasibility variables + * @{ + */ + c_float *delta_y; ///< difference between consecutive dual iterates + c_float *Atdelta_y; ///< A' * delta_y + + /** @} */ + + /** + * @name Dual infeasibility variables + * @{ + */ + c_float *delta_x; ///< difference between consecutive primal iterates + c_float *Pdelta_x; ///< P * delta_x + c_float *Adelta_x; ///< A * delta_x + + /** @} */ + + /** + * @name Temporary vectors used in scaling + * @{ + */ + + c_float *D_temp; ///< temporary primal variable scaling vectors + c_float *D_temp_A; ///< temporary primal variable scaling vectors storing norms of A columns + c_float *E_temp; ///< temporary constraints scaling vectors storing norms of A' columns + + + /** @} */ + + OSQPSettings *settings; ///< problem settings + OSQPScaling *scaling; ///< scaling vectors + OSQPSolution *solution; ///< problem solution + OSQPInfo *info; ///< solver information + +# ifdef PROFILING + OSQPTimer *timer; ///< timer object + + /// flag indicating whether the solve function has been run before + c_int first_run; + + /// flag indicating whether the update_time should be cleared + c_int clear_update_time; + + /// flag indicating that osqp_update_rho is called from osqp_solve function + c_int rho_update_from_solve; +# endif // ifdef PROFILING + +# ifdef PRINTING + c_int summary_printed; ///< Has last summary been printed? (true/false) +# endif // ifdef PRINTING + +} OSQPWorkspace; + + +/** + * Define linsys_solver prototype structure + * + * NB: The details are defined when the linear solver is initialized depending + * on the choice + */ +struct linsys_solver { + enum linsys_solver_type type; ///< linear system solver type functions + c_int (*solve)(LinSysSolver *self, + c_float *b); ///< solve linear system + +# ifndef EMBEDDED + void (*free)(LinSysSolver *self); ///< free linear system solver (only in desktop version) +# endif // ifndef EMBEDDED + +# if EMBEDDED != 1 + c_int (*update_matrices)(LinSysSolver *s, + const csc *P, ///< update matrices P + const csc *A); // and A in the solver + + c_int (*update_rho_vec)(LinSysSolver *s, + const c_float *rho_vec); ///< Update rho_vec +# endif // if EMBEDDED != 1 + +# ifndef EMBEDDED + c_int nthreads; ///< number of threads active +# endif // ifndef EMBEDDED +}; + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef OSQP_TYPES_H diff --git a/src/lib/allocator_qp/include/util.h b/src/lib/allocator_qp/include/util.h new file mode 100644 index 000000000000..6b2aac342363 --- /dev/null +++ b/src/lib/allocator_qp/include/util.h @@ -0,0 +1,222 @@ +#ifndef UTIL_H +# define UTIL_H + +# ifdef __cplusplus +extern "C" { +# endif // ifdef __cplusplus + +# include "types.h" +# include "constants.h" + +/****************** +* Versioning * +******************/ + +/** + * Return OSQP version + * @return OSQP version + */ +const char* osqp_version(void); + + +/********************** +* Utility Functions * +**********************/ + +# ifndef EMBEDDED + +/** + * Copy settings creating a new settings structure (uses MALLOC) + * @param settings Settings to be copied + * @return New settings structure + */ +OSQPSettings* copy_settings(const OSQPSettings *settings); + +# endif // #ifndef EMBEDDED + +/** + * Custom string copy to avoid string.h library + * @param dest destination string + * @param source source string + */ +void c_strcpy(char dest[], + const char source[]); + + +# ifdef PRINTING + +/** + * Print Header before running the algorithm + * @param work osqp workspace + */ +void print_setup_header(const OSQPWorkspace *work); + +/** + * Print header with data to be displayed per iteration + */ +void print_header(void); + +/** + * Print iteration summary + * @param work current workspace + */ +void print_summary(OSQPWorkspace *work); + +/** + * Print information after polish + * @param work current workspace + */ +void print_polish(OSQPWorkspace *work); + +/** + * Print footer when algorithm terminates + * @param info info structure + * @param polish is polish enabled? + */ +void print_footer(OSQPInfo *info, + c_int polish); + + +# endif // ifdef PRINTING + + +/********************************* +* Timer Structs and Functions * * +*********************************/ + +/*! \cond PRIVATE */ + +# ifdef PROFILING + +// Windows +# ifdef IS_WINDOWS + + // Some R packages clash with elements + // of the windows.h header, so use a + // slimmer version for conflict avoidance +# ifdef R_LANG +#define NOGDI +# endif + +# include + +struct OSQP_TIMER { + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +}; + +// Mac +# elif defined IS_MAC + +# include + +/* Use MAC OSX mach_time for timing */ +struct OSQP_TIMER { + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +}; + +// Linux +# else // ifdef IS_WINDOWS + +/* Use POSIX clock_gettime() for timing on non-Windows machines */ +# include +# include + + +struct OSQP_TIMER { + struct timespec tic; + struct timespec toc; +}; + +# endif // ifdef IS_WINDOWS + +/*! \endcond */ + +/** + * Timer Methods + */ + +/** + * Start timer + * @param t Timer object + */ +void osqp_tic(OSQPTimer *t); + +/** + * Report time + * @param t Timer object + * @return Reported time + */ +c_float osqp_toc(OSQPTimer *t); + +# endif /* END #ifdef PROFILING */ + + +/* ================================= DEBUG FUNCTIONS ======================= */ + +/*! \cond PRIVATE */ + + +# ifndef EMBEDDED + +/* Compare CSC matrices */ +c_int is_eq_csc(csc *A, + csc *B, + c_float tol); + +/* Convert sparse CSC to dense */ +c_float* csc_to_dns(csc *M); + +# endif // #ifndef EMBEDDED + + +# ifdef PRINTING +# include + + +/* Print a csc sparse matrix */ +void print_csc_matrix(csc *M, + const char *name); + +/* Dump csc sparse matrix to file */ +void dump_csc_matrix(csc *M, + const char *file_name); + +/* Print a triplet format sparse matrix */ +void print_trip_matrix(csc *M, + const char *name); + +/* Print a dense matrix */ +void print_dns_matrix(c_float *M, + c_int m, + c_int n, + const char *name); + +/* Print vector */ +void print_vec(c_float *v, + c_int n, + const char *name); + +/* Dump vector to file */ +void dump_vec(c_float *v, + c_int len, + const char *file_name); + +// Print int array +void print_vec_int(c_int *x, + c_int n, + const char *name); + +# endif // ifdef PRINTING + +/*! \endcond */ + + +# ifdef __cplusplus +} +# endif // ifdef __cplusplus + +#endif // ifndef UTIL_H diff --git a/src/lib/allocator_qp/include/workspace.h b/src/lib/allocator_qp/include/workspace.h new file mode 100644 index 000000000000..8b13db745ae2 --- /dev/null +++ b/src/lib/allocator_qp/include/workspace.h @@ -0,0 +1,85 @@ +#ifndef WORKSPACE_H +#define WORKSPACE_H + +/* + * This file was autogenerated by OSQP-Python on October 21, 2022 at 12:11:41. + * + * This file contains the prototypes for all the workspace variables needed + * by OSQP. The actual data is contained inside workspace.c. + */ + +#include "types.h" +#include "qdldl_interface.h" + +// Data structure prototypes +extern csc Pdata; +extern csc Adata; +extern c_float qdata[4]; +extern c_float ldata[4]; +extern c_float udata[4]; +extern OSQPData data; + +// Settings structure prototype +extern OSQPSettings settings; + +// Scaling structure prototypes +extern c_float Dscaling[4]; +extern c_float Dinvscaling[4]; +extern c_float Escaling[4]; +extern c_float Einvscaling[4]; +extern OSQPScaling scaling; + +// Prototypes for linsys_solver structure +extern csc linsys_solver_L; +extern c_float linsys_solver_Dinv[8]; +extern c_int linsys_solver_P[8]; +extern c_float linsys_solver_bp[8]; +extern c_float linsys_solver_sol[8]; +extern c_float linsys_solver_rho_inv_vec[4]; +extern c_int linsys_solver_Pdiag_idx[4]; +extern csc linsys_solver_KKT; +extern c_int linsys_solver_PtoKKT[10]; +extern c_int linsys_solver_AtoKKT[4]; +extern c_int linsys_solver_rhotoKKT[4]; +extern QDLDL_float linsys_solver_D[8]; +extern QDLDL_int linsys_solver_etree[8]; +extern QDLDL_int linsys_solver_Lnz[8]; +extern QDLDL_int linsys_solver_iwork[24]; +extern QDLDL_bool linsys_solver_bwork[8]; +extern QDLDL_float linsys_solver_fwork[8]; +extern qdldl_solver linsys_solver; + +// Prototypes for solution +extern c_float xsolution[4]; +extern c_float ysolution[4]; + +extern OSQPSolution solution; + +// Prototype for info structure +extern OSQPInfo info; + +// Prototypes for the workspace +extern c_float work_rho_vec[4]; +extern c_float work_rho_inv_vec[4]; +extern c_int work_constr_type[4]; +extern c_float work_x[4]; +extern c_float work_y[4]; +extern c_float work_z[4]; +extern c_float work_xz_tilde[8]; +extern c_float work_x_prev[4]; +extern c_float work_z_prev[4]; +extern c_float work_Ax[4]; +extern c_float work_Px[4]; +extern c_float work_Aty[4]; +extern c_float work_delta_y[4]; +extern c_float work_Atdelta_y[4]; +extern c_float work_delta_x[4]; +extern c_float work_Pdelta_x[4]; +extern c_float work_Adelta_x[4]; +extern c_float work_D_temp[4]; +extern c_float work_D_temp_A[4]; +extern c_float work_E_temp[4]; + +extern OSQPWorkspace workspace; + +#endif // ifndef WORKSPACE_H diff --git a/src/lib/allocator_qp/qp_generator.py b/src/lib/allocator_qp/qp_generator.py new file mode 100644 index 000000000000..89648724325e --- /dev/null +++ b/src/lib/allocator_qp/qp_generator.py @@ -0,0 +1,23 @@ +import osqp +import numpy as np +import scipy as sp +from scipy import sparse + +G = sparse.random(4,4,1.0) + +H = np.diag([1,1,0.1, 0.5]) + +P = sparse.csc_matrix(G.T @ H @ G) + +mu_ref = np.array([1,2,3,4]) + +q = G.T @ H @ mu_ref + +A = sparse.csc_matrix(np.eye(4)) +l = np.zeros(4) +u = 10.0 * np.ones(4) + +prob = osqp.OSQP() +prob.setup(P, q, A, l, u) + +prob.codegen('allocator_qp', parameters='matrices', project_type='Makefile') diff --git a/src/lib/allocator_qp/src/build/lib.linux-x86_64-3.8/emosqp.cpython-38-x86_64-linux-gnu.so b/src/lib/allocator_qp/src/build/lib.linux-x86_64-3.8/emosqp.cpython-38-x86_64-linux-gnu.so new file mode 100755 index 000000000000..fbd26b5e4853 Binary files /dev/null and b/src/lib/allocator_qp/src/build/lib.linux-x86_64-3.8/emosqp.cpython-38-x86_64-linux-gnu.so differ diff --git a/src/lib/allocator_qp/src/emosqp.cpython-38-x86_64-linux-gnu.so b/src/lib/allocator_qp/src/emosqp.cpython-38-x86_64-linux-gnu.so new file mode 100755 index 000000000000..fbd26b5e4853 Binary files /dev/null and b/src/lib/allocator_qp/src/emosqp.cpython-38-x86_64-linux-gnu.so differ diff --git a/src/lib/allocator_qp/src/emosqpmodule.c b/src/lib/allocator_qp/src/emosqpmodule.c new file mode 100644 index 000000000000..2a189b3e32e6 --- /dev/null +++ b/src/lib/allocator_qp/src/emosqpmodule.c @@ -0,0 +1,718 @@ +// Use not deprecated Numpy API (numpy > 1.7) +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION + +#include "Python.h" // Python API +#include "numpy/arrayobject.h" // Numpy C API +#include "numpy/npy_math.h" // For infinity values +#include "osqp.h" // OSQP API + +#include "workspace.h" // Include code-generated OSQP workspace + + + + +/********************************* + * Timer Structs and Functions * * + *********************************/ + +// Windows +#ifdef IS_WINDOWS + +#include + +typedef struct { + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} PyTimer; + +// Mac +#elif defined IS_MAC + +#include + +/* Use MAC OSX mach_time for timing */ +typedef struct { + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} PyTimer; + +// Linux +#else + +/* Use POSIX clocl_gettime() for timing on non-Windows machines */ +#include +#include + +typedef struct { + struct timespec tic; + struct timespec toc; +} PyTimer; + +#endif + +/** + * Timer Methods + */ + +// Windows +#ifdef IS_WINDOWS + +void tic(PyTimer* t) { + QueryPerformanceFrequency(&t->freq); + QueryPerformanceCounter(&t->tic); +} + +c_float toc(PyTimer* t) { + QueryPerformanceCounter(&t->toc); + return ((t->toc.QuadPart - t->tic.QuadPart) / (c_float)t->freq.QuadPart); +} + +// Mac +#elif defined IS_MAC + +void tic(PyTimer* t) { + /* read current clock cycles */ + t->tic = mach_absolute_time(); +} + +c_float toc(PyTimer* t) { + uint64_t duration; /* elapsed time in clock cycles*/ + + t->toc = mach_absolute_time(); + duration = t->toc - t->tic; + + /*conversion from clock cycles to nanoseconds*/ + mach_timebase_info(&(t->tinfo)); + duration *= t->tinfo.numer; + duration /= t->tinfo.denom; + +return (c_float)duration / 1e9; +} + + +// Linux +#else + +/* read current time */ +void tic(PyTimer* t) +{ + clock_gettime(CLOCK_MONOTONIC, &t->tic); +} + + +/* return time passed since last call to tic on this timer */ +c_float toc(PyTimer* t) { + struct timespec temp; + + clock_gettime(CLOCK_MONOTONIC, &t->toc); + + if ((t->toc.tv_nsec - t->tic.tv_nsec)<0) { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec-1; + temp.tv_nsec = 1e9+t->toc.tv_nsec - t->tic.tv_nsec; + } else { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; + } + return (c_float)temp.tv_sec + (c_float)temp.tv_nsec / 1e9; +} + + +#endif + + + + +/* The PyInt variable is a PyLong in Python3.x. + */ +// #if PY_MAJOR_VERSION >= 3 +// #define PyInt_AsLong PyLong_AsLong +// #define PyInt_Check PyLong_Check +// #endif + + +// Get float type from OSQP setup +static int get_float_type(void) { + switch (sizeof(c_float)) { + case 2: + return NPY_FLOAT16; + case 4: + return NPY_FLOAT32; + case 8: + return NPY_FLOAT64; + default: + return NPY_FLOAT64; /* defaults to double */ + } +} + +static PyArrayObject * PyArrayFromCArray(c_float *arrayin, npy_intp * nd){ + int i; + PyArrayObject * arrayout; + double * data; + + arrayout = (PyArrayObject *)PyArray_SimpleNew(1, nd, NPY_DOUBLE); + data = PyArray_DATA(arrayout); + + // Copy array into Python array + for (i=0; i< nd[0]; i++){ + data[i] = (double)arrayin[i]; + } + + return arrayout; + +} + +// Old function. Not working. +// static PyObject * PyArrayFromCArray(c_float *arrayin, npy_intp * nd, +// int typenum){ +// int i; +// PyObject * arrayout; +// c_float *x_arr; +// +// // Allocate solutions +// x_arr = PyMem_Malloc(nd[0] * sizeof(c_float)); +// +// // copy elements to x_arr +// for (i=0; i< nd[0]; i++){ +// x_arr[i] = arrayin[i]; +// } +// +// arrayout = PyArray_SimpleNewFromData(1, nd, typenum, x_arr); +// // Set x to own x_arr so that it is freed when x is freed +// PyArray_ENABLEFLAGS((PyArrayObject *) arrayout, NPY_ARRAY_OWNDATA); +// +// +// return arrayout; +// +// } + + +/* gets the pointer to the block of contiguous C memory + * the overhead should be small unless the numpy array has been + * reordered in some way or the data type doesn't quite match + */ +static PyArrayObject *get_contiguous(PyArrayObject *array, int typenum) { + /* + * the "tmp_arr" pointer has to have Py_DECREF called on it; new_owner + * owns the "new" array object created by PyArray_Cast + */ + PyArrayObject *tmp_arr; + PyArrayObject *new_owner; + tmp_arr = PyArray_GETCONTIGUOUS(array); + new_owner = (PyArrayObject *) PyArray_Cast(tmp_arr, typenum); + Py_DECREF(tmp_arr); + return new_owner; +} + + +/************************ +* Interface Methods * +************************/ + + +// Solve Optimization Problem +static PyObject * OSQP_solve(PyObject *self, PyObject *args) +{ + // Allocate timer + PyTimer * timer; + c_float solve_time; + + // Create solution objects + PyObject * x, *y; + + // Temporary solution + npy_intp nd[] = {(npy_intp)(&workspace)->data->n}; // Dimensions in R^n + npy_intp md[] = {(npy_intp)(&workspace)->data->m}; // Dimensions in R^m + + + // Initialize timer + timer = PyMem_Malloc(sizeof(PyTimer)); + tic(timer); + + /** + * Solve QP Problem + */ + if (osqp_solve((&workspace)) == -1){ + PySys_WriteStdout("Error: Workspace not initialized!\n"); + } + + // Stop timer + solve_time = toc(timer); + + // If problem is not primal or dual infeasible store it + if (((&workspace)->info->status_val != OSQP_PRIMAL_INFEASIBLE) && + ((&workspace)->info->status_val != OSQP_PRIMAL_INFEASIBLE_INACCURATE) && + ((&workspace)->info->status_val != OSQP_DUAL_INFEASIBLE) && + ((&workspace)->info->status_val != OSQP_DUAL_INFEASIBLE_INACCURATE)) { + + // Construct primal and dual solution arrays + x = (PyObject *)PyArrayFromCArray((&workspace)->solution->x, nd); + y = (PyObject *)PyArrayFromCArray((&workspace)->solution->y, md); + + } else { // Problem primal or dual infeasible -> None values for x,y + x = PyArray_EMPTY(1, nd, NPY_OBJECT, 0); + y = PyArray_EMPTY(1, nd, NPY_OBJECT, 0); + } + + // Free timer + PyMem_Free(timer); + + // Return struct + return Py_BuildValue("OOiid", x, y, (&workspace)->info->status_val, + (&workspace)->info->iter, solve_time); + +} + + + +static PyObject *OSQP_update_lin_cost(PyObject *self, PyObject *args){ + PyArrayObject *q, *q_cont; + c_float * q_arr; + int float_type = get_float_type(); + + static char * argparse_string = "O!"; + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, + &PyArray_Type, &q)) { + return NULL; + } + + // Check dimension + if (PyArray_DIM(q, 0) != (&workspace)->data->n){ + PySys_WriteStdout("Error in linear cost dimension!\n"); + return NULL; + } + + // Get contiguous data structure + q_cont = get_contiguous(q, float_type); + + // Copy array into c_float array + q_arr = (c_float *)PyArray_DATA(q_cont); + + // Update linear cost + osqp_update_lin_cost((&workspace), q_arr); + + // Free data + Py_DECREF(q_cont); + + // Return None + Py_INCREF(Py_None); + return Py_None; + +} + +static PyObject *OSQP_update_lower_bound(PyObject *self, PyObject *args){ + PyArrayObject *l, *l_cont; + c_float * l_arr; + int float_type = get_float_type(); + + static char * argparse_string = "O!"; + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, + &PyArray_Type, &l)) { + return NULL; + } + + // Check dimension + if (PyArray_DIM(l, 0) != (&workspace)->data->m){ + PySys_WriteStdout("Error in lower bound dimension!\n"); + return NULL; + } + + // Get contiguous data structure + l_cont = get_contiguous(l, float_type); + + // Copy array into c_float array + l_arr = (c_float *)PyArray_DATA(l_cont); + + // Update linear cost + osqp_update_lower_bound((&workspace), l_arr); + + // Free data + Py_DECREF(l_cont); + + // Return None + Py_INCREF(Py_None); + return Py_None; + +} + +static PyObject *OSQP_update_upper_bound(PyObject *self, PyObject *args){ + PyArrayObject *u, *u_cont; + c_float * u_arr; + int float_type = get_float_type(); + + static char * argparse_string = "O!"; + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, + &PyArray_Type, &u)) { + return NULL; + } + + // Check dimension + if (PyArray_DIM(u, 0) != (&workspace)->data->m){ + PySys_WriteStdout("Error in upper bound dimension!\n"); + return NULL; + } + + // Get contiguous data structure + u_cont = get_contiguous(u, float_type); + + // Copy array into c_float array + u_arr = (c_float *)PyArray_DATA(u_cont); + + // Update linear cost + osqp_update_upper_bound((&workspace), u_arr); + + // Free data + Py_DECREF(u_cont); + + // Return None + Py_INCREF(Py_None); + return Py_None; + +} + + +static PyObject *OSQP_update_bounds(PyObject *self, PyObject *args){ + PyArrayObject *l, *l_cont, *u, *u_cont; + c_float * l_arr, * u_arr; + int float_type = get_float_type(); + + static char * argparse_string = "O!O!"; + + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, + &PyArray_Type, &l, + &PyArray_Type, &u)) { + return NULL; + } + + // Check dimension + if (PyArray_DIM(u, 0) != (&workspace)->data->m){ + PySys_WriteStdout("Error in upper bound dimension!\n"); + return NULL; + } + + // Check dimension + if (PyArray_DIM(l, 0) != (&workspace)->data->m){ + PySys_WriteStdout("Error in lower bound dimension!\n"); + return NULL; + } + + + // Get contiguous data structure + u_cont = get_contiguous(u, float_type); + + // Get contiguous data structure + l_cont = get_contiguous(l, float_type); + + // Copy array into c_float array + u_arr = (c_float *)PyArray_DATA(u_cont); + + // Copy array into c_float array + l_arr = (c_float *)PyArray_DATA(l_cont); + + // Update linear cost + osqp_update_bounds((&workspace), l_arr, u_arr); + + // Free data + Py_DECREF(u_cont); + Py_DECREF(l_cont); + + // Return None + Py_INCREF(Py_None); + return Py_None; + +} + + +#if EMBEDDED != 1 + +// Get integer type from OSQP setup +static int get_int_type(void) { + switch (sizeof(c_int)) { + case 1: + return NPY_INT8; + case 2: + return NPY_INT16; + case 4: + return NPY_INT32; + case 8: + return NPY_INT64; + default: + return NPY_INT32; /* defaults to 4 byte int */ + } +} + +// Update elements of matrix P +static PyObject * OSQP_update_P(PyObject *self, PyObject *args) { + PyArrayObject *Px, *Px_cont, *Px_idx, *Px_idx_cont; + c_float * Px_arr; + c_int * Px_idx_arr; + c_int Px_n; + c_int return_val; + int float_type = get_float_type(); + int int_type = get_int_type(); + + #ifdef DLONG + static char * argparse_string = "OOL"; + #else + static char * argparse_string = "OOi"; + #endif + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, &Px, &Px_idx, &Px_n)) { + return NULL; + } + + // Check if Px_idx is passed + if((PyObject *)Px_idx != Py_None){ + Px_idx_cont = get_contiguous(Px_idx, int_type); + Px_idx_arr = (c_int *)PyArray_DATA(Px_idx_cont); + } else { + Px_idx_cont = OSQP_NULL; + Px_idx_arr = OSQP_NULL; + } + + + // Get contiguous data structure + Px_cont = get_contiguous(Px, float_type); + + // Copy array into c_float and c_int arrays + Px_arr = (c_float *)PyArray_DATA(Px_cont); + + // Check dimension + if ((PyObject *)Px_idx != Py_None && PyArray_DIM(Px, 0) != PyArray_DIM(Px_idx, 0)){ + PyErr_SetString(PyExc_ValueError, "Error in updating P: Px and Px_idx must have the same length!"); + return (PyObject *) NULL; + } + + // Update matrix P + return_val = osqp_update_P((&workspace), Px_arr, Px_idx_arr, Px_n); + + // Free data + Py_DECREF(Px_cont); + if ((PyObject *)Px_idx != Py_None) Py_DECREF(Px_idx_cont); + + if (return_val == 1) { + PyErr_SetString(PyExc_ValueError, "Error in updating P: length of Px and Px_idx is too large!"); + return (PyObject *) NULL; + } else if (return_val < 0) {\ + PyErr_SetString(PyExc_ValueError, "Error in updating P: new KKT matrix is not quasidefinite!"); + return (PyObject *) NULL; + } + + // Return None + Py_INCREF(Py_None); + return Py_None; +} + +// Update elements of matrix A +static PyObject * OSQP_update_A(PyObject *self, PyObject *args) { + PyArrayObject *Ax, *Ax_cont, *Ax_idx, *Ax_idx_cont; + c_float * Ax_arr; + c_int * Ax_idx_arr; + c_int Ax_n; + c_int return_val; + int float_type = get_float_type(); + int int_type = get_int_type(); + + #ifdef DLONG + static char * argparse_string = "OOL"; + #else + static char * argparse_string = "OOi"; + #endif + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, &Ax, &Ax_idx, &Ax_n)) { + return NULL; + } + + // Check if Ax_idx is passed + if((PyObject *)Ax_idx != Py_None){ + Ax_idx_cont = get_contiguous(Ax_idx, int_type); + Ax_idx_arr = (c_int *)PyArray_DATA(Ax_idx_cont); + } else { + Ax_idx_cont = OSQP_NULL; + Ax_idx_arr = OSQP_NULL; + } + + // Get contiguous data structure + Ax_cont = get_contiguous(Ax, float_type); + + // Copy array into c_float and c_int arrays + Ax_arr = (c_float *)PyArray_DATA(Ax_cont); + + // Check dimension + if ((PyObject *)Ax_idx != Py_None && PyArray_DIM(Ax, 0) != PyArray_DIM(Ax_idx, 0)){ + PyErr_SetString(PyExc_ValueError, "Error in updating A: Ax and Ax_idx must have the same length!"); + return (PyObject *) NULL; + } + + // Update matrix P + return_val = osqp_update_A((&workspace), Ax_arr, Ax_idx_arr, Ax_n); + + // Free data + Py_DECREF(Ax_cont); + if ((PyObject *)Ax_idx != Py_None) Py_DECREF(Ax_idx_cont); + + if (return_val == 1) { + PyErr_SetString(PyExc_ValueError, "Error in updating A: length of Ax and Ax_idx is too large!"); + return (PyObject *) NULL; + } else if (return_val < 0) {\ + PyErr_SetString(PyExc_ValueError, "Error in updating A: new KKT matrix is not quasidefinite!"); + return (PyObject *) NULL; + } + + // Return None + Py_INCREF(Py_None); + return Py_None; +} + +// Update elements of matrix A +static PyObject * OSQP_update_P_A(PyObject *self, PyObject *args) { + PyArrayObject *Px, *Px_cont, *Px_idx, *Px_idx_cont; + PyArrayObject *Ax, *Ax_cont, *Ax_idx, *Ax_idx_cont; + c_float * Px_arr, * Ax_arr; + c_int * Px_idx_arr, * Ax_idx_arr; + c_int Px_n, Ax_n; + c_int return_val; + int float_type = get_float_type(); + int int_type = get_int_type(); + + #ifdef DLONG + static char * argparse_string = "OOLOOL"; + #else + static char * argparse_string = "OOiOOi"; + #endif + + // Parse arguments + if( !PyArg_ParseTuple(args, argparse_string, &Px, &Px_idx, &Px_n, + &Ax, &Ax_idx, &Ax_n)) { + return NULL; + } + + // Ax_idx is passed + if((PyObject *)Ax_idx != Py_None){ + Ax_idx_cont = get_contiguous(Ax_idx, int_type); + Ax_idx_arr = (c_int *)PyArray_DATA(Ax_idx_cont); + } else { + Ax_idx_cont = OSQP_NULL; + Ax_idx_arr = OSQP_NULL; + } + + // Px_idx is passed + if((PyObject *)Px_idx != Py_None){ + Px_idx_cont = get_contiguous(Px_idx, int_type); + Px_idx_arr = (c_int *)PyArray_DATA(Px_idx_cont); + } else { + Px_idx_cont = OSQP_NULL; + Px_idx_arr = OSQP_NULL; + } + + // Get contiguous data structure + Px_cont = get_contiguous(Px, float_type); + Ax_cont = get_contiguous(Ax, float_type); + + // Copy array into c_float and c_int arrays + Px_arr = (c_float *)PyArray_DATA(Px_cont); + Ax_arr = (c_float *)PyArray_DATA(Ax_cont); + + // Check dimension + if ((PyObject *)Px_idx != Py_None && PyArray_DIM(Px, 0) != PyArray_DIM(Px_idx, 0)){ + PyErr_SetString(PyExc_ValueError, "Error in updating P and A: Px and Px_idx must have the same length!"); + return (PyObject *) NULL; + } + if ((PyObject *)Ax_idx != Py_None && PyArray_DIM(Ax, 0) != PyArray_DIM(Ax_idx, 0)){ + PyErr_SetString(PyExc_ValueError, "Error in updating P and A: Ax and Ax_idx must have the same length!"); + return (PyObject *) NULL; + } + + // Update matrices P and A + return_val = osqp_update_P_A((&workspace), Px_arr, Px_idx_arr, Px_n, Ax_arr, Ax_idx_arr, Ax_n); + + // Free data + Py_DECREF(Px_cont); + if ((PyObject *)Px_idx != Py_None) Py_DECREF(Px_idx_cont); + Py_DECREF(Ax_cont); + if ((PyObject *)Ax_idx != Py_None) Py_DECREF(Ax_idx_cont); + + // LEFT for DEBUG + if (return_val == 1) { + PySys_WriteStdout("Size of Px and Px_idx is too large!"); + return NULL; + } else if (return_val == 2) { + PySys_WriteStdout("Size of Ax and Ax_idx is too large!"); + return NULL; + } else if (return_val < 0) { + PySys_WriteStdout("New KKT matrix is not quasidefinite!"); + return NULL; + } + + // Return None + Py_INCREF(Py_None); + return Py_None; +} + +#endif // end EMBEDDED + + +static PyMethodDef emosqp_methods[] = { + {"solve", (PyCFunction)OSQP_solve, METH_NOARGS, "Solve QP"}, + {"update_lin_cost", (PyCFunction)OSQP_update_lin_cost, METH_VARARGS, "Update linear cost"}, + {"update_lower_bound", (PyCFunction)OSQP_update_lower_bound, METH_VARARGS, "Update lower bound"}, + {"update_upper_bound", (PyCFunction)OSQP_update_upper_bound, METH_VARARGS, "Update upper bound"}, + {"update_bounds", (PyCFunction)OSQP_update_bounds, METH_VARARGS, "Update bounds"}, +#if EMBEDDED != 1 + {"update_P", (PyCFunction)OSQP_update_P, METH_VARARGS, "Update matrix P"}, + {"update_A", (PyCFunction)OSQP_update_A, METH_VARARGS, "Update matrix A"}, + {"update_P_A", (PyCFunction)OSQP_update_P_A, METH_VARARGS, "Update matrices P and A"}, +#endif + {NULL, NULL, 0, NULL} +}; + + + +/* Module initialization for Python 3*/ +static struct PyModuleDef moduledef = { + PyModuleDef_HEAD_INIT, "emosqp", /* m_name */ + "Embedded OSQP solver", /* m_doc */ + -1, /* m_size */ + emosqp_methods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ +}; + + + +static PyObject * moduleinit(void){ + + PyObject *m; + + // Initialize module + m = PyModule_Create(&moduledef); + + if (m == NULL) + return NULL; + + return m; +} + + + + +// Init Osqp Internal module +PyMODINIT_FUNC PyInit_emosqp(void) +{ + import_array(); /* for numpy arrays */ + + return moduleinit(); +} diff --git a/src/lib/allocator_qp/src/example.c b/src/lib/allocator_qp/src/example.c new file mode 100644 index 000000000000..beac16eea643 --- /dev/null +++ b/src/lib/allocator_qp/src/example.c @@ -0,0 +1,20 @@ +#include "stdio.h" +#include + +#include "workspace.h" +#include "osqp.h" + +int main(int argc, char **argv) { + + // Solve Problem + osqp_solve(&workspace); + + // Print status + printf("Status: %s\n", (&workspace)->info->status); + printf("Number of iterations: %d\n", (int)((&workspace)->info->iter)); + printf("Objective value: %.4e\n", (&workspace)->info->obj_val); + printf("Primal residual: %.4e\n", (&workspace)->info->pri_res); + printf("Dual residual: %.4e\n", (&workspace)->info->dua_res); + + return 0; +} diff --git a/src/lib/allocator_qp/src/osqp/CMakeLists.txt b/src/lib/allocator_qp/src/osqp/CMakeLists.txt new file mode 100644 index 000000000000..02bf7f1dad9b --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/CMakeLists.txt @@ -0,0 +1,47 @@ +# Add the OSQP sources +set( + osqp_src + "${CMAKE_CURRENT_SOURCE_DIR}/auxil.c" + "${CMAKE_CURRENT_SOURCE_DIR}/error.c" + "${CMAKE_CURRENT_SOURCE_DIR}/lin_alg.c" + "${CMAKE_CURRENT_SOURCE_DIR}/osqp.c" + "${CMAKE_CURRENT_SOURCE_DIR}/proj.c" + "${CMAKE_CURRENT_SOURCE_DIR}/scaling.c" + "${CMAKE_CURRENT_SOURCE_DIR}/util.c" +) + +# Add the KKT update only in normal mode and matrix-updating embedded mode (not mode 1) +if (NOT (EMBEDDED EQUAL 1)) + list( + APPEND + osqp_src + "${CMAKE_CURRENT_SOURCE_DIR}/kkt.c" + ) +endif() + +# Add more files that should only be in non-embedded code +if (NOT DEFINED EMBEDDED) + list( + APPEND + osqp_src + "${CMAKE_CURRENT_SOURCE_DIR}/cs.c" + "${CMAKE_CURRENT_SOURCE_DIR}/polish.c" + "${CMAKE_CURRENT_SOURCE_DIR}/lin_sys.c" + ) +endif() + +# Add the ctrl-c handler if enabled +if (CTRLC) + list( + APPEND + osqp_src + "${CMAKE_CURRENT_SOURCE_DIR}/ctrlc.c" + ) +endif() + +# Pass the source list up to the main CMakeLists scope +set( + osqp_src + "${osqp_src}" + PARENT_SCOPE +) diff --git a/src/lib/allocator_qp/src/osqp/auxil.c b/src/lib/allocator_qp/src/osqp/auxil.c new file mode 100644 index 000000000000..89c7de72dcf7 --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/auxil.c @@ -0,0 +1,1067 @@ +#include "osqp.h" // For OSQP rho update +#include "auxil.h" +#include "proj.h" +#include "lin_alg.h" +#include "constants.h" +#include "scaling.h" +#include "util.h" + +/*********************************************************** +* Auxiliary functions needed to compute ADMM iterations * * +***********************************************************/ +#if EMBEDDED != 1 +c_float compute_rho_estimate(OSQPWorkspace *work) { + c_int n, m; // Dimensions + c_float pri_res, dua_res; // Primal and dual residuals + c_float pri_res_norm, dua_res_norm; // Normalization for the residuals + c_float temp_res_norm; // Temporary residual norm + c_float rho_estimate; // Rho estimate value + + // Get problem dimensions + n = work->data->n; + m = work->data->m; + + // Get primal and dual residuals + pri_res = vec_norm_inf(work->z_prev, m); + dua_res = vec_norm_inf(work->x_prev, n); + + // Normalize primal residual + pri_res_norm = vec_norm_inf(work->z, m); // ||z|| + temp_res_norm = vec_norm_inf(work->Ax, m); // ||Ax|| + pri_res_norm = c_max(pri_res_norm, temp_res_norm); // max (||z||,||Ax||) + pri_res /= (pri_res_norm + OSQP_DIVISION_TOL); // Normalize primal + // residual (prevent 0 + // division) + + // Normalize dual residual + dua_res_norm = vec_norm_inf(work->data->q, n); // ||q|| + temp_res_norm = vec_norm_inf(work->Aty, n); // ||A' y|| + dua_res_norm = c_max(dua_res_norm, temp_res_norm); + temp_res_norm = vec_norm_inf(work->Px, n); // ||P x|| + dua_res_norm = c_max(dua_res_norm, temp_res_norm); // max(||q||,||A' y||,||P + // x||) + dua_res /= (dua_res_norm + OSQP_DIVISION_TOL); // Normalize dual residual + // (prevent 0 division) + + + // Return rho estimate + rho_estimate = work->settings->rho * c_sqrt(pri_res / dua_res); + rho_estimate = c_min(c_max(rho_estimate, RHO_MIN), RHO_MAX); // Constrain + // rho values + return rho_estimate; +} + +c_int adapt_rho(OSQPWorkspace *work) { + c_int exitflag; // Exitflag + c_float rho_new; // New rho value + + exitflag = 0; // Initialize exitflag to 0 + + // Compute new rho + rho_new = compute_rho_estimate(work); + + // Set rho estimate in info + work->info->rho_estimate = rho_new; + + // Check if the new rho is large or small enough and update it in case + if ((rho_new > work->settings->rho * work->settings->adaptive_rho_tolerance) || + (rho_new < work->settings->rho / work->settings->adaptive_rho_tolerance)) { + exitflag = osqp_update_rho(work, rho_new); + work->info->rho_updates += 1; + } + + return exitflag; +} + +void set_rho_vec(OSQPWorkspace *work) { + c_int i; + + work->settings->rho = c_min(c_max(work->settings->rho, RHO_MIN), RHO_MAX); + + for (i = 0; i < work->data->m; i++) { + if ((work->data->l[i] < -OSQP_INFTY * MIN_SCALING) && + (work->data->u[i] > OSQP_INFTY * MIN_SCALING)) { + // Loose bounds + work->constr_type[i] = -1; + work->rho_vec[i] = RHO_MIN; + } else if (work->data->u[i] - work->data->l[i] < RHO_TOL) { + // Equality constraints + work->constr_type[i] = 1; + work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho; + } else { + // Inequality constraints + work->constr_type[i] = 0; + work->rho_vec[i] = work->settings->rho; + } + work->rho_inv_vec[i] = 1. / work->rho_vec[i]; + } +} + +c_int update_rho_vec(OSQPWorkspace *work) { + c_int i, exitflag, constr_type_changed; + + exitflag = 0; + constr_type_changed = 0; + + for (i = 0; i < work->data->m; i++) { + if ((work->data->l[i] < -OSQP_INFTY * MIN_SCALING) && + (work->data->u[i] > OSQP_INFTY * MIN_SCALING)) { + // Loose bounds + if (work->constr_type[i] != -1) { + work->constr_type[i] = -1; + work->rho_vec[i] = RHO_MIN; + work->rho_inv_vec[i] = 1. / RHO_MIN; + constr_type_changed = 1; + } + } else if (work->data->u[i] - work->data->l[i] < RHO_TOL) { + // Equality constraints + if (work->constr_type[i] != 1) { + work->constr_type[i] = 1; + work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho; + work->rho_inv_vec[i] = 1. / work->rho_vec[i]; + constr_type_changed = 1; + } + } else { + // Inequality constraints + if (work->constr_type[i] != 0) { + work->constr_type[i] = 0; + work->rho_vec[i] = work->settings->rho; + work->rho_inv_vec[i] = 1. / work->settings->rho; + constr_type_changed = 1; + } + } + } + + // Update rho_vec in KKT matrix if constraints type has changed + if (constr_type_changed == 1) { + exitflag = work->linsys_solver->update_rho_vec(work->linsys_solver, + work->rho_vec); + } + + return exitflag; +} + +#endif // EMBEDDED != 1 + + +void swap_vectors(c_float **a, c_float **b) { + c_float *temp; + + temp = *b; + *b = *a; + *a = temp; +} + +void cold_start(OSQPWorkspace *work) { + vec_set_scalar(work->x, 0., work->data->n); + vec_set_scalar(work->z, 0., work->data->m); + vec_set_scalar(work->y, 0., work->data->m); +} + +static void compute_rhs(OSQPWorkspace *work) { + c_int i; // Index + + for (i = 0; i < work->data->n; i++) { + // Cycle over part related to x variables + work->xz_tilde[i] = work->settings->sigma * work->x_prev[i] - + work->data->q[i]; + } + + for (i = 0; i < work->data->m; i++) { + // Cycle over dual variable in the first step (nu) + work->xz_tilde[i + work->data->n] = work->z_prev[i] - work->rho_inv_vec[i] * + work->y[i]; + } +} + +void update_xz_tilde(OSQPWorkspace *work) { + // Compute right-hand side + compute_rhs(work); + + // Solve linear system + work->linsys_solver->solve(work->linsys_solver, work->xz_tilde); +} + +void update_x(OSQPWorkspace *work) { + c_int i; + + // update x + for (i = 0; i < work->data->n; i++) { + work->x[i] = work->settings->alpha * work->xz_tilde[i] + + ((c_float)1.0 - work->settings->alpha) * work->x_prev[i]; + } + + // update delta_x + for (i = 0; i < work->data->n; i++) { + work->delta_x[i] = work->x[i] - work->x_prev[i]; + } +} + +void update_z(OSQPWorkspace *work) { + c_int i; + + // update z + for (i = 0; i < work->data->m; i++) { + work->z[i] = work->settings->alpha * work->xz_tilde[i + work->data->n] + + ((c_float)1.0 - work->settings->alpha) * work->z_prev[i] + + work->rho_inv_vec[i] * work->y[i]; + } + + // project z + project(work, work->z); +} + +void update_y(OSQPWorkspace *work) { + c_int i; // Index + + for (i = 0; i < work->data->m; i++) { + work->delta_y[i] = work->rho_vec[i] * + (work->settings->alpha * + work->xz_tilde[i + work->data->n] + + ((c_float)1.0 - work->settings->alpha) * work->z_prev[i] - + work->z[i]); + work->y[i] += work->delta_y[i]; + } +} + +c_float compute_obj_val(OSQPWorkspace *work, c_float *x) { + c_float obj_val; + + obj_val = quad_form(work->data->P, x) + + vec_prod(work->data->q, x, work->data->n); + + if (work->settings->scaling) { + obj_val *= work->scaling->cinv; + } + + return obj_val; +} + +c_float compute_pri_res(OSQPWorkspace *work, c_float *x, c_float *z) { + // NB: Use z_prev as working vector + // pr = Ax - z + + mat_vec(work->data->A, x, work->Ax, 0); // Ax + vec_add_scaled(work->z_prev, work->Ax, z, work->data->m, -1); + + // If scaling active -> rescale residual + if (work->settings->scaling && !work->settings->scaled_termination) { + return vec_scaled_norm_inf(work->scaling->Einv, work->z_prev, work->data->m); + } + + // Return norm of the residual + return vec_norm_inf(work->z_prev, work->data->m); +} + +c_float compute_pri_tol(OSQPWorkspace *work, c_float eps_abs, c_float eps_rel) { + c_float max_rel_eps, temp_rel_eps; + + // max_rel_eps = max(||z||, ||A x||) + if (work->settings->scaling && !work->settings->scaled_termination) { + // ||Einv * z|| + max_rel_eps = + vec_scaled_norm_inf(work->scaling->Einv, work->z, work->data->m); + + // ||Einv * A * x|| + temp_rel_eps = vec_scaled_norm_inf(work->scaling->Einv, + work->Ax, + work->data->m); + + // Choose maximum + max_rel_eps = c_max(max_rel_eps, temp_rel_eps); + } else { // No unscaling required + // ||z|| + max_rel_eps = vec_norm_inf(work->z, work->data->m); + + // ||A * x|| + temp_rel_eps = vec_norm_inf(work->Ax, work->data->m); + + // Choose maximum + max_rel_eps = c_max(max_rel_eps, temp_rel_eps); + } + + // eps_prim + return eps_abs + eps_rel * max_rel_eps; +} + +c_float compute_dua_res(OSQPWorkspace *work, c_float *x, c_float *y) { + // NB: Use x_prev as temporary vector + // NB: Only upper triangular part of P is stored. + // dr = q + A'*y + P*x + + // dr = q + prea_vec_copy(work->data->q, work->x_prev, work->data->n); + + // P * x (upper triangular part) + mat_vec(work->data->P, x, work->Px, 0); + + // P' * x (lower triangular part with no diagonal) + mat_tpose_vec(work->data->P, x, work->Px, 1, 1); + + // dr += P * x (full P matrix) + vec_add_scaled(work->x_prev, work->x_prev, work->Px, work->data->n, 1); + + // dr += A' * y + if (work->data->m > 0) { + mat_tpose_vec(work->data->A, y, work->Aty, 0, 0); + vec_add_scaled(work->x_prev, work->x_prev, work->Aty, work->data->n, 1); + } + + // If scaling active -> rescale residual + if (work->settings->scaling && !work->settings->scaled_termination) { + return work->scaling->cinv * vec_scaled_norm_inf(work->scaling->Dinv, + work->x_prev, + work->data->n); + } + + return vec_norm_inf(work->x_prev, work->data->n); +} + +c_float compute_dua_tol(OSQPWorkspace *work, c_float eps_abs, c_float eps_rel) { + c_float max_rel_eps, temp_rel_eps; + + // max_rel_eps = max(||q||, ||A' y|, ||P x||) + if (work->settings->scaling && !work->settings->scaled_termination) { + // || Dinv q|| + max_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv, + work->data->q, + work->data->n); + + // || Dinv A' y || + temp_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv, + work->Aty, + work->data->n); + max_rel_eps = c_max(max_rel_eps, temp_rel_eps); + + // || Dinv P x|| + temp_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv, + work->Px, + work->data->n); + max_rel_eps = c_max(max_rel_eps, temp_rel_eps); + + // Multiply by cinv + max_rel_eps *= work->scaling->cinv; + } else { // No scaling required + // ||q|| + max_rel_eps = vec_norm_inf(work->data->q, work->data->n); + + // ||A'*y|| + temp_rel_eps = vec_norm_inf(work->Aty, work->data->n); + max_rel_eps = c_max(max_rel_eps, temp_rel_eps); + + // ||P*x|| + temp_rel_eps = vec_norm_inf(work->Px, work->data->n); + max_rel_eps = c_max(max_rel_eps, temp_rel_eps); + } + + // eps_dual + return eps_abs + eps_rel * max_rel_eps; +} + +c_int is_primal_infeasible(OSQPWorkspace *work, c_float eps_prim_inf) { + // This function checks for the primal infeasibility termination criteria. + // + // 1) A' * delta_y < eps * ||delta_y|| + // + // 2) u'*max(delta_y, 0) + l'*min(delta_y, 0) < eps * ||delta_y|| + // + + c_int i; // Index for loops + c_float norm_delta_y; + c_float ineq_lhs = 0.0; + + // Project delta_y onto the polar of the recession cone of [l,u] + for (i = 0; i < work->data->m; i++) { + if (work->data->u[i] > OSQP_INFTY * MIN_SCALING) { // Infinite upper bound + if (work->data->l[i] < -OSQP_INFTY * MIN_SCALING) { // Infinite lower bound + // Both bounds infinite + work->delta_y[i] = 0.0; + } else { + // Only upper bound infinite + work->delta_y[i] = c_min(work->delta_y[i], 0.0); + } + } else if (work->data->l[i] < -OSQP_INFTY * MIN_SCALING) { // Infinite lower bound + // Only lower bound infinite + work->delta_y[i] = c_max(work->delta_y[i], 0.0); + } + } + + // Compute infinity norm of delta_y (unscale if necessary) + if (work->settings->scaling && !work->settings->scaled_termination) { + // Use work->Adelta_x as temporary vector + vec_ew_prod(work->scaling->E, work->delta_y, work->Adelta_x, work->data->m); + norm_delta_y = vec_norm_inf(work->Adelta_x, work->data->m); + } else { + norm_delta_y = vec_norm_inf(work->delta_y, work->data->m); + } + + if (norm_delta_y > OSQP_DIVISION_TOL) { + + for (i = 0; i < work->data->m; i++) { + ineq_lhs += work->data->u[i] * c_max(work->delta_y[i], 0) + \ + work->data->l[i] * c_min(work->delta_y[i], 0); + } + + // Check if the condition is satisfied: ineq_lhs < -eps + if (ineq_lhs < eps_prim_inf * norm_delta_y) { + // Compute and return ||A'delta_y|| < eps_prim_inf + mat_tpose_vec(work->data->A, work->delta_y, work->Atdelta_y, 0, 0); + + // Unscale if necessary + if (work->settings->scaling && !work->settings->scaled_termination) { + vec_ew_prod(work->scaling->Dinv, + work->Atdelta_y, + work->Atdelta_y, + work->data->n); + } + + return vec_norm_inf(work->Atdelta_y, work->data->n) < eps_prim_inf * norm_delta_y; + } + } + + // Conditions not satisfied -> not primal infeasible + return 0; +} + +c_int is_dual_infeasible(OSQPWorkspace *work, c_float eps_dual_inf) { + // This function checks for the scaled dual infeasibility termination + // criteria. + // + // 1) q * delta_x < eps * || delta_x || + // + // 2) ||P * delta_x || < eps * || delta_x || + // + // 3) -> (A * delta_x)_i > -eps * || delta_x ||, l_i != -inf + // -> (A * delta_x)_i < eps * || delta_x ||, u_i != inf + // + + + c_int i; // Index for loops + c_float norm_delta_x; + c_float cost_scaling; + + // Compute norm of delta_x + if (work->settings->scaling && !work->settings->scaled_termination) { // Unscale + // if + // necessary + norm_delta_x = vec_scaled_norm_inf(work->scaling->D, + work->delta_x, + work->data->n); + cost_scaling = work->scaling->c; + } else { + norm_delta_x = vec_norm_inf(work->delta_x, work->data->n); + cost_scaling = 1.0; + } + + // Prevent 0 division || delta_x || > 0 + if (norm_delta_x > OSQP_DIVISION_TOL) { + // Normalize delta_x by its norm + + /* vec_mult_scalar(work->delta_x, 1./norm_delta_x, work->data->n); */ + + // Check first if q'*delta_x < 0 + if (vec_prod(work->data->q, work->delta_x, work->data->n) < + cost_scaling * eps_dual_inf * norm_delta_x) { + // Compute product P * delta_x (NB: P is store in upper triangular form) + mat_vec(work->data->P, work->delta_x, work->Pdelta_x, 0); + mat_tpose_vec(work->data->P, work->delta_x, work->Pdelta_x, 1, 1); + + // Scale if necessary + if (work->settings->scaling && !work->settings->scaled_termination) { + vec_ew_prod(work->scaling->Dinv, + work->Pdelta_x, + work->Pdelta_x, + work->data->n); + } + + // Check if || P * delta_x || = 0 + if (vec_norm_inf(work->Pdelta_x, work->data->n) < + cost_scaling * eps_dual_inf * norm_delta_x) { + // Compute A * delta_x + mat_vec(work->data->A, work->delta_x, work->Adelta_x, 0); + + // Scale if necessary + if (work->settings->scaling && !work->settings->scaled_termination) { + vec_ew_prod(work->scaling->Einv, + work->Adelta_x, + work->Adelta_x, + work->data->m); + } + + // De Morgan Law Applied to dual infeasibility conditions for A * x + // NB: Note that MIN_SCALING is used to adjust the infinity value + // in case the problem is scaled. + for (i = 0; i < work->data->m; i++) { + if (((work->data->u[i] < OSQP_INFTY * MIN_SCALING) && + (work->Adelta_x[i] > eps_dual_inf * norm_delta_x)) || + ((work->data->l[i] > -OSQP_INFTY * MIN_SCALING) && + (work->Adelta_x[i] < -eps_dual_inf * norm_delta_x))) { + // At least one condition not satisfied -> not dual infeasible + return 0; + } + } + + // All conditions passed -> dual infeasible + return 1; + } + } + } + + // Conditions not satisfied -> not dual infeasible + return 0; +} + +c_int has_solution(OSQPInfo * info){ + + return ((info->status_val != OSQP_PRIMAL_INFEASIBLE) && + (info->status_val != OSQP_PRIMAL_INFEASIBLE_INACCURATE) && + (info->status_val != OSQP_DUAL_INFEASIBLE) && + (info->status_val != OSQP_DUAL_INFEASIBLE_INACCURATE) && + (info->status_val != OSQP_NON_CVX)); + +} + +void store_solution(OSQPWorkspace *work) { +#ifndef EMBEDDED + c_float norm_vec; +#endif /* ifndef EMBEDDED */ + + if (has_solution(work->info)) { + prea_vec_copy(work->x, work->solution->x, work->data->n); // primal + prea_vec_copy(work->y, work->solution->y, work->data->m); // dual + + // Unscale solution if scaling has been performed + if (work->settings->scaling) + unscale_solution(work); + } else { + // No solution present. Solution is NaN + vec_set_scalar(work->solution->x, OSQP_NAN, work->data->n); + vec_set_scalar(work->solution->y, OSQP_NAN, work->data->m); + +#ifndef EMBEDDED + + // Normalize infeasibility certificates if embedded is off + // NB: It requires a division + if ((work->info->status_val == OSQP_PRIMAL_INFEASIBLE) || + ((work->info->status_val == OSQP_PRIMAL_INFEASIBLE_INACCURATE))) { + norm_vec = vec_norm_inf(work->delta_y, work->data->m); + vec_mult_scalar(work->delta_y, 1. / norm_vec, work->data->m); + } + + if ((work->info->status_val == OSQP_DUAL_INFEASIBLE) || + ((work->info->status_val == OSQP_DUAL_INFEASIBLE_INACCURATE))) { + norm_vec = vec_norm_inf(work->delta_x, work->data->n); + vec_mult_scalar(work->delta_x, 1. / norm_vec, work->data->n); + } + +#endif /* ifndef EMBEDDED */ + + // Cold start iterates to 0 for next runs (they cannot start from NaN) + cold_start(work); + } +} + +void update_info(OSQPWorkspace *work, + c_int iter, + c_int compute_objective, + c_int polish) { + c_float *x, *z, *y; // Allocate pointers to variables + c_float *obj_val, *pri_res, *dua_res; // objective value, residuals + +#ifdef PROFILING + c_float *run_time; // Execution time +#endif /* ifdef PROFILING */ + +#ifndef EMBEDDED + + if (polish) { + x = work->pol->x; + y = work->pol->y; + z = work->pol->z; + obj_val = &work->pol->obj_val; + pri_res = &work->pol->pri_res; + dua_res = &work->pol->dua_res; +# ifdef PROFILING + run_time = &work->info->polish_time; +# endif /* ifdef PROFILING */ + } else { +#endif // EMBEDDED + x = work->x; + y = work->y; + z = work->z; + obj_val = &work->info->obj_val; + pri_res = &work->info->pri_res; + dua_res = &work->info->dua_res; + work->info->iter = iter; // Update iteration number +#ifdef PROFILING + run_time = &work->info->solve_time; +#endif /* ifdef PROFILING */ +#ifndef EMBEDDED +} + +#endif /* ifndef EMBEDDED */ + + + // Compute the objective if needed + if (compute_objective) { + *obj_val = compute_obj_val(work, x); + } + + // Compute primal residual + if (work->data->m == 0) { + // No constraints -> Always primal feasible + *pri_res = 0.; + } else { + *pri_res = compute_pri_res(work, x, z); + } + + // Compute dual residual + *dua_res = compute_dua_res(work, x, y); + + // Update timing +#ifdef PROFILING + *run_time = osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + +#ifdef PRINTING + work->summary_printed = 0; // The just updated info have not been printed +#endif /* ifdef PRINTING */ +} + + +void reset_info(OSQPInfo *info) { +#ifdef PROFILING + + // Initialize info values. + info->solve_time = 0.0; // Solve time to zero +# ifndef EMBEDDED + info->polish_time = 0.0; // Polish time to zero +# endif /* ifndef EMBEDDED */ + + // NB: We do not reset the setup_time because it is performed only once +#endif /* ifdef PROFILING */ + + update_status(info, OSQP_UNSOLVED); // Problem is unsolved + +#if EMBEDDED != 1 + info->rho_updates = 0; // Rho updates are now 0 +#endif /* if EMBEDDED != 1 */ +} + +void update_status(OSQPInfo *info, c_int status_val) { + // Update status value + info->status_val = status_val; + + // Update status string depending on status val + if (status_val == OSQP_SOLVED) c_strcpy(info->status, "solved"); + + if (status_val == OSQP_SOLVED_INACCURATE) c_strcpy(info->status, + "solved inaccurate"); + else if (status_val == OSQP_PRIMAL_INFEASIBLE) c_strcpy(info->status, + "primal infeasible"); + else if (status_val == OSQP_PRIMAL_INFEASIBLE_INACCURATE) c_strcpy(info->status, + "primal infeasible inaccurate"); + else if (status_val == OSQP_UNSOLVED) c_strcpy(info->status, "unsolved"); + else if (status_val == OSQP_DUAL_INFEASIBLE) c_strcpy(info->status, + "dual infeasible"); + else if (status_val == OSQP_DUAL_INFEASIBLE_INACCURATE) c_strcpy(info->status, + "dual infeasible inaccurate"); + else if (status_val == OSQP_MAX_ITER_REACHED) c_strcpy(info->status, + "maximum iterations reached"); +#ifdef PROFILING + else if (status_val == OSQP_TIME_LIMIT_REACHED) c_strcpy(info->status, + "run time limit reached"); +#endif /* ifdef PROFILING */ + else if (status_val == OSQP_SIGINT) c_strcpy(info->status, "interrupted"); + + else if (status_val == OSQP_NON_CVX) c_strcpy(info->status, "problem non convex"); + +} + +c_int check_termination(OSQPWorkspace *work, c_int approximate) { + c_float eps_prim, eps_dual, eps_prim_inf, eps_dual_inf; + c_int exitflag; + c_int prim_res_check, dual_res_check, prim_inf_check, dual_inf_check; + c_float eps_abs, eps_rel; + + // Initialize variables to 0 + exitflag = 0; + prim_res_check = 0; dual_res_check = 0; + prim_inf_check = 0; dual_inf_check = 0; + + // Initialize tolerances + eps_abs = work->settings->eps_abs; + eps_rel = work->settings->eps_rel; + eps_prim_inf = work->settings->eps_prim_inf; + eps_dual_inf = work->settings->eps_dual_inf; + + // If residuals are too large, the problem is probably non convex + if ((work->info->pri_res > OSQP_INFTY) || + (work->info->dua_res > OSQP_INFTY)){ + // Looks like residuals are diverging. Probably the problem is non convex! + // Terminate and report it + update_status(work->info, OSQP_NON_CVX); + work->info->obj_val = OSQP_NAN; + return 1; + } + + // If approximate solution required, increase tolerances by 10 + if (approximate) { + eps_abs *= 10; + eps_rel *= 10; + eps_prim_inf *= 10; + eps_dual_inf *= 10; + } + + // Check residuals + if (work->data->m == 0) { + prim_res_check = 1; // No constraints -> Primal feasibility always satisfied + } + else { + // Compute primal tolerance + eps_prim = compute_pri_tol(work, eps_abs, eps_rel); + + // Primal feasibility check + if (work->info->pri_res < eps_prim) { + prim_res_check = 1; + } else { + // Primal infeasibility check + prim_inf_check = is_primal_infeasible(work, eps_prim_inf); + } + } // End check if m == 0 + + // Compute dual tolerance + eps_dual = compute_dua_tol(work, eps_abs, eps_rel); + + // Dual feasibility check + if (work->info->dua_res < eps_dual) { + dual_res_check = 1; + } else { + // Check dual infeasibility + dual_inf_check = is_dual_infeasible(work, eps_dual_inf); + } + + // Compare checks to determine solver status + if (prim_res_check && dual_res_check) { + // Update final information + if (approximate) { + update_status(work->info, OSQP_SOLVED_INACCURATE); + } else { + update_status(work->info, OSQP_SOLVED); + } + exitflag = 1; + } + else if (prim_inf_check) { + // Update final information + if (approximate) { + update_status(work->info, OSQP_PRIMAL_INFEASIBLE_INACCURATE); + } else { + update_status(work->info, OSQP_PRIMAL_INFEASIBLE); + } + + if (work->settings->scaling && !work->settings->scaled_termination) { + // Update infeasibility certificate + vec_ew_prod(work->scaling->E, work->delta_y, work->delta_y, work->data->m); + } + work->info->obj_val = OSQP_INFTY; + exitflag = 1; + } + else if (dual_inf_check) { + // Update final information + if (approximate) { + update_status(work->info, OSQP_DUAL_INFEASIBLE_INACCURATE); + } else { + update_status(work->info, OSQP_DUAL_INFEASIBLE); + } + + if (work->settings->scaling && !work->settings->scaled_termination) { + // Update infeasibility certificate + vec_ew_prod(work->scaling->D, work->delta_x, work->delta_x, work->data->n); + } + work->info->obj_val = -OSQP_INFTY; + exitflag = 1; + } + + return exitflag; +} + + +#ifndef EMBEDDED + +c_int validate_data(const OSQPData *data) { + c_int j, ptr; + + if (!data) { +# ifdef PRINTING + c_eprint("Missing data"); +# endif + return 1; + } + + if (!(data->P)) { +# ifdef PRINTING + c_eprint("Missing matrix P"); +# endif + return 1; + } + + if (!(data->A)) { +# ifdef PRINTING + c_eprint("Missing matrix A"); +# endif + return 1; + } + + if (!(data->q)) { +# ifdef PRINTING + c_eprint("Missing vector q"); +# endif + return 1; + } + + // General dimensions Tests + if ((data->n <= 0) || (data->m < 0)) { +# ifdef PRINTING + c_eprint("n must be positive and m nonnegative; n = %i, m = %i", + (int)data->n, (int)data->m); +# endif /* ifdef PRINTING */ + return 1; + } + + // Matrix P + if (data->P->m != data->n) { +# ifdef PRINTING + c_eprint("P does not have dimension n x n with n = %i", (int)data->n); +# endif /* ifdef PRINTING */ + return 1; + } + + if (data->P->m != data->P->n) { +# ifdef PRINTING + c_eprint("P is not square"); +# endif /* ifdef PRINTING */ + return 1; + } + + for (j = 0; j < data->n; j++) { // COLUMN + for (ptr = data->P->p[j]; ptr < data->P->p[j + 1]; ptr++) { + if (data->P->i[ptr] > j) { // if ROW > COLUMN +# ifdef PRINTING + c_eprint("P is not upper triangular"); +# endif /* ifdef PRINTING */ + return 1; + } + } + } + + // Matrix A + if ((data->A->m != data->m) || (data->A->n != data->n)) { +# ifdef PRINTING + c_eprint("A does not have dimension %i x %i", (int)data->m, (int)data->n); +# endif /* ifdef PRINTING */ + return 1; + } + + // Lower and upper bounds + for (j = 0; j < data->m; j++) { + if (data->l[j] > data->u[j]) { +# ifdef PRINTING + c_eprint("Lower bound at index %d is greater than upper bound: %.4e > %.4e", + (int)j, data->l[j], data->u[j]); +# endif /* ifdef PRINTING */ + return 1; + } + } + + // TODO: Complete with other checks + + return 0; +} + +c_int validate_linsys_solver(c_int linsys_solver) { + if ((linsys_solver != QDLDL_SOLVER) && + (linsys_solver != MKL_PARDISO_SOLVER)) { + return 1; + } + + // TODO: Add more solvers in case + + // Valid solver + return 0; +} + +c_int validate_settings(const OSQPSettings *settings) { + if (!settings) { +# ifdef PRINTING + c_eprint("Missing settings!"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->scaling < 0) { +# ifdef PRINTING + c_eprint("scaling must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } + + if ((settings->adaptive_rho != 0) && (settings->adaptive_rho != 1)) { +# ifdef PRINTING + c_eprint("adaptive_rho must be either 0 or 1"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->adaptive_rho_interval < 0) { +# ifdef PRINTING + c_eprint("adaptive_rho_interval must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } +# ifdef PROFILING + + if (settings->adaptive_rho_fraction <= 0) { +# ifdef PRINTING + c_eprint("adaptive_rho_fraction must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } +# endif /* ifdef PROFILING */ + + if (settings->adaptive_rho_tolerance < 1.0) { +# ifdef PRINTING + c_eprint("adaptive_rho_tolerance must be >= 1"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->polish_refine_iter < 0) { +# ifdef PRINTING + c_eprint("polish_refine_iter must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->rho <= 0.0) { +# ifdef PRINTING + c_eprint("rho must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->sigma <= 0.0) { +# ifdef PRINTING + c_eprint("sigma must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->delta <= 0.0) { +# ifdef PRINTING + c_eprint("delta must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->max_iter <= 0) { +# ifdef PRINTING + c_eprint("max_iter must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->eps_abs < 0.0) { +# ifdef PRINTING + c_eprint("eps_abs must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->eps_rel < 0.0) { +# ifdef PRINTING + c_eprint("eps_rel must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } + + if ((settings->eps_rel == 0.0) && + (settings->eps_abs == 0.0)) { +# ifdef PRINTING + c_eprint("at least one of eps_abs and eps_rel must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->eps_prim_inf <= 0.0) { +# ifdef PRINTING + c_eprint("eps_prim_inf must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->eps_dual_inf <= 0.0) { +# ifdef PRINTING + c_eprint("eps_dual_inf must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + if ((settings->alpha <= 0.0) || + (settings->alpha >= 2.0)) { +# ifdef PRINTING + c_eprint("alpha must be strictly between 0 and 2"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (validate_linsys_solver(settings->linsys_solver)) { +# ifdef PRINTING + c_eprint("linsys_solver not recognized"); +# endif /* ifdef PRINTING */ + return 1; + } + + if ((settings->verbose != 0) && + (settings->verbose != 1)) { +# ifdef PRINTING + c_eprint("verbose must be either 0 or 1"); +# endif /* ifdef PRINTING */ + return 1; + } + + if ((settings->scaled_termination != 0) && + (settings->scaled_termination != 1)) { +# ifdef PRINTING + c_eprint("scaled_termination must be either 0 or 1"); +# endif /* ifdef PRINTING */ + return 1; + } + + if (settings->check_termination < 0) { +# ifdef PRINTING + c_eprint("check_termination must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } + + if ((settings->warm_start != 0) && + (settings->warm_start != 1)) { +# ifdef PRINTING + c_eprint("warm_start must be either 0 or 1"); +# endif /* ifdef PRINTING */ + return 1; + } +# ifdef PROFILING + + if (settings->time_limit < 0.0) { +# ifdef PRINTING + c_eprint("time_limit must be nonnegative\n"); +# endif /* ifdef PRINTING */ + return 1; + } +# endif /* ifdef PROFILING */ + + return 0; +} + +#endif // #ifndef EMBEDDED diff --git a/src/lib/allocator_qp/src/osqp/error.c b/src/lib/allocator_qp/src/osqp/error.c new file mode 100644 index 000000000000..e0c069e65ce8 --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/error.c @@ -0,0 +1,21 @@ +#include "error.h" + +const char *OSQP_ERROR_MESSAGE[] = { + "Problem data validation.", + "Solver settings validation.", + "Linear system solver not available.\nTried to obtain it from shared library.", + "Linear system solver initialization.", + "KKT matrix factorization.\nThe problem seems to be non-convex.", + "Memory allocation.", + "Solver workspace not initialized.", +}; + + +c_int _osqp_error(enum osqp_error_type error_code, + const char * function_name) { +# ifdef PRINTING + c_print("ERROR in %s: %s\n", function_name, OSQP_ERROR_MESSAGE[error_code-1]); +# endif + return (c_int)error_code; +} + diff --git a/src/lib/allocator_qp/src/osqp/kkt.c b/src/lib/allocator_qp/src/osqp/kkt.c new file mode 100644 index 000000000000..f89aea0bf50c --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/kkt.c @@ -0,0 +1,224 @@ +#include "kkt.h" + +#ifndef EMBEDDED + + +csc* form_KKT(const csc *P, + const csc *A, + c_int format, + c_float param1, + c_float *param2, + c_int *PtoKKT, + c_int *AtoKKT, + c_int **Pdiag_idx, + c_int *Pdiag_n, + c_int *param2toKKT) { + c_int nKKT, nnzKKTmax; // Size, number of nonzeros and max number of nonzeros + // in KKT matrix + csc *KKT_trip, *KKT; // KKT matrix in triplet format and CSC format + c_int ptr, i, j; // Counters for elements (i,j) and index pointer + c_int zKKT = 0; // Counter for total number of elements in P and in + // KKT + c_int *KKT_TtoC; // Pointer to vector mapping from KKT in triplet form + // to CSC + + // Get matrix dimensions + nKKT = P->m + A->m; + + // Get maximum number of nonzero elements (only upper triangular part) + nnzKKTmax = P->p[P->n] + // Number of elements in P + P->m + // Number of elements in param1 * I + A->p[A->n] + // Number of nonzeros in A + A->m; // Number of elements in - diag(param2) + + // Preallocate KKT matrix in triplet format + KKT_trip = csc_spalloc(nKKT, nKKT, nnzKKTmax, 1, 1); + + if (!KKT_trip) return OSQP_NULL; // Failed to preallocate matrix + + // Allocate vector of indices on the diagonal. Worst case it has m elements + if (Pdiag_idx != OSQP_NULL) { + (*Pdiag_idx) = c_malloc(P->m * sizeof(c_int)); + *Pdiag_n = 0; // Set 0 diagonal elements to start + } + + // Allocate Triplet matrices + // P + param1 I + for (j = 0; j < P->n; j++) { // cycle over columns + // No elements in column j => add diagonal element param1 + if (P->p[j] == P->p[j + 1]) { + KKT_trip->i[zKKT] = j; + KKT_trip->p[zKKT] = j; + KKT_trip->x[zKKT] = param1; + zKKT++; + } + + for (ptr = P->p[j]; ptr < P->p[j + 1]; ptr++) { // cycle over rows + // Get current row + i = P->i[ptr]; + + // Add element of P + KKT_trip->i[zKKT] = i; + KKT_trip->p[zKKT] = j; + KKT_trip->x[zKKT] = P->x[ptr]; + + if (PtoKKT != OSQP_NULL) PtoKKT[ptr] = zKKT; // Update index from P to + // KKTtrip + + if (i == j) { // P has a diagonal element, + // add param1 + KKT_trip->x[zKKT] += param1; + + // If index vector pointer supplied -> Store the index + if (Pdiag_idx != OSQP_NULL) { + (*Pdiag_idx)[*Pdiag_n] = ptr; + (*Pdiag_n)++; + } + } + zKKT++; + + // Add diagonal param1 in case + if ((i < j) && // Diagonal element not reached + (ptr + 1 == P->p[j + 1])) { // last element of column j + // Add diagonal element param1 + KKT_trip->i[zKKT] = j; + KKT_trip->p[zKKT] = j; + KKT_trip->x[zKKT] = param1; + zKKT++; + } + } + } + + if (Pdiag_idx != OSQP_NULL) { + // Realloc Pdiag_idx so that it contains exactly *Pdiag_n diagonal elements + (*Pdiag_idx) = c_realloc((*Pdiag_idx), (*Pdiag_n) * sizeof(c_int)); + } + + + // A' at top right + for (j = 0; j < A->n; j++) { // Cycle over columns of A + for (ptr = A->p[j]; ptr < A->p[j + 1]; ptr++) { + KKT_trip->p[zKKT] = P->m + A->i[ptr]; // Assign column index from + // row index of A + KKT_trip->i[zKKT] = j; // Assign row index from + // column index of A + KKT_trip->x[zKKT] = A->x[ptr]; // Assign A value element + + if (AtoKKT != OSQP_NULL) AtoKKT[ptr] = zKKT; // Update index from A to + // KKTtrip + zKKT++; + } + } + + // - diag(param2) at bottom right + for (j = 0; j < A->m; j++) { + KKT_trip->i[zKKT] = j + P->n; + KKT_trip->p[zKKT] = j + P->n; + KKT_trip->x[zKKT] = -param2[j]; + + if (param2toKKT != OSQP_NULL) param2toKKT[j] = zKKT; // Update index from + // param2 to KKTtrip + zKKT++; + } + + // Allocate number of nonzeros + KKT_trip->nz = zKKT; + + // Convert triplet matrix to csc format + if (!PtoKKT && !AtoKKT && !param2toKKT) { + // If no index vectors passed, do not store KKT mapping from Trip to CSC/CSR + if (format == 0) KKT = triplet_to_csc(KKT_trip, OSQP_NULL); + else KKT = triplet_to_csr(KKT_trip, OSQP_NULL); + } + else { + // Allocate vector of indices from triplet to csc + KKT_TtoC = c_malloc((zKKT) * sizeof(c_int)); + + if (!KKT_TtoC) { + // Error in allocating KKT_TtoC vector + csc_spfree(KKT_trip); + c_free(*Pdiag_idx); + return OSQP_NULL; + } + + // Store KKT mapping from Trip to CSC/CSR + if (format == 0) + KKT = triplet_to_csc(KKT_trip, KKT_TtoC); + else + KKT = triplet_to_csr(KKT_trip, KKT_TtoC); + + // Update vectors of indices from P, A, param2 to KKT (now in CSC format) + if (PtoKKT != OSQP_NULL) { + for (i = 0; i < P->p[P->n]; i++) { + PtoKKT[i] = KKT_TtoC[PtoKKT[i]]; + } + } + + if (AtoKKT != OSQP_NULL) { + for (i = 0; i < A->p[A->n]; i++) { + AtoKKT[i] = KKT_TtoC[AtoKKT[i]]; + } + } + + if (param2toKKT != OSQP_NULL) { + for (i = 0; i < A->m; i++) { + param2toKKT[i] = KKT_TtoC[param2toKKT[i]]; + } + } + + // Free mapping + c_free(KKT_TtoC); + } + + // Clean matrix in triplet format and return result + csc_spfree(KKT_trip); + + return KKT; +} + +#endif /* ifndef EMBEDDED */ + + +#if EMBEDDED != 1 + +void update_KKT_P(csc *KKT, + const csc *P, + const c_int *PtoKKT, + const c_float param1, + const c_int *Pdiag_idx, + const c_int Pdiag_n) { + c_int i, j; // Iterations + + // Update elements of KKT using P + for (i = 0; i < P->p[P->n]; i++) { + KKT->x[PtoKKT[i]] = P->x[i]; + } + + // Update diagonal elements of KKT by adding sigma + for (i = 0; i < Pdiag_n; i++) { + j = Pdiag_idx[i]; // Extract index of the element on the + // diagonal + KKT->x[PtoKKT[j]] += param1; + } +} + +void update_KKT_A(csc *KKT, const csc *A, const c_int *AtoKKT) { + c_int i; // Iterations + + // Update elements of KKT using A + for (i = 0; i < A->p[A->n]; i++) { + KKT->x[AtoKKT[i]] = A->x[i]; + } +} + +void update_KKT_param2(csc *KKT, const c_float *param2, + const c_int *param2toKKT, const c_int m) { + c_int i; // Iterations + + // Update elements of KKT using param2 + for (i = 0; i < m; i++) { + KKT->x[param2toKKT[i]] = -param2[i]; + } +} + +#endif // EMBEDDED != 1 diff --git a/src/lib/allocator_qp/src/osqp/lin_alg.c b/src/lib/allocator_qp/src/osqp/lin_alg.c new file mode 100644 index 000000000000..ba896f5d46bb --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/lin_alg.c @@ -0,0 +1,413 @@ +#include "lin_alg.h" + + +/* VECTOR FUNCTIONS ----------------------------------------------------------*/ + + +void vec_add_scaled(c_float *c, + const c_float *a, + const c_float *b, + c_int n, + c_float sc) { + c_int i; + + for (i = 0; i < n; i++) { + c[i] = a[i] + sc * b[i]; + } +} + +c_float vec_scaled_norm_inf(const c_float *S, const c_float *v, c_int l) { + c_int i; + c_float abs_Sv_i; + c_float max = 0.0; + + for (i = 0; i < l; i++) { + abs_Sv_i = c_absval(S[i] * v[i]); + + if (abs_Sv_i > max) max = abs_Sv_i; + } + return max; +} + +c_float vec_norm_inf(const c_float *v, c_int l) { + c_int i; + c_float abs_v_i; + c_float max = 0.0; + + for (i = 0; i < l; i++) { + abs_v_i = c_absval(v[i]); + + if (abs_v_i > max) max = abs_v_i; + } + return max; +} + +c_float vec_norm_inf_diff(const c_float *a, const c_float *b, c_int l) { + c_float nmDiff = 0.0, tmp; + c_int i; + + for (i = 0; i < l; i++) { + tmp = c_absval(a[i] - b[i]); + + if (tmp > nmDiff) nmDiff = tmp; + } + return nmDiff; +} + +c_float vec_mean(const c_float *a, c_int n) { + c_float mean = 0.0; + c_int i; + + for (i = 0; i < n; i++) { + mean += a[i]; + } + mean /= (c_float)n; + + return mean; +} + +void int_vec_set_scalar(c_int *a, c_int sc, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] = sc; + } +} + +void vec_set_scalar(c_float *a, c_float sc, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] = sc; + } +} + +void vec_add_scalar(c_float *a, c_float sc, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] += sc; + } +} + +void vec_mult_scalar(c_float *a, c_float sc, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] *= sc; + } +} + +#ifndef EMBEDDED +c_float* vec_copy(c_float *a, c_int n) { + c_float *b; + c_int i; + + b = c_malloc(n * sizeof(c_float)); + if (!b) return OSQP_NULL; + + for (i = 0; i < n; i++) { + b[i] = a[i]; + } + + return b; +} + +#endif // end EMBEDDED + + +void prea_int_vec_copy(const c_int *a, c_int *b, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + b[i] = a[i]; + } +} + +void prea_vec_copy(const c_float *a, c_float *b, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + b[i] = a[i]; + } +} + +void vec_ew_recipr(const c_float *a, c_float *b, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + b[i] = (c_float)1.0 / a[i]; + } +} + +c_float vec_prod(const c_float *a, const c_float *b, c_int n) { + c_float prod = 0.0; + c_int i; // Index + + for (i = 0; i < n; i++) { + prod += a[i] * b[i]; + } + + return prod; +} + +void vec_ew_prod(const c_float *a, const c_float *b, c_float *c, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + c[i] = b[i] * a[i]; + } +} + +#if EMBEDDED != 1 +void vec_ew_sqrt(c_float *a, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] = c_sqrt(a[i]); + } +} + +void vec_ew_max(c_float *a, c_int n, c_float max_val) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] = c_max(a[i], max_val); + } +} + +void vec_ew_min(c_float *a, c_int n, c_float min_val) { + c_int i; + + for (i = 0; i < n; i++) { + a[i] = c_min(a[i], min_val); + } +} + +void vec_ew_max_vec(const c_float *a, const c_float *b, c_float *c, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + c[i] = c_max(a[i], b[i]); + } +} + +void vec_ew_min_vec(const c_float *a, const c_float *b, c_float *c, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + c[i] = c_min(a[i], b[i]); + } +} + +#endif // EMBEDDED != 1 + + +/* MATRIX FUNCTIONS ----------------------------------------------------------*/ + +/* multiply scalar to matrix */ +void mat_mult_scalar(csc *A, c_float sc) { + c_int i, nnzA; + + nnzA = A->p[A->n]; + + for (i = 0; i < nnzA; i++) { + A->x[i] *= sc; + } +} + +void mat_premult_diag(csc *A, const c_float *d) { + c_int j, i; + + for (j = 0; j < A->n; j++) { // Cycle over columns + for (i = A->p[j]; i < A->p[j + 1]; i++) { // Cycle every row in the column + A->x[i] *= d[A->i[i]]; // Scale by corresponding element + // of d for row i + } + } +} + +void mat_postmult_diag(csc *A, const c_float *d) { + c_int j, i; + + for (j = 0; j < A->n; j++) { // Cycle over columns j + for (i = A->p[j]; i < A->p[j + 1]; i++) { // Cycle every row i in column j + A->x[i] *= d[j]; // Scale by corresponding element + // of d for column j + } + } +} + +void mat_vec(const csc *A, const c_float *x, c_float *y, c_int plus_eq) { + c_int i, j; + + if (!plus_eq) { + // y = 0 + for (i = 0; i < A->m; i++) { + y[i] = 0; + } + } + + // if A is empty + if (A->p[A->n] == 0) { + return; + } + + if (plus_eq == -1) { + // y -= A*x + for (j = 0; j < A->n; j++) { + for (i = A->p[j]; i < A->p[j + 1]; i++) { + y[A->i[i]] -= A->x[i] * x[j]; + } + } + } else { + // y += A*x + for (j = 0; j < A->n; j++) { + for (i = A->p[j]; i < A->p[j + 1]; i++) { + y[A->i[i]] += A->x[i] * x[j]; + } + } + } +} + +void mat_tpose_vec(const csc *A, const c_float *x, c_float *y, + c_int plus_eq, c_int skip_diag) { + c_int i, j, k; + + if (!plus_eq) { + // y = 0 + for (i = 0; i < A->n; i++) { + y[i] = 0; + } + } + + // if A is empty + if (A->p[A->n] == 0) { + return; + } + + if (plus_eq == -1) { + // y -= A*x + if (skip_diag) { + for (j = 0; j < A->n; j++) { + for (k = A->p[j]; k < A->p[j + 1]; k++) { + i = A->i[k]; + y[j] -= i == j ? 0 : A->x[k] * x[i]; + } + } + } else { + for (j = 0; j < A->n; j++) { + for (k = A->p[j]; k < A->p[j + 1]; k++) { + y[j] -= A->x[k] * x[A->i[k]]; + } + } + } + } else { + // y += A*x + if (skip_diag) { + for (j = 0; j < A->n; j++) { + for (k = A->p[j]; k < A->p[j + 1]; k++) { + i = A->i[k]; + y[j] += i == j ? 0 : A->x[k] * x[i]; + } + } + } else { + for (j = 0; j < A->n; j++) { + for (k = A->p[j]; k < A->p[j + 1]; k++) { + y[j] += A->x[k] * x[A->i[k]]; + } + } + } + } +} + +#if EMBEDDED != 1 +void mat_inf_norm_cols(const csc *M, c_float *E) { + c_int j, ptr; + + // Initialize zero max elements + for (j = 0; j < M->n; j++) { + E[j] = 0.; + } + + // Compute maximum across columns + for (j = 0; j < M->n; j++) { + for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) { + E[j] = c_max(c_absval(M->x[ptr]), E[j]); + } + } +} + +void mat_inf_norm_rows(const csc *M, c_float *E) { + c_int i, j, ptr; + + // Initialize zero max elements + for (j = 0; j < M->m; j++) { + E[j] = 0.; + } + + // Compute maximum across rows + for (j = 0; j < M->n; j++) { + for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) { + i = M->i[ptr]; + E[i] = c_max(c_absval(M->x[ptr]), E[i]); + } + } +} + +void mat_inf_norm_cols_sym_triu(const csc *M, c_float *E) { + c_int i, j, ptr; + c_float abs_x; + + // Initialize zero max elements + for (j = 0; j < M->n; j++) { + E[j] = 0.; + } + + // Compute maximum across columns + // Note that element (i, j) contributes to + // -> Column j (as expected in any matrices) + // -> Column i (which is equal to row i for symmetric matrices) + for (j = 0; j < M->n; j++) { + for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) { + i = M->i[ptr]; + abs_x = c_absval(M->x[ptr]); + E[j] = c_max(abs_x, E[j]); + + if (i != j) { + E[i] = c_max(abs_x, E[i]); + } + } + } +} + +#endif /* if EMBEDDED != 1 */ + + +c_float quad_form(const csc *P, const c_float *x) { + c_float quad_form = 0.; + c_int i, j, ptr; // Pointers to iterate over + // matrix: (i,j) a element + // pointer + + for (j = 0; j < P->n; j++) { // Iterate over columns + for (ptr = P->p[j]; ptr < P->p[j + 1]; ptr++) { // Iterate over rows + i = P->i[ptr]; // Row index + + if (i == j) { // Diagonal element + quad_form += (c_float).5 * P->x[ptr] * x[i] * x[i]; + } + else if (i < j) { // Off-diagonal element + quad_form += P->x[ptr] * x[i] * x[j]; + } + else { // Element in lower diagonal + // part +#ifdef PRINTING + c_eprint("quad_form matrix is not upper triangular"); +#endif /* ifdef PRINTING */ + return OSQP_NULL; + } + } + } + return quad_form; +} diff --git a/src/lib/allocator_qp/src/osqp/osqp.c b/src/lib/allocator_qp/src/osqp/osqp.c new file mode 100644 index 000000000000..76aae9547f08 --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/osqp.c @@ -0,0 +1,1604 @@ +#include "osqp.h" +#include "auxil.h" +#include "util.h" +#include "scaling.h" +#include "glob_opts.h" +#include "error.h" + + +#ifndef EMBEDDED +# include "polish.h" +#endif /* ifndef EMBEDDED */ + +#ifdef CTRLC +# include "ctrlc.h" +#endif /* ifdef CTRLC */ + +#ifndef EMBEDDED +# include "lin_sys.h" +#endif /* ifndef EMBEDDED */ + +/********************** +* Main API Functions * +**********************/ +void osqp_set_default_settings(OSQPSettings *settings) { + + settings->rho = (c_float)RHO; /* ADMM step */ + settings->sigma = (c_float)SIGMA; /* ADMM step */ + settings->scaling = SCALING; /* heuristic problem scaling */ +#if EMBEDDED != 1 + settings->adaptive_rho = ADAPTIVE_RHO; + settings->adaptive_rho_interval = ADAPTIVE_RHO_INTERVAL; + settings->adaptive_rho_tolerance = (c_float)ADAPTIVE_RHO_TOLERANCE; + +# ifdef PROFILING + settings->adaptive_rho_fraction = (c_float)ADAPTIVE_RHO_FRACTION; +# endif /* ifdef PROFILING */ +#endif /* if EMBEDDED != 1 */ + + settings->max_iter = MAX_ITER; /* maximum iterations to + take */ + settings->eps_abs = (c_float)EPS_ABS; /* absolute convergence + tolerance */ + settings->eps_rel = (c_float)EPS_REL; /* relative convergence + tolerance */ + settings->eps_prim_inf = (c_float)EPS_PRIM_INF; /* primal infeasibility + tolerance */ + settings->eps_dual_inf = (c_float)EPS_DUAL_INF; /* dual infeasibility + tolerance */ + settings->alpha = (c_float)ALPHA; /* relaxation parameter */ + settings->linsys_solver = LINSYS_SOLVER; /* relaxation parameter */ + +#ifndef EMBEDDED + settings->delta = DELTA; /* regularization parameter + for polish */ + settings->polish = POLISH; /* ADMM solution polish: 1 + */ + settings->polish_refine_iter = POLISH_REFINE_ITER; /* iterative refinement + steps in polish */ + settings->verbose = VERBOSE; /* print output */ +#endif /* ifndef EMBEDDED */ + + settings->scaled_termination = SCALED_TERMINATION; /* Evaluate scaled + termination criteria*/ + settings->check_termination = CHECK_TERMINATION; /* Interval for evaluating + termination criteria */ + settings->warm_start = WARM_START; /* warm starting */ + +#ifdef PROFILING + settings->time_limit = TIME_LIMIT; +#endif /* ifdef PROFILING */ +} + +#ifndef EMBEDDED + + +c_int osqp_setup(OSQPWorkspace** workp, const OSQPData *data, const OSQPSettings *settings) { + c_int exitflag; + + OSQPWorkspace * work; + + // Validate data + if (validate_data(data)) return osqp_error(OSQP_DATA_VALIDATION_ERROR); + + // Validate settings + if (validate_settings(settings)) return osqp_error(OSQP_SETTINGS_VALIDATION_ERROR); + + // Allocate empty workspace + work = c_calloc(1, sizeof(OSQPWorkspace)); + if (!(work)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + *workp = work; + + // Start and allocate directly timer +# ifdef PROFILING + work->timer = c_malloc(sizeof(OSQPTimer)); + if (!(work->timer)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + osqp_tic(work->timer); +# endif /* ifdef PROFILING */ + + // Copy problem data into workspace + work->data = c_malloc(sizeof(OSQPData)); + if (!(work->data)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + work->data->n = data->n; + work->data->m = data->m; + + // Cost function + work->data->P = copy_csc_mat(data->P); + work->data->q = vec_copy(data->q, data->n); + if (!(work->data->P) || !(work->data->q)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Constraints + work->data->A = copy_csc_mat(data->A); + if (!(work->data->A)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + work->data->l = vec_copy(data->l, data->m); + work->data->u = vec_copy(data->u, data->m); + if ( data->m && (!(work->data->l) || !(work->data->u)) ) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Vectorized rho parameter + work->rho_vec = c_malloc(data->m * sizeof(c_float)); + work->rho_inv_vec = c_malloc(data->m * sizeof(c_float)); + if ( data->m && (!(work->rho_vec) || !(work->rho_inv_vec)) ) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Type of constraints + work->constr_type = c_calloc(data->m, sizeof(c_int)); + if (data->m && !(work->constr_type)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Allocate internal solver variables (ADMM steps) + work->x = c_calloc(data->n, sizeof(c_float)); + work->z = c_calloc(data->m, sizeof(c_float)); + work->xz_tilde = c_calloc(data->n + data->m, sizeof(c_float)); + work->x_prev = c_calloc(data->n, sizeof(c_float)); + work->z_prev = c_calloc(data->m, sizeof(c_float)); + work->y = c_calloc(data->m, sizeof(c_float)); + if (!(work->x) || !(work->xz_tilde) || !(work->x_prev)) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + if ( data->m && (!(work->z) || !(work->z_prev) || !(work->y)) ) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Initialize variables x, y, z to 0 + cold_start(work); + + // Primal and dual residuals variables + work->Ax = c_calloc(data->m, sizeof(c_float)); + work->Px = c_calloc(data->n, sizeof(c_float)); + work->Aty = c_calloc(data->n, sizeof(c_float)); + + // Primal infeasibility variables + work->delta_y = c_calloc(data->m, sizeof(c_float)); + work->Atdelta_y = c_calloc(data->n, sizeof(c_float)); + + // Dual infeasibility variables + work->delta_x = c_calloc(data->n, sizeof(c_float)); + work->Pdelta_x = c_calloc(data->n, sizeof(c_float)); + work->Adelta_x = c_calloc(data->m, sizeof(c_float)); + + if (!(work->Px) || !(work->Aty) || !(work->Atdelta_y) || + !(work->delta_x) || !(work->Pdelta_x)) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + if ( data->m && (!(work->Ax) || !(work->delta_y) || !(work->Adelta_x)) ) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Copy settings + work->settings = copy_settings(settings); + if (!(work->settings)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Perform scaling + if (settings->scaling) { + // Allocate scaling structure + work->scaling = c_malloc(sizeof(OSQPScaling)); + if (!(work->scaling)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + work->scaling->D = c_malloc(data->n * sizeof(c_float)); + work->scaling->Dinv = c_malloc(data->n * sizeof(c_float)); + work->scaling->E = c_malloc(data->m * sizeof(c_float)); + work->scaling->Einv = c_malloc(data->m * sizeof(c_float)); + if (!(work->scaling->D) || !(work->scaling->Dinv)) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + if ( data->m && (!(work->scaling->E) || !(work->scaling->Einv)) ) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + + + // Allocate workspace variables used in scaling + work->D_temp = c_malloc(data->n * sizeof(c_float)); + work->D_temp_A = c_malloc(data->n * sizeof(c_float)); + work->E_temp = c_malloc(data->m * sizeof(c_float)); + // if (!(work->D_temp) || !(work->D_temp_A) || !(work->E_temp)) + // return osqp_error(OSQP_MEM_ALLOC_ERROR); + if (!(work->D_temp) || !(work->D_temp_A)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + if (data->m && !(work->E_temp)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Scale data + scale_data(work); + } else { + work->scaling = OSQP_NULL; + work->D_temp = OSQP_NULL; + work->D_temp_A = OSQP_NULL; + work->E_temp = OSQP_NULL; + } + + // Set type of constraints + set_rho_vec(work); + + // Load linear system solver + if (load_linsys_solver(work->settings->linsys_solver)) return osqp_error(OSQP_LINSYS_SOLVER_LOAD_ERROR); + + // Initialize linear system solver structure + exitflag = init_linsys_solver(&(work->linsys_solver), work->data->P, work->data->A, + work->settings->sigma, work->rho_vec, + work->settings->linsys_solver, 0); + + if (exitflag) { + return osqp_error(exitflag); + } + + // Initialize active constraints structure + work->pol = c_malloc(sizeof(OSQPPolish)); + if (!(work->pol)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + work->pol->Alow_to_A = c_malloc(data->m * sizeof(c_int)); + work->pol->Aupp_to_A = c_malloc(data->m * sizeof(c_int)); + work->pol->A_to_Alow = c_malloc(data->m * sizeof(c_int)); + work->pol->A_to_Aupp = c_malloc(data->m * sizeof(c_int)); + work->pol->x = c_malloc(data->n * sizeof(c_float)); + work->pol->z = c_malloc(data->m * sizeof(c_float)); + work->pol->y = c_malloc(data->m * sizeof(c_float)); + if (!(work->pol->x)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + if ( data->m && (!(work->pol->Alow_to_A) || !(work->pol->Aupp_to_A) || + !(work->pol->A_to_Alow) || !(work->pol->A_to_Aupp) || + !(work->pol->z) || !(work->pol->y)) ) + return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Allocate solution + work->solution = c_calloc(1, sizeof(OSQPSolution)); + if (!(work->solution)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + work->solution->x = c_calloc(1, data->n * sizeof(c_float)); + work->solution->y = c_calloc(1, data->m * sizeof(c_float)); + if (!(work->solution->x)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + if (data->m && !(work->solution->y)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + + // Allocate and initialize information + work->info = c_calloc(1, sizeof(OSQPInfo)); + if (!(work->info)) return osqp_error(OSQP_MEM_ALLOC_ERROR); + work->info->status_polish = 0; // Polishing not performed + update_status(work->info, OSQP_UNSOLVED); +# ifdef PROFILING + work->info->solve_time = 0.0; // Solve time to zero + work->info->update_time = 0.0; // Update time to zero + work->info->polish_time = 0.0; // Polish time to zero + work->info->run_time = 0.0; // Total run time to zero + work->info->setup_time = osqp_toc(work->timer); // Update timer information + + work->first_run = 1; + work->clear_update_time = 0; + work->rho_update_from_solve = 0; +# endif /* ifdef PROFILING */ + work->info->rho_updates = 0; // Rho updates set to 0 + work->info->rho_estimate = work->settings->rho; // Best rho estimate + + // Print header +# ifdef PRINTING + if (work->settings->verbose) print_setup_header(work); + work->summary_printed = 0; // Initialize last summary to not printed +# endif /* ifdef PRINTING */ + + + // If adaptive rho and automatic interval, but profiling disabled, we need to + // set the interval to a default value +# ifndef PROFILING + if (work->settings->adaptive_rho && !work->settings->adaptive_rho_interval) { + if (work->settings->check_termination) { + // If check_termination is enabled, we set it to a multiple of the check + // termination interval + work->settings->adaptive_rho_interval = ADAPTIVE_RHO_MULTIPLE_TERMINATION * + work->settings->check_termination; + } else { + // If check_termination is disabled we set it to a predefined fix number + work->settings->adaptive_rho_interval = ADAPTIVE_RHO_FIXED; + } + } +# endif /* ifndef PROFILING */ + + // Return exit flag + return 0; +} + +#endif // #ifndef EMBEDDED + + +c_int osqp_solve(OSQPWorkspace *work) { + + c_int exitflag; + c_int iter; + c_int compute_cost_function; // Boolean: compute the cost function in the loop or not + c_int can_check_termination; // Boolean: check termination or not + +#ifdef PROFILING + c_float temp_run_time; // Temporary variable to store current run time +#endif /* ifdef PROFILING */ + +#ifdef PRINTING + c_int can_print; // Boolean whether you can print +#endif /* ifdef PRINTING */ + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) + work->info->update_time = 0.0; + work->rho_update_from_solve = 1; +#endif /* ifdef PROFILING */ + + // Initialize variables + exitflag = 0; + can_check_termination = 0; +#ifdef PRINTING + can_print = work->settings->verbose; +#endif /* ifdef PRINTING */ +#ifdef PRINTING + compute_cost_function = work->settings->verbose; // Compute cost function only + // if verbose is on +#else /* ifdef PRINTING */ + compute_cost_function = 0; // Never compute cost + // function during the + // iterations if no printing + // enabled +#endif /* ifdef PRINTING */ + + + +#ifdef PROFILING + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + +#ifdef PRINTING + + if (work->settings->verbose) { + // Print Header for every column + print_header(); + } +#endif /* ifdef PRINTING */ + +#ifdef CTRLC + + // initialize Ctrl-C support + osqp_start_interrupt_listener(); +#endif /* ifdef CTRLC */ + + // Initialize variables (cold start or warm start depending on settings) + if (!work->settings->warm_start) cold_start(work); // If not warm start -> + // set x, z, y to zero + + // Main ADMM algorithm + for (iter = 1; iter <= work->settings->max_iter; iter++) { + // Update x_prev, z_prev (preallocated, no malloc) + swap_vectors(&(work->x), &(work->x_prev)); + swap_vectors(&(work->z), &(work->z_prev)); + + /* ADMM STEPS */ + /* Compute \tilde{x}^{k+1}, \tilde{z}^{k+1} */ + update_xz_tilde(work); + + /* Compute x^{k+1} */ + update_x(work); + + /* Compute z^{k+1} */ + update_z(work); + + /* Compute y^{k+1} */ + update_y(work); + + /* End of ADMM Steps */ + +#ifdef CTRLC + + // Check the interrupt signal + if (osqp_is_interrupted()) { + update_status(work->info, OSQP_SIGINT); +# ifdef PRINTING + c_print("Solver interrupted\n"); +# endif /* ifdef PRINTING */ + exitflag = 1; + goto exit; + } +#endif /* ifdef CTRLC */ + +#ifdef PROFILING + + // Check if solver time_limit is enabled. In case, check if the current + // run time is more than the time_limit option. + if (work->first_run) { + temp_run_time = work->info->setup_time + osqp_toc(work->timer); + } + else { + temp_run_time = work->info->update_time + osqp_toc(work->timer); + } + + if (work->settings->time_limit && + (temp_run_time >= work->settings->time_limit)) { + update_status(work->info, OSQP_TIME_LIMIT_REACHED); +# ifdef PRINTING + if (work->settings->verbose) c_print("run time limit reached\n"); + can_print = 0; // Not printing at this iteration +# endif /* ifdef PRINTING */ + break; + } +#endif /* ifdef PROFILING */ + + + // Can we check for termination ? + can_check_termination = work->settings->check_termination && + (iter % work->settings->check_termination == 0); + +#ifdef PRINTING + + // Can we print ? + can_print = work->settings->verbose && + ((iter % PRINT_INTERVAL == 0) || (iter == 1)); + + if (can_check_termination || can_print) { // Update status in either of + // these cases + // Update information + update_info(work, iter, compute_cost_function, 0); + + if (can_print) { + // Print summary + print_summary(work); + } + + if (can_check_termination) { + // Check algorithm termination + if (check_termination(work, 0)) { + // Terminate algorithm + break; + } + } + } +#else /* ifdef PRINTING */ + + if (can_check_termination) { + // Update information and compute also objective value + update_info(work, iter, compute_cost_function, 0); + + // Check algorithm termination + if (check_termination(work, 0)) { + // Terminate algorithm + break; + } + } +#endif /* ifdef PRINTING */ + + +#if EMBEDDED != 1 +# ifdef PROFILING + + // If adaptive rho with automatic interval, check if the solve time is a + // certain fraction + // of the setup time. + if (work->settings->adaptive_rho && !work->settings->adaptive_rho_interval) { + // Check time + if (osqp_toc(work->timer) > + work->settings->adaptive_rho_fraction * work->info->setup_time) { + // Enough time has passed. We now get the number of iterations between + // the updates. + if (work->settings->check_termination) { + // If check_termination is enabled, we round the number of iterations + // between + // rho updates to the closest multiple of check_termination + work->settings->adaptive_rho_interval = (c_int)c_roundmultiple(iter, + work->settings->check_termination); + } else { + // If check_termination is disabled, we round the number of iterations + // between + // updates to the closest multiple of the default check_termination + // interval. + work->settings->adaptive_rho_interval = (c_int)c_roundmultiple(iter, + CHECK_TERMINATION); + } + + // Make sure the interval is not 0 and at least check_termination times + work->settings->adaptive_rho_interval = c_max( + work->settings->adaptive_rho_interval, + work->settings->check_termination); + } // If time condition is met + } // If adaptive rho enabled and interval set to auto +# endif // PROFILING + + // Adapt rho + if (work->settings->adaptive_rho && + work->settings->adaptive_rho_interval && + (iter % work->settings->adaptive_rho_interval == 0)) { + // Update info with the residuals if it hasn't been done before +# ifdef PRINTING + + if (!can_check_termination && !can_print) { + // Information has not been computed neither for termination or printing + // reasons + update_info(work, iter, compute_cost_function, 0); + } +# else /* ifdef PRINTING */ + + if (!can_check_termination) { + // Information has not been computed before for termination check + update_info(work, iter, compute_cost_function, 0); + } +# endif /* ifdef PRINTING */ + + // Actually update rho + if (adapt_rho(work)) { +# ifdef PRINTING + c_eprint("Failed rho update"); +# endif // PRINTING + exitflag = 1; + goto exit; + } + } +#endif // EMBEDDED != 1 + + } // End of ADMM for loop + + + // Update information and check termination condition if it hasn't been done + // during last iteration (max_iter reached or check_termination disabled) + if (!can_check_termination) { + /* Update information */ +#ifdef PRINTING + + if (!can_print) { + // Update info only if it hasn't been updated before for printing + // reasons + update_info(work, iter - 1, compute_cost_function, 0); + } +#else /* ifdef PRINTING */ + + // If no printing is enabled, update info directly + update_info(work, iter - 1, compute_cost_function, 0); +#endif /* ifdef PRINTING */ + +#ifdef PRINTING + + /* Print summary */ + if (work->settings->verbose && !work->summary_printed) print_summary(work); +#endif /* ifdef PRINTING */ + + /* Check whether a termination criterion is triggered */ + check_termination(work, 0); + } + + // Compute objective value in case it was not + // computed during the iterations + if (!compute_cost_function && has_solution(work->info)){ + work->info->obj_val = compute_obj_val(work, work->x); + } + + +#ifdef PRINTING + /* Print summary for last iteration */ + if (work->settings->verbose && !work->summary_printed) { + print_summary(work); + } +#endif /* ifdef PRINTING */ + + /* if max iterations reached, change status accordingly */ + if (work->info->status_val == OSQP_UNSOLVED) { + if (!check_termination(work, 1)) { // Try to check for approximate + update_status(work->info, OSQP_MAX_ITER_REACHED); + } + } + +#ifdef PROFILING + /* if time-limit reached check termination and update status accordingly */ + if (work->info->status_val == OSQP_TIME_LIMIT_REACHED) { + if (!check_termination(work, 1)) { // Try for approximate solutions + update_status(work->info, OSQP_TIME_LIMIT_REACHED); /* Change update status back to OSQP_TIME_LIMIT_REACHED */ + } + } +#endif /* ifdef PROFILING */ + + +#if EMBEDDED != 1 + /* Update rho estimate */ + work->info->rho_estimate = compute_rho_estimate(work); +#endif /* if EMBEDDED != 1 */ + + /* Update solve time */ +#ifdef PROFILING + work->info->solve_time = osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + +#ifndef EMBEDDED + // Polish the obtained solution + if (work->settings->polish && (work->info->status_val == OSQP_SOLVED)) + polish(work); +#endif /* ifndef EMBEDDED */ + +#ifdef PROFILING + /* Update total time */ + if (work->first_run) { + // total time: setup + solve + polish + work->info->run_time = work->info->setup_time + + work->info->solve_time + + work->info->polish_time; + } else { + // total time: update + solve + polish + work->info->run_time = work->info->update_time + + work->info->solve_time + + work->info->polish_time; + } + + // Indicate that the solve function has already been executed + if (work->first_run) work->first_run = 0; + + // Indicate that the update_time should be set to zero + work->clear_update_time = 1; + + // Indicate that osqp_update_rho is not called from osqp_solve + work->rho_update_from_solve = 0; +#endif /* ifdef PROFILING */ + +#ifdef PRINTING + /* Print final footer */ + if (work->settings->verbose) print_footer(work->info, work->settings->polish); +#endif /* ifdef PRINTING */ + + // Store solution + store_solution(work); + + +// Define exit flag for quitting function +#if defined(PROFILING) || defined(CTRLC) || EMBEDDED != 1 +exit: +#endif /* if defined(PROFILING) || defined(CTRLC) || EMBEDDED != 1 */ + +#ifdef CTRLC + // Restore previous signal handler + osqp_end_interrupt_listener(); +#endif /* ifdef CTRLC */ + + return exitflag; +} + + +#ifndef EMBEDDED + +c_int osqp_cleanup(OSQPWorkspace *work) { + c_int exitflag = 0; + + if (work) { // If workspace has been allocated + // Free Data + if (work->data) { + if (work->data->P) csc_spfree(work->data->P); + if (work->data->A) csc_spfree(work->data->A); + if (work->data->q) c_free(work->data->q); + if (work->data->l) c_free(work->data->l); + if (work->data->u) c_free(work->data->u); + c_free(work->data); + } + + // Free scaling variables + if (work->scaling){ + if (work->scaling->D) c_free(work->scaling->D); + if (work->scaling->Dinv) c_free(work->scaling->Dinv); + if (work->scaling->E) c_free(work->scaling->E); + if (work->scaling->Einv) c_free(work->scaling->Einv); + c_free(work->scaling); + } + + // Free temp workspace variables for scaling + if (work->D_temp) c_free(work->D_temp); + if (work->D_temp_A) c_free(work->D_temp_A); + if (work->E_temp) c_free(work->E_temp); + + // Free linear system solver structure + if (work->linsys_solver) { + if (work->linsys_solver->free) { + work->linsys_solver->free(work->linsys_solver); + } + } + + // Unload linear system solver after free + if (work->settings) { + exitflag = unload_linsys_solver(work->settings->linsys_solver); + } + +#ifndef EMBEDDED + // Free active constraints structure + if (work->pol) { + if (work->pol->Alow_to_A) c_free(work->pol->Alow_to_A); + if (work->pol->Aupp_to_A) c_free(work->pol->Aupp_to_A); + if (work->pol->A_to_Alow) c_free(work->pol->A_to_Alow); + if (work->pol->A_to_Aupp) c_free(work->pol->A_to_Aupp); + if (work->pol->x) c_free(work->pol->x); + if (work->pol->z) c_free(work->pol->z); + if (work->pol->y) c_free(work->pol->y); + c_free(work->pol); + } +#endif /* ifndef EMBEDDED */ + + // Free other Variables + if (work->rho_vec) c_free(work->rho_vec); + if (work->rho_inv_vec) c_free(work->rho_inv_vec); +#if EMBEDDED != 1 + if (work->constr_type) c_free(work->constr_type); +#endif + if (work->x) c_free(work->x); + if (work->z) c_free(work->z); + if (work->xz_tilde) c_free(work->xz_tilde); + if (work->x_prev) c_free(work->x_prev); + if (work->z_prev) c_free(work->z_prev); + if (work->y) c_free(work->y); + if (work->Ax) c_free(work->Ax); + if (work->Px) c_free(work->Px); + if (work->Aty) c_free(work->Aty); + if (work->delta_y) c_free(work->delta_y); + if (work->Atdelta_y) c_free(work->Atdelta_y); + if (work->delta_x) c_free(work->delta_x); + if (work->Pdelta_x) c_free(work->Pdelta_x); + if (work->Adelta_x) c_free(work->Adelta_x); + + // Free Settings + if (work->settings) c_free(work->settings); + + // Free solution + if (work->solution) { + if (work->solution->x) c_free(work->solution->x); + if (work->solution->y) c_free(work->solution->y); + c_free(work->solution); + } + + // Free information + if (work->info) c_free(work->info); + +# ifdef PROFILING + // Free timer + if (work->timer) c_free(work->timer); +# endif /* ifdef PROFILING */ + + // Free work + c_free(work); + } + + return exitflag; +} + +#endif // #ifndef EMBEDDED + + +/************************ +* Update problem data * +************************/ +c_int osqp_update_lin_cost(OSQPWorkspace *work, const c_float *q_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + // Replace q by the new vector + prea_vec_copy(q_new, work->data->q, work->data->n); + + // Scaling + if (work->settings->scaling) { + vec_ew_prod(work->scaling->D, work->data->q, work->data->q, work->data->n); + vec_mult_scalar(work->data->q, work->scaling->c, work->data->n); + } + + // Reset solver information + reset_info(work->info); + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return 0; +} + +c_int osqp_update_bounds(OSQPWorkspace *work, + const c_float *l_new, + const c_float *u_new) { + c_int i, exitflag = 0; + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + // Check if lower bound is smaller than upper bound + for (i = 0; i < work->data->m; i++) { + if (l_new[i] > u_new[i]) { +#ifdef PRINTING + c_eprint("lower bound must be lower than or equal to upper bound"); +#endif /* ifdef PRINTING */ + return 1; + } + } + + // Replace l and u by the new vectors + prea_vec_copy(l_new, work->data->l, work->data->m); + prea_vec_copy(u_new, work->data->u, work->data->m); + + // Scaling + if (work->settings->scaling) { + vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m); + vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m); + } + + // Reset solver information + reset_info(work->info); + +#if EMBEDDED != 1 + // Update rho_vec and refactor if constraints type changes + exitflag = update_rho_vec(work); +#endif // EMBEDDED != 1 + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + +c_int osqp_update_lower_bound(OSQPWorkspace *work, const c_float *l_new) { + c_int i, exitflag = 0; + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + // Replace l by the new vector + prea_vec_copy(l_new, work->data->l, work->data->m); + + // Scaling + if (work->settings->scaling) { + vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m); + } + + // Check if lower bound is smaller than upper bound + for (i = 0; i < work->data->m; i++) { + if (work->data->l[i] > work->data->u[i]) { +#ifdef PRINTING + c_eprint("upper bound must be greater than or equal to lower bound"); +#endif /* ifdef PRINTING */ + return 1; + } + } + + // Reset solver information + reset_info(work->info); + +#if EMBEDDED != 1 + // Update rho_vec and refactor if constraints type changes + exitflag = update_rho_vec(work); +#endif // EMBEDDED ! =1 + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + +c_int osqp_update_upper_bound(OSQPWorkspace *work, const c_float *u_new) { + c_int i, exitflag = 0; + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + // Replace u by the new vector + prea_vec_copy(u_new, work->data->u, work->data->m); + + // Scaling + if (work->settings->scaling) { + vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m); + } + + // Check if upper bound is greater than lower bound + for (i = 0; i < work->data->m; i++) { + if (work->data->u[i] < work->data->l[i]) { +#ifdef PRINTING + c_eprint("lower bound must be lower than or equal to upper bound"); +#endif /* ifdef PRINTING */ + return 1; + } + } + + // Reset solver information + reset_info(work->info); + +#if EMBEDDED != 1 + // Update rho_vec and refactor if constraints type changes + exitflag = update_rho_vec(work); +#endif // EMBEDDED != 1 + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + +c_int osqp_warm_start(OSQPWorkspace *work, const c_float *x, const c_float *y) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Update warm_start setting to true + if (!work->settings->warm_start) work->settings->warm_start = 1; + + // Copy primal and dual variables into the iterates + prea_vec_copy(x, work->x, work->data->n); + prea_vec_copy(y, work->y, work->data->m); + + // Scale iterates + if (work->settings->scaling) { + vec_ew_prod(work->scaling->Dinv, work->x, work->x, work->data->n); + vec_ew_prod(work->scaling->Einv, work->y, work->y, work->data->m); + vec_mult_scalar(work->y, work->scaling->c, work->data->m); + } + + // Compute Ax = z and store it in z + mat_vec(work->data->A, work->x, work->z, 0); + + return 0; +} + +c_int osqp_warm_start_x(OSQPWorkspace *work, const c_float *x) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Update warm_start setting to true + if (!work->settings->warm_start) work->settings->warm_start = 1; + + // Copy primal variable into the iterate x + prea_vec_copy(x, work->x, work->data->n); + + // Scale iterate + if (work->settings->scaling) { + vec_ew_prod(work->scaling->Dinv, work->x, work->x, work->data->n); + } + + // Compute Ax = z and store it in z + mat_vec(work->data->A, work->x, work->z, 0); + + return 0; +} + +c_int osqp_warm_start_y(OSQPWorkspace *work, const c_float *y) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Update warm_start setting to true + if (!work->settings->warm_start) work->settings->warm_start = 1; + + // Copy dual variable into the iterate y + prea_vec_copy(y, work->y, work->data->m); + + // Scale iterate + if (work->settings->scaling) { + vec_ew_prod(work->scaling->Einv, work->y, work->y, work->data->m); + vec_mult_scalar(work->y, work->scaling->c, work->data->m); + } + + return 0; +} + + +#if EMBEDDED != 1 + +c_int osqp_update_P(OSQPWorkspace *work, + const c_float *Px_new, + const c_int *Px_new_idx, + c_int P_new_n) { + c_int i; // For indexing + c_int exitflag; // Exit flag + c_int nnzP; // Number of nonzeros in P + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + nnzP = work->data->P->p[work->data->P->n]; + + if (Px_new_idx) { // Passing the index of elements changed + // Check if number of elements is less or equal than the total number of + // nonzeros in P + if (P_new_n > nnzP) { +# ifdef PRINTING + c_eprint("new number of elements (%i) greater than elements in P (%i)", + (int)P_new_n, + (int)nnzP); +# endif /* ifdef PRINTING */ + return 1; + } + } + + if (work->settings->scaling) { + // Unscale data + unscale_data(work); + } + + // Update P elements + if (Px_new_idx) { // Change only Px_new_idx + for (i = 0; i < P_new_n; i++) { + work->data->P->x[Px_new_idx[i]] = Px_new[i]; + } + } + else // Change whole P + { + for (i = 0; i < nnzP; i++) { + work->data->P->x[i] = Px_new[i]; + } + } + + if (work->settings->scaling) { + // Scale data + scale_data(work); + } + + // Update linear system structure with new data + exitflag = work->linsys_solver->update_matrices(work->linsys_solver, + work->data->P, + work->data->A); + + // Reset solver information + reset_info(work->info); + +# ifdef PRINTING + + if (exitflag < 0) { + c_eprint("new KKT matrix is not quasidefinite"); + } +# endif /* ifdef PRINTING */ + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + + +c_int osqp_update_A(OSQPWorkspace *work, + const c_float *Ax_new, + const c_int *Ax_new_idx, + c_int A_new_n) { + c_int i; // For indexing + c_int exitflag; // Exit flag + c_int nnzA; // Number of nonzeros in A + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + nnzA = work->data->A->p[work->data->A->n]; + + if (Ax_new_idx) { // Passing the index of elements changed + // Check if number of elements is less or equal than the total number of + // nonzeros in A + if (A_new_n > nnzA) { +# ifdef PRINTING + c_eprint("new number of elements (%i) greater than elements in A (%i)", + (int)A_new_n, + (int)nnzA); +# endif /* ifdef PRINTING */ + return 1; + } + } + + if (work->settings->scaling) { + // Unscale data + unscale_data(work); + } + + // Update A elements + if (Ax_new_idx) { // Change only Ax_new_idx + for (i = 0; i < A_new_n; i++) { + work->data->A->x[Ax_new_idx[i]] = Ax_new[i]; + } + } + else { // Change whole A + for (i = 0; i < nnzA; i++) { + work->data->A->x[i] = Ax_new[i]; + } + } + + if (work->settings->scaling) { + // Scale data + scale_data(work); + } + + // Update linear system structure with new data + exitflag = work->linsys_solver->update_matrices(work->linsys_solver, + work->data->P, + work->data->A); + + // Reset solver information + reset_info(work->info); + +# ifdef PRINTING + + if (exitflag < 0) { + c_eprint("new KKT matrix is not quasidefinite"); + } +# endif /* ifdef PRINTING */ + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + + +c_int osqp_update_P_A(OSQPWorkspace *work, + const c_float *Px_new, + const c_int *Px_new_idx, + c_int P_new_n, + const c_float *Ax_new, + const c_int *Ax_new_idx, + c_int A_new_n) { + c_int i; // For indexing + c_int exitflag; // Exit flag + c_int nnzP, nnzA; // Number of nonzeros in P and A + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + +#ifdef PROFILING + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer +#endif /* ifdef PROFILING */ + + nnzP = work->data->P->p[work->data->P->n]; + nnzA = work->data->A->p[work->data->A->n]; + + + if (Px_new_idx) { // Passing the index of elements changed + // Check if number of elements is less or equal than the total number of + // nonzeros in P + if (P_new_n > nnzP) { +# ifdef PRINTING + c_eprint("new number of elements (%i) greater than elements in P (%i)", + (int)P_new_n, + (int)nnzP); +# endif /* ifdef PRINTING */ + return 1; + } + } + + + if (Ax_new_idx) { // Passing the index of elements changed + // Check if number of elements is less or equal than the total number of + // nonzeros in A + if (A_new_n > nnzA) { +# ifdef PRINTING + c_eprint("new number of elements (%i) greater than elements in A (%i)", + (int)A_new_n, + (int)nnzA); +# endif /* ifdef PRINTING */ + return 2; + } + } + + if (work->settings->scaling) { + // Unscale data + unscale_data(work); + } + + // Update P elements + if (Px_new_idx) { // Change only Px_new_idx + for (i = 0; i < P_new_n; i++) { + work->data->P->x[Px_new_idx[i]] = Px_new[i]; + } + } + else // Change whole P + { + for (i = 0; i < nnzP; i++) { + work->data->P->x[i] = Px_new[i]; + } + } + + // Update A elements + if (Ax_new_idx) { // Change only Ax_new_idx + for (i = 0; i < A_new_n; i++) { + work->data->A->x[Ax_new_idx[i]] = Ax_new[i]; + } + } + else { // Change whole A + for (i = 0; i < nnzA; i++) { + work->data->A->x[i] = Ax_new[i]; + } + } + + if (work->settings->scaling) { + // Scale data + scale_data(work); + } + + // Update linear system structure with new data + exitflag = work->linsys_solver->update_matrices(work->linsys_solver, + work->data->P, + work->data->A); + + // Reset solver information + reset_info(work->info); + +# ifdef PRINTING + + if (exitflag < 0) { + c_eprint("new KKT matrix is not quasidefinite"); + } +# endif /* ifdef PRINTING */ + +#ifdef PROFILING + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + +c_int osqp_update_rho(OSQPWorkspace *work, c_float rho_new) { + c_int exitflag, i; + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check value of rho + if (rho_new <= 0) { +# ifdef PRINTING + c_eprint("rho must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + +#ifdef PROFILING + if (work->rho_update_from_solve == 0) { + if (work->clear_update_time == 1) { + work->clear_update_time = 0; + work->info->update_time = 0.0; + } + osqp_tic(work->timer); // Start timer + } +#endif /* ifdef PROFILING */ + + // Update rho in settings + work->settings->rho = c_min(c_max(rho_new, RHO_MIN), RHO_MAX); + + // Update rho_vec and rho_inv_vec + for (i = 0; i < work->data->m; i++) { + if (work->constr_type[i] == 0) { + // Inequalities + work->rho_vec[i] = work->settings->rho; + work->rho_inv_vec[i] = 1. / work->settings->rho; + } + else if (work->constr_type[i] == 1) { + // Equalities + work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho; + work->rho_inv_vec[i] = 1. / work->rho_vec[i]; + } + } + + // Update rho_vec in KKT matrix + exitflag = work->linsys_solver->update_rho_vec(work->linsys_solver, + work->rho_vec); + +#ifdef PROFILING + if (work->rho_update_from_solve == 0) + work->info->update_time += osqp_toc(work->timer); +#endif /* ifdef PROFILING */ + + return exitflag; +} + +#endif // EMBEDDED != 1 + +/**************************** +* Update problem settings * +****************************/ +c_int osqp_update_max_iter(OSQPWorkspace *work, c_int max_iter_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that max_iter is positive + if (max_iter_new <= 0) { +#ifdef PRINTING + c_eprint("max_iter must be positive"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update max_iter + work->settings->max_iter = max_iter_new; + + return 0; +} + +c_int osqp_update_eps_abs(OSQPWorkspace *work, c_float eps_abs_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that eps_abs is positive + if (eps_abs_new < 0.) { +#ifdef PRINTING + c_eprint("eps_abs must be nonnegative"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update eps_abs + work->settings->eps_abs = eps_abs_new; + + return 0; +} + +c_int osqp_update_eps_rel(OSQPWorkspace *work, c_float eps_rel_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that eps_rel is positive + if (eps_rel_new < 0.) { +#ifdef PRINTING + c_eprint("eps_rel must be nonnegative"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update eps_rel + work->settings->eps_rel = eps_rel_new; + + return 0; +} + +c_int osqp_update_eps_prim_inf(OSQPWorkspace *work, c_float eps_prim_inf_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that eps_prim_inf is positive + if (eps_prim_inf_new < 0.) { +#ifdef PRINTING + c_eprint("eps_prim_inf must be nonnegative"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update eps_prim_inf + work->settings->eps_prim_inf = eps_prim_inf_new; + + return 0; +} + +c_int osqp_update_eps_dual_inf(OSQPWorkspace *work, c_float eps_dual_inf_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that eps_dual_inf is positive + if (eps_dual_inf_new < 0.) { +#ifdef PRINTING + c_eprint("eps_dual_inf must be nonnegative"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update eps_dual_inf + work->settings->eps_dual_inf = eps_dual_inf_new; + + + return 0; +} + +c_int osqp_update_alpha(OSQPWorkspace *work, c_float alpha_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that alpha is between 0 and 2 + if ((alpha_new <= 0.) || (alpha_new >= 2.)) { +#ifdef PRINTING + c_eprint("alpha must be between 0 and 2"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update alpha + work->settings->alpha = alpha_new; + + return 0; +} + +c_int osqp_update_warm_start(OSQPWorkspace *work, c_int warm_start_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that warm_start is either 0 or 1 + if ((warm_start_new != 0) && (warm_start_new != 1)) { +#ifdef PRINTING + c_eprint("warm_start should be either 0 or 1"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update warm_start + work->settings->warm_start = warm_start_new; + + return 0; +} + +c_int osqp_update_scaled_termination(OSQPWorkspace *work, c_int scaled_termination_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that scaled_termination is either 0 or 1 + if ((scaled_termination_new != 0) && (scaled_termination_new != 1)) { +#ifdef PRINTING + c_eprint("scaled_termination should be either 0 or 1"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update scaled_termination + work->settings->scaled_termination = scaled_termination_new; + + return 0; +} + +c_int osqp_update_check_termination(OSQPWorkspace *work, c_int check_termination_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that check_termination is nonnegative + if (check_termination_new < 0) { +#ifdef PRINTING + c_eprint("check_termination should be nonnegative"); +#endif /* ifdef PRINTING */ + return 1; + } + + // Update check_termination + work->settings->check_termination = check_termination_new; + + return 0; +} + +#ifndef EMBEDDED + +c_int osqp_update_delta(OSQPWorkspace *work, c_float delta_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that delta is positive + if (delta_new <= 0.) { +# ifdef PRINTING + c_eprint("delta must be positive"); +# endif /* ifdef PRINTING */ + return 1; + } + + // Update delta + work->settings->delta = delta_new; + + return 0; +} + +c_int osqp_update_polish(OSQPWorkspace *work, c_int polish_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that polish is either 0 or 1 + if ((polish_new != 0) && (polish_new != 1)) { +# ifdef PRINTING + c_eprint("polish should be either 0 or 1"); +# endif /* ifdef PRINTING */ + return 1; + } + + // Update polish + work->settings->polish = polish_new; + +# ifdef PROFILING + + // Reset polish time to zero + work->info->polish_time = 0.0; +# endif /* ifdef PROFILING */ + + return 0; +} + +c_int osqp_update_polish_refine_iter(OSQPWorkspace *work, c_int polish_refine_iter_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that polish_refine_iter is nonnegative + if (polish_refine_iter_new < 0) { +# ifdef PRINTING + c_eprint("polish_refine_iter must be nonnegative"); +# endif /* ifdef PRINTING */ + return 1; + } + + // Update polish_refine_iter + work->settings->polish_refine_iter = polish_refine_iter_new; + + return 0; +} + +c_int osqp_update_verbose(OSQPWorkspace *work, c_int verbose_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that verbose is either 0 or 1 + if ((verbose_new != 0) && (verbose_new != 1)) { +# ifdef PRINTING + c_eprint("verbose should be either 0 or 1"); +# endif /* ifdef PRINTING */ + return 1; + } + + // Update verbose + work->settings->verbose = verbose_new; + + return 0; +} + +#endif // EMBEDDED + +#ifdef PROFILING + +c_int osqp_update_time_limit(OSQPWorkspace *work, c_float time_limit_new) { + + // Check if workspace has been initialized + if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR); + + // Check that time_limit is nonnegative + if (time_limit_new < 0.) { +# ifdef PRINTING + c_print("time_limit must be nonnegative\n"); +# endif /* ifdef PRINTING */ + return 1; + } + + // Update time_limit + work->settings->time_limit = time_limit_new; + + return 0; +} +#endif /* ifdef PROFILING */ diff --git a/src/lib/allocator_qp/src/osqp/proj.c b/src/lib/allocator_qp/src/osqp/proj.c new file mode 100644 index 000000000000..c7fdb458a944 --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/proj.c @@ -0,0 +1,29 @@ +#include "proj.h" + + +void project(OSQPWorkspace *work, c_float *z) { + c_int i, m; + + m = work->data->m; + + for (i = 0; i < m; i++) { + z[i] = c_min(c_max(z[i], + work->data->l[i]), // Between lower + work->data->u[i]); // and upper bounds + } +} + +void project_normalcone(OSQPWorkspace *work, c_float *z, c_float *y) { + c_int i, m; + + // NB: Use z_prev as temporary vector + + m = work->data->m; + + for (i = 0; i < m; i++) { + work->z_prev[i] = z[i] + y[i]; + z[i] = c_min(c_max(work->z_prev[i], work->data->l[i]), + work->data->u[i]); + y[i] = work->z_prev[i] - z[i]; + } +} diff --git a/src/lib/allocator_qp/src/osqp/qdldl.c b/src/lib/allocator_qp/src/osqp/qdldl.c new file mode 100644 index 000000000000..8018b6dd017b --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/qdldl.c @@ -0,0 +1,286 @@ +#include "qdldl.h" + +#define QDLDL_UNKNOWN (-1) +#define QDLDL_USED (1) +#define QDLDL_UNUSED (0) + +/* Compute the elimination tree for a quasidefinite matrix + in compressed sparse column form. +*/ + +QDLDL_int QDLDL_etree(const QDLDL_int n, + const QDLDL_int* Ap, + const QDLDL_int* Ai, + QDLDL_int* work, + QDLDL_int* Lnz, + QDLDL_int* etree){ + + QDLDL_int sumLnz; + QDLDL_int i,j,p; + + + for(i = 0; i < n; i++){ + // zero out Lnz and work. Set all etree values to unknown + work[i] = 0; + Lnz[i] = 0; + etree[i] = QDLDL_UNKNOWN; + + //Abort if A doesn't have at least one entry + //one entry in every column + if(Ap[i] == Ap[i+1]){ + return -1; + } + + } + + for(j = 0; j < n; j++){ + work[j] = j; + for(p = Ap[j]; p < Ap[j+1]; p++){ + i = Ai[p]; + if(i > j){return -1;}; //abort if entries on lower triangle + while(work[i] != j){ + if(etree[i] == QDLDL_UNKNOWN){ + etree[i] = j; + } + Lnz[i]++; //nonzeros in this column + work[i] = j; + i = etree[i]; + } + } + } + + //compute the total nonzeros in L. This much + //space is required to store Li and Lx. Return + //error code -2 if the nonzero count will overflow + //its unteger type. + sumLnz = 0; + for(i = 0; i < n; i++){ + if(sumLnz > QDLDL_INT_MAX - Lnz[i]){ + sumLnz = -2; + break; + } + else{ + sumLnz += Lnz[i]; + } + } + + return sumLnz; +} + + + +QDLDL_int QDLDL_factor(const QDLDL_int n, + const QDLDL_int* Ap, + const QDLDL_int* Ai, + const QDLDL_float* Ax, + QDLDL_int* Lp, + QDLDL_int* Li, + QDLDL_float* Lx, + QDLDL_float* D, + QDLDL_float* Dinv, + const QDLDL_int* Lnz, + const QDLDL_int* etree, + QDLDL_bool* bwork, + QDLDL_int* iwork, + QDLDL_float* fwork){ + + QDLDL_int i,j,k,nnzY, bidx, cidx, nextIdx, nnzE, tmpIdx; + QDLDL_int *yIdx, *elimBuffer, *LNextSpaceInCol; + QDLDL_float *yVals; + QDLDL_float yVals_cidx; + QDLDL_bool *yMarkers; + QDLDL_int positiveValuesInD = 0; + + //partition working memory into pieces + yMarkers = bwork; + yIdx = iwork; + elimBuffer = iwork + n; + LNextSpaceInCol = iwork + n*2; + yVals = fwork; + + + Lp[0] = 0; //first column starts at index zero + + for(i = 0; i < n; i++){ + + //compute L column indices + Lp[i+1] = Lp[i] + Lnz[i]; //cumsum, total at the end + + // set all Yidx to be 'unused' initially + //in each column of L, the next available space + //to start is just the first space in the column + yMarkers[i] = QDLDL_UNUSED; + yVals[i] = 0.0; + D[i] = 0.0; + LNextSpaceInCol[i] = Lp[i]; + } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" + // First element of the diagonal D. + D[0] = Ax[0]; + if(D[0] == 0.0){return -1;} + if(D[0] > 0.0){positiveValuesInD++;} + Dinv[0] = 1/D[0]; +#pragma GCC diagnostic pop + + //Start from 1 here. The upper LH corner is trivially 0 + //in L b/c we are only computing the subdiagonal elements + for(k = 1; k < n; k++){ + + //NB : For each k, we compute a solution to + //y = L(0:(k-1),0:k-1))\b, where b is the kth + //column of A that sits above the diagonal. + //The solution y is then the kth row of L, + //with an implied '1' at the diagonal entry. + + //number of nonzeros in this row of L + nnzY = 0; //number of elements in this row + + //This loop determines where nonzeros + //will go in the kth row of L, but doesn't + //compute the actual values + tmpIdx = Ap[k+1]; + + for(i = Ap[k]; i < tmpIdx; i++){ + + bidx = Ai[i]; // we are working on this element of b + + //Initialize D[k] as the element of this column + //corresponding to the diagonal place. Don't use + //this element as part of the elimination step + //that computes the k^th row of L + if(bidx == k){ + D[k] = Ax[i]; + continue; + } + + yVals[bidx] = Ax[i]; // initialise y(bidx) = b(bidx) + + // use the forward elimination tree to figure + // out which elements must be eliminated after + // this element of b + nextIdx = bidx; + + if(yMarkers[nextIdx] == QDLDL_UNUSED){ //this y term not already visited + + yMarkers[nextIdx] = QDLDL_USED; //I touched this one + elimBuffer[0] = nextIdx; // It goes at the start of the current list + nnzE = 1; //length of unvisited elimination path from here + + nextIdx = etree[bidx]; + + while(nextIdx != QDLDL_UNKNOWN && nextIdx < k){ + if(yMarkers[nextIdx] == QDLDL_USED) break; + + yMarkers[nextIdx] = QDLDL_USED; //I touched this one + elimBuffer[nnzE] = nextIdx; //It goes in the current list + nnzE++; //the list is one longer than before + nextIdx = etree[nextIdx]; //one step further along tree + + } //end while + + // now I put the buffered elimination list into + // my current ordering in reverse order + while(nnzE){ + yIdx[nnzY++] = elimBuffer[--nnzE]; + } //end while + } //end if + + } //end for i + + //This for loop places nonzeros values in the k^th row + for(i = (nnzY-1); i >=0; i--){ + + //which column are we working on? + cidx = yIdx[i]; + + // loop along the elements in this + // column of L and subtract to solve to y + tmpIdx = LNextSpaceInCol[cidx]; + yVals_cidx = yVals[cidx]; + for(j = Lp[cidx]; j < tmpIdx; j++){ + yVals[Li[j]] -= Lx[j]*yVals_cidx; + } + + //Now I have the cidx^th element of y = L\b. + //so compute the corresponding element of + //this row of L and put it into the right place + Li[tmpIdx] = k; + Lx[tmpIdx] = yVals_cidx *Dinv[cidx]; + + //D[k] -= yVals[cidx]*yVals[cidx]*Dinv[cidx]; + D[k] -= yVals_cidx*Lx[tmpIdx]; + LNextSpaceInCol[cidx]++; + + //reset the yvalues and indices back to zero and QDLDL_UNUSED + //once I'm done with them + yVals[cidx] = 0.0; + yMarkers[cidx] = QDLDL_UNUSED; + + } //end for i + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" + //Maintain a count of the positive entries + //in D. If we hit a zero, we can't factor + //this matrix, so abort + if(D[k] == 0.0){return -1;} + if(D[k] > 0.0){positiveValuesInD++;} +#pragma GCC diagnostic pop + + //compute the inverse of the diagonal + Dinv[k]= 1/D[k]; + + } //end for k + + return positiveValuesInD; + +} + +// Solves (L+I)x = b +void QDLDL_Lsolve(const QDLDL_int n, + const QDLDL_int* Lp, + const QDLDL_int* Li, + const QDLDL_float* Lx, + QDLDL_float* x){ + + QDLDL_int i,j; + for(i = 0; i < n; i++){ + QDLDL_float val = x[i]; + for(j = Lp[i]; j < Lp[i+1]; j++){ + x[Li[j]] -= Lx[j]*val; + } + } +} + +// Solves (L+I)'x = b +void QDLDL_Ltsolve(const QDLDL_int n, + const QDLDL_int* Lp, + const QDLDL_int* Li, + const QDLDL_float* Lx, + QDLDL_float* x){ + + QDLDL_int i,j; + for(i = n-1; i>=0; i--){ + QDLDL_float val = x[i]; + for(j = Lp[i]; j < Lp[i+1]; j++){ + val -= Lx[j]*x[Li[j]]; + } + x[i] = val; + } +} + +// Solves Ax = b where A has given LDL factors +void QDLDL_solve(const QDLDL_int n, + const QDLDL_int* Lp, + const QDLDL_int* Li, + const QDLDL_float* Lx, + const QDLDL_float* Dinv, + QDLDL_float* x){ + + QDLDL_int i; + + QDLDL_Lsolve(n,Lp,Li,Lx,x); + for(i = 0; i < n; i++) x[i] *= Dinv[i]; + QDLDL_Ltsolve(n,Lp,Li,Lx,x); +} diff --git a/src/lib/allocator_qp/src/osqp/qdldl_interface.c b/src/lib/allocator_qp/src/osqp/qdldl_interface.c new file mode 100644 index 000000000000..43f30ef34c8d --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/qdldl_interface.c @@ -0,0 +1,413 @@ +#include "glob_opts.h" + +#include "qdldl.h" +#include "qdldl_interface.h" + +#ifndef EMBEDDED +#include "amd.h" +#endif + +#if EMBEDDED != 1 +#include "kkt.h" +#endif + +#ifndef EMBEDDED + +// Free LDL Factorization structure +void free_linsys_solver_qdldl(qdldl_solver *s) { + if (s) { + if (s->L) csc_spfree(s->L); + if (s->P) c_free(s->P); + if (s->Dinv) c_free(s->Dinv); + if (s->bp) c_free(s->bp); + if (s->sol) c_free(s->sol); + if (s->rho_inv_vec) c_free(s->rho_inv_vec); + + // These are required for matrix updates + if (s->Pdiag_idx) c_free(s->Pdiag_idx); + if (s->KKT) csc_spfree(s->KKT); + if (s->PtoKKT) c_free(s->PtoKKT); + if (s->AtoKKT) c_free(s->AtoKKT); + if (s->rhotoKKT) c_free(s->rhotoKKT); + + // QDLDL workspace + if (s->D) c_free(s->D); + if (s->etree) c_free(s->etree); + if (s->Lnz) c_free(s->Lnz); + if (s->iwork) c_free(s->iwork); + if (s->bwork) c_free(s->bwork); + if (s->fwork) c_free(s->fwork); + c_free(s); + + } +} + + +/** + * Compute LDL factorization of matrix A + * @param A Matrix to be factorized + * @param p Private workspace + * @param nvar Number of QP variables + * @return exitstatus (0 is good) + */ +static c_int LDL_factor(csc *A, qdldl_solver * p, c_int nvar){ + + c_int sum_Lnz; + c_int factor_status; + + // Compute elimination tree + sum_Lnz = QDLDL_etree(A->n, A->p, A->i, p->iwork, p->Lnz, p->etree); + + if (sum_Lnz < 0){ + // Error +#ifdef PRINTING + c_eprint("Error in KKT matrix LDL factorization when computing the elimination tree."); + if(sum_Lnz == -1){ + c_eprint("Matrix is not perfectly upper triangular."); + } + else if(sum_Lnz == -2){ + c_eprint("Integer overflow in L nonzero count."); + } +#endif + return sum_Lnz; + } + + // Allocate memory for Li and Lx + p->L->i = (c_int *)c_malloc(sizeof(c_int)*sum_Lnz); + p->L->x = (c_float *)c_malloc(sizeof(c_float)*sum_Lnz); + p->L->nzmax = sum_Lnz; + + // Factor matrix + factor_status = QDLDL_factor(A->n, A->p, A->i, A->x, + p->L->p, p->L->i, p->L->x, + p->D, p->Dinv, p->Lnz, + p->etree, p->bwork, p->iwork, p->fwork); + + + if (factor_status < 0){ + // Error +#ifdef PRINTING + c_eprint("Error in KKT matrix LDL factorization when computing the nonzero elements. There are zeros in the diagonal matrix"); +#endif + return factor_status; + } else if (factor_status < nvar) { + // Error: Number of positive elements of D should be equal to nvar +#ifdef PRINTING + c_eprint("Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex"); +#endif + return -2; + } + + return 0; + +} + + +static c_int permute_KKT(csc ** KKT, qdldl_solver * p, c_int Pnz, c_int Anz, c_int m, c_int * PtoKKT, c_int * AtoKKT, c_int * rhotoKKT){ + c_float *info; + c_int amd_status; + c_int * Pinv; + csc *KKT_temp; + c_int * KtoPKPt; + c_int i; // Indexing + + info = (c_float *)c_malloc(AMD_INFO * sizeof(c_float)); + + // Compute permutation matrix P using AMD +#ifdef DLONG + amd_status = amd_l_order((*KKT)->n, (*KKT)->p, (*KKT)->i, p->P, (c_float *)OSQP_NULL, info); +#else + amd_status = amd_order((*KKT)->n, (*KKT)->p, (*KKT)->i, p->P, (c_float *)OSQP_NULL, info); +#endif + if (amd_status < 0) { + // Free Amd info and return an error + c_free(info); + return amd_status; + } + + + // Inverse of the permutation vector + Pinv = csc_pinv(p->P, (*KKT)->n); + + // Permute KKT matrix + if (!PtoKKT && !AtoKKT && !rhotoKKT){ // No vectors to be stored + // Assign values of mapping + KKT_temp = csc_symperm((*KKT), Pinv, OSQP_NULL, 1); + } + else { + // Allocate vector of mappings from unpermuted to permuted + KtoPKPt = c_malloc((*KKT)->p[(*KKT)->n] * sizeof(c_int)); + KKT_temp = csc_symperm((*KKT), Pinv, KtoPKPt, 1); + + // Update vectors PtoKKT, AtoKKT and rhotoKKT + if (PtoKKT){ + for (i = 0; i < Pnz; i++){ + PtoKKT[i] = KtoPKPt[PtoKKT[i]]; + } + } + if (AtoKKT){ + for (i = 0; i < Anz; i++){ + AtoKKT[i] = KtoPKPt[AtoKKT[i]]; + } + } + if (rhotoKKT){ + for (i = 0; i < m; i++){ + rhotoKKT[i] = KtoPKPt[rhotoKKT[i]]; + } + } + + // Cleanup vector of mapping + c_free(KtoPKPt); + } + + // Cleanup + // Free previous KKT matrix and assign pointer to new one + csc_spfree((*KKT)); + (*KKT) = KKT_temp; + // Free Pinv + c_free(Pinv); + // Free Amd info + c_free(info); + + return 0; +} + + +// Initialize LDL Factorization structure +c_int init_linsys_solver_qdldl(qdldl_solver ** sp, const csc * P, const csc * A, c_float sigma, const c_float * rho_vec, c_int polish){ + + // Define Variables + csc * KKT_temp; // Temporary KKT pointer + c_int i; // Loop counter + c_int n_plus_m; // Define n_plus_m dimension + + // Allocate private structure to store KKT factorization + qdldl_solver *s; + s = c_calloc(1, sizeof(qdldl_solver)); + *sp = s; + + // Size of KKT + s->n = P->n; + s->m = A->m; + n_plus_m = s->n + s->m; + + // Sigma parameter + s->sigma = sigma; + + // Polishing flag + s->polish = polish; + + // Link Functions + s->solve = &solve_linsys_qdldl; + +#ifndef EMBEDDED + s->free = &free_linsys_solver_qdldl; +#endif + +#if EMBEDDED != 1 + s->update_matrices = &update_linsys_solver_matrices_qdldl; + s->update_rho_vec = &update_linsys_solver_rho_vec_qdldl; +#endif + + // Assign type + s->type = QDLDL_SOLVER; + + // Set number of threads to 1 (single threaded) + s->nthreads = 1; + + // Sparse matrix L (lower triangular) + // NB: We don not allocate L completely (CSC elements) + // L will be allocated during the factorization depending on the + // resulting number of elements. + s->L = c_malloc(sizeof(csc)); + s->L->m = n_plus_m; + s->L->n = n_plus_m; + s->L->nz = -1; + + // Diagonal matrix stored as a vector D + s->Dinv = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m); + s->D = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m); + + // Permutation vector P + s->P = (QDLDL_int *)c_malloc(sizeof(QDLDL_int) * n_plus_m); + + // Working vector + s->bp = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m); + + // Solution vector + s->sol = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m); + + // Parameter vector + s->rho_inv_vec = (c_float *)c_malloc(sizeof(c_float) * s->m); + + // Elimination tree workspace + s->etree = (QDLDL_int *)c_malloc(n_plus_m * sizeof(QDLDL_int)); + s->Lnz = (QDLDL_int *)c_malloc(n_plus_m * sizeof(QDLDL_int)); + + // Preallocate L matrix (Lx and Li are sparsity dependent) + s->L->p = (c_int *)c_malloc((n_plus_m+1) * sizeof(QDLDL_int)); + + // Lx and Li are sparsity dependent, so set them to + // null initially so we don't try to free them prematurely + s->L->i = OSQP_NULL; + s->L->x = OSQP_NULL; + + // Preallocate workspace + s->iwork = (QDLDL_int *)c_malloc(sizeof(QDLDL_int)*(3*n_plus_m)); + s->bwork = (QDLDL_bool *)c_malloc(sizeof(QDLDL_bool)*n_plus_m); + s->fwork = (QDLDL_float *)c_malloc(sizeof(QDLDL_float)*n_plus_m); + + // Form and permute KKT matrix + if (polish){ // Called from polish() + // Use s->rho_inv_vec for storing param2 = vec(delta) + for (i = 0; i < A->m; i++){ + s->rho_inv_vec[i] = sigma; + } + + KKT_temp = form_KKT(P, A, 0, sigma, s->rho_inv_vec, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL); + + // Permute matrix + if (KKT_temp) + permute_KKT(&KKT_temp, s, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL); + } + else { // Called from ADMM algorithm + + // Allocate vectors of indices + s->PtoKKT = c_malloc((P->p[P->n]) * sizeof(c_int)); + s->AtoKKT = c_malloc((A->p[A->n]) * sizeof(c_int)); + s->rhotoKKT = c_malloc((A->m) * sizeof(c_int)); + + // Use p->rho_inv_vec for storing param2 = rho_inv_vec + for (i = 0; i < A->m; i++){ + s->rho_inv_vec[i] = 1. / rho_vec[i]; + } + + KKT_temp = form_KKT(P, A, 0, sigma, s->rho_inv_vec, + s->PtoKKT, s->AtoKKT, + &(s->Pdiag_idx), &(s->Pdiag_n), s->rhotoKKT); + + // Permute matrix + if (KKT_temp) + permute_KKT(&KKT_temp, s, P->p[P->n], A->p[A->n], A->m, s->PtoKKT, s->AtoKKT, s->rhotoKKT); + } + + // Check if matrix has been created + if (!KKT_temp){ +#ifdef PRINTING + c_eprint("Error forming and permuting KKT matrix"); +#endif + free_linsys_solver_qdldl(s); + *sp = OSQP_NULL; + return OSQP_LINSYS_SOLVER_INIT_ERROR; + } + + // Factorize the KKT matrix + if (LDL_factor(KKT_temp, s, P->n) < 0) { + csc_spfree(KKT_temp); + free_linsys_solver_qdldl(s); + *sp = OSQP_NULL; + return OSQP_NONCVX_ERROR; + } + + if (polish){ // If KKT passed, assign it to KKT_temp + // Polish, no need for KKT_temp + csc_spfree(KKT_temp); + } + else { // If not embedded option 1 copy pointer to KKT_temp. Do not free it. + s->KKT = KKT_temp; + } + + + // No error + return 0; +} + +#endif // EMBEDDED + + +// Permute x = P*b using P +void permute_x(c_int n, c_float * x, const c_float * b, const c_int * P) { + c_int j; + for (j = 0 ; j < n ; j++) x[j] = b[P[j]]; +} + +// Permute x = P'*b using P +void permutet_x(c_int n, c_float * x, const c_float * b, const c_int * P) { + c_int j; + for (j = 0 ; j < n ; j++) x[P[j]] = b[j]; +} + + +static void LDLSolve(c_float *x, c_float *b, const csc *L, const c_float *Dinv, const c_int *P, c_float *bp) { + /* solves P'LDL'P x = b for x */ + permute_x(L->n, bp, b, P); + QDLDL_solve(L->n, L->p, L->i, L->x, Dinv, bp); + permutet_x(L->n, x, bp, P); + +} + + +c_int solve_linsys_qdldl(qdldl_solver * s, c_float * b) { + c_int j; + +#ifndef EMBEDDED + if (s->polish) { + /* stores solution to the KKT system in b */ + LDLSolve(b, b, s->L, s->Dinv, s->P, s->bp); + } else { +#endif + /* stores solution to the KKT system in s->sol */ + LDLSolve(s->sol, b, s->L, s->Dinv, s->P, s->bp); + + /* copy x_tilde from s->sol */ + for (j = 0 ; j < s->n ; j++) { + b[j] = s->sol[j]; + } + + /* compute z_tilde from b and s->sol */ + for (j = 0 ; j < s->m ; j++) { + b[j + s->n] += s->rho_inv_vec[j] * s->sol[j + s->n]; + } +#ifndef EMBEDDED + } +#endif + + return 0; +} + + +#if EMBEDDED != 1 +// Update private structure with new P and A +c_int update_linsys_solver_matrices_qdldl(qdldl_solver * s, const csc *P, const csc *A) { + + // Update KKT matrix with new P + update_KKT_P(s->KKT, P, s->PtoKKT, s->sigma, s->Pdiag_idx, s->Pdiag_n); + + // Update KKT matrix with new A + update_KKT_A(s->KKT, A, s->AtoKKT); + + return (QDLDL_factor(s->KKT->n, s->KKT->p, s->KKT->i, s->KKT->x, + s->L->p, s->L->i, s->L->x, s->D, s->Dinv, s->Lnz, + s->etree, s->bwork, s->iwork, s->fwork) < 0); + +} + + +c_int update_linsys_solver_rho_vec_qdldl(qdldl_solver * s, const c_float * rho_vec){ + c_int i; + + // Update internal rho_inv_vec + for (i = 0; i < s->m; i++){ + s->rho_inv_vec[i] = 1. / rho_vec[i]; + } + + // Update KKT matrix with new rho_vec + update_KKT_param2(s->KKT, s->rho_inv_vec, s->rhotoKKT, s->m); + + return (QDLDL_factor(s->KKT->n, s->KKT->p, s->KKT->i, s->KKT->x, + s->L->p, s->L->i, s->L->x, s->D, s->Dinv, s->Lnz, + s->etree, s->bwork, s->iwork, s->fwork) < 0); +} + + +#endif diff --git a/src/lib/allocator_qp/src/osqp/scaling.c b/src/lib/allocator_qp/src/osqp/scaling.c new file mode 100644 index 000000000000..74616a7303a2 --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/scaling.c @@ -0,0 +1,192 @@ +#include "scaling.h" + +#if EMBEDDED != 1 + + +// Set values lower than threshold SCALING_REG to 1 +void limit_scaling(c_float *D, c_int n) { + c_int i; + + for (i = 0; i < n; i++) { + D[i] = D[i] < MIN_SCALING ? 1.0 : D[i]; + D[i] = D[i] > MAX_SCALING ? MAX_SCALING : D[i]; + } +} + +/** + * Compute infinite norm of the columns of the KKT matrix without forming it + * + * The norm is stored in the vector v = (D, E) + * + * @param P Cost matrix + * @param A Constraints matrix + * @param D Norm of columns related to variables + * @param D_temp_A Temporary vector for norm of columns of A + * @param E Norm of columns related to constraints + * @param n Dimension of KKT matrix + */ +void compute_inf_norm_cols_KKT(const csc *P, const csc *A, + c_float *D, c_float *D_temp_A, + c_float *E, c_int n) { + // First half + // [ P ] + // [ A ] + mat_inf_norm_cols_sym_triu(P, D); + mat_inf_norm_cols(A, D_temp_A); + vec_ew_max_vec(D, D_temp_A, D, n); + + // Second half + // [ A'] + // [ 0 ] + mat_inf_norm_rows(A, E); +} + +c_int scale_data(OSQPWorkspace *work) { + // Scale KKT matrix + // + // [ P A'] + // [ A 0 ] + // + // with diagonal matrix + // + // S = [ D ] + // [ E ] + // + + c_int i; // Iterations index + c_int n, m; // Number of constraints and variables + c_float c_temp; // Cost function scaling + c_float inf_norm_q; // Infinity norm of q + + n = work->data->n; + m = work->data->m; + + // Initialize scaling to 1 + work->scaling->c = 1.0; + vec_set_scalar(work->scaling->D, 1., work->data->n); + vec_set_scalar(work->scaling->Dinv, 1., work->data->n); + vec_set_scalar(work->scaling->E, 1., work->data->m); + vec_set_scalar(work->scaling->Einv, 1., work->data->m); + + + for (i = 0; i < work->settings->scaling; i++) { + // + // First Ruiz step + // + + // Compute norm of KKT columns + compute_inf_norm_cols_KKT(work->data->P, work->data->A, + work->D_temp, work->D_temp_A, + work->E_temp, n); + + // Set to 1 values with 0 norms (avoid crazy scaling) + limit_scaling(work->D_temp, n); + limit_scaling(work->E_temp, m); + + // Take square root of norms + vec_ew_sqrt(work->D_temp, n); + vec_ew_sqrt(work->E_temp, m); + + // Divide scalings D and E by themselves + vec_ew_recipr(work->D_temp, work->D_temp, n); + vec_ew_recipr(work->E_temp, work->E_temp, m); + + // Equilibrate matrices P and A and vector q + // P <- DPD + mat_premult_diag(work->data->P, work->D_temp); + mat_postmult_diag(work->data->P, work->D_temp); + + // A <- EAD + mat_premult_diag(work->data->A, work->E_temp); + mat_postmult_diag(work->data->A, work->D_temp); + + // q <- Dq + vec_ew_prod(work->D_temp, work->data->q, work->data->q, n); + + // Update equilibration matrices D and E + vec_ew_prod(work->scaling->D, work->D_temp, work->scaling->D, n); + vec_ew_prod(work->scaling->E, work->E_temp, work->scaling->E, m); + + // + // Cost normalization step + // + + // Compute avg norm of cols of P + mat_inf_norm_cols_sym_triu(work->data->P, work->D_temp); + c_temp = vec_mean(work->D_temp, n); + + // Compute inf norm of q + inf_norm_q = vec_norm_inf(work->data->q, n); + + // If norm_q == 0, set it to 1 (ignore it in the scaling) + // NB: Using the same function as with vectors here + limit_scaling(&inf_norm_q, 1); + + // Compute max between avg norm of cols of P and inf norm of q + c_temp = c_max(c_temp, inf_norm_q); + + // Limit scaling (use same function as with vectors) + limit_scaling(&c_temp, 1); + + // Invert scaling c = 1 / cost_measure + c_temp = 1. / c_temp; + + // Scale P + mat_mult_scalar(work->data->P, c_temp); + + // Scale q + vec_mult_scalar(work->data->q, c_temp, n); + + // Update cost scaling + work->scaling->c *= c_temp; + } + + + // Store cinv, Dinv, Einv + work->scaling->cinv = 1. / work->scaling->c; + vec_ew_recipr(work->scaling->D, work->scaling->Dinv, work->data->n); + vec_ew_recipr(work->scaling->E, work->scaling->Einv, work->data->m); + + + // Scale problem vectors l, u + vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m); + vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m); + + return 0; +} + +#endif // EMBEDDED + +c_int unscale_data(OSQPWorkspace *work) { + // Unscale cost + mat_mult_scalar(work->data->P, work->scaling->cinv); + mat_premult_diag(work->data->P, work->scaling->Dinv); + mat_postmult_diag(work->data->P, work->scaling->Dinv); + vec_mult_scalar(work->data->q, work->scaling->cinv, work->data->n); + vec_ew_prod(work->scaling->Dinv, work->data->q, work->data->q, work->data->n); + + // Unscale constraints + mat_premult_diag(work->data->A, work->scaling->Einv); + mat_postmult_diag(work->data->A, work->scaling->Dinv); + vec_ew_prod(work->scaling->Einv, work->data->l, work->data->l, work->data->m); + vec_ew_prod(work->scaling->Einv, work->data->u, work->data->u, work->data->m); + + return 0; +} + +c_int unscale_solution(OSQPWorkspace *work) { + // primal + vec_ew_prod(work->scaling->D, + work->solution->x, + work->solution->x, + work->data->n); + + // dual + vec_ew_prod(work->scaling->E, + work->solution->y, + work->solution->y, + work->data->m); + vec_mult_scalar(work->solution->y, work->scaling->cinv, work->data->m); + + return 0; +} diff --git a/src/lib/allocator_qp/src/osqp/util.c b/src/lib/allocator_qp/src/osqp/util.c new file mode 100644 index 000000000000..8e8d04abcd3f --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/util.c @@ -0,0 +1,487 @@ +#include "util.h" + +/*************** +* Versioning * +***************/ +const char* osqp_version(void) { + return OSQP_VERSION; +} + +/************************************ +* Printing Constants to set Layout * +************************************/ +#ifdef PRINTING +# define HEADER_LINE_LEN 65 +#endif /* ifdef PRINTING */ + +/********************** +* Utility Functions * +**********************/ +void c_strcpy(char dest[], const char source[]) { + int i = 0; + + while (1) { + dest[i] = source[i]; + + if (dest[i] == '\0') break; + i++; + } +} + +#ifdef PRINTING + +static void print_line(void) { + char the_line[HEADER_LINE_LEN + 1]; + c_int i; + + for (i = 0; i < HEADER_LINE_LEN; ++i) the_line[i] = '-'; + the_line[HEADER_LINE_LEN] = '\0'; + c_print("%s\n", the_line); +} + +void print_header(void) { + // Different indentation required for windows +#if defined(IS_WINDOWS) && !defined(PYTHON) + c_print("iter "); +#else + c_print("iter "); +#endif + + // Main information + c_print("objective pri res dua res rho"); +# ifdef PROFILING + c_print(" time"); +# endif /* ifdef PROFILING */ + c_print("\n"); +} + +void print_setup_header(const OSQPWorkspace *work) { + OSQPData *data; + OSQPSettings *settings; + c_int nnz; // Number of nonzeros in the problem + + data = work->data; + settings = work->settings; + + // Number of nonzeros + nnz = data->P->p[data->P->n] + data->A->p[data->A->n]; + + print_line(); + c_print(" OSQP v%s - Operator Splitting QP Solver\n" + " (c) Bartolomeo Stellato, Goran Banjac\n" + " University of Oxford - Stanford University 2021\n", + OSQP_VERSION); + print_line(); + + // Print variables and constraints + c_print("problem: "); + c_print("variables n = %i, constraints m = %i\n ", + (int)data->n, + (int)data->m); + c_print("nnz(P) + nnz(A) = %i\n", (int)nnz); + + // Print Settings + c_print("settings: "); + c_print("linear system solver = %s", + LINSYS_SOLVER_NAME[settings->linsys_solver]); + + if (work->linsys_solver->nthreads != 1) { + c_print(" (%d threads)", (int)work->linsys_solver->nthreads); + } + c_print(",\n "); + + c_print("eps_abs = %.1e, eps_rel = %.1e,\n ", + settings->eps_abs, settings->eps_rel); + c_print("eps_prim_inf = %.1e, eps_dual_inf = %.1e,\n ", + settings->eps_prim_inf, settings->eps_dual_inf); + c_print("rho = %.2e ", settings->rho); + + if (settings->adaptive_rho) { + c_print("(adaptive)"); + } + c_print(",\n "); + c_print("sigma = %.2e, alpha = %.2f, ", + settings->sigma, settings->alpha); + c_print("max_iter = %i\n", (int)settings->max_iter); + + if (settings->check_termination) { + c_print(" check_termination: on (interval %i),\n", + (int)settings->check_termination); + } else {c_print(" check_termination: off,\n");} +# ifdef PROFILING + if (settings->time_limit) { + c_print(" time_limit: %.2e sec,\n", settings->time_limit); + } +# endif /* ifdef PROFILING */ + + if (settings->scaling) { + c_print(" scaling: on, "); + } else { + c_print(" scaling: off, "); + } + + if (settings->scaled_termination) { + c_print("scaled_termination: on\n"); + } else { + c_print("scaled_termination: off\n"); + } + + if (settings->warm_start) { + c_print(" warm start: on, "); + } else { + c_print(" warm start: off, "); + } + + if (settings->polish) { + c_print("polish: on, "); + } else { + c_print("polish: off, "); + } + +# ifdef PROFILING + if (settings->time_limit) { + c_print("time_limit: %.2e sec\n", settings->time_limit); + } else { + c_print("time_limit: off\n"); + } +# endif + + c_print("\n"); +} + +void print_summary(OSQPWorkspace *work) { + OSQPInfo *info; + + info = work->info; + + c_print("%4i", (int)info->iter); + c_print(" %12.4e", info->obj_val); + c_print(" %9.2e", info->pri_res); + c_print(" %9.2e", info->dua_res); + c_print(" %9.2e", work->settings->rho); +# ifdef PROFILING + + if (work->first_run) { + // total time: setup + solve + c_print(" %9.2es", info->setup_time + info->solve_time); + } else { + // total time: update + solve + c_print(" %9.2es", info->update_time + info->solve_time); + } +# endif /* ifdef PROFILING */ + c_print("\n"); + + work->summary_printed = 1; // Summary has been printed +} + +void print_polish(OSQPWorkspace *work) { + OSQPInfo *info; + + info = work->info; + + c_print("%4s", "plsh"); + c_print(" %12.4e", info->obj_val); + c_print(" %9.2e", info->pri_res); + c_print(" %9.2e", info->dua_res); + + // Different characters for windows/unix +#if defined(IS_WINDOWS) && !defined(PYTHON) + c_print(" ---------"); +#else + c_print(" --------"); +#endif + +# ifdef PROFILING + if (work->first_run) { + // total time: setup + solve + c_print(" %9.2es", info->setup_time + info->solve_time + + info->polish_time); + } else { + // total time: update + solve + c_print(" %9.2es", info->update_time + info->solve_time + + info->polish_time); + } +# endif /* ifdef PROFILING */ + c_print("\n"); +} + +void print_footer(OSQPInfo *info, c_int polish) { + c_print("\n"); // Add space after iterations + + c_print("status: %s\n", info->status); + + if (polish && (info->status_val == OSQP_SOLVED)) { + if (info->status_polish == 1) { + c_print("solution polish: successful\n"); + } else if (info->status_polish < 0) { + c_print("solution polish: unsuccessful\n"); + } + } + + c_print("number of iterations: %i\n", (int)info->iter); + + if ((info->status_val == OSQP_SOLVED) || + (info->status_val == OSQP_SOLVED_INACCURATE)) { + c_print("optimal objective: %.4f\n", info->obj_val); + } + +# ifdef PROFILING + c_print("run time: %.2es\n", info->run_time); +# endif /* ifdef PROFILING */ + +# if EMBEDDED != 1 + c_print("optimal rho estimate: %.2e\n", info->rho_estimate); +# endif /* if EMBEDDED != 1 */ + c_print("\n"); +} + +#endif /* End #ifdef PRINTING */ + + +#ifndef EMBEDDED + +OSQPSettings* copy_settings(const OSQPSettings *settings) { + OSQPSettings *new = c_malloc(sizeof(OSQPSettings)); + + if (!new) return OSQP_NULL; + + // Copy settings + // NB. Copying them explicitly because memcpy is not + // defined when PRINTING is disabled (appears in string.h) + new->rho = settings->rho; + new->sigma = settings->sigma; + new->scaling = settings->scaling; + +# if EMBEDDED != 1 + new->adaptive_rho = settings->adaptive_rho; + new->adaptive_rho_interval = settings->adaptive_rho_interval; + new->adaptive_rho_tolerance = settings->adaptive_rho_tolerance; +# ifdef PROFILING + new->adaptive_rho_fraction = settings->adaptive_rho_fraction; +# endif +# endif // EMBEDDED != 1 + new->max_iter = settings->max_iter; + new->eps_abs = settings->eps_abs; + new->eps_rel = settings->eps_rel; + new->eps_prim_inf = settings->eps_prim_inf; + new->eps_dual_inf = settings->eps_dual_inf; + new->alpha = settings->alpha; + new->linsys_solver = settings->linsys_solver; + new->delta = settings->delta; + new->polish = settings->polish; + new->polish_refine_iter = settings->polish_refine_iter; + new->verbose = settings->verbose; + new->scaled_termination = settings->scaled_termination; + new->check_termination = settings->check_termination; + new->warm_start = settings->warm_start; +# ifdef PROFILING + new->time_limit = settings->time_limit; +# endif + + return new; +} + +#endif // #ifndef EMBEDDED + + +/******************* +* Timer Functions * +*******************/ + +#ifdef PROFILING + +// Windows +# ifdef IS_WINDOWS + +void osqp_tic(OSQPTimer *t) +{ + QueryPerformanceFrequency(&t->freq); + QueryPerformanceCounter(&t->tic); +} + +c_float osqp_toc(OSQPTimer *t) +{ + QueryPerformanceCounter(&t->toc); + return (t->toc.QuadPart - t->tic.QuadPart) / (c_float)t->freq.QuadPart; +} + +// Mac +# elif defined IS_MAC + +void osqp_tic(OSQPTimer *t) +{ + /* read current clock cycles */ + t->tic = mach_absolute_time(); +} + +c_float osqp_toc(OSQPTimer *t) +{ + uint64_t duration; /* elapsed time in clock cycles*/ + + t->toc = mach_absolute_time(); + duration = t->toc - t->tic; + + /*conversion from clock cycles to nanoseconds*/ + mach_timebase_info(&(t->tinfo)); + duration *= t->tinfo.numer; + duration /= t->tinfo.denom; + + return (c_float)duration / 1e9; +} + +// Linux +# else /* ifdef IS_WINDOWS */ + +/* read current time */ +void osqp_tic(OSQPTimer *t) +{ + clock_gettime(CLOCK_MONOTONIC, &t->tic); +} + +/* return time passed since last call to tic on this timer */ +c_float osqp_toc(OSQPTimer *t) +{ + struct timespec temp; + + clock_gettime(CLOCK_MONOTONIC, &t->toc); + + if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_nsec = 1e9 + t->toc.tv_nsec - t->tic.tv_nsec; + } else { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; + } + return (c_float)temp.tv_sec + (c_float)temp.tv_nsec / 1e9; +} + +# endif /* ifdef IS_WINDOWS */ + +#endif // If Profiling end + + +/* ==================== DEBUG FUNCTIONS ======================= */ + + + +// If debug mode enabled +#ifdef DDEBUG + +#ifdef PRINTING + +void print_csc_matrix(csc *M, const char *name) +{ + c_int j, i, row_start, row_stop; + c_int k = 0; + + // Print name + c_print("%s :\n", name); + + for (j = 0; j < M->n; j++) { + row_start = M->p[j]; + row_stop = M->p[j + 1]; + + if (row_start == row_stop) continue; + else { + for (i = row_start; i < row_stop; i++) { + c_print("\t[%3u,%3u] = %.3g\n", (int)M->i[i], (int)j, M->x[k++]); + } + } + } +} + +void dump_csc_matrix(csc *M, const char *file_name) { + c_int j, i, row_strt, row_stop; + c_int k = 0; + FILE *f = fopen(file_name, "w"); + + if (f != NULL) { + for (j = 0; j < M->n; j++) { + row_strt = M->p[j]; + row_stop = M->p[j + 1]; + + if (row_strt == row_stop) continue; + else { + for (i = row_strt; i < row_stop; i++) { + fprintf(f, "%d\t%d\t%20.18e\n", + (int)M->i[i] + 1, (int)j + 1, M->x[k++]); + } + } + } + fprintf(f, "%d\t%d\t%20.18e\n", (int)M->m, (int)M->n, 0.0); + fclose(f); + c_print("File %s successfully written.\n", file_name); + } else { + c_eprint("Error during writing file %s.\n", file_name); + } +} + +void print_trip_matrix(csc *M, const char *name) +{ + c_int k = 0; + + // Print name + c_print("%s :\n", name); + + for (k = 0; k < M->nz; k++) { + c_print("\t[%3u, %3u] = %.3g\n", (int)M->i[k], (int)M->p[k], M->x[k]); + } +} + +void print_dns_matrix(c_float *M, c_int m, c_int n, const char *name) +{ + c_int i, j; + + c_print("%s : \n\t", name); + + for (i = 0; i < m; i++) { // Cycle over rows + for (j = 0; j < n; j++) { // Cycle over columns + if (j < n - 1) + // c_print("% 14.12e, ", M[j*m+i]); + c_print("% .3g, ", M[j * m + i]); + + else + // c_print("% 14.12e; ", M[j*m+i]); + c_print("% .3g; ", M[j * m + i]); + } + + if (i < m - 1) { + c_print("\n\t"); + } + } + c_print("\n"); +} + +void print_vec(c_float *v, c_int n, const char *name) { + print_dns_matrix(v, 1, n, name); +} + +void dump_vec(c_float *v, c_int len, const char *file_name) { + c_int i; + FILE *f = fopen(file_name, "w"); + + if (f != NULL) { + for (i = 0; i < len; i++) { + fprintf(f, "%20.18e\n", v[i]); + } + fclose(f); + c_print("File %s successfully written.\n", file_name); + } else { + c_print("Error during writing file %s.\n", file_name); + } +} + +void print_vec_int(c_int *x, c_int n, const char *name) { + c_int i; + + c_print("%s = [", name); + + for (i = 0; i < n; i++) { + c_print(" %i ", (int)x[i]); + } + c_print("]\n"); +} + +#endif // PRINTING + +#endif // DEBUG MODE diff --git a/src/lib/allocator_qp/src/osqp/workspace.c b/src/lib/allocator_qp/src/osqp/workspace.c new file mode 100644 index 000000000000..d36aa94d5102 --- /dev/null +++ b/src/lib/allocator_qp/src/osqp/workspace.c @@ -0,0 +1,352 @@ +/* + * This file was autogenerated by OSQP-Python on October 21, 2022 at 12:11:41. + * + * This file contains the workspace variables needed by OSQP. + */ + +#include "types.h" +#include "qdldl_interface.h" + +// Define data structure +c_int Pdata_i[10] = { +0, +0, +1, +0, +1, +2, +0, +1, +2, +3, +}; +c_int Pdata_p[5] = { +0, +1, +3, +6, +10, +}; +c_float Pdata_x[10] = { +(c_float)0.18632775557178593573, +(c_float)0.16310043703762008849, +(c_float)0.16351787344762380716, +(c_float)0.18801560136866851680, +(c_float)0.20738454767796993261, +(c_float)0.33209636075693677038, +(c_float)0.13095872534237409934, +(c_float)0.12846980145768741033, +(c_float)0.18093282286191703090, +(c_float)0.10929161570006164228, +}; +csc Pdata = {10, 4, 4, Pdata_p, Pdata_i, Pdata_x, -1}; +c_int Adata_i[4] = { +0, +1, +2, +3, +}; +c_int Adata_p[5] = { +0, +1, +2, +3, +4, +}; +c_float Adata_x[4] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000022204, +(c_float)1.00000000000000000000, +}; +csc Adata = {4, 4, 4, Adata_p, Adata_i, Adata_x, -1}; +c_float qdata[4] = { +(c_float)0.86548400311251072470, +(c_float)0.80993937655462866498, +(c_float)1.00000000000000000000, +(c_float)0.65027997381137192523, +}; +c_float ldata[4] = { +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +}; +c_float udata[4] = { +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.53568345279424178784, +(c_float)10.00000000000000000000, +}; +OSQPData data = {4, 4, &Pdata, &Adata, qdata, ldata, udata}; + +// Define settings structure +OSQPSettings settings = {(c_float)0.10000000000000000555, (c_float)0.00000100000000000000, 10, 1, 0, (c_float)5.00000000000000000000, 4000, (c_float)0.00100000000000000002, (c_float)0.00100000000000000002, (c_float)0.00010000000000000000, (c_float)0.00010000000000000000, (c_float)1.60000000000000008882, (enum linsys_solver_type) LINSYS_SOLVER, 0, 25, 1, }; + +// Define scaling structure +c_float Dscaling[4] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)0.94915532008963621191, +(c_float)1.00000000000000000000, +}; +c_float Dinvscaling[4] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.05356834527942400115, +(c_float)1.00000000000000000000, +}; +c_float Escaling[4] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.05356834527942422319, +(c_float)1.00000000000000000000, +}; +c_float Einvscaling[4] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)0.94915532008963598987, +(c_float)1.00000000000000000000, +}; +OSQPScaling scaling = {(c_float)0.29918422379257630928, Dscaling, Escaling, (c_float)3.34242222843039193947, Dinvscaling, Einvscaling}; + +// Define linsys_solver structure +c_int linsys_solver_L_i[10] = { +7, +4, +5, +6, +5, +6, +7, +6, +7, +7, +}; +c_int linsys_solver_L_p[9] = { +0, +1, +2, +3, +4, +7, +9, +10, +10, +}; +c_float linsys_solver_L_x[10] = { +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000003331, +(c_float)-0.10000000000000000555, +(c_float)0.78698176325950719434, +(c_float)0.48751650983063898881, +(c_float)0.61893265899240201833, +(c_float)0.29688563841677856336, +(c_float)0.22187006561367000890, +(c_float)0.27433689030327057523, +}; +csc linsys_solver_L = {10, 8, 8, linsys_solver_L_p, linsys_solver_L_i, linsys_solver_L_x, -1}; +c_float linsys_solver_Dinv[8] = { +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)3.79479460775228627512, +(c_float)3.71899976028910517201, +(c_float)8.13264311014344798423, +(c_float)6.13911332785330010609, +}; +c_int linsys_solver_P[8] = { +4, +5, +6, +7, +1, +2, +3, +0, +}; +c_float linsys_solver_bp[8]; +c_float linsys_solver_sol[8]; +c_float linsys_solver_rho_inv_vec[4] = { +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +}; +c_int linsys_solver_Pdiag_idx[4] = { +0, +2, +5, +9, +}; +c_int linsys_solver_KKT_i[18] = { +0, +1, +2, +3, +4, +1, +4, +5, +2, +4, +5, +6, +3, +7, +4, +5, +6, +0, +}; +c_int linsys_solver_KKT_p[9] = { +0, +1, +2, +3, +4, +6, +9, +13, +18, +}; +c_float linsys_solver_KKT_x[18] = { +(c_float)-10.00000000000000000000, +(c_float)-10.00000000000000000000, +(c_float)-10.00000000000000000000, +(c_float)-10.00000000000000000000, +(c_float)0.16351887344762380816, +(c_float)1.00000000000000000000, +(c_float)0.20738454767796993261, +(c_float)0.33209736075693674362, +(c_float)1.00000000000000022204, +(c_float)0.12846980145768741033, +(c_float)0.18093282286191703090, +(c_float)0.10929261570006164328, +(c_float)1.00000000000000000000, +(c_float)0.18632875557178593673, +(c_float)0.16310043703762008849, +(c_float)0.18801560136866851680, +(c_float)0.13095872534237409934, +(c_float)1.00000000000000000000, +}; +csc linsys_solver_KKT = {18, 8, 8, linsys_solver_KKT_p, linsys_solver_KKT_i, linsys_solver_KKT_x, -1}; +c_int linsys_solver_PtoKKT[10] = { +13, +14, +4, +15, +6, +7, +16, +9, +10, +11, +}; +c_int linsys_solver_AtoKKT[4] = { +17, +5, +8, +12, +}; +c_int linsys_solver_rhotoKKT[4] = { +0, +1, +2, +3, +}; +QDLDL_float linsys_solver_D[8] = { +-10, +-10, +-10, +-10, +0, +0, +0, +0, +}; +QDLDL_int linsys_solver_etree[8] = { +7, +4, +5, +6, +5, +6, +7, +-1, +}; +QDLDL_int linsys_solver_Lnz[8] = { +1, +1, +1, +1, +3, +2, +1, +0, +}; +QDLDL_int linsys_solver_iwork[24]; +QDLDL_bool linsys_solver_bwork[8]; +QDLDL_float linsys_solver_fwork[8]; +qdldl_solver linsys_solver = {QDLDL_SOLVER, &solve_linsys_qdldl, &update_linsys_solver_matrices_qdldl, &update_linsys_solver_rho_vec_qdldl, &linsys_solver_L, linsys_solver_Dinv, linsys_solver_P, linsys_solver_bp, linsys_solver_sol, linsys_solver_rho_inv_vec, (c_float)0.00000100000000000000, 4, 4, linsys_solver_Pdiag_idx, 4, &linsys_solver_KKT, linsys_solver_PtoKKT, linsys_solver_AtoKKT, linsys_solver_rhotoKKT, linsys_solver_D, linsys_solver_etree, linsys_solver_Lnz, linsys_solver_iwork, linsys_solver_bwork, linsys_solver_fwork, }; + +// Define solution +c_float xsolution[4]; +c_float ysolution[4]; + +OSQPSolution solution = {xsolution, ysolution}; + +// Define info +OSQPInfo info = {0, "Unsolved", OSQP_UNSOLVED, 0.0, 0.0, 0.0}; + +// Define workspace +c_float work_rho_vec[4] = { +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +}; +c_float work_rho_inv_vec[4] = { +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +}; +c_int work_constr_type[4] = { +0, +0, +0, +0, +}; +c_float work_x[4]; +c_float work_y[4]; +c_float work_z[4]; +c_float work_xz_tilde[8]; +c_float work_x_prev[4]; +c_float work_z_prev[4]; +c_float work_Ax[4]; +c_float work_Px[4]; +c_float work_Aty[4]; +c_float work_delta_y[4]; +c_float work_Atdelta_y[4]; +c_float work_delta_x[4]; +c_float work_Pdelta_x[4]; +c_float work_Adelta_x[4]; +c_float work_D_temp[4]; +c_float work_D_temp_A[4]; +c_float work_E_temp[4]; + +OSQPWorkspace workspace = { +&data, (LinSysSolver *)&linsys_solver, +work_rho_vec, work_rho_inv_vec, +work_constr_type, +work_x, work_y, work_z, work_xz_tilde, +work_x_prev, work_z_prev, +work_Ax, work_Px, work_Aty, +work_delta_y, work_Atdelta_y, +work_delta_x, work_Pdelta_x, work_Adelta_x, +work_D_temp, work_D_temp_A, work_E_temp, +&settings, &scaling, &solution, &info}; + diff --git a/src/lib/allocator_qp/src/setup.py b/src/lib/allocator_qp/src/setup.py new file mode 100644 index 000000000000..f5104f99ae9c --- /dev/null +++ b/src/lib/allocator_qp/src/setup.py @@ -0,0 +1,86 @@ +from setuptools import setup, Extension +from setuptools.command.build_ext import build_ext +import distutils.sysconfig as sysconfig +from platform import system +from glob import glob +import os +import shutil as sh +from subprocess import call + + +class build_ext_osqp(build_ext): + def finalize_options(self): + build_ext.finalize_options(self) + # Prevent numpy from thinking it is still in its setup process: + __builtins__.__NUMPY_SETUP__ = False + import numpy + self.include_dirs.append(numpy.get_include()) + + +''' +Define macros +''' +# Pass EMBEDDED flag to cmake to generate osqp_configure.h +# and qdldl_types.h files +cmake_args = [] +embedded_flag = 2 +cmake_args += ['-DEMBEDDED:INT=%i' % embedded_flag] + +# Pass Python flag to compile interface +define_macros = [] +define_macros += [('PYTHON', None)] + +# Generate glob_opts.h file by running cmake +current_dir = os.getcwd() +os.chdir('..') +if os.path.exists('build'): + sh.rmtree('build') +os.makedirs('build') +os.chdir('build') +call(['cmake'] + cmake_args + ['..'], stdout=open(os.devnull, 'wb')) +os.chdir(current_dir) + +''' +Define compiler flags +''' +if system() != 'Windows': + compile_args = ["-O3"] +else: + compile_args = [] + +# Add additional libraries +libraries = [] +if system() == 'Linux': + libraries = ['rt'] + +''' +Include directory +''' +include_dirs = [os.path.join('..', 'include')] # OSQP includes + +''' +Source files +''' +sources_files = ['emosqpmodule.c'] # Python wrapper +sources_files += glob(os.path.join('osqp', '*.c')) # OSQP files + + +emosqp = Extension('emosqp', + define_macros=define_macros, + libraries=libraries, + include_dirs=include_dirs, + sources=sources_files, + extra_compile_args=compile_args) + + +setup(name='emosqp', + version='0.6.2.post5', + author='Bartolomeo Stellato, Goran Banjac', + author_email='bartolomeo.stellato@gmail.com', + description='This is the Python module for embedded OSQP: ' + + 'Operator Splitting solver for Quadratic Programs.', + setup_requires=["numpy >= 1.7"], + install_requires=["numpy >= 1.7", "future"], + license='Apache 2.0', + cmdclass={'build_ext': build_ext_osqp}, + ext_modules=[emosqp]) diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 159d2bcbfbad..749a5f52ff19 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -88,6 +88,7 @@ class LowPassFilter2p if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) { disable(); } + } /** @@ -158,6 +159,12 @@ class LowPassFilter2p _a1 = 0.f; _a2 = 0.f; + + + } + + bool is_disabled(){ + return _sample_freq <= 0.0f; } protected: @@ -174,6 +181,7 @@ class LowPassFilter2p float _cutoff_freq{0.f}; float _sample_freq{0.f}; + }; } // namespace math diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 20ecd67f2c8d..2907c2cbdb0a 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -572,5 +572,7 @@ SquareMatrix choleskyInv(const SquareMatrix &A) using Matrix3f = SquareMatrix; using Matrix3d = SquareMatrix; +using Matrix4f = SquareMatrix; +using Matrix4d = SquareMatrix; } // namespace matrix diff --git a/src/lib/matrix/matrix/Vector.hpp b/src/lib/matrix/matrix/Vector.hpp index 892be7ce0b6b..73c01cc5d60e 100644 --- a/src/lib/matrix/matrix/Vector.hpp +++ b/src/lib/matrix/matrix/Vector.hpp @@ -148,6 +148,38 @@ class Vector : public Matrix return r; } + + Vector signed_sqrt() const + { + const Vector &a(*this); + Vector r; + + for (size_t i = 0; i < M; i++) { + Type v = a(i); + r(i) = (v < 0) ? Type(-std::sqrt(-v)) : Type(std::sqrt(v)); + } + + return r; + } + + bool has_nan() const + { + const Vector &a(*this); + + for (size_t i=0; i < M; i++){ + if (std::isnan(a(i))){ + return true; + } + } + return false; + } + + Vector zero_if_nan() const + { + return has_nan() ? Vector() : *this; + + } + }; } // namespace matrix diff --git a/src/lib/matrix/matrix/Vector4.hpp b/src/lib/matrix/matrix/Vector4.hpp new file mode 100644 index 000000000000..56122b647043 --- /dev/null +++ b/src/lib/matrix/matrix/Vector4.hpp @@ -0,0 +1,123 @@ +/** + * @file Vector4.hpp + * + * @author Devansh Agrawal + * + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Matrix; + +template +class Vector; + +template +class Dcm; + + +template +class Vector4 : public Vector +{ +public: + + using Matrix41 = Matrix; + + Vector4() = default; + + Vector4(const Matrix41 &other) : + Vector(other) + { + } + + explicit Vector4(const Type data_[4]) : + Vector(data_) + { + } + + Vector4(Type w, Type x, Type y, Type z) + { + Vector4 &v(*this); + v(0) = w; + v(1) = x; + v(2) = y; + v(3) = z; + } + + template + Vector4(const Slice &slice_in) : Vector(slice_in) + { + } + + template + Vector4(const Slice &slice_in) : Vector(slice_in) + { + } + + + /** + * Override matrix ops so Vector4 type is returned + */ + + inline Vector4 operator+(Vector4 other) const + { + return Matrix41::operator+(other); + } + + inline Vector4 operator+(Type scalar) const + { + return Matrix41::operator+(scalar); + } + + inline Vector4 operator-(Vector4 other) const + { + return Matrix41::operator-(other); + } + + inline Vector4 operator-(Type scalar) const + { + return Matrix41::operator-(scalar); + } + + inline Vector4 operator-() const + { + return Matrix41::operator-(); + } + + inline Vector4 operator*(Type scalar) const + { + return Matrix41::operator*(scalar); + } + + inline Type operator*(Vector4 b) const + { + return Vector::operator*(b); + } + + + /** + * Override vector ops so Vector4 type is returned + */ + inline Vector4 unit() const + { + return Vector4(Vector::unit()); + } + + inline Vector4 normalized() const + { + return unit(); + } + + +}; + +using Vector4f = Vector4; +using Vector4d = Vector4; + +} // namespace matrix diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp index 24a43723bf8c..f55b136d4178 100644 --- a/src/lib/matrix/matrix/math.hpp +++ b/src/lib/matrix/matrix/math.hpp @@ -10,6 +10,7 @@ #include "Vector.hpp" #include "Vector2.hpp" #include "Vector3.hpp" +#include "Vector4.hpp" #include "Euler.hpp" #include "Dcm.hpp" #include "Scalar.hpp" diff --git a/src/modules/indi_control/CMakeLists.txt b/src/modules/indi_control/CMakeLists.txt new file mode 100644 index 000000000000..348557c1ac3a --- /dev/null +++ b/src/modules/indi_control/CMakeLists.txt @@ -0,0 +1,22 @@ +# Indi Controller +# Devansh Agrawal Oct 2022 + +# add_subdirectory(allocator_qp) + +px4_add_module( + MODULE modules__indi_control + MAIN indi_control + COMPILE_FLAGS + -O3 + SRCS + IndiControl.cpp + IndiControlPubs.cpp + IndiControlCbs.cpp + IndiControlAllocator.cpp + IndiControl.hpp + DEPENDS + px4_work_queue + mathlib + allocator_qp +) + diff --git a/src/modules/indi_control/IndiControl.cpp b/src/modules/indi_control/IndiControl.cpp new file mode 100644 index 000000000000..627c32c1b0e8 --- /dev/null +++ b/src/modules/indi_control/IndiControl.cpp @@ -0,0 +1,557 @@ +// Indi Controller +// Devansh Agrawal Oct 2022 + +#include "IndiControl.hpp" + +using namespace matrix; + +IndiControl::IndiControl() + : ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + + // set default setpoint + _setpoint.position[0] = 0.0; + _setpoint.position[1] = 0.0; + _setpoint.position[2] = -1.25; + _setpoint.yaw = M_PI * 0.5; + + // set up filters + _tau_bz_filter.set_cutoff_frequency(RATE_TAU_BZ, RATE_LPF); + _ang_vel_filter.set_cutoff_frequency(RATE_ANG_VEL, RATE_LPF); + _ang_accel_filter.set_cutoff_frequency(RATE_ANG_ACC, RATE_LPF); + _a_filter.set_cutoff_frequency(RATE_ACCEL, RATE_LPF); + _torque_filter.set_cutoff_frequency(RATE_TORQUE, RATE_LPF); + _manual_control_z_filter.set_cutoff_frequency(RATE_TORQUE, RATE_MANUAL_Z); +} + +IndiControl::~IndiControl() { + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +bool IndiControl::init() { + + update_parameters(); + + if (!_vehicle_angular_velocity_sub.registerCallback()) { + return false; + } + + // Run on fixed interval + _interval = (uint32_t)(1.0e6 / ((double)RATE_TORQUE)); + // ScheduleOnInterval(_interval); // 500 us interval, 2000 Hz rate // TODO: + // Make this a parameter + + return true; +} + +void IndiControl::update_parameters() { + + // update all the constants + _mass = _param_mass.get(); + _thrust_constant = _param_thrust_constant.get(); + _torque_constant = _param_torque_constant.get(); + _J(0, 0) = _param_Jxx.get(); + _J(0, 1) = _param_Jxy.get(); + _J(0, 2) = _param_Jxz.get(); + _J(1, 0) = _param_Jxy.get(); + _J(1, 1) = _param_Jyy.get(); + _J(1, 2) = _param_Jyz.get(); + _J(2, 0) = _param_Jxz.get(); + _J(2, 1) = _param_Jyz.get(); + _J(2, 2) = _param_Jzz.get(); + + const float lx = 0.13; + const float ly = 0.22; + // assuming the motors are in the pattern (body FRD frame) + // +bx + // ^ + // | + // | + // + // 2 0 + // x --> +by + // 1 3 + // and motor 0 spins ccw (so neg bz direction) + + Vector4f motor_pos_x(lx, -lx, lx, -lx); + Vector4f motor_pos_y(ly, -ly, -ly, ly); + Vector4f motor_dir( + 1, 1, -1, -1); // positive for torque in +bz direction (body FRD frame) + + // construct the effectiveness matrix + Matrix4f G; + G.zero(); + for (size_t i = 0; i < 4; i++) { + // torque about body x: + G(0, i) = -motor_pos_y(i) * _thrust_constant; + // torque about body y: + G(1, i) = motor_pos_x(i) * _thrust_constant; + // torque about body z: + G(2, i) = motor_dir(i) * _torque_constant * _thrust_constant; + // thrust in body z: + G(3, i) = _thrust_constant; + } + + // construct the cost matrix for the allocator QP + Matrix4f H; + H.zero(); + H(0, 0) = 1.0; + H(1, 1) = 1.0; + H(2, 2) = 0.1; + H(3, 3) = 0.5; + + // update the matrices in the allocator + update_allocator_matrices(H, G); + + // set rpm bounds + update_allocator_rpm_bounds(0.0, _max_motor_rpm); +} + +void IndiControl::service_subscriptions() { + + cb_params(); + cb_vehicle_status(); + cb_vehicle_control_mode(); + cb_manual_control_setpoint(); + cb_vehicle_local_position(); + cb_vehicle_attitude(); + cb_vehicle_angular_velocity(); + cb_vehicle_angular_acceleration(); + cb_sensor_accel(); + cb_trajectory_setpoint(); +} + +void IndiControl::compute_cmd_accel() { + + Vector3f k_pos(3, 3, 2); + Vector3f k_vel(1, 1, 0.75); + Vector3f k_acc(0.0, 0.0, 0.0); + + Vector3f x(_local_position.x, _local_position.y, _local_position.z); + Vector3f v(_local_position.vx, _local_position.vy, _local_position.vz); + + Vector3f x_ref(_setpoint.position); + Vector3f v_ref(_setpoint.velocity); + Vector3f a_ref(_setpoint.acceleration); + + // acceleration command (EZRA, EQ:17) + _a_cmd = a_ref.zero_if_nan() + k_pos.emult((x_ref - x).zero_if_nan()) + + k_vel.emult((v_ref - v).zero_if_nan()) + + k_acc.emult((a_ref - _a_filt).zero_if_nan()); +} + +void IndiControl::compute_cmd_thrust() { + bool use_geometric = false; + + if (use_geometric) { + + float g = 9.81; + Vector3f _z(0, 0, 1); + + Vector3f acc = _a_cmd - g * _z; + + float coll_acc = acc.dot(Quatf(_attitude.q).rotateVector(_z)); + _thrust_cmd = -coll_acc * _mass; + } else { + // EZRA, EQ:20 + _tau_bz_cmd = _tau_bz_filt + _a_cmd - _a_filt; + + // EZRA, EQ:21 + _thrust_cmd = _mass * _tau_bz_cmd.norm(); + } +} + +void IndiControl::compute_cmd_thrust_manual() { + + const float max_thrust = + 0.75f * 4.0f * _thrust_constant * _max_motor_rpm * _max_motor_rpm; + + _thrust_cmd = + _manual_control_z_filter.apply(_manual_control_setpoint.z) * max_thrust; +} + +void IndiControl::compute_cmd_quaternion() { + bool use_geometric = true; + if (use_geometric) { + const float g = 9.81; + Vector3f _z(0, 0, 1); + + Vector3f acc = _a_cmd - g * _z; + + Vector3f b3d = (acc.norm() < 0.01f) ? _z : -acc.unit(); + + float yaw_ref = _setpoint.yaw; + + float b1dx = cosf(yaw_ref); + float b1dy = sinf(yaw_ref); + float b1dz = 0.0f; + + const Vector3f b1d(b1dx, b1dy, b1dz); + const Vector3f b2d = b3d.cross(b1d).unit(); + const Vector3f b1d_new = b2d.cross(b3d).unit(); + + // construct desired rot matrix + Dcm rotDes; + for (size_t i = 0; i < 3; i++) { + rotDes(i, 0) = b1d_new(i); + rotDes(i, 1) = b2d(i); + rotDes(i, 2) = b3d(i); + } + + _xi_cmd = Quatf(rotDes); + } else { + + // get body to inertial quaterion + Quatf xi(_attitude.q); + + // EZRA, EQ:22 + Vector3f neg_bz_cmd_body = xi.rotateVectorInverse(-_tau_bz_cmd); + + // EZRA, EQ:23 + Vector3f iz(0, 0, 1); + Quatf xi_bar(iz, neg_bz_cmd_body); // min rotation constructor (defined in + // matrix/Quaternion.hpp) + + // EZRA, EQ:24 + const float yaw_ref = _setpoint.yaw; + Vector3f heading_vec_inertial(std::sin(yaw_ref), -std::cos(yaw_ref), 0); + Quatf xi_xi_bar = xi * xi_bar; + Vector3f heading_vec_bar = + xi_xi_bar.rotateVectorInverse(heading_vec_inertial); + + // EZRA, EQ:25 + Quatf xi_yaw(1, 0, 0, -heading_vec_bar(0) / heading_vec_bar(1)); + xi_yaw.normalize(); + + // EZRA, EQ:26 + _xi_cmd = xi_bar * xi_yaw; + } + // publish_xi_cmd(); +} + +void IndiControl::compute_cmd_quaternion_manual() { + + const float g = 9.81; + const Vector3f _z(0, 0, 1); + + const float max_angle = 30.0f; + + const float max_acc = g * std::tan(max_angle * ((float)M_PI) / 180.f); + + const float max_yaw_rate = 2.0f * (float)M_PI / 8.0f; + + // update yaw setpoint + _manual_yaw_setpoint += + _manual_control_setpoint.r * max_yaw_rate / RATE_TORQUE; + + // determine desired roll and pitch + float p_acc = _manual_control_setpoint.x * max_acc; + float r_acc = _manual_control_setpoint.y * max_acc; + + float c = cosf(_manual_yaw_setpoint); + float s = sinf(_manual_yaw_setpoint); + + const Vector3f acc = + Vector3f(c * p_acc - s * r_acc, s * p_acc + c * r_acc, -g); + + // get desired body z axis + Vector3f b3d = (acc.norm() < 0.01f) ? _z : -acc.unit(); + + float b1dx = cosf(_manual_yaw_setpoint); + float b1dy = sinf(_manual_yaw_setpoint); + float b1dz = 0.0f; + + const Vector3f b1d(b1dx, b1dy, b1dz); + const Vector3f b2d = b3d.cross(b1d).unit(); + const Vector3f b1d_new = b2d.cross(b3d).unit(); + + // construct desired rot matrix + Dcm rotDes; + for (size_t i = 0; i < 3; i++) { + rotDes(i, 0) = b1d_new(i); + rotDes(i, 1) = b2d(i); + rotDes(i, 2) = b3d(i); + } + + // convert to quaternion + _xi_cmd = Quatf(rotDes); +} + +void IndiControl::compute_cmd_ang_accel() { + + bool use_geometric = true; + + if (use_geometric) { + + const Vector3f kR(1.0, 1.0, 0.5); + const Vector3f kOmega(0.125, 0.125, 0.125 / 2.0); + + // get the desired rotation matrix + Dcm rotDes(_xi_cmd); + + // construct current rot matrix + Dcm rotMat(Quatf(_attitude.q)); + + // get rotation error + Vector3f rotMat_err = + 0.5f * + ((Dcm)((rotDes.T() * rotMat - rotMat.T() * rotDes))).vee(); + + // error in angular rate + Vector3f ang_rate(_ang_vel); + Vector3f ang_rate_sp(0, 0, 0); + Vector3f ang_rate_err = ang_rate - rotMat.T() * rotDes * ang_rate_sp; + + if (false) { + // desired moments + Vector3f omega_cross_Jomega = ang_rate.cross(_J * ang_rate); + + Vector3f moments = + -kR.emult(rotMat_err) - kOmega.emult(ang_rate_err) + + omega_cross_Jomega - + _J * (ang_rate.hat() * rotMat.T() * rotDes * ang_rate_sp); + + _ang_accel_cmd = _J.I() * (moments - omega_cross_Jomega); + + } else { + + _ang_accel_cmd = + _J.I() * (-kR.emult(rotMat_err) - kOmega.emult(ang_rate_err)); + } + + } else { + // // EZRA, EQ:27 + // float xi_w = _xi_cmd(0); + // Vector3f xi_e = (2.0f * std::acos(xi_w) / std::sqrt(1.0f - xi_w * xi_w)) + // * _xi_cmd.imag(); + + // Vector3f k_xi(175, 175, 82); + // Vector3f k_omega(19.5, 19.5, 19.2); + + // k_xi *= 0.01; + // k_omega *= 0.01; + + // Vector3f ang_vel_ref(0, 0, 0); //TODO: FIX + // Vector3f ang_accel_ref(0, 0, 0); // TODO: FIX + + // // EZRA, EQ:28 + // _ang_accel_cmd = ang_accel_ref.zero_if_nan() + // + k_xi.emult(xi_e.zero_if_nan()) + // + k_omega.emult((ang_vel_ref - _ang_vel_filt).zero_if_nan()); + // // why do i need the filtered version? + } + // publish_ang_accel_cmd(); +} + +void IndiControl::compute_cmd_torque() { + + // EZRA, EQ:31 + _torque_cmd = _torque_filt.zero_if_nan() + + _J * ((_ang_accel_cmd - _ang_accel).zero_if_nan()); +} + +void IndiControl::compute_cmd_pwm() { + + // note RPM is actually in units of (1000 radians) per second + + // translate from thrust and torque setpoints to motor RPM + Vector4f mu_T(_torque_cmd(0), _torque_cmd(1), _torque_cmd(2), _thrust_cmd); + + Vector4f rpm_sq = solve_allocator_qp(mu_T); + + // catch negatives + for (size_t i = 0; i < 4; i++) { + if (rpm_sq(i) < 0.0f) { + rpm_sq(i) = 0.0f; + } + } + + // translate from motor RPM to motor PWM + Vector4f rpm = rpm_sq.sqrt(); // in units of kilo-rad/s + + // map to RPM + for (size_t i = 0; i < 4; i++) { + _pwm_cmd(i) = rpm(i) / _max_motor_rpm; + _pwm_cmd(i) = math::constrain(_pwm_cmd(i), -1.0f, 1.0f); + } + + // update filters + Vector4f mu_T_applied = _G * rpm_sq; + + // // update tau_bz filter + float thrust = mu_T_applied(3); + Vector3f tau_bz = -(thrust / _mass) * _bz; + if (!tau_bz.has_nan()) { + _tau_bz_filt = _tau_bz_filter.apply(tau_bz); + } + // update torque filt + Vector3f torque_applied; + for (size_t i = 0; i < 3; i++) { + torque_applied(i) = mu_T_applied(i); + } + if (!torque_applied.has_nan()) { + _torque_filt = _torque_filter.apply(torque_applied); + } + + // update accel filter + if (!_a.has_nan()) { + _a_filt = _a_filter.apply(_a); + } + + publish_pwm_cmd(); +} + +bool IndiControl::check_flight_mode() { + + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_altitude_enabled && + !_vehicle_control_mode.flag_control_velocity_enabled && + !_vehicle_control_mode.flag_control_position_enabled) { + // is in stabilized mode + return true; + } else { + return false; + } +} + +void IndiControl::compute_cmd() { + + bool mode_manual = check_flight_mode(); + + if (!mode_manual) { + // run the main controller + compute_cmd_accel(); + compute_cmd_thrust(); + compute_cmd_quaternion(); + } else { + compute_cmd_thrust_manual(); + compute_cmd_quaternion_manual(); + } + compute_cmd_ang_accel(); + compute_cmd_torque(); + compute_cmd_pwm(); + + // // print pos error + // Vector3f x(_local_position.x, _local_position.y, _local_position.z); + // Vector3f x_ref(_setpoint.position); + // if (!(x - x_ref).has_nan() && _armed) { + // running_pos_err.update((x - x_ref)); + // // Vector3f mu = running_pos_err.mean(); + // // Vector3f sigma = running_pos_err.variance().sqrt(); + + // if (running_pos_err.count() > 10000) { + // running_pos_err.reset(); + // } + + // // PX4_INFO("POS ERROR: %f, %f, %f", + // // (double)(x-x_ref)(0), + // // (double)(x-x_ref)(1), + // // (double)(x-x_ref)(2)); + + // // PX4_INFO("POS ERROR: (%f +- %f) (%f +- %f) (%f +- %f)", + // (double)mu(0), + // // (double)sigma(0), (double)mu(1), (double)sigma(1), + // (double)mu(2), + // // (double)sigma(2)); + // } +} + +void IndiControl::construct_setpoint() { + float time_since_start_s = (float)(_now - _start) * 1.0e-6f; + float seconds_per_rev = 5.0f; + + float omega = 2.0f * (float)M_PI / seconds_per_rev; + float phase = omega * time_since_start_s; + + float amplitude = 0.5f; + + _setpoint.position[0] = amplitude * cos(phase); + _setpoint.position[1] = amplitude * sin(phase); + _setpoint.position[2] = -1.25f; + + _setpoint.velocity[0] = -amplitude * omega * sin(phase); + _setpoint.velocity[1] = amplitude * omega * cos(phase); + _setpoint.velocity[2] = 0.0f; + + _setpoint.acceleration[0] = -amplitude * omega * omega * cos(phase); + _setpoint.acceleration[1] = -amplitude * omega * omega * sin(phase); + _setpoint.acceleration[2] = 0.0f; + + _setpoint.yaw = -0.0f * 0.5f * phase; +} + +void IndiControl::Run() { + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + _now = hrt_absolute_time(); + service_subscriptions(); + construct_setpoint(); + compute_cmd(); + + // PX4_INFO("PERIOD: %lu INTERVAL: %u", (_now - _last), _interval); + _last = _now; + + perf_end(_loop_perf); +} + +int IndiControl::task_spawn(int argc, char *argv[]) { + IndiControl *instance = new IndiControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int IndiControl::print_status() { + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; +} + +int IndiControl::custom_command(int argc, char *argv[]) { + return print_usage("unknown command"); +} + +int IndiControl::print_usage(const char *reason) { + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Indi Controller +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("indi_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int indi_control_main(int argc, char *argv[]) { + return IndiControl::main(argc, argv); +} diff --git a/src/modules/indi_control/IndiControl.hpp b/src/modules/indi_control/IndiControl.hpp new file mode 100644 index 000000000000..f84f540ee0e2 --- /dev/null +++ b/src/modules/indi_control/IndiControl.hpp @@ -0,0 +1,222 @@ +// Indi Controller +// Devansh Agrawal Oct 2022 + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace time_literals; +using namespace matrix; + +class IndiControl : public ModuleBase, + public ModuleParams, + public px4::WorkItem { +public: + IndiControl(); + ~IndiControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + int print_status() override; + +private: + void Run() override; + + hrt_abstime _now; + hrt_abstime _last; + uint32_t _interval; + hrt_abstime _start; + + void service_subscriptions(); + void cb_params(); + void cb_vehicle_status(); + void cb_vehicle_control_mode(); + void cb_vehicle_local_position(); + void cb_vehicle_attitude(); + void cb_vehicle_angular_velocity(); + void cb_vehicle_angular_acceleration(); + void cb_sensor_accel(); + void cb_trajectory_setpoint(); + void cb_manual_control_setpoint(); + void update_parameters(); + + void construct_setpoint(); + + bool check_flight_mode(); + + void compute_cmd(); + void compute_cmd_accel(); + void compute_cmd_thrust(); + void compute_cmd_thrust_manual(); + void compute_cmd_quaternion(); + void compute_cmd_quaternion_manual(); + void compute_cmd_ang_accel(); + void compute_cmd_torque(); + void compute_cmd_pwm(); + void update_allocator_rpm_bounds(const float min, const float max); + void update_allocator_matrices(const Matrix4f H, const Matrix4f G); + Vector4f solve_allocator_qp(const Vector4f mu_ref); + + void publish_thrust_cmd(); + void publish_xi_cmd(); + void publish_ang_accel_cmd(); + void publish_torque_cmd(); + void publish_pwm_cmd(); + void publish_setpoints(const float acc, const Vector3f ang_accel); + void publish_attitude_setpoint(const Dcm rotDes); + void publish_rates_setpoint(const Vector3f ang_acc); + + // Quad_params + float _mass; + Matrix3f _J; + float _max_rpm; // used for RPM -> PWM conversion + float _thrust_constant; // Newtons -> PWM normalization factor + float _torque_constant; + float _max_motor_rpm = 1.100; // in kilo-rad/s + Matrix4f _G; + Matrix4f _H; + + // Private Variables + Vector3f _a; + Vector3f _a_cmd; + Vector3f _a_filt; + Vector3f _bz; + Vector3f _tau_bz_cmd; + Vector3f _tau_bz_filt; + float _thrust_cmd; + Quatf _xi_cmd; + Vector3f _ang_vel; + Vector3f _ang_vel_filt; + Vector3f _ang_accel; + Vector3f _ang_accel_cmd; + Vector3f _ang_accel_filt; + Vector3f _torque_filt; + Vector3f _torque_cmd; + Vector4f _rpm_cmd; + Vector4f _pwm_cmd; + float _manual_yaw_setpoint = 0.0f; + + math::WelfordMean running_pos_err; + + math::LowPassFilter2p _a_filter; + math::LowPassFilter2p _tau_bz_filter; + math::LowPassFilter2p _ang_vel_filter; + math::LowPassFilter2p _torque_filter; + math::LowPassFilter2p _ang_accel_filter; + math::LowPassFilter2p _manual_control_z_filter; + + // todo: make these into parameters (or grab them from the respective modules) + const float RATE_LPF = 10.0; // desired cutoff frequency for low-pass filter + const float RATE_TORQUE = 250.0; + // const float RATE_ACCEL = 250.0; // frequency of new acceleration messages + const float RATE_ACCEL = + RATE_TORQUE; // frequency of new acceleration messages + const float RATE_TAU_BZ = RATE_TORQUE; + const float RATE_ANG_VEL = 250.0; + const float RATE_ANG_ACC = 250.0; + const float RATE_MANUAL_Z = 2.0f; + + vehicle_status_s _vehicle_status; + vehicle_local_position_s _local_position; + vehicle_attitude_s _attitude; + vehicle_angular_velocity_s _angular_velocity; + trajectory_setpoint_s _setpoint; + vehicle_control_mode_s _vehicle_control_mode; + manual_control_setpoint_s _manual_control_setpoint; + // Publications + uORB::Publication _vehicle_torque_setpoint_pub{ + ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ + ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _actuator_motors_pub{ + ORB_ID(actuator_motors)}; + uORB::Publication _actuator_controls_pub{ + ORB_ID(actuator_controls_0)}; + + uORB::Publication _vehicle_attitude_setpoint_pub{ + ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _vehicle_rates_setpoint_pub{ + ORB_ID(vehicle_rates_setpoint)}; + + // Subscriptions + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_local_position_sub{ + ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{ + this, ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_angular_acceleration_sub{ + ORB_ID(vehicle_angular_acceleration)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)}; + uORB::Subscription _manual_control_setpoint_sub{ + ORB_ID(manual_control_setpoint)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), + 1_s}; + uORB::SubscriptionInterval _vehicle_control_mode_sub{ + ORB_ID(vehicle_control_mode), 1_s}; + + // Performance (perf) counters + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")}; + perf_counter_t _loop_interval_perf{ + perf_alloc(PC_INTERVAL, MODULE_NAME ": interval")}; + + // Parameters + DEFINE_PARAMETERS( + //(ParamInt) _param_sys_autostart, /**< + // example parameter */ (ParamInt) + //_param_sys_autoconfig /**< another parameter */ + (ParamFloat)_param_mass, + (ParamFloat)_param_thrust_constant, + (ParamFloat)_param_torque_constant, + (ParamFloat)_param_Jxx, + (ParamFloat)_param_Jyy, + (ParamFloat)_param_Jzz, + (ParamFloat)_param_Jxy, + (ParamFloat)_param_Jxz, + (ParamFloat)_param_Jyz) + + bool _armed{false}; +}; diff --git a/src/modules/indi_control/IndiControlAllocator.cpp b/src/modules/indi_control/IndiControlAllocator.cpp new file mode 100644 index 000000000000..8eacd9e01891 --- /dev/null +++ b/src/modules/indi_control/IndiControlAllocator.cpp @@ -0,0 +1,96 @@ +#include "IndiControl.hpp" + +using namespace matrix; + +void IndiControl::update_allocator_rpm_bounds(const float min_rpm, + const float max_rpm) { + + c_float l_new[4] = {min_rpm, min_rpm, min_rpm, min_rpm}; + c_float u_new[4] = {max_rpm, max_rpm, max_rpm, max_rpm}; + + // update bounds + c_int res = osqp_update_bounds(&workspace, l_new, u_new); + if (res != 0) { + PX4_WARN("unable to update bounds"); + } +} + +void IndiControl::update_allocator_matrices(const Matrix4f H, + const Matrix4f G) { + + _H = H; + _G = G; + + Matrix4f GHG = _G.transpose() * _H * _G; + + // update the CSC variables + c_float P_x_new[10]; + int ind = 0; + for (size_t j = 0; j < 4; j++) { + for (size_t i = 0; i <= j; i++) { + P_x_new[ind] = GHG(i, j); + ind++; + } + } + + c_int res = osqp_update_P(&workspace, P_x_new, OSQP_NULL, 10); + if (res != 0) { + PX4_WARN("unable to update quadratic cost"); + } +} + +Vector4f IndiControl::solve_allocator_qp(const Vector4f mu_ref) { + + // construct the linear cost + Vector4f q = -_G.transpose() * _H * mu_ref; + + c_float q_new[4]; + for (size_t i = 0; i < 4; i++) { + q_new[i] = q(i); + } + + // update linear cost + c_int res = osqp_update_lin_cost(&workspace, q_new); + if (res != 0) { + PX4_WARN("unable to update linear cost"); + } + + // solve + res = osqp_solve(&workspace); + if (res != 0) { + PX4_WARN("SOLVE EXIT FLAG != 0"); + } + + // check status + if (workspace.info->status_val != OSQP_SOLVED) { + PX4_WARN("OSQP DIDNT SOLVE SUCCESSFULLY! RETURNED: %d", + workspace.info->status_val); + } + + // construct solution + Vector4f omegas; + for (size_t i = 0; i < 4; i++) { + omegas(i) = workspace.solution->x[i]; + } + + // Vector4f mu_out = _G * omegas; + + // PX4_INFO("IN: %f, %f, %f, %f, OUT: %f, %f, %f, %f", + // (double)mu_ref(0), + // (double)mu_ref(1), + // (double)mu_ref(2), + // (double)mu_ref(3), + // (double)mu_out(0), + // (double)mu_out(1), + // (double)mu_out(2), + // (double)mu_out(3) + // ); + + // PX4_INFO("MU_DIFF: %f, %f, %f, %f", + // (double)(mu_out(0) - mu_ref(0)), + // (double)(mu_out(1) - mu_ref(1)), + // (double)(mu_out(2) - mu_ref(2)), + // (double)(mu_out(3) - mu_ref(3)) + // ); + return omegas; +} diff --git a/src/modules/indi_control/IndiControlCbs.cpp b/src/modules/indi_control/IndiControlCbs.cpp new file mode 100644 index 000000000000..5953592e6c0c --- /dev/null +++ b/src/modules/indi_control/IndiControlCbs.cpp @@ -0,0 +1,111 @@ +// Indi Controller Callbacks +// Devansh Agrawal Oct 2022 + +#include "IndiControl.hpp" + +void IndiControl::cb_params() { + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); // update module parameters (in DEFINE_PARAMETERS) + update_parameters(); + } +} + +void IndiControl::cb_vehicle_status() { + if (_vehicle_status_sub.updated()) { + + if (_vehicle_status_sub.copy(&_vehicle_status)) { + + const bool armed = (_vehicle_status.arming_state == + vehicle_status_s::ARMING_STATE_ARMED); + + if (armed && !_armed) { + PX4_WARN("vehicle armed due to %d", + _vehicle_status.latest_arming_reason); + _start = hrt_absolute_time(); + + } else if (!armed && _armed) { + PX4_INFO("vehicle disarmed due to %d", + _vehicle_status.latest_disarming_reason); + } + + _armed = armed; + } + } +} + +void IndiControl::cb_vehicle_control_mode() { + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } +} + +void IndiControl::cb_manual_control_setpoint() { + + if (_manual_control_setpoint_sub.updated()) { + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + } +} + +void IndiControl::cb_vehicle_local_position() { + if (_vehicle_local_position_sub.updated()) { + _vehicle_local_position_sub.copy(&_local_position); + // const float g = 9.81; + // Vector3f a ( _local_position.ax, _local_position.ay, _local_position.az + + // g); _a_filt = _a_filter.apply(a); + } +} + +void IndiControl::cb_vehicle_attitude() { + if (_vehicle_attitude_sub.updated()) { + _vehicle_attitude_sub.copy(&_attitude); + Quatf q(_attitude.q); + Vector3f ez(0, 0, 1); + _bz = q.rotateVector(ez); + } +} + +void IndiControl::cb_sensor_accel() { + if (_sensor_accel_sub.updated()) { + sensor_accel_s accel; + _sensor_accel_sub.copy(&accel); + Vector3f a_body(accel.x, accel.y, accel.z); + Quatf q(_attitude.q); + _a = q.rotateVector(a_body) + Vector3f(0, 0, 9.81); + } +} + +void IndiControl::cb_vehicle_angular_velocity() { + if (_vehicle_angular_velocity_sub.updated()) { + _vehicle_angular_velocity_sub.copy(&_angular_velocity); + for (size_t i = 0; i < 3; i++) { + _ang_vel(i) = _angular_velocity.xyz[i]; + } + if (!_ang_vel.has_nan()) { + _ang_vel_filt = _ang_vel_filter.apply(_ang_vel); + } + } +} + +void IndiControl::cb_vehicle_angular_acceleration() { + if (_vehicle_angular_acceleration_sub.updated()) { + vehicle_angular_acceleration_s msg; + _vehicle_angular_acceleration_sub.copy(&msg); + for (size_t i = 0; i < 3; i++) { + _ang_accel(i) = msg.xyz[i]; + } + if (!_ang_accel.has_nan()) { + _ang_accel_filt = _ang_accel_filter.apply(_ang_accel); + } + } +} + +void IndiControl::cb_trajectory_setpoint() { + if (_trajectory_setpoint_sub.updated()) { + _trajectory_setpoint_sub.copy(&_setpoint); + } +} diff --git a/src/modules/indi_control/IndiControlPubs.cpp b/src/modules/indi_control/IndiControlPubs.cpp new file mode 100644 index 000000000000..db783af21a75 --- /dev/null +++ b/src/modules/indi_control/IndiControlPubs.cpp @@ -0,0 +1,98 @@ + +// Indi Controller publish functions +// Devansh Agrawal Oct 2022 + +#include "IndiControl.hpp" + +void IndiControl::publish_thrust_cmd() { + + vehicle_thrust_setpoint_s msg; + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = _now; + msg.xyz[0] = 0.0; + msg.xyz[1] = 0.0; + msg.xyz[2] = + math::constrain(_thrust_cmd * 1.5f * _thrust_constant, -1.0f, 1.0f); + + if (_armed) { + // PX4_INFO("THRUST: %f, MSG: %f, K: %f", (double)_thrust_cmd, + // (double)msg.xyz[2], (double)_thrust_constant); + } + + _vehicle_thrust_setpoint_pub.publish(msg); +} + +void IndiControl::publish_xi_cmd() { + // TODO +} + +void IndiControl::publish_ang_accel_cmd() { + // TODO +} + +void IndiControl::publish_torque_cmd() { + + // vehicle_torque_setpoint_s msg{}; + // msg.timestamp = hrt_absolute_time(); + // msg.timestamp_sample = _now; + + // for (size_t i = 0; i < 3; i++) { + // msg.xyz[i] = math::constrain(_torque_cmd(i) * 0.005f * _torque_constant, + // -1.0f, 1.0f); + // } + + // _vehicle_torque_setpoint_pub.publish(msg); +} + +void IndiControl::publish_pwm_cmd() { + + // directly publish the cmd to actuator_motors + actuator_motors_s msg; + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = _now; + + msg.reversible_flags = 255; // _param_r_rev.get(); // TODO check! + + for (size_t i = 0; i < 4; i++) { + msg.control[i] = _pwm_cmd(i); + } + + _actuator_motors_pub.publish(msg); + + actuator_controls_s msg2; + msg2.timestamp = msg.timestamp; + msg2.timestamp_sample = _now; + msg2.control[actuator_controls_s::INDEX_ROLL] = _pwm_cmd(0); + msg2.control[actuator_controls_s::INDEX_PITCH] = _pwm_cmd(1); + msg2.control[actuator_controls_s::INDEX_YAW] = _pwm_cmd(2); + msg2.control[actuator_controls_s::INDEX_THROTTLE] = _pwm_cmd(3); + + _actuator_controls_pub.publish(msg2); +} + +void IndiControl::publish_attitude_setpoint(const Dcm rotDes) { + + vehicle_attitude_setpoint_s msg; + msg.timestamp = hrt_absolute_time(); + Euler euler(rotDes); + + msg.roll_body = euler.phi(); + msg.pitch_body = euler.theta(); + msg.yaw_body = euler.psi(); + + Quatf q_d(rotDes); + q_d.copyTo(msg.q_d); + + _vehicle_attitude_setpoint_pub.publish(msg); +} + +void IndiControl::publish_rates_setpoint(const Vector3f ang_acc) { + + vehicle_rates_setpoint_s msg; + msg.timestamp = hrt_absolute_time(); + msg.roll = ang_acc(0); + msg.pitch = ang_acc(1); + msg.yaw = ang_acc(2); + + _vehicle_rates_setpoint_pub.publish(msg); +} diff --git a/src/modules/indi_control/Kconfig b/src/modules/indi_control/Kconfig new file mode 100644 index 000000000000..351ae181aab8 --- /dev/null +++ b/src/modules/indi_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_INDI_CONTROL + bool "indi_control" + default n + ---help--- + Enable Indi Control diff --git a/src/modules/indi_control/indi_control_params.c b/src/modules/indi_control/indi_control_params.c new file mode 100644 index 000000000000..4b3cc4842d3e --- /dev/null +++ b/src/modules/indi_control/indi_control_params.c @@ -0,0 +1,13 @@ +#include + +PARAM_DEFINE_FLOAT(INDI_QUAD_MASS, 1.5f); + +PARAM_DEFINE_FLOAT(INDI_THRUST_K, 5.48f); // Newtons per kilo-rad/s +PARAM_DEFINE_FLOAT(INDI_TORQUE_K, 0.06f); // 0.06*5.48f // but is wrong? // Newton-Meters per kilo-rad/s + +PARAM_DEFINE_FLOAT(INDI_JXX, 0.03f); +PARAM_DEFINE_FLOAT(INDI_JXY, 0.0f); +PARAM_DEFINE_FLOAT(INDI_JXZ, 0.0f); +PARAM_DEFINE_FLOAT(INDI_JYY, 0.03f); +PARAM_DEFINE_FLOAT(INDI_JYZ, 0.0f); +PARAM_DEFINE_FLOAT(INDI_JZZ, 0.055f); diff --git a/src/modules/mc_rate_control/GeometricControl/GeometricControl.cpp b/src/modules/mc_rate_control/GeometricControl/GeometricControl.cpp index b71c3bd9888d..70e2fc134f4a 100644 --- a/src/modules/mc_rate_control/GeometricControl/GeometricControl.cpp +++ b/src/modules/mc_rate_control/GeometricControl/GeometricControl.cpp @@ -41,89 +41,93 @@ using namespace matrix; -void GeometricControl::set_gains(float kx, float kv, float kR, float komega){ +void GeometricControl::set_gains(float kx, float kv, float kR, float komega) +{ - _kx = kx; - _kv = kv; - _kR = kR; - _kOmega = komega; + _kx = kx; + _kv = kv; + _kR = kR; + _kOmega = komega; } -void GeometricControl::set_inertia(float Jxx, float Jyy, float Jzz, float Jxy, float Jxz, float Jyz){ - _J(0,0) = Jxx; - _J(1,1) = Jyy; - _J(2,2) = Jzz; +void GeometricControl::set_inertia(float Jxx, float Jyy, float Jzz, float Jxy, float Jxz, float Jyz) +{ + _J(0, 0) = Jxx; + _J(1, 1) = Jyy; + _J(2, 2) = Jzz; - _J(0,2) = Jxz; - _J(2,0) = Jxz; + _J(0, 2) = Jxz; + _J(2, 0) = Jxz; - _J(1,2) = Jyz; - _J(2,1) = Jyz; + _J(1, 2) = Jyz; + _J(2, 1) = Jyz; - _J(0,1) = Jxy; - _J(1,0) = Jxy; + _J(0, 1) = Jxy; + _J(1, 0) = Jxy; } matrix::Vector GeometricControl::update( - const matrix::Vector3f &pos, // pass in current state - const matrix::Vector3f &vel, - const matrix::Quatf &ang_att, - const matrix::Vector3f &ang_rate, - const vehicle_local_position_setpoint_s &setpoint, // pass in setpoint - const vehicle_control_mode_s &control_mode // determines which mode we should control it in - ) + const matrix::Vector3f &pos, // pass in current state + const matrix::Vector3f &vel, + const matrix::Quatf &ang_att, + const matrix::Vector3f &ang_rate, + const vehicle_local_position_setpoint_s &setpoint, // pass in setpoint + const vehicle_control_mode_s &control_mode // determines which mode we should control it in +) { // construction the required acceleration vector - matrix::Vector3f acc = -g*_z; - - // check the mode before we add stuff into it - if (control_mode.flag_control_position_enabled){ - - // error in position - const matrix::Vector3f pos_sp(setpoint.x, setpoint.y, setpoint.z); - const matrix::Vector3f pos_err = pos - pos_sp; - - // add the error to the acceleration vector - acc = acc - _kx * pos_err; - - } - - if (control_mode.flag_control_velocity_enabled){ - - // error in velocity - matrix::Vector3f vel_sp(0.0, 0.0, 0.0); - - if (PX4_ISFINITE(setpoint.vx)) {vel_sp(0) += setpoint.vx;} - if (PX4_ISFINITE(setpoint.vy)) {vel_sp(1) += setpoint.vy;} - if (PX4_ISFINITE(setpoint.vz)) {vel_sp(2) += setpoint.vz;} - - const matrix::Vector3f vel_err = vel - vel_sp; - - // add in the velocity correction - acc = acc - _kv*vel_err; - } - - if (control_mode.flag_control_acceleration_enabled){ - - // error in acceleration - matrix::Vector3f acc_sp(0.0, 0.0, 0.0); - - for (uint8_t i=0; i< 3; i++){ - if (PX4_ISFINITE(setpoint.acceleration[i])) {acc_sp(i) += setpoint.acceleration[i];} - } - - // add in the setpoints - acc = acc + acc_sp; - } + matrix::Vector3f acc = -g * _z; + + // check the mode before we add stuff into it + if (control_mode.flag_control_position_enabled) { + + // error in position + const matrix::Vector3f pos_sp(setpoint.x, setpoint.y, setpoint.z); + const matrix::Vector3f pos_err = pos - pos_sp; + + // add the error to the acceleration vector + acc = acc - _kx * pos_err; + + } + + if (control_mode.flag_control_velocity_enabled) { + + // error in velocity + matrix::Vector3f vel_sp(0.0, 0.0, 0.0); + + if (PX4_ISFINITE(setpoint.vx)) {vel_sp(0) += setpoint.vx;} + + if (PX4_ISFINITE(setpoint.vy)) {vel_sp(1) += setpoint.vy;} + + if (PX4_ISFINITE(setpoint.vz)) {vel_sp(2) += setpoint.vz;} + + const matrix::Vector3f vel_err = vel - vel_sp; + + // add in the velocity correction + acc = acc - _kv * vel_err; + } + + if (control_mode.flag_control_acceleration_enabled) { + + // error in acceleration + matrix::Vector3f acc_sp(0.0, 0.0, 0.0); + + for (uint8_t i = 0; i < 3; i++) { + if (PX4_ISFINITE(setpoint.acceleration[i])) {acc_sp(i) += setpoint.acceleration[i];} + } + + // add in the setpoints + acc = acc + acc_sp; + } // get the unit vector in the desired thrust direction - const matrix::Vector3f b3d = (acc.norm() < 0.01f) ? Vector3f(0,0,1) : -acc.unit(); + const matrix::Vector3f b3d = (acc.norm() < 0.01f) ? Vector3f(0, 0, 1) : -acc.unit(); // get the unit vector pointing in the heading direction - + float b1dx = cosf(setpoint.yaw); float b1dy = sinf(setpoint.yaw); float b1dz = 0.0f; @@ -139,40 +143,42 @@ matrix::Vector GeometricControl::update( // construct the desired rotation matrix matrix::Dcm rotDes; - for (uint i=0; i< 3; i++){ - rotDes(i,0) = b1d_new(i); - rotDes(i,1) = b2d(i); - rotDes(i,2) = b3d(i); - } + + for (uint i = 0; i < 3; i++) { + rotDes(i, 0) = b1d_new(i); + rotDes(i, 1) = b2d(i); + rotDes(i, 2) = b3d(i); + } // construct the current rotation matrix const matrix::Dcm rotMat(ang_att); - + // error in rotation - matrix::Vector3f rotMat_err = 0.5f * skew_to_vector( rotDes.T() * rotMat - rotMat.T() * rotDes); - + matrix::Vector3f rotMat_err = 0.5f * skew_to_vector(rotDes.T() * rotMat - rotMat.T() * rotDes); + // grab angular velocity and acceleration setpoints - const Vector3f ang_rate_sp(0,0,0); - const Vector3f ang_acc_sp(0,0,0); - + const Vector3f ang_rate_sp(0, 0, 0); + const Vector3f ang_acc_sp(0, 0, 0); + // error in angular rates const Vector3f ang_rate_err = ang_rate - rotMat.T() * rotDes * ang_rate_sp; // compute the desired total thrust - const float coll_acc = -acc.dot( rotMat * _z); + const float coll_acc = -acc.dot(rotMat * _z); // compute the desired moments - const matrix::Vector3f omega_cross_Jomega = ang_rate.cross ( _J * ang_rate); + const matrix::Vector3f omega_cross_Jomega = ang_rate.cross(_J * ang_rate); const matrix::Vector3f moments = -_kR * rotMat_err - _kOmega * ang_rate_err - + omega_cross_Jomega - - _J * (vector_to_skew(ang_rate) * rotMat.T() * rotDes * ang_rate_sp); // technically should have additional angular acceleration terms - ignored in this implementation. + + omega_cross_Jomega + - _J * (vector_to_skew(ang_rate) * rotMat.T() * rotDes * + ang_rate_sp); // technically should have additional angular acceleration terms - ignored in this implementation. + + // get the angular acceleration + const matrix::Vector3f ang_acc_des = _J.I() * (moments - omega_cross_Jomega); - // get the angular acceleration - const matrix::Vector3f ang_acc_des = _J.I() * ( moments - omega_cross_Jomega ); - // // normalize terms - convert from SI into arbitrary ranges for the drones // matrix::Vector3f ang_acc_des_normalized = ang_acc_des / torque_constant; // float coll_thrust_normalized = math::min(1.0f, math::max(0.0f, coll_acc / g * hover_throttle)); // clamp the thrust to a reasonable rangle @@ -180,8 +186,8 @@ matrix::Vector GeometricControl::update( // // bound the max angular acceleration // if ( ang_acc_des_normalized.norm() > torque_max) { - // ang_acc_des_normalized = ang_acc_des_normalized.unit() * torque_max; - // } + // ang_acc_des_normalized = ang_acc_des_normalized.unit() * torque_max; + // } // return things matrix::Vector res; @@ -189,36 +195,38 @@ matrix::Vector GeometricControl::update( res(1) = ang_acc_des(1); res(2) = ang_acc_des(2); res(3) = coll_acc; - - return res; + + return res; } -matrix::SquareMatrix GeometricControl::vector_to_skew(matrix::Vector3f v){ +matrix::SquareMatrix GeometricControl::vector_to_skew(matrix::Vector3f v) +{ matrix::SquareMatrix M; - M(0,0) = 0; - M(0,1) = -v(2); - M(0,2) = v(1); - - M(1,0) = v(2); - M(1,1) = 0; - M(1,2) = -v(0); - - M(2,0) = -v(1); - M(2,1) = v(0); - M(2,2) = 0; + M(0, 0) = 0; + M(0, 1) = -v(2); + M(0, 2) = v(1); + + M(1, 0) = v(2); + M(1, 1) = 0; + M(1, 2) = -v(0); + + M(2, 0) = -v(1); + M(2, 1) = v(0); + M(2, 2) = 0; return M; } -matrix::Vector3f GeometricControl::skew_to_vector(matrix::SquareMatrix M){ +matrix::Vector3f GeometricControl::skew_to_vector(matrix::SquareMatrix M) +{ matrix::Vector3f v; - v(0) = 0.5f * (M(2,1) - M(1,2)); - v(1) = 0.5f * (M(0,2) - M(2,0)); - v(2) = 0.5f * (M(1,0) - M(0,1)); + v(0) = 0.5f * (M(2, 1) - M(1, 2)); + v(1) = 0.5f * (M(0, 2) - M(2, 0)); + v(2) = 0.5f * (M(1, 0) - M(0, 1)); return v; } diff --git a/src/modules/mc_rate_control/GeometricControl/GeometricControl.hpp b/src/modules/mc_rate_control/GeometricControl/GeometricControl.hpp index e2a525e981af..a61dd79d374a 100644 --- a/src/modules/mc_rate_control/GeometricControl/GeometricControl.hpp +++ b/src/modules/mc_rate_control/GeometricControl/GeometricControl.hpp @@ -50,32 +50,33 @@ class GeometricControl { public: - GeometricControl(){ - _J.setIdentity(); + GeometricControl() + { + _J.setIdentity(); } ~GeometricControl() = default; - // functions to set internal parameters - void set_gains(float kx, float kv, float kR, float komega); - void set_inertia(float Jxx, float Jyy, float Jzz, float Jxy=0.0f, float Jxz=0.0f, float Jyz=0.0f); + // functions to set internal parameters + void set_gains(float kx, float kv, float kR, float komega); + void set_inertia(float Jxx, float Jyy, float Jzz, float Jxy = 0.0f, float Jxz = 0.0f, float Jyz = 0.0f); /** * Run one control loop cycle calculation - * pass in SI units, returns in SI units - * Returns [moments, _thrust_sp] + * pass in SI units, returns in SI units + * Returns [moments, _thrust_sp] */ matrix::Vector update( - const matrix::Vector3f &pos, // pass in current state - const matrix::Vector3f &vel, - const matrix::Quatf &ang_att, - const matrix::Vector3f &ang_rate, - const vehicle_local_position_setpoint_s &setpoint, // pass in setpoint - const vehicle_control_mode_s &control_mode // determines which mode we should control it in - ); - + const matrix::Vector3f &pos, // pass in current state + const matrix::Vector3f &vel, + const matrix::Quatf &ang_att, + const matrix::Vector3f &ang_rate, + const vehicle_local_position_setpoint_s &setpoint, // pass in setpoint + const vehicle_control_mode_s &control_mode // determines which mode we should control it in + ); + private: - const matrix::Vector3f _z = matrix::Vector3f(0,0,1); + const matrix::Vector3f _z = matrix::Vector3f(0, 0, 1); static constexpr float g = 9.81f; matrix::SquareMatrix _J; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index c04add815295..ba1ec02e82a8 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -94,9 +94,10 @@ MulticopterRateControl::parameters_updated() // manual rate control acro mode rate limits _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get())); - // DASC CUSTOM - _geometric_control.set_gains(_param_geo_kx.get(), _param_geo_kv.get(), _param_geo_kR.get(), _param_geo_kOmega.get()); - _geometric_control.set_inertia(_param_geo_Jxx.get(), _param_geo_Jyy.get(), _param_geo_Jzz.get(), _param_geo_Jxy.get(), _param_geo_Jxz.get(), _param_geo_Jyz.get()); + // DASC CUSTOM + _geometric_control.set_gains(_param_geo_kx.get(), _param_geo_kv.get(), _param_geo_kR.get(), _param_geo_kOmega.get()); + _geometric_control.set_inertia(_param_geo_Jxx.get(), _param_geo_Jyy.get(), _param_geo_Jzz.get(), _param_geo_Jxy.get(), + _param_geo_Jxz.get(), _param_geo_Jyz.get()); } @@ -174,10 +175,11 @@ MulticopterRateControl::Run() // use rates setpoint topic vehicle_rates_setpoint_s vehicle_rates_setpoint{}; - // DASC CUSTOM: check if the external control mode has been changed - _external_controller_sub.update(&_external_controller); - if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { - // generate the rate setpoint from sticks + // DASC CUSTOM: check if the external control mode has been changed + _external_controller_sub.update(&_external_controller); + + if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks manual_control_setpoint_s manual_control_setpoint; @@ -251,70 +253,70 @@ MulticopterRateControl::Run() // hijack px4 and insert our own geometric controller if sufficient flags are met // check that we are in offboard mode and want to use the custom geometric controller - if (_vehicle_control_mode.flag_control_offboard_enabled && _external_controller.use_geometric_control){ - // load other states as well - _vehicle_local_position_sub.update(&_vehicle_local_position); - _trajectory_setpoint_sub.update(&_trajectory_setpoint); - _vehicle_attitude_sub.update(&_vehicle_attitude); - - // if (true) { - // // reinitialize for testing purposes - // _trajectory_setpoint.x = 0.0; - // _trajectory_setpoint.y = 0.0; - // _trajectory_setpoint.z = -1.0; - // _trajectory_setpoint.yaw = 0.0; - // _trajectory_setpoint.yawspeed = 0.0; - // _trajectory_setpoint.vx = 0.0; - // _trajectory_setpoint.vy = 0.0; - // _trajectory_setpoint.vz = 0.0; - // _trajectory_setpoint.acceleration[0] = 0.0; - // _trajectory_setpoint.acceleration[1] = 0.0; - // _trajectory_setpoint.acceleration[2] = 0.0; - // _trajectory_setpoint.jerk[0] = 0.0; - // _trajectory_setpoint.jerk[1] = 0.0; - // _trajectory_setpoint.jerk[2] = 0.0; - // _trajectory_setpoint.thrust[0] = 0.0; - // _trajectory_setpoint.thrust[1] = 0.0; - // _trajectory_setpoint.thrust[2] = 0.0; - // } - - // PX4_INFO("traj setpoint xyz: %f, %f, %f", double(_trajectory_setpoint.x), double(_trajectory_setpoint.y), double(_trajectory_setpoint.z)); - - // construct current state - matrix::Vector3f _pos( - _vehicle_local_position.x, - _vehicle_local_position.y, - _vehicle_local_position.z ); - - matrix::Vector3f _vel( - _vehicle_local_position.vx, - _vehicle_local_position.vy, - _vehicle_local_position.vz ); + if (_vehicle_control_mode.flag_control_offboard_enabled && _external_controller.use_geometric_control) { + // load other states as well + _vehicle_local_position_sub.update(&_vehicle_local_position); + _trajectory_setpoint_sub.update(&_trajectory_setpoint); + _vehicle_attitude_sub.update(&_vehicle_attitude); + + // if (true) { + // // reinitialize for testing purposes + // _trajectory_setpoint.x = 0.0; + // _trajectory_setpoint.y = 0.0; + // _trajectory_setpoint.z = -1.0; + // _trajectory_setpoint.yaw = 0.0; + // _trajectory_setpoint.yawspeed = 0.0; + // _trajectory_setpoint.vx = 0.0; + // _trajectory_setpoint.vy = 0.0; + // _trajectory_setpoint.vz = 0.0; + // _trajectory_setpoint.acceleration[0] = 0.0; + // _trajectory_setpoint.acceleration[1] = 0.0; + // _trajectory_setpoint.acceleration[2] = 0.0; + // _trajectory_setpoint.jerk[0] = 0.0; + // _trajectory_setpoint.jerk[1] = 0.0; + // _trajectory_setpoint.jerk[2] = 0.0; + // _trajectory_setpoint.thrust[0] = 0.0; + // _trajectory_setpoint.thrust[1] = 0.0; + // _trajectory_setpoint.thrust[2] = 0.0; + // } + + // PX4_INFO("traj setpoint xyz: %f, %f, %f", double(_trajectory_setpoint.x), double(_trajectory_setpoint.y), double(_trajectory_setpoint.z)); + + // construct current state + matrix::Vector3f _pos( + _vehicle_local_position.x, + _vehicle_local_position.y, + _vehicle_local_position.z); + + matrix::Vector3f _vel( + _vehicle_local_position.vx, + _vehicle_local_position.vy, + _vehicle_local_position.vz); matrix::Quatf _ang_att(_vehicle_attitude.q); - // run the geometric controller + // run the geometric controller const matrix::Vector res = _geometric_control.update( - _pos, _vel, _ang_att, rates, _trajectory_setpoint, _vehicle_control_mode); + _pos, _vel, _ang_att, rates, _trajectory_setpoint, _vehicle_control_mode); - // PX4_INFO("completed geometric controller"); + // PX4_INFO("completed geometric controller"); - // PX4_INFO("SI: thrust, att: %f, %f, %f, %f", double(res(3)), double(res(0)), double(res(1)), double(res(2))); + // PX4_INFO("SI: thrust, att: %f, %f, %f, %f", double(res(3)), double(res(0)), double(res(1)), double(res(2))); - // normalize the values to appropriate ranges - _thrust_setpoint(2) = math::min(1.0f, math::max(0.0f, res(3) / 9.81f * _param_geo_hover_thrust.get())); + // normalize the values to appropriate ranges + _thrust_setpoint(2) = math::min(1.0f, math::max(0.0f, res(3) / 9.81f * _param_geo_hover_thrust.get())); - att_control(0) = res(0) / _param_geo_torq_const.get(); - att_control(1) = res(1) / _param_geo_torq_const.get(); - att_control(2) = res(2) / _param_geo_torq_const.get(); + att_control(0) = res(0) / _param_geo_torq_const.get(); + att_control(1) = res(1) / _param_geo_torq_const.get(); + att_control(2) = res(2) / _param_geo_torq_const.get(); - if (att_control.norm() > _param_geo_torq_max.get()){ - att_control = att_control.unit() * _param_geo_torq_max.get(); - } + if (att_control.norm() > _param_geo_torq_max.get()) { + att_control = att_control.unit() * _param_geo_torq_max.get(); + } - // PX4_INFO("thrust, att: %f, %f, %f, %f", double(_thrust_sp), double(att_control(0)), double(att_control(1)), double(att_control(2))); + // PX4_INFO("thrust, att: %f, %f, %f, %f", double(_thrust_sp), double(att_control(0)), double(att_control(1)), double(att_control(2))); - // PX4_INFO("exiting geometric controller"); + // PX4_INFO("exiting geometric controller"); } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 8a4de73cdf60..58504b915d92 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -118,11 +118,11 @@ class MulticopterRateControl : public ModuleBase, public uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - // DASC CUSTOM + // DASC CUSTOM uORB::Subscription _external_controller_sub{ORB_ID(external_controller)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -140,12 +140,12 @@ class MulticopterRateControl : public ModuleBase, public vehicle_control_mode_s _vehicle_control_mode{}; vehicle_status_s _vehicle_status{}; - // DASC CUSTOM - external_controller_s _external_controller{}; + // DASC CUSTOM + external_controller_s _external_controller{}; vehicle_attitude_s _vehicle_attitude{}; - vehicle_local_position_s _vehicle_local_position{}; - vehicle_local_position_setpoint_s _trajectory_setpoint{}; - bool _landed{true}; + vehicle_local_position_s _vehicle_local_position{}; + vehicle_local_position_setpoint_s _trajectory_setpoint{}; + bool _landed{true}; bool _maybe_landed{true}; hrt_abstime _last_run{0}; @@ -186,7 +186,7 @@ class MulticopterRateControl : public ModuleBase, public (ParamFloat) _param_mc_yawrate_ff, (ParamFloat) _param_mc_yawrate_k, - (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + //(ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ (ParamFloat) _param_mc_acro_r_max, (ParamFloat) _param_mc_acro_p_max, @@ -198,20 +198,20 @@ class MulticopterRateControl : public ModuleBase, public (ParamBool) _param_mc_bat_scale_en, - // DASC CUSTOM PARAMS - (ParamFloat) _param_geo_kx, - (ParamFloat) _param_geo_kv, - (ParamFloat) _param_geo_kR, - (ParamFloat) _param_geo_kOmega, - (ParamFloat) _param_geo_Jxx, - (ParamFloat) _param_geo_Jyy, - (ParamFloat) _param_geo_Jzz, - (ParamFloat) _param_geo_Jxy, - (ParamFloat) _param_geo_Jxz, - (ParamFloat) _param_geo_Jyz, - (ParamFloat) _param_geo_torq_max, - (ParamFloat) _param_geo_torq_const, - (ParamFloat) _param_geo_hover_thrust - ) + // DASC CUSTOM PARAMS + (ParamFloat) _param_geo_kx, + (ParamFloat) _param_geo_kv, + (ParamFloat) _param_geo_kR, + (ParamFloat) _param_geo_kOmega, + (ParamFloat) _param_geo_Jxx, + (ParamFloat) _param_geo_Jyy, + (ParamFloat) _param_geo_Jzz, + (ParamFloat) _param_geo_Jxy, + (ParamFloat) _param_geo_Jxz, + (ParamFloat) _param_geo_Jyz, + (ParamFloat) _param_geo_torq_max, + (ParamFloat) _param_geo_torq_const, + (ParamFloat) _param_geo_hover_thrust + ) };