A sparse MPC solver for walking motion generation (old version).
solver/problem_param.cpp
Go to the documentation of this file.
00001 
00009 /****************************************
00010  * INCLUDES 
00011  ****************************************/
00012 
00013 #include "problem_param.h"
00014 
00015 #include <cmath> //cos,sin
00016 
00017 /****************************************
00018  * FUNCTIONS 
00019  ****************************************/
00020 
00021 problem_parameters::problem_parameters (
00022     const int N_,
00023     const double Alpha,
00024     const double Beta,
00025     const double Gamma,
00026     const double regularization)
00027 {
00028     N = N_;
00029 
00030     i2Q[0] = 1/(2*(Beta/2));
00031     i2Q[1] = 1/(2*(Alpha/2));
00032     i2Q[2] = 1/(2*regularization);
00033 
00034     i2P = 1/(2 * (Gamma/2));
00035 
00036     spar = new state_parameters[N];
00037 }
00038 
00039 
00040 
00041 problem_parameters::~problem_parameters()
00042 {
00043     if (spar != NULL)
00044         delete [] spar;
00045 }
00046 
00047 
00048 
00055 void problem_parameters::set_state_parameters (
00056     const double* T_,
00057     const double* h_,
00058     const double h_initial,
00059     const double* angle)
00060 {
00061     for (int i = 0; i < N; i++)
00062     {
00063         spar[i].cos = cos(angle[i]);
00064         spar[i].sin = sin(angle[i]);
00065 
00066         if (i == 0)
00067         {
00068             spar[i].A6 = T_[i]*T_[i]/2 - (h_[0] - h_initial);
00069         }
00070         else
00071         {
00072             spar[i].A6 = T_[i]*T_[i]/2 - (h_[i] - h_[i-1]);
00073         }
00074 
00075         spar[i].T = T_[i];
00076         spar[i].h = h_[i];
00077 
00078         spar[i].B[2] = T_[i];
00079         spar[i].B[1] = T_[i]*T_[i]/2;
00080         spar[i].B[0] = spar[i].B[1]*T_[i]/3 - h_[i]*T_[i];
00081 
00082         spar[i].A3 = T_[i];
00083     }
00084 }