A sparse MPC solver for walking motion generation.
state_handling.cpp
Go to the documentation of this file.
1 
9 /****************************************
10  * INCLUDES
11  ****************************************/
12 
13 #include "state_handling.h"
14 
15 /****************************************
16  * FUNCTIONS
17  ****************************************/
18 
19 namespace state_handling
20 {
28  void tilde_to_bar (const double sinA, const double cosA, double *state)
29  {
30  double tmp = cosA*state[0] + sinA*state[3];
31  state[3] = -sinA*state[0] + cosA*state[3];
32  state[0] = tmp;
33  }
34 
42  void bar_to_tilde (const double sinA, const double cosA, double *state)
43  {
44  double tmp = cosA*state[0] - sinA*state[3];
45  state[3] = sinA*state[0] + cosA*state[3];
46  state[0] = tmp;
47  }
48 
49 
56  void tilde_to_orig (const double h, double *state)
57  {
58  state[0] = state[0] + h * state[2];
59  state[3] = state[3] + h * state[5];
60  }
61 
68  void orig_to_tilde (const double h, double *state)
69  {
70  state[0] = state[0] - h * state[2];
71  state[3] = state[3] - h * state[5];
72  }
73 
74 
83  void get_controls (const int preview_window_size, const double *X, const int ind, double *controls)
84  {
85  int index;
86  if (ind >= preview_window_size)
87  {
88  index = preview_window_size-1;
89  }
90  else
91  {
92  index = ind;
93  }
94  controls[0] = X[preview_window_size*SMPC_NUM_STATE_VAR + index*SMPC_NUM_CONTROL_VAR + 0];
95  controls[1] = X[preview_window_size*SMPC_NUM_STATE_VAR + index*SMPC_NUM_CONTROL_VAR + 1];
96  }
97 }
#define SMPC_NUM_STATE_VAR
Number of state variables.
Definition: smpc_solver.h:24
void bar_to_tilde(const double sinA, const double cosA, double *state)
Converts state from X_tilde to X_bar.
void tilde_to_bar(const double sinA, const double cosA, double *state)
Converts state from X_tilde to X_bar.
Various operations on the vector of states.
#define SMPC_NUM_CONTROL_VAR
Number of control variables.
Definition: smpc_solver.h:26
void orig_to_tilde(const double h, double *state)
Converts state from original variables to X_tilde.
void get_controls(const int preview_window_size, const double *X, const int ind, double *controls)
Returns the controls,that must be applied to reach the next state.
void tilde_to_orig(const double h, double *state)
Converts state from X_tilde to original variables.