Chun Feng Huang / ANTI_SLIP_FUZZY_CONTROL

A new type of anti-slip controller based on TS fuzzy models

ANTI_SLIP_FUZZY_CONTROL.cpp@1:773d8ae11c1a, 2017-02-26 (annotated)

Committer:
benson516
Date:
Sun Feb 26 05:36:16 2017 +0000
Revision:
1:773d8ae11c1a
Parent:
0:bfcd2371f3dc 
Ver. 1.00

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:bfcd2371f3dc 1 #include "ANTI_SLIP_FUZZY_CONTROL.h"
benson516 0:bfcd2371f3dc 2
benson516 0:bfcd2371f3dc 3 //------------------------------------------//
benson516 0:bfcd2371f3dc 4 // The template for building a library
benson516 0:bfcd2371f3dc 5 // for control system apllication
benson516 0:bfcd2371f3dc 6 //------------------------------------------//
benson516 0:bfcd2371f3dc 7
benson516 0:bfcd2371f3dc 8 // The plant is a (p, n, q) system
benson516 0:bfcd2371f3dc 9 // Dimensions:
benson516 0:bfcd2371f3dc 10 //
benson516 0:bfcd2371f3dc 11 // Inputs, u | States, x | outputs, y
benson516 0:bfcd2371f3dc 12 // p --> n --> q
benson516 0:bfcd2371f3dc 13 //
benson516 0:bfcd2371f3dc 14
benson516 0:bfcd2371f3dc 15 //
benson516 0:bfcd2371f3dc 16 // The number of vertex systems is m_vertex.
benson516 0:bfcd2371f3dc 17 //
benson516 0:bfcd2371f3dc 18
benson516 0:bfcd2371f3dc 19 ANTI_SLIP_FUZZY_CONTROL::ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime):
benson516 0:bfcd2371f3dc 20 n(num_state), p(num_in), q(num_out), m_vertex(num_vertex), Ts(samplingTime),
benson516 0:bfcd2371f3dc 21 E_out(num_out, vector<float>(num_state,0.0)),
benson516 1:773d8ae11c1a 22 Nxd(num_state, vector<float>(num_in,0.0)),
benson516 1:773d8ae11c1a 23 Nud(num_in, vector<float>(num_in,0.0)),
benson516 0:bfcd2371f3dc 24 ver_K_matrix(num_vertex ,vector<vector<float> >(num_in, vector<float>( (num_state + num_out), 0.0)) ),
benson516 0:bfcd2371f3dc 25 SA_r(1.0, 0.0),
benson516 0:bfcd2371f3dc 26 SA_l(1.0, 0.0)
benson516 0:bfcd2371f3dc 27 {
benson516 1:773d8ae11c1a 28
benson516 1:773d8ae11c1a 29 // To enble, run *.start() function
benson516 1:773d8ae11c1a 30 enable = false;
benson516 1:773d8ae11c1a 31 // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d
benson516 1:773d8ae11c1a 32 is_usingFeedForward = false;
benson516 1:773d8ae11c1a 33
benson516 1:773d8ae11c1a 34
benson516 0:bfcd2371f3dc 35 // Normally, q = p
benson516 0:bfcd2371f3dc 36 if (q > p)
benson516 0:bfcd2371f3dc 37 q = p;
benson516 0:bfcd2371f3dc 38 //
benson516 0:bfcd2371f3dc 39 zeros_n.assign(n, 0.0);
benson516 0:bfcd2371f3dc 40 zeros_p.assign(p, 0.0);
benson516 0:bfcd2371f3dc 41 zeros_q.assign(q, 0.0);
benson516 0:bfcd2371f3dc 42 zeros_nPq.assign((n+q), 0.0); // (n+q)
benson516 0:bfcd2371f3dc 43 zeros_m_vertex.assign(m_vertex, 0.0);
benson516 0:bfcd2371f3dc 44 //
benson516 0:bfcd2371f3dc 45 ones_p.assign(p, 1.0);
benson516 0:bfcd2371f3dc 46
benson516 0:bfcd2371f3dc 47 // States
benson516 0:bfcd2371f3dc 48 // Input signal ---
benson516 0:bfcd2371f3dc 49 states = zeros_n;
benson516 0:bfcd2371f3dc 50 command = zeros_q; // r, commands, q = p
benson516 0:bfcd2371f3dc 51 // Output signal ---
benson516 0:bfcd2371f3dc 52 sys_inputs = zeros_p;
benson516 0:bfcd2371f3dc 53
benson516 0:bfcd2371f3dc 54 // Internal states ---
benson516 0:bfcd2371f3dc 55 ver_u.assign(m_vertex, zeros_p);
benson516 0:bfcd2371f3dc 56 sys_outputs = zeros_q;
benson516 0:bfcd2371f3dc 57 // Integral state
benson516 0:bfcd2371f3dc 58 state_int = zeros_q;
benson516 0:bfcd2371f3dc 59 // Total states, [states; state_int]
benson516 0:bfcd2371f3dc 60 state_total = zeros_nPq;
benson516 0:bfcd2371f3dc 61
benson516 1:773d8ae11c1a 62 // Equalibrium states for command tracking
benson516 1:773d8ae11c1a 63 x_d = zeros_n;
benson516 1:773d8ae11c1a 64 u_d = zeros_p;
benson516 0:bfcd2371f3dc 65
benson516 0:bfcd2371f3dc 66 // The composition ratio of each vertex system
benson516 0:bfcd2371f3dc 67 ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
benson516 1:773d8ae11c1a 68 ver_ratio[3] = 1.0; // Ratio for no-slip system
benson516 0:bfcd2371f3dc 69
benson516 0:bfcd2371f3dc 70 }
benson516 1:773d8ae11c1a 71 void ANTI_SLIP_FUZZY_CONTROL::start(){
benson516 1:773d8ae11c1a 72 enable = true;
benson516 1:773d8ae11c1a 73 }
benson516 1:773d8ae11c1a 74 void ANTI_SLIP_FUZZY_CONTROL::pause(){
benson516 1:773d8ae11c1a 75 enable = false;
benson516 1:773d8ae11c1a 76 }
benson516 1:773d8ae11c1a 77 void ANTI_SLIP_FUZZY_CONTROL::stop(){
benson516 1:773d8ae11c1a 78 if (!enable){
benson516 1:773d8ae11c1a 79 return;
benson516 1:773d8ae11c1a 80 }
benson516 1:773d8ae11c1a 81 enable = false;
benson516 1:773d8ae11c1a 82 // Reset
benson516 1:773d8ae11c1a 83 reset();
benson516 1:773d8ae11c1a 84 }
benson516 1:773d8ae11c1a 85 void ANTI_SLIP_FUZZY_CONTROL::reset(){
benson516 1:773d8ae11c1a 86 //
benson516 1:773d8ae11c1a 87 // States
benson516 1:773d8ae11c1a 88 // Input signal ---
benson516 1:773d8ae11c1a 89 states = zeros_n;
benson516 1:773d8ae11c1a 90 command = zeros_q; // r, commands, q = p
benson516 1:773d8ae11c1a 91 // Output signal ---
benson516 1:773d8ae11c1a 92 sys_inputs = zeros_p;
benson516 1:773d8ae11c1a 93
benson516 1:773d8ae11c1a 94 // Internal states ---
benson516 1:773d8ae11c1a 95 // ver_u.assign(m_vertex, zeros_p);
benson516 1:773d8ae11c1a 96 sys_outputs = zeros_q;
benson516 1:773d8ae11c1a 97 // Integral state
benson516 1:773d8ae11c1a 98 state_int = zeros_q;
benson516 1:773d8ae11c1a 99 // Total states, [states; state_int]
benson516 1:773d8ae11c1a 100 state_total = zeros_nPq;
benson516 1:773d8ae11c1a 101
benson516 1:773d8ae11c1a 102 // Equalibrium states for command tracking
benson516 1:773d8ae11c1a 103 x_d = zeros_n;
benson516 1:773d8ae11c1a 104 u_d = zeros_p;
benson516 1:773d8ae11c1a 105
benson516 1:773d8ae11c1a 106 // The composition ratio of each vertex system
benson516 1:773d8ae11c1a 107 ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
benson516 1:773d8ae11c1a 108 ver_ratio[3] = 1.0; // Ratio for no-slip system
benson516 1:773d8ae11c1a 109
benson516 1:773d8ae11c1a 110 }
benson516 1:773d8ae11c1a 111 void ANTI_SLIP_FUZZY_CONTROL::reset_integrator(){ // Reset the state_int only
benson516 1:773d8ae11c1a 112 // Integral state
benson516 1:773d8ae11c1a 113 state_int = zeros_q;
benson516 1:773d8ae11c1a 114 }
benson516 0:bfcd2371f3dc 115 // Assign Parameters
benson516 0:bfcd2371f3dc 116 void ANTI_SLIP_FUZZY_CONTROL::assign_E_out(float* E_out_in){
benson516 0:bfcd2371f3dc 117 // E_out_in is the pointer of a mutidimentional array with size q by n
benson516 0:bfcd2371f3dc 118 E_out.assign(q, zeros_n);
benson516 0:bfcd2371f3dc 119 //
benson516 0:bfcd2371f3dc 120 for (size_t i = 0; i < q; ++i){
benson516 0:bfcd2371f3dc 121 for (size_t j = 0; j < n; ++j){
benson516 0:bfcd2371f3dc 122 // E_out[i][j] = E_out_in[i][j];
benson516 0:bfcd2371f3dc 123 E_out[i][j] = *E_out_in;
benson516 0:bfcd2371f3dc 124 E_out_in++;
benson516 0:bfcd2371f3dc 125 }
benson516 0:bfcd2371f3dc 126 }
benson516 0:bfcd2371f3dc 127 }
benson516 1:773d8ae11c1a 128 void ANTI_SLIP_FUZZY_CONTROL::assign_Nxd(float* Nxd_in){
benson516 1:773d8ae11c1a 129 // Nxd_in is the pointer of a mutidimentional array with size n by p
benson516 1:773d8ae11c1a 130 Nxd.assign(n, zeros_p);
benson516 1:773d8ae11c1a 131 //
benson516 1:773d8ae11c1a 132 for (size_t i = 0; i < n; ++i){
benson516 1:773d8ae11c1a 133 for (size_t j = 0; j < p; ++j){
benson516 1:773d8ae11c1a 134 // Nxd[i][j] = Nxd_in[i][j];
benson516 1:773d8ae11c1a 135 Nxd[i][j] = *Nxd_in;
benson516 1:773d8ae11c1a 136 Nxd_in++;
benson516 1:773d8ae11c1a 137 }
benson516 1:773d8ae11c1a 138 }
benson516 1:773d8ae11c1a 139 //
benson516 1:773d8ae11c1a 140 is_usingFeedForward = true;
benson516 1:773d8ae11c1a 141 }
benson516 1:773d8ae11c1a 142 void ANTI_SLIP_FUZZY_CONTROL::assign_Nud(float* Nud_in){
benson516 1:773d8ae11c1a 143 // Nxd_in is the pointer of a mutidimentional array with size p by p
benson516 1:773d8ae11c1a 144 Nud.assign(p, zeros_p);
benson516 1:773d8ae11c1a 145 //
benson516 1:773d8ae11c1a 146 for (size_t i = 0; i < p; ++i){
benson516 1:773d8ae11c1a 147 for (size_t j = 0; j < p; ++j){
benson516 1:773d8ae11c1a 148 // Nud[i][j] = Nud_in[i][j];
benson516 1:773d8ae11c1a 149 Nud[i][j] = *Nud_in;
benson516 1:773d8ae11c1a 150 Nud_in++;
benson516 1:773d8ae11c1a 151 }
benson516 1:773d8ae11c1a 152 }
benson516 1:773d8ae11c1a 153 //
benson516 1:773d8ae11c1a 154 is_usingFeedForward = true;
benson516 1:773d8ae11c1a 155 }
benson516 0:bfcd2371f3dc 156 // 1st controller Parameters (no-slip plant, sys_dVs)
benson516 0:bfcd2371f3dc 157 void ANTI_SLIP_FUZZY_CONTROL::assign_ver_K_matrix(float* ver_K_matrix_in){
benson516 0:bfcd2371f3dc 158 // ver_K_matrix_in is the pointer of a list of mutidimentional array with size p by (n+q)
benson516 0:bfcd2371f3dc 159 ver_K_matrix.assign(m_vertex, vector<vector<float> >(p, zeros_nPq) );
benson516 0:bfcd2371f3dc 160 //
benson516 0:bfcd2371f3dc 161 for (size_t k = 0; k < m_vertex; ++k){
benson516 0:bfcd2371f3dc 162 // For each vertex
benson516 0:bfcd2371f3dc 163 for (size_t i = 0; i < p; ++i){
benson516 0:bfcd2371f3dc 164 for (size_t j = 0; j < (n+q); ++j){
benson516 0:bfcd2371f3dc 165 // (ver_K_matrix[k])[i][j] = ver_K_matrix_in[i][j];
benson516 0:bfcd2371f3dc 166 (ver_K_matrix[k])[i][j] = *ver_K_matrix_in;
benson516 0:bfcd2371f3dc 167 ver_K_matrix_in++;
benson516 0:bfcd2371f3dc 168 }
benson516 0:bfcd2371f3dc 169 }
benson516 0:bfcd2371f3dc 170 //
benson516 0:bfcd2371f3dc 171 }
benson516 0:bfcd2371f3dc 172 //
benson516 0:bfcd2371f3dc 173 }
benson516 0:bfcd2371f3dc 174
benson516 0:bfcd2371f3dc 175
benson516 0:bfcd2371f3dc 176 //
benson516 0:bfcd2371f3dc 177 void ANTI_SLIP_FUZZY_CONTROL::set_ver_ratio(float ratio_ft_right, float ratio_ft_left){
benson516 0:bfcd2371f3dc 178 float one_ratio_ft_right;
benson516 0:bfcd2371f3dc 179 float one_ratio_ft_left;
benson516 0:bfcd2371f3dc 180 // Input Saturation
benson516 0:bfcd2371f3dc 181 ratio_ft_right = SA_r.filter(ratio_ft_right);
benson516 0:bfcd2371f3dc 182 ratio_ft_left = SA_l.filter(ratio_ft_left);
benson516 0:bfcd2371f3dc 183 //
benson516 0:bfcd2371f3dc 184 one_ratio_ft_right = 1.0 - ratio_ft_right;
benson516 0:bfcd2371f3dc 185 one_ratio_ft_left = 1.0 - ratio_ft_left;
benson516 0:bfcd2371f3dc 186 //
benson516 0:bfcd2371f3dc 187 ver_ratio[0] = one_ratio_ft_right * one_ratio_ft_left;
benson516 0:bfcd2371f3dc 188 ver_ratio[1] = ratio_ft_right * one_ratio_ft_left;
benson516 0:bfcd2371f3dc 189 ver_ratio[2] = one_ratio_ft_right * ratio_ft_left;
benson516 0:bfcd2371f3dc 190 ver_ratio[3] = ratio_ft_right * ratio_ft_left;
benson516 0:bfcd2371f3dc 191 }
benson516 0:bfcd2371f3dc 192 //
benson516 1:773d8ae11c1a 193 void ANTI_SLIP_FUZZY_CONTROL::iterateOnce(void){
benson516 0:bfcd2371f3dc 194
benson516 1:773d8ae11c1a 195 if(!enable){
benson516 1:773d8ae11c1a 196 return;
benson516 1:773d8ae11c1a 197 }
benson516 1:773d8ae11c1a 198
benson516 1:773d8ae11c1a 199 //
benson516 1:773d8ae11c1a 200 if (is_usingFeedForward){
benson516 1:773d8ae11c1a 201 get_equilibriumState();
benson516 0:bfcd2371f3dc 202 // Get the state_total
benson516 0:bfcd2371f3dc 203 get_state_total();
benson516 1:773d8ae11c1a 204 // u = u_d + yc;
benson516 1:773d8ae11c1a 205 sys_inputs = u_d;
benson516 0:bfcd2371f3dc 206 }else{
benson516 1:773d8ae11c1a 207 // Get the state_total
benson516 1:773d8ae11c1a 208 get_state_total();
benson516 1:773d8ae11c1a 209 // u = yc;
benson516 0:bfcd2371f3dc 210 sys_inputs = zeros_p;
benson516 0:bfcd2371f3dc 211 }
benson516 0:bfcd2371f3dc 212
benson516 1:773d8ae11c1a 213
benson516 1:773d8ae11c1a 214 /*
benson516 1:773d8ae11c1a 215 // Slower solution, record the outout of each vertex controller
benson516 1:773d8ae11c1a 216 sys_inputs = zeros_p;
benson516 1:773d8ae11c1a 217 //
benson516 1:773d8ae11c1a 218 for (size_t k = 0; k < m_vertex; ++k){
benson516 1:773d8ae11c1a 219 ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 );
benson516 1:773d8ae11c1a 220 Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // +=
benson516 1:773d8ae11c1a 221 }
benson516 1:773d8ae11c1a 222 */
benson516 1:773d8ae11c1a 223
benson516 1:773d8ae11c1a 224 // Faster solution, no recording the output of each vertex controller
benson516 1:773d8ae11c1a 225 // sys_inputs = zeros_p;
benson516 1:773d8ae11c1a 226 //
benson516 1:773d8ae11c1a 227 for (size_t k = 0; k < m_vertex; ++k){
benson516 1:773d8ae11c1a 228 // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
benson516 1:773d8ae11c1a 229 Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
benson516 1:773d8ae11c1a 230 }
benson516 1:773d8ae11c1a 231
benson516 1:773d8ae11c1a 232 /*
benson516 1:773d8ae11c1a 233 // Add the u_d
benson516 1:773d8ae11c1a 234 if (is_usingFeedForward){
benson516 1:773d8ae11c1a 235 Get_VectorIncrement(sys_inputs, u_d, false); // +=
benson516 1:773d8ae11c1a 236 }
benson516 1:773d8ae11c1a 237 */
benson516 1:773d8ae11c1a 238
benson516 0:bfcd2371f3dc 239 // Integral action
benson516 1:773d8ae11c1a 240 get_integral();
benson516 0:bfcd2371f3dc 241 }
benson516 0:bfcd2371f3dc 242
benson516 0:bfcd2371f3dc 243 // Private functions
benson516 1:773d8ae11c1a 244 // Calculate the equilibrium states
benson516 1:773d8ae11c1a 245 void ANTI_SLIP_FUZZY_CONTROL::get_equilibriumState(void){
benson516 1:773d8ae11c1a 246 Mat_multiply_Vec(x_d, Nxd, command);
benson516 1:773d8ae11c1a 247 Mat_multiply_Vec(u_d, Nud, command);
benson516 1:773d8ae11c1a 248 }
benson516 0:bfcd2371f3dc 249 // Calculate the sys_outputs
benson516 0:bfcd2371f3dc 250 void ANTI_SLIP_FUZZY_CONTROL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
benson516 0:bfcd2371f3dc 251 // sys_outputs = E_out*states
benson516 1:773d8ae11c1a 252 Mat_multiply_Vec(sys_outputs, E_out, states);
benson516 1:773d8ae11c1a 253 // sys_outputs = Mat_multiply_Vec(E_out, states);
benson516 0:bfcd2371f3dc 254 }
benson516 0:bfcd2371f3dc 255 // Calculate the Integral
benson516 1:773d8ae11c1a 256 void ANTI_SLIP_FUZZY_CONTROL::get_integral(){ // Calculate the state_int
benson516 1:773d8ae11c1a 257 if(!enable){
benson516 1:773d8ae11c1a 258 return;
benson516 1:773d8ae11c1a 259 }
benson516 0:bfcd2371f3dc 260 //
benson516 0:bfcd2371f3dc 261 // Calculate the sys_outputs
benson516 0:bfcd2371f3dc 262 get_sys_outputs();
benson516 1:773d8ae11c1a 263 //
benson516 0:bfcd2371f3dc 264 // Integral action
benson516 0:bfcd2371f3dc 265 // state_int += sys_outputs - command
benson516 1:773d8ae11c1a 266 Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
benson516 0:bfcd2371f3dc 267 }
benson516 0:bfcd2371f3dc 268 // Concatenate the state and state_int
benson516 0:bfcd2371f3dc 269 void ANTI_SLIP_FUZZY_CONTROL::get_state_total(void){ // Total states, [states; state_int]
benson516 0:bfcd2371f3dc 270 //
benson516 0:bfcd2371f3dc 271 size_t idx = 0;
benson516 1:773d8ae11c1a 272
benson516 0:bfcd2371f3dc 273 // states
benson516 1:773d8ae11c1a 274 if (is_usingFeedForward){
benson516 1:773d8ae11c1a 275 for (size_t i = 0; i < n; ++i){
benson516 1:773d8ae11c1a 276 state_total[idx] = states[i] - x_d[i];
benson516 1:773d8ae11c1a 277 idx++;
benson516 1:773d8ae11c1a 278 }
benson516 1:773d8ae11c1a 279 }else{
benson516 1:773d8ae11c1a 280 for (size_t i = 0; i < n; ++i){
benson516 1:773d8ae11c1a 281 state_total[idx] = states[i];
benson516 1:773d8ae11c1a 282 idx++;
benson516 1:773d8ae11c1a 283 }
benson516 0:bfcd2371f3dc 284 }
benson516 1:773d8ae11c1a 285
benson516 0:bfcd2371f3dc 286 // state_int
benson516 0:bfcd2371f3dc 287 for (size_t i = 0; i < q; ++i){
benson516 0:bfcd2371f3dc 288 state_total[idx] = state_int[i];
benson516 0:bfcd2371f3dc 289 idx++;
benson516 0:bfcd2371f3dc 290 }
benson516 0:bfcd2371f3dc 291 }
benson516 0:bfcd2371f3dc 292
benson516 0:bfcd2371f3dc 293 // Utilities
benson516 0:bfcd2371f3dc 294 void ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:bfcd2371f3dc 295
benson516 0:bfcd2371f3dc 296 // Size check
benson516 0:bfcd2371f3dc 297 if (v_out.size() != m_left.size()){
benson516 0:bfcd2371f3dc 298 v_out.resize(m_left.size());
benson516 0:bfcd2371f3dc 299 }
benson516 0:bfcd2371f3dc 300
benson516 0:bfcd2371f3dc 301 /*
benson516 0:bfcd2371f3dc 302 // Iterators
benson516 0:bfcd2371f3dc 303 static vector<float>::iterator it_out;
benson516 0:bfcd2371f3dc 304 static vector<float>::iterator it_m_row;
benson516 0:bfcd2371f3dc 305 static vector<float>::iterator it_v;
benson516 0:bfcd2371f3dc 306
benson516 0:bfcd2371f3dc 307 //
benson516 0:bfcd2371f3dc 308 it_out = v_out.begin();
benson516 0:bfcd2371f3dc 309 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 310 *it_out = 0.0;
benson516 0:bfcd2371f3dc 311 it_m_row = vector<vector<float> >(m_left)[i].begin();
benson516 0:bfcd2371f3dc 312 it_v = vector<float>(v_right).begin();
benson516 0:bfcd2371f3dc 313 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:bfcd2371f3dc 314 // *it_out += m_left[i][j] * v_right[j];
benson516 0:bfcd2371f3dc 315 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:bfcd2371f3dc 316 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 317 }else{
benson516 0:bfcd2371f3dc 318 // (*it_out) += 0.0
benson516 0:bfcd2371f3dc 319 }
benson516 0:bfcd2371f3dc 320 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 321 //
benson516 0:bfcd2371f3dc 322 it_m_row++;
benson516 0:bfcd2371f3dc 323 it_v++;
benson516 0:bfcd2371f3dc 324 }
benson516 0:bfcd2371f3dc 325 it_out++;
benson516 0:bfcd2371f3dc 326 }
benson516 0:bfcd2371f3dc 327 */
benson516 0:bfcd2371f3dc 328
benson516 0:bfcd2371f3dc 329 // Indexing
benson516 0:bfcd2371f3dc 330 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 331 //
benson516 0:bfcd2371f3dc 332 v_out[i] = 0.0;
benson516 0:bfcd2371f3dc 333 //
benson516 0:bfcd2371f3dc 334 for (size_t j = 0; j < v_right.size(); ++j){
benson516 0:bfcd2371f3dc 335 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 0:bfcd2371f3dc 336 v_out[i] += m_left[i][j]*v_right[j];
benson516 0:bfcd2371f3dc 337 }
benson516 0:bfcd2371f3dc 338 }
benson516 0:bfcd2371f3dc 339
benson516 0:bfcd2371f3dc 340 }
benson516 0:bfcd2371f3dc 341 vector<float> ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:bfcd2371f3dc 342 vector<float> v_out;
benson516 0:bfcd2371f3dc 343 // Size check
benson516 0:bfcd2371f3dc 344 if (v_out.size() != m_left.size()){
benson516 0:bfcd2371f3dc 345 v_out.resize(m_left.size());
benson516 0:bfcd2371f3dc 346 }
benson516 0:bfcd2371f3dc 347
benson516 0:bfcd2371f3dc 348 /*
benson516 0:bfcd2371f3dc 349 // Iterators
benson516 0:bfcd2371f3dc 350 static vector<float>::iterator it_out;
benson516 0:bfcd2371f3dc 351 static vector<const float>::iterator it_m_row;
benson516 0:bfcd2371f3dc 352 static vector<const float>::iterator it_v;
benson516 0:bfcd2371f3dc 353 //
benson516 0:bfcd2371f3dc 354 it_out = v_out.begin();
benson516 0:bfcd2371f3dc 355 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 356 *it_out = 0.0;
benson516 0:bfcd2371f3dc 357 it_m_row = m_left[i].begin();
benson516 0:bfcd2371f3dc 358 it_v = v_right.begin();
benson516 0:bfcd2371f3dc 359 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:bfcd2371f3dc 360 // *it_out += m_left[i][j] * v_right[j];
benson516 0:bfcd2371f3dc 361 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:bfcd2371f3dc 362 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 363 }else{
benson516 0:bfcd2371f3dc 364 // (*it_out) += 0.0
benson516 0:bfcd2371f3dc 365 }
benson516 0:bfcd2371f3dc 366 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 367 //
benson516 0:bfcd2371f3dc 368 it_m_row++;
benson516 0:bfcd2371f3dc 369 it_v++;
benson516 0:bfcd2371f3dc 370 }
benson516 0:bfcd2371f3dc 371 it_out++;
benson516 0:bfcd2371f3dc 372 }
benson516 0:bfcd2371f3dc 373 */
benson516 0:bfcd2371f3dc 374
benson516 0:bfcd2371f3dc 375 // Indexing
benson516 0:bfcd2371f3dc 376 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 377 //
benson516 0:bfcd2371f3dc 378 v_out[i] = 0.0;
benson516 0:bfcd2371f3dc 379 //
benson516 0:bfcd2371f3dc 380 for (size_t j = 0; j < v_right.size(); ++j){
benson516 0:bfcd2371f3dc 381 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 0:bfcd2371f3dc 382 v_out[i] += m_left[i][j]*v_right[j];
benson516 0:bfcd2371f3dc 383 }
benson516 0:bfcd2371f3dc 384 }
benson516 0:bfcd2371f3dc 385
benson516 0:bfcd2371f3dc 386 return v_out;
benson516 0:bfcd2371f3dc 387 }
benson516 0:bfcd2371f3dc 388 vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 0:bfcd2371f3dc 389 {
benson516 0:bfcd2371f3dc 390 static vector<float> v_c;
benson516 0:bfcd2371f3dc 391 // Size check
benson516 0:bfcd2371f3dc 392 if (v_c.size() != v_a.size()){
benson516 0:bfcd2371f3dc 393 v_c.resize(v_a.size());
benson516 0:bfcd2371f3dc 394 }
benson516 0:bfcd2371f3dc 395 //
benson516 0:bfcd2371f3dc 396 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:bfcd2371f3dc 397 if (is_minus){
benson516 0:bfcd2371f3dc 398 v_c[i] = v_a[i] - v_b[i];
benson516 0:bfcd2371f3dc 399 }else{
benson516 0:bfcd2371f3dc 400 v_c[i] = v_a[i] + v_b[i];
benson516 0:bfcd2371f3dc 401 }
benson516 0:bfcd2371f3dc 402 }
benson516 0:bfcd2371f3dc 403 return v_c;
benson516 0:bfcd2371f3dc 404 }
benson516 0:bfcd2371f3dc 405 vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:bfcd2371f3dc 406 {
benson516 0:bfcd2371f3dc 407 static vector<float> v_c;
benson516 0:bfcd2371f3dc 408 // Size check
benson516 0:bfcd2371f3dc 409 if (v_c.size() != v_a.size()){
benson516 0:bfcd2371f3dc 410 v_c.resize(v_a.size());
benson516 0:bfcd2371f3dc 411 }
benson516 0:bfcd2371f3dc 412 // for pure negative
benson516 0:bfcd2371f3dc 413 if (scale == -1.0){
benson516 0:bfcd2371f3dc 414 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:bfcd2371f3dc 415 v_c[i] = -v_a[i];
benson516 0:bfcd2371f3dc 416 }
benson516 0:bfcd2371f3dc 417 return v_c;
benson516 0:bfcd2371f3dc 418 }
benson516 0:bfcd2371f3dc 419 // else
benson516 0:bfcd2371f3dc 420 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:bfcd2371f3dc 421 v_c[i] = scale*v_a[i];
benson516 0:bfcd2371f3dc 422
benson516 0:bfcd2371f3dc 423 }
benson516 0:bfcd2371f3dc 424 return v_c;
benson516 0:bfcd2371f3dc 425 }
benson516 0:bfcd2371f3dc 426 // Increment
benson516 0:bfcd2371f3dc 427 void ANTI_SLIP_FUZZY_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 0:bfcd2371f3dc 428 // Size check
benson516 0:bfcd2371f3dc 429 if (v_a.size() != v_b.size()){
benson516 0:bfcd2371f3dc 430 v_a.resize(v_b.size());
benson516 0:bfcd2371f3dc 431 }
benson516 0:bfcd2371f3dc 432 //
benson516 0:bfcd2371f3dc 433 if (is_minus){ // -=
benson516 0:bfcd2371f3dc 434 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:bfcd2371f3dc 435 v_a[i] -= v_b[i];
benson516 0:bfcd2371f3dc 436 }
benson516 0:bfcd2371f3dc 437 }else{ // +=
benson516 0:bfcd2371f3dc 438 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:bfcd2371f3dc 439 v_a[i] += v_b[i];
benson516 0:bfcd2371f3dc 440 }
benson516 0:bfcd2371f3dc 441 }
benson516 0:bfcd2371f3dc 442
benson516 0:bfcd2371f3dc 443 }