Record the Control Constraint as a CppAD Function Object

rec_constraint

@(@\newcommand{\B}[1]{{\bf #1}} \newcommand{\R}[1]{{\rm #1}}@)@Record the Control Constraint as a CppAD Function Object
Syntax
control::rec_constraint(L_fun, J, delta_t, p)

Prototype


template <class Base> void
rec_constraint(
     CppAD::ADFun<Base>&             L_fun   ,
     size_t                          J       ,
     const Base                      delta_t ,
     const VECTOR(Base)&             p       )

L_fun
The constraint function @(@ L : \B{R}^{2 \times J} \times \B{R}^{4 \times J} \rightarrow \B{R}^{4 \times J} @)@ is stored in L_fun . We use u to denote an L_fun argument vector (size 6*J ) and v a result vector (size 4*J ); e.g.,



     v = L_fun.Forward(0, u)

J
is the number of time points in the control problem.

delta_t
is the step size between time points.

p
Is the initial value for the state variable vector in the control problem.

x
For @(@ i = 0, 1 @)@ and @(@ j = 0, \ldots , J-1 @)@, @[@ u_{j * 2 + i} = x_{i,j} @]@

y
For @(@ i = 0, \ldots, 3 @)@ and @(@ j = 0, \ldots , J-1 @)@, @[@ u_{2 * J + j * 4 + i} = y_{i,j} @]@

L
For @(@ i = 0, \ldots, 3 @)@ and @(@ j = 0, \ldots , J-1 @)@, @[@ v_{j * 4 + i} = L_{i,j} @]@

Example

ADFun<double>
The following example records the constraint as an ADFun<double> object.

bool test_control_rec_constraint(void)
{    bool ok = true;
     double eps99   = 99.0 * std::numeric_limits<double>::epsilon();
     //
     CppAD::ADFun<double>       L_fun;
     size_t                     J = 4;
     double                     delta_t = 0.5;
     VECTOR(double) p(4);
     for(size_t i = 0; i < 4; i++)
          p[i] = double(5 - i);
     control::rec_constraint(L_fun, J, delta_t, p);
     //
     // argument values
     MATRIX(double) x(2, J), y(4, J);
     for(size_t i = 0; i < 4; i++)
     {    for(size_t j = 0; j < J; j++)
          {    y(i, j) = double(2 * J + 4 * j + i);
               if( i < 2 )
                    x(i, j) = double(2 * j + i);
          }
     }
     // pack (x, y) into xy_vec
     VECTOR(double) xy_vec(6 * J);
     for(size_t j = 0; j < J; j++)
     {    for(size_t i = 0; i < 2; i++)
               xy_vec[ j * 2 + i] = x(i, j);
          for(size_t i = 0; i < 4; i++)
               xy_vec[ 2 * J + j * 4 + i] = y(i, j);
     }
     // compute constraint function using L_fun
     VECTOR(double) L_vec = L_fun.Forward(0, xy_vec);
     ok &= size_t( L_vec.size() ) == 4 * J;
     //
     // unpack L_vec
     MATRIX(double) L(4, J);
     for(size_t j = 0; j < J; j++)
     {    for(size_t i = 0; i < 4; i++)
               L(i, j) = L_vec[ j * 4 + i];
     }
     //
     for(size_t i = 0; i < 4; i++)
     {    double check = y(i, 0) - p[i];
          ok &= CppAD::NearEqual(L(i, 0), check, eps99, eps99);
     }
     for(size_t j = 1; j < J; j++)
     {    // use notation in Park2006 reference
          double u1 = x(0,j-1);
          double u2 = x(1,j-1);
          double x1 = y(0,j-1);
          double x2 = y(1,j-1);
          double x3 = y(2,j-1);
          double x4 = y(3,j-1);
          double r  = sqrt( (x1 + 1.0) * (x1 + 1.0) + x2 * x2 );
          double a  = 1.0 / (r * r * r) - 1.0;
          //
          double dxdt[4];
          dxdt[0] = x3;
          dxdt[1] = x4;
          dxdt[2] = + 2.0 * x4 - (1.0 + x1) * a + u1;
          dxdt[3] = - 2.0 * x3 -         x2 * a + u2;
          //
          for(size_t i = 0; i < 4; i++)
          {    double check = y(i,j) - y(i,j-1) - dxdt[i] * delta_t;
               ok &= CppAD::NearEqual(L(i, j), check, eps99, eps99);
          }
     }
     //
     return ok;
}

ADFun< AD<double> >
The following example records the constraint as an ADFun< AD<double> > object.

bool test_control_ad_rec_constraint(void)
{    bool ok = true;
     double eps99 = 99.0 * std::numeric_limits<double>::epsilon();
     //
     // ---------------------------------------------------------------------
     // create afun
     typedef CppAD::AD<double> adouble;
     CppAD::ADFun<adouble>      afun;
     size_t                     J = 3;
     adouble                    adelta_t = 0.5;
     VECTOR(adouble) ap(4);
     for(size_t i = 0; i < 4; i++)
          ap[i] = adouble(5 - i);
     control::rec_constraint(afun, J, adelta_t, ap);
     size_t m = afun.Range();
     size_t n = afun.Domain() - m;
     // ----------------------------------------------------------------------
     // Create L_fun using afun
     VECTOR(adouble) axy(n + m), aL(m);
     for(size_t i = 0; i < n + m; i++)
          axy[i] = adouble(0.0);
     CppAD::Independent( axy );
     aL = afun.Forward(0, axy);
     CppAD::ADFun<double> L_fun(axy, aL);
     // ----------------------------------------------------------------------
     // check L_fun
     // ----------------------------------------------------------------------
     // argument values
     MATRIX(double) x(2, J);
     MATRIX(double) y(4, J);
     for(size_t i = 0; i < 4; i++)
     {    for(size_t j = 0; j < J; j++)
          {    y(i, j) = double(2 * J + 4 * j + i);
               if( i < 2 )
                    x(i, j) = double(2 * j + i);
          }
     }
     // pack (x, y) into xy_vec
     VECTOR(double) xy_vec(6 * J);
     for(size_t j = 0; j < J; j++)
     {    for(size_t i = 0; i < 2; i++)
               xy_vec[ j * 2 + i] = x(i, j);
          for(size_t i = 0; i < 4; i++)
               xy_vec[ 2 * J + j * 4 + i] = y(i, j);
     }
     // compute constraint function using L_fun
     VECTOR(double) L_vec = L_fun.Forward(0, xy_vec);
     ok &= size_t( L_vec.size() ) == 4 * J;
     //
     // unpack L_vec
     MATRIX(double) L(4, J);
     for(size_t j = 0; j < J; j++)
     {    for(size_t i = 0; i < 4; i++)
               L(i, j) = L_vec[ j * 4 + i];
     }
     //
     for(size_t i = 0; i < 4; i++)
     {    double check = y(i, 0) - Value(ap[i]);
          ok &= CppAD::NearEqual(L(i, 0), check, eps99, eps99);
     }
     for(size_t j = 1; j < J; j++)
     {    // use notation in Park2006 reference
          double u1 = x(0,j-1);
          double u2 = x(1,j-1);
          double x1 = y(0,j-1);
          double x2 = y(1,j-1);
          double x3 = y(2,j-1);
          double x4 = y(3,j-1);
          double r  = sqrt( (x1 + 1.0) * (x1 + 1.0) + x2 * x2 );
          double a  = 1.0 / (r * r * r) - 1.0;
          //
          double dxdt[4];
          dxdt[0] = x3;
          dxdt[1] = x4;
          dxdt[2] = + 2.0 * x4 - (1.0 + x1) * a + u1;
          dxdt[3] = - 2.0 * x3 -         x2 * a + u2;
          //
          for(size_t i = 0; i < 4; i++)
          {    double check = y(i,j) - y(i,j-1) - dxdt[i] * Value(adelta_t);
               ok &= CppAD::NearEqual(L(i, j), check, eps99, eps99);
          }
     }
     //
     return ok;
}

Input File: src/control.hpp