acados · FreyJo · Oct 25, 2023 · Oct 25, 2023 · Oct 25, 2023 · Oct 25, 2023
diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.c b/acados/ocp_nlp/ocp_nlp_cost_conl.c
@@ -179,6 +179,7 @@ void *ocp_nlp_cost_conl_model_assign(void *config_, void *dims_, void *raw_memor
 
     // default initialization
     model->scaling = 1.0;
+    model->t = 0.0;
 
     // assert
     assert((char *) raw_memory + ocp_nlp_cost_conl_model_calculate_size(config_, dims) >= c_ptr);
@@ -623,8 +624,8 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode
 
     if (opts->integrator_cost == 0)
     {
-        ext_fun_arg_t conl_fun_jac_hess_type_in[4];
-        void *conl_fun_jac_hess_in[4];
+        ext_fun_arg_t conl_fun_jac_hess_type_in[5];
+        void *conl_fun_jac_hess_in[5];
         ext_fun_arg_t conl_fun_jac_hess_type_out[5];
         void *conl_fun_jac_hess_out[5];
 
@@ -647,6 +648,9 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode
         conl_fun_jac_hess_type_in[3] = BLASFEO_DVEC;
         conl_fun_jac_hess_in[3] = &model->y_ref;
 
+        conl_fun_jac_hess_type_in[4] = COLMAJ;
+        conl_fun_jac_hess_in[4] = &model->t;
+
         // OUTPUT
         conl_fun_jac_hess_type_out[0] = COLMAJ;
         conl_fun_jac_hess_out[0] = &memory->fun;         // fun: scalar
@@ -735,8 +739,8 @@ void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_,
     if (opts->integrator_cost == 0)
     {
         /* specify input types and pointers for external cost function */
-        ext_fun_arg_t ext_fun_type_in[4];
-        void *ext_fun_in[4];
+        ext_fun_arg_t ext_fun_type_in[5];
+        void *ext_fun_in[5];
         ext_fun_arg_t ext_fun_type_out[1];
         void *ext_fun_out[1];
 
@@ -757,6 +761,8 @@ void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_,
         ext_fun_in[2] = memory->z_alg;
         ext_fun_type_in[3] = BLASFEO_DVEC;
         ext_fun_in[3] = &model->y_ref;
+        ext_fun_type_in[4] = COLMAJ;
+        ext_fun_in[4] = &model->t;
 
         // OUTPUT
         ext_fun_type_out[0] = COLMAJ;

diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.h b/acados/ocp_nlp/ocp_nlp_cost_conl.h
@@ -94,6 +94,7 @@ typedef struct
     struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
     struct blasfeo_dvec z;              // gradient of slacks as vector
     double scaling;
+    double t; // time (always zero) to match signature of external function wrt cost integration
 } ocp_nlp_cost_conl_model;
 
 //

diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c
@@ -194,6 +194,7 @@ void *ocp_nlp_cost_nls_model_assign(void *config_, void *dims_, void *raw_memory
 
     // default initialization
     model->scaling = 1.0;
+    model->t = 0.0;
 
     // initialize to 1 to update factorization of W in precompute
     model->W_changed = 1;
@@ -706,8 +707,8 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model
     int ny = dims->ny;
     int ns = dims->ns;
 
-    ext_fun_arg_t ext_fun_type_in[4];
-    void *ext_fun_in[4];
+    ext_fun_arg_t ext_fun_type_in[5];
+    void *ext_fun_in[5];
     ext_fun_arg_t ext_fun_type_out[3];
     void *ext_fun_out[3];
 
@@ -728,6 +729,8 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model
         ext_fun_in[1] = &u_in;
         ext_fun_type_in[2] = BLASFEO_DVEC;
         ext_fun_in[2] = memory->z_alg;
+        ext_fun_type_in[3] = COLMAJ;
+        ext_fun_in[3] = &model->t;
 
         ext_fun_type_out[0] = BLASFEO_DVEC;
         ext_fun_out[0] = &memory->res;  // fun: ny
@@ -811,9 +814,11 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model
             // the product < r, d2_d[x,u] r >, where the cost is 0.5 * norm2(r(x,u))^2
             // exact hessian of ls cost
 
-            // ext_fun_[type_]in 0,1 are the same as before.
+            // ext_fun_[type_]in 0, 1, 2 are the same as before.
             ext_fun_type_in[3] = BLASFEO_DVEC;
             ext_fun_in[3] = &work->tmp_ny;  // fun: ny
+            ext_fun_type_in[4] = COLMAJ;
+            ext_fun_in[4] = &model->t;
 
             ext_fun_type_out[0] = BLASFEO_DMAT;
             ext_fun_out[0] = &work->tmp_nv_nv;   // hess*fun: (nu+nx) * (nu+nx)
@@ -881,8 +886,8 @@ void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_,
 
     if (opts->integrator_cost == 0)
     {
-        ext_fun_arg_t nls_y_fun_type_in[3];
-        void *nls_y_fun_in[3];
+        ext_fun_arg_t nls_y_fun_type_in[4];
+        void *nls_y_fun_in[4];
         ext_fun_arg_t nls_y_fun_type_out[1];
         void *nls_y_fun_out[1];
 
@@ -904,6 +909,9 @@ void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_,
         nls_y_fun_type_in[2] = BLASFEO_DVEC;
         nls_y_fun_in[2] = memory->z_alg;
 
+        nls_y_fun_type_in[3] = COLMAJ;
+        nls_y_fun_in[3] = &model->t;
+
         nls_y_fun_type_out[0] = BLASFEO_DVEC;
         nls_y_fun_out[0] = &memory->res;  // fun: ny
 

diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.h b/acados/ocp_nlp/ocp_nlp_cost_nls.h
@@ -94,6 +94,7 @@ typedef struct
     struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
     struct blasfeo_dvec z;              // gradient of slacks as vector
     double scaling;
+    double t; // time (always zero) to match signature of external function wrt cost integration
     int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
 } ocp_nlp_cost_nls_model;
 

diff --git a/acados/sim/sim_common.c b/acados/sim/sim_common.c
@@ -127,6 +127,7 @@ sim_in *sim_in_assign(void *config_, void *dims, void *raw_memory)
     config->dims_get(config_, dims, "nz", &nz);
     int NF = nx + nu;
     in->identity_seed = false;
+    in->t0 = 0.0;
 
     // align
     align_char_to(8, &c_ptr);
@@ -155,6 +156,11 @@ int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void
         double *T = value;
         in->T = T[0];
     }
+    else if (!strcmp(field, "t0"))
+    {
+        double *t0 = value;
+        in->t0 = t0[0];
+    }
     else if (!strcmp(field, "x"))
     {
         int nx;

diff --git a/acados/sim/sim_common.h b/acados/sim/sim_common.h
@@ -83,6 +83,7 @@ typedef struct
     void *model;
 
     double T;  // simulation time
+    double t0; // initial time (only relevant for time dependent dynamics)
 
 } sim_in;