Skip to content
Permalink
Browse files

sim: added 'bool identity_seed' to in for more efficient forw propaga…

…tion; lifted_irk: add LA_time
  • Loading branch information...
FreyJo committed Jul 9, 2019
1 parent 9433b71 commit ab6bb62c41b222dc639cd917cf34d3b347fac523
@@ -640,10 +640,14 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void *

// initialize seeds
// TODO fix dims if nx!=nx1 !!!!!!!!!!!!!!!!!
// set S_forw = [eye(nx), zeros(nx x nu)]
for(jj = 0; jj < nx1 * (nx + nu); jj++)
work->sim_in->S_forw[jj] = 0.0;
for(jj = 0; jj < nx1; jj++)
work->sim_in->S_forw[jj * (nx + 1)] = 1.0;
work->sim_in->identity_seed = true;

// adjoint seed
for(jj = 0; jj < nx + nu; jj++)
work->sim_in->S_adj[jj] = 0.0;
blasfeo_unpack_dvec(nx1, mem->pi, 0, work->sim_in->S_adj);
@@ -121,6 +121,8 @@ sim_in *sim_in_assign(void *config_, void *dims, void *raw_memory)
assign_and_advance_double(nx * NF, &in->S_forw, &c_ptr);
assign_and_advance_double(NF, &in->S_adj, &c_ptr);

in->identity_seed = false;

in->model = config->model_assign(config, dims, c_ptr);
c_ptr += config->model_calculate_size(config, dims);

@@ -66,6 +66,8 @@ typedef struct
double *S_forw; // forward seed [Sx, Su]
double *S_adj; // backward seed // TODO rename seed_adj ???

bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)]

void *model;

double T; // simulation time
@@ -1083,10 +1083,18 @@ int sim_gnsf_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, vo
0, 0, ELO_inv_ALO, 0, 0);
}

// generate all sensitivities
// generate sensitivities
mem->first_call = true;
if (model->tmp_fully_linear)
{
// set identity seed
for (int jj = 0; jj < nx * (nx + nu); jj++)
in->S_forw[jj] = 0.0;
for (int jj = 0; jj < nx; jj++)
in->S_forw[jj * (nx + 1)] = 1.0;
in->identity_seed = true;
sim_gnsf(config_, in, out, opts_, mem_, work_);
}
mem->first_call = false;

return status;
@@ -1795,14 +1803,17 @@ int sim_gnsf(void *config, sim_in *in, sim_out *out, void *args, void *mem_, voi
x0_traj, nx * num_steps, x0_traj, nx * num_steps);

// sensitivities are constant and already computed - only multiply with seed
// pack seed into S_forw_new
blasfeo_pack_dmat(nx, nx + nu, &in->S_forw[0], nx, S_forw_new, 0, 0);
// S_forw_new_x = S_forw_x * S_forw_new_x
blasfeo_dgemm_nn(nx, nx, nx, 1.0, S_forw, 0, 0, S_forw_new, 0, 0, 0.0, S_forw_new, 0, 0,
S_forw_new, 0, 0);
// S_forw_new_u = S_forw_x * S_forw_new_u + S_forw_new_u
blasfeo_dgemm_nn(nx, nu, nx, 1.0, S_forw, 0, 0, S_forw_new, 0, nx, 1.0, S_forw_new, 0, nx,
S_forw_new, 0, nx);
if (!in->identity_seed)
{
// pack seed into S_forw_new
blasfeo_pack_dmat(nx, nx + nu, &in->S_forw[0], nx, S_forw_new, 0, 0);
// S_forw_new_x = S_forw_x * S_forw_new_x
blasfeo_dgemm_nn(nx, nx, nx, 1.0, S_forw, 0, 0, S_forw_new, 0, 0, 0.0, S_forw_new, 0, 0,
S_forw_new, 0, 0);
// S_forw_new_u = S_forw_x * S_forw_new_u + S_forw_new_u
blasfeo_dgemm_nn(nx, nu, nx, 1.0, S_forw, 0, 0, S_forw_new, 0, nx, 1.0, S_forw_new, 0, nx,
S_forw_new, 0, nx);
}

// adjoint
blasfeo_dgemv_t(nx, nx+nu, 1.0, S_forw, 0, 0, lambda_old, 0, 0.0, lambda_old, 0, lambda, 0);
@@ -2231,11 +2242,17 @@ int sim_gnsf(void *config, sim_in *in, sim_out *out, void *args, void *mem_, voi
// copy from precomputed dx2f_dx2u
blasfeo_dgecp(nx2, nx2 + nu, dx2f_dx2u, 0, 0, dxf_dwn, nx1, nx1);
}
blasfeo_dgemm_nn(nx, nx, nx, 1.0, dxf_dwn, 0, 0, S_forw, 0, 0, 0.0, S_forw_new, 0, 0,
S_forw_new, 0, 0);
blasfeo_dgemm_nn(nx, nu, nx, 1.0, dxf_dwn, 0, 0, S_forw, 0, nx, 1.0, dxf_dwn, 0, nx,
S_forw_new, 0, nx);
blasfeo_dgecp(nx, nx + nu, S_forw_new, 0, 0, S_forw, 0, 0);
// omit matrix multiplication for identity seed
if (in->identity_seed && ss == 0)
blasfeo_dgecp(nx, nx + nu, dxf_dwn, 0, 0, S_forw, 0, 0);
else
{
blasfeo_dgemm_nn(nx, nx, nx, 1.0, dxf_dwn, 0, 0, S_forw, 0, 0, 0.0, S_forw_new, 0, 0,
S_forw_new, 0, 0);
blasfeo_dgemm_nn(nx, nu, nx, 1.0, dxf_dwn, 0, 0, S_forw, 0, nx, 1.0, dxf_dwn, 0, nx,
S_forw_new, 0, nx);
blasfeo_dgecp(nx, nx + nu, S_forw_new, 0, 0, S_forw, 0, 0);
}
}

/* output z and propagate corresponding sensitivities */
@@ -935,15 +935,21 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo
acados_tic(&timer_la);
blasfeo_dgetrf_rp(nK, nK, dG_dK_ss, 0, 0, dG_dK_ss, 0, 0, ipiv_ss);
timing_la += acados_toc(&timer_la);

/* obtain dK_dxu */
// set up right hand side
// dK_dw = 0 * dK_dw + 1 * dG_dx * S_forw_old
blasfeo_dgemm_nn(nK, nx + nu, nx, 1.0, dG_dxu_ss, 0, 0, S_forw_ss, 0,
0, 0.0, dK_dxu_ss, 0, 0, dK_dxu_ss, 0, 0);
// printf("dG_dxu = \n");
// blasfeo_print_exp_dmat(nx + nz, nx+nu, dG_dxu_ss, 0, 0);
// dK_du = dK_du + 1 * dG_du
blasfeo_dgead(nK, nu, 1.0, dG_dxu_ss, 0, nx, dK_dxu_ss, 0, nx);
if (in->identity_seed && ss == 0) // omit matrix multiplication for identity seed
blasfeo_dgecp(nK, nx + nu, dG_dxu_ss, 0, 0, dK_dxu_ss, 0, 0);
else
{
// dK_dw = 0 * dK_dw + 1 * dG_dx * S_forw_old
blasfeo_dgemm_nn(nK, nx + nu, nx, 1.0, dG_dxu_ss, 0, 0, S_forw_ss, 0,
0, 0.0, dK_dxu_ss, 0, 0, dK_dxu_ss, 0, 0);
// printf("dG_dxu = \n");
// blasfeo_print_exp_dmat(nx + nz, nx+nu, dG_dxu_ss, 0, 0);
// dK_du = dK_du + 1 * dG_du
blasfeo_dgead(nK, nu, 1.0, dG_dxu_ss, 0, nx, dK_dxu_ss, 0, nx);
}
// solve linear system
acados_tic(&timer_la);
blasfeo_drowpe(nK, ipiv_ss, dK_dxu_ss);
@@ -607,8 +607,9 @@ int sim_lifted_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *m

lifted_irk_model *model = in->model;

acados_timer timer, timer_ad;
acados_timer timer, timer_ad, timer_la;
double timing_ad = 0.0;
out->info->LAtime = 0.0;

if (opts->sens_adj)
{
@@ -782,14 +783,22 @@ int sim_lifted_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *m
blasfeo_daxpy(nx, -step * b_vec[ii], rG, ii * nx, dxn, 0, dxn, 0);

// update JKf
blasfeo_dgemm_nn(nx * ns, nx + nu, nx, 1.0, JGf, 0, 0, S_forw, 0, 0, 0.0, &JKf[ss], 0, 0,
&JKf[ss], 0, 0);

blasfeo_dgead(nx * ns, nu, 1.0, JGf, 0, nx, &JKf[ss], 0, nx);
// JKf[ss] = JGf * S_forw;
if (in->identity_seed && ss == 0) // omit matrix multiplication for identity seed
blasfeo_dgecp(nx * ns, nx + nu, JGf, 0, 0, &JKf[ss], 0, 0);
else
{
blasfeo_dgemm_nn(nx * ns, nx + nu, nx, 1.0, JGf, 0, 0, S_forw, 0, 0, 0.0, &JKf[ss], 0, 0,
&JKf[ss], 0, 0);
blasfeo_dgead(nx * ns, nu, 1.0, JGf, 0, nx, &JKf[ss], 0, nx);
}

// solve linear system
acados_tic(&timer_la);
blasfeo_drowpe(nx * ns, ipiv, &JKf[ss]);
blasfeo_dtrsm_llnu(nx * ns, nx + nu, 1.0, JGK, 0, 0, &JKf[ss], 0, 0, &JKf[ss], 0, 0);
blasfeo_dtrsm_lunn(nx * ns, nx + nu, 1.0, JGK, 0, 0, &JKf[ss], 0, 0, &JKf[ss], 0, 0);
out->info->LAtime += acados_toc(&timer_la);

// update forward sensitivity
for (jj = 0; jj < ns; jj++)
@@ -808,7 +817,6 @@ int sim_lifted_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *m
blasfeo_unpack_dmat(nx, nx + nu, S_forw, 0, 0, S_forw_out, nx);

out->info->CPUtime = acados_toc(&timer);
out->info->LAtime = 0.0;
out->info->ADtime = timing_ad;

return 0;
@@ -313,6 +313,7 @@ TEST_CASE("crane_dae_example", "[integrators]")
in->S_forw[ii] = 0.0;
for (int ii = 0; ii < nx; ii++)
in->S_forw[ii * (nx + 1)] = 1.0;
in->identity_seed = true;

// seeds adj
for (int ii = 0; ii < nx; ii++)
@@ -292,6 +292,8 @@ TEST_CASE("pendulum_hessians", "[integrators]")
in->S_forw[ii] = 0.0;
for (int ii = 0; ii < nx; ii++)
in->S_forw[ii * (nx + 1)] = 1.0;
in->identity_seed = true;


// seeds adj
for (int ii = 0; ii < nx; ii++)
@@ -277,6 +277,7 @@ TEST_CASE("wt_nx3_example", "[integrators]")
in->S_forw[ii] = 0.0;
for (ii = 0; ii < nx; ii++)
in->S_forw[ii * (nx + 1)] = 1.0;
in->identity_seed = true;

// seeds adj
for (ii = 0; ii < nx; ii++)

0 comments on commit ab6bb62

Please sign in to comment.
You can’t perform that action at this time.