Skip to content

Commit

Permalink
proper memory assignment in ocp qp ooqp
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnovoselnik
bnovoselnik committed Jul 24, 2018
1 parent 3e5201e commit f671927
Show file tree
Hide file tree
Showing 4 changed files with 93 additions and 70 deletions.
13 changes: 7 additions & 6 deletions acados/dense_qp/dense_qp_ooqp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include "acados/dense_qp/dense_qp_ooqp.h"
#include "acados/utils/timing.h"
#include "acados/utils/print.h"
#include "acados/utils/mem.h"

static void update_gradient(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
{
Expand Down Expand Up @@ -318,14 +319,14 @@ int dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void
size += sizeof(dense_qp_ooqp_memory);

size += 1 * nv * nv * sizeof(double); // dQ
size += 1 * nv * sizeof(double); // c
size += 2 * nv * sizeof(char); // ixlow, ixupp
size += 2 * nv * sizeof(double); // xlow, xupp
size += 1 * nv * sizeof(double); // c
size += 2 * nv * sizeof(char); // ixlow, ixupp
size += 2 * nv * sizeof(double); // xlow, xupp
size += 1 * nv * ne * sizeof(double); // dA
size += 1 * ne * sizeof(double); // bA
size += 1 * ne * sizeof(double); // bA
size += 1 * nv * ng * sizeof(double); // dC
size += 2 * ng * sizeof(double); // clow, cupp
size += 2 * ng * sizeof(char); // iclow, icupp
size += 2 * ng * sizeof(double); // clow, cupp
size += 2 * ng * sizeof(char); // iclow, icupp

make_int_multiple_of(8, &size);

Expand Down
142 changes: 84 additions & 58 deletions acados/ocp_qp/ocp_qp_ooqp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include "acados/ocp_qp/ocp_qp_ooqp.h"
#include "acados/utils/timing.h"
#include "acados/utils/print.h"
#include "acados/utils/mem.h"

#define FLIP_BOUNDS

Expand Down Expand Up @@ -141,14 +142,11 @@ static int get_number_of_inequalities(const ocp_qp_dims *dims)
return mz;
}

static int get_nnzQ(const ocp_qp_dims *dims, const ocp_qp_ooqp_opts *opts)
static int get_nnzQ(const ocp_qp_dims *dims)
{
int kk;
int nnzQ = 0;

// dummy command
if (opts->printLevel) kk = 0;

for (kk = 0; kk <= dims->N; kk++)
{
nnzQ += (dims->nx[kk] * dims->nx[kk] - dims->nx[kk]) / 2 + dims->nx[kk];
Expand All @@ -159,14 +157,11 @@ static int get_nnzQ(const ocp_qp_dims *dims, const ocp_qp_ooqp_opts *opts)
return nnzQ;
}

static int get_nnzA(const ocp_qp_dims *dims, const ocp_qp_ooqp_opts *opts)
static int get_nnzA(const ocp_qp_dims *dims)
{
int kk;
int nnzA = 0;

// dummy command
if (opts->printLevel) kk = 0;

for (kk = 0; kk < dims->N; kk++)
{
nnzA += dims->nx[kk + 1] * (dims->nx[kk] + dims->nu[kk] + 1);
Expand All @@ -175,14 +170,11 @@ static int get_nnzA(const ocp_qp_dims *dims, const ocp_qp_ooqp_opts *opts)
return nnzA;
}

static int get_nnzC(const ocp_qp_dims *dims, const ocp_qp_ooqp_opts *opts)
static int get_nnzC(const ocp_qp_dims *dims)
{
int kk;
int nnzC = 0;

// dummy command
if (opts->printLevel) kk = 0;

for (kk = 0; kk <= dims->N; kk++)
{
nnzC += dims->ng[kk] * (dims->nx[kk] + dims->nu[kk]);
Expand Down Expand Up @@ -799,8 +791,32 @@ void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_)

int ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_)
{
int nx = get_number_of_primal_vars(dims);
int my = get_number_of_equalities(dims);
int mz = get_number_of_inequalities(dims);
int nnzQ = get_nnzQ(dims);
int nnzA = get_nnzA(dims);
int nnzC = get_nnzC(dims);
int nnz = max_of_three(nnzQ, nnzA, nnzC);

int size = 0;
size += sizeof(ocp_qp_ooqp_memory);

size += 1 * nx * sizeof(double); // c
size += 1 * nnzQ * sizeof(double); // dQ
size += 3 * nnzQ * sizeof(int); // irowQ, jcolQ, orderQ
size += 2 * nx * sizeof(double); // xlow, xupp
size += 2 * nx * sizeof(char); // ixlow, ixupp
size += 1 * nnzA * sizeof(double); // dA
size += 3 * nnzA * sizeof(int); // irowA, jcolA, orderA
size += 1 * my * sizeof(double); // bA
size += 1 * nnzC * sizeof(double); // dC
size += 3 * nnzC * sizeof(int); // irowC, jcolC, orderC
size += 2 * mz * sizeof(double); // clow, cupp
size += 2 * mz * sizeof(char); // iclow, icupp

make_int_multiple_of(8, &size);

return size;
}

Expand All @@ -809,33 +825,66 @@ void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, v
ocp_qp_ooqp_opts *opts = (ocp_qp_ooqp_opts *)opts_;
ocp_qp_ooqp_memory *mem;

int nx = get_number_of_primal_vars(dims);
int my = get_number_of_equalities(dims);
int mz = get_number_of_inequalities(dims);
int nnzQ = get_nnzQ(dims);
int nnzA = get_nnzA(dims);
int nnzC = get_nnzC(dims);
int nnz = max_of_three(nnzQ, nnzA, nnzC);

// char pointer
char *c_ptr = (char *) raw_memory;

mem = (ocp_qp_ooqp_memory *) c_ptr;
c_ptr += sizeof(ocp_qp_ooqp_memory);

assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!");

assign_and_advance_double(nx, &mem->c, &c_ptr);
assign_and_advance_double(nnzQ, &mem->dQ, &c_ptr);
assign_and_advance_double(nx, &mem->xlow, &c_ptr);
assign_and_advance_double(nx, &mem->xupp, &c_ptr);
assign_and_advance_double(nnzA, &mem->dA, &c_ptr);
assign_and_advance_double(my, &mem->bA, &c_ptr);
assign_and_advance_double(nnzC, &mem->dC, &c_ptr);
assign_and_advance_double(mz, &mem->clow, &c_ptr);
assign_and_advance_double(mz, &mem->cupp, &c_ptr);

assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!");

assign_and_advance_int(nnzQ, &mem->irowQ, &c_ptr);
assign_and_advance_int(nnzQ, &mem->jcolQ, &c_ptr);
assign_and_advance_int(nnzQ, &mem->orderQ, &c_ptr);
assign_and_advance_int(nnzA, &mem->irowA, &c_ptr);
assign_and_advance_int(nnzA, &mem->jcolA, &c_ptr);
assign_and_advance_int(nnzA, &mem->orderA, &c_ptr);
assign_and_advance_int(nnzC, &mem->irowC, &c_ptr);
assign_and_advance_int(nnzC, &mem->jcolC, &c_ptr);
assign_and_advance_int(nnzC, &mem->orderC, &c_ptr);

assign_and_advance_char(nx, &mem->ixlow, &c_ptr);
assign_and_advance_char(nx, &mem->ixupp, &c_ptr);
assign_and_advance_char(mz, &mem->iclow, &c_ptr);
assign_and_advance_char(mz, &mem->icupp, &c_ptr);

// initialize memory
mem->firstRun = 1;
mem->nx = get_number_of_primal_vars(dims);
mem->my = get_number_of_equalities(dims);
mem->mz = get_number_of_inequalities(dims);
mem->nnzQ = get_nnzQ(dims, opts);
mem->nnzA = get_nnzA(dims, opts);
mem->nnzC = get_nnzC(dims, opts);
mem->nnz = max_of_three(mem->nnzQ, mem->nnzA, mem->nnzC);

int ooqp_failed;
newQpGenSparse(&mem->c, mem->nx, &mem->irowQ, mem->nnzQ, &mem->jcolQ, &mem->dQ, &mem->xlow,
&mem->ixlow, &mem->xupp, &mem->ixupp, &mem->irowA, mem->nnzA, &mem->jcolA,
&mem->dA, &mem->bA, mem->my, &mem->irowC, mem->nnzC, &mem->jcolC, &mem->dC,
&mem->clow, mem->mz, &mem->iclow, &mem->cupp, &mem->icupp, &ooqp_failed);

mem->orderQ = (int *)calloc(mem->nnzQ, sizeof(*mem->orderQ));
mem->orderA = (int *)calloc(mem->nnzA, sizeof(*mem->orderA));
mem->orderC = (int *)calloc(mem->nnzC, sizeof(*mem->orderC));

if (ooqp_failed) return NULL;
mem->nx = nx;
mem->my = my;
mem->mz = mz;
mem->nnzQ = nnzQ;
mem->nnzA = nnzA;
mem->nnzC = nnzC;
mem->nnz = max_of_three(nnzQ, nnzA, nnzC);

for (int ii = 0; ii < nnzQ; ii++) mem->orderQ[ii] = 0;
for (int ii = 0; ii < nnzA; ii++) mem->orderA[ii] = 0;
for (int ii = 0; ii < nnzC; ii++) mem->orderC[ii] = 0;

assert((char *) raw_memory + ocp_qp_ooqp_memory_calculate_size(config_, dims, opts_) >=
c_ptr);

return mem;
}

Expand All @@ -849,9 +898,9 @@ int ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void
nx = get_number_of_primal_vars(dims);
my = get_number_of_equalities(dims);
mz = get_number_of_inequalities(dims);
nnzQ = get_nnzQ(dims, opts);
nnzA = get_nnzA(dims, opts);
nnzC = get_nnzC(dims, opts);
nnzQ = get_nnzQ(dims);
nnzA = get_nnzA(dims);
nnzC = get_nnzC(dims);
nnz = max_of_three(nnzQ, nnzA, nnzC);

size += sizeof(ocp_qp_ooqp_workspace);
Expand Down Expand Up @@ -887,20 +936,6 @@ static void ocp_qp_ooqp_cast_workspace(ocp_qp_ooqp_workspace *work, ocp_qp_ooqp_
// ptr += (mem->nnz)*sizeof(double);
}

void ocp_qp_ooqp_free_memory(void *mem_)
{
ocp_qp_ooqp_memory *mem = (ocp_qp_ooqp_memory *)mem_;

freeQpGenSparse(&mem->c, &mem->irowQ, &mem->jcolQ, &mem->dQ, &mem->xlow, &mem->ixlow,
&mem->xupp, &mem->ixupp, &mem->irowA, &mem->jcolA, &mem->dA, &mem->bA,
&mem->irowC, &mem->jcolC, &mem->dC, &mem->clow, &mem->iclow, &mem->cupp,
&mem->icupp);

free(mem->orderQ);
free(mem->orderA);
free(mem->orderC);
}

int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
void *work_)
{
Expand Down Expand Up @@ -958,19 +993,10 @@ int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_
return acados_status;
}

// void ocp_qp_ooqp_initialize(const ocp_qp_in *qp_in, void *opts_, void **mem, void **work)
// {
// ocp_qp_ooqp_opts *opts = (ocp_qp_ooqp_opts *)opts_;

// *mem = ocp_qp_ooqp_create_memory(qp_in, opts);
// int work_space_size = ocp_qp_ooqp_calculate_workspace_size(qp_in, opts);
// *work = (void *)malloc(work_space_size);
// }

void ocp_qp_ooqp_destroy(void *mem_, void *work)
{
free(work);
ocp_qp_ooqp_free_memory(mem_);
free(mem_);
}

void ocp_qp_ooqp_config_initialize_default(void *config_)
Expand Down
4 changes: 0 additions & 4 deletions acados/ocp_qp/ocp_qp_ooqp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,13 +112,9 @@ void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, v
//
int ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
void ocp_qp_ooqp_free_memory(void *mem_);
//
int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
void *work_);
//
// void ocp_qp_ooqp_initialize(const ocp_qp_in *qp_in, void *opts_, void **mem, void **work);
//
void ocp_qp_ooqp_destroy(void *mem_, void *work);
//
void ocp_qp_ooqp_config_initialize_default(void *config_);
Expand Down
4 changes: 2 additions & 2 deletions examples/c/mass_spring_example.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ int main() {
int num_N2_values = 3;
int N2_values[3] = {15,10,5};

int ii_max = 2;
int ii_max = 3;

#ifndef ACADOS_WITH_HPMPC
ii_max--;
Expand Down Expand Up @@ -128,7 +128,7 @@ int main() {
// FULL_CONDENSING_QPOASES,
#endif
#ifdef ACADOS_WITH_OOQP
// PARTIAL_CONDENSING_OOQP,
PARTIAL_CONDENSING_OOQP,
FULL_CONDENSING_OOQP
#endif
};
Expand Down

0 comments on commit f671927

Please sign in to comment.