Diff: ANTI_SLIP_FUZZY_CONTROL.cpp

Revision:

0:bfcd2371f3dc

Child:

1:773d8ae11c1a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ANTI_SLIP_FUZZY_CONTROL.cpp	Thu Feb 23 11:12:35 2017 +0000
@@ -0,0 +1,346 @@
+#include "ANTI_SLIP_FUZZY_CONTROL.h"
+
+//------------------------------------------//
+// The template for building a library
+//   for control system apllication
+//------------------------------------------//
+
+// The plant is a (p, n, q) system
+// Dimensions:
+//
+// Inputs, u   |   States, x    |   outputs, y
+//     p     -->      n        -->      q
+//
+
+//
+// The number of vertex systems is m_vertex.
+//
+
+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):
+        n(num_state), p(num_in), q(num_out), m_vertex(num_vertex), Ts(samplingTime),
+        E_out(num_out, vector<float>(num_state,0.0)),
+        ver_K_matrix(num_vertex ,vector<vector<float> >(num_in, vector<float>( (num_state + num_out), 0.0)) ),
+        SA_r(1.0, 0.0),
+        SA_l(1.0, 0.0)
+{
+    // Normally, q = p
+    if (q > p)
+        q = p;
+    //
+    zeros_n.assign(n, 0.0);
+    zeros_p.assign(p, 0.0);
+    zeros_q.assign(q, 0.0);
+    zeros_nPq.assign((n+q), 0.0); // (n+q)
+    zeros_m_vertex.assign(m_vertex, 0.0);
+    //
+    ones_p.assign(p, 1.0);
+
+    // States
+    // Input signal ---
+    states = zeros_n;
+    command = zeros_q; // r, commands, q = p
+    // Output signal ---
+    sys_inputs = zeros_p;
+
+    // Internal states ---
+    ver_u.assign(m_vertex, zeros_p);
+    sys_outputs = zeros_q;
+    // Integral state
+    state_int = zeros_q;
+    // Total states, [states; state_int]
+    state_total = zeros_nPq;
+
+
+    // The composition ratio of each vertex system
+    ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
+    ver_ratio[3] = 1.0; // Vertex of no-slip system
+
+}
+// Assign Parameters
+void ANTI_SLIP_FUZZY_CONTROL::assign_E_out(float* E_out_in){
+    // E_out_in is the pointer of a mutidimentional array with size q by n
+    E_out.assign(q, zeros_n);
+    //
+    for (size_t i = 0; i < q; ++i){
+        for (size_t j = 0; j < n; ++j){
+            // E_out[i][j] = E_out_in[i][j];
+            E_out[i][j] = *E_out_in;
+            E_out_in++;
+        }
+    }
+}
+// 1st controller Parameters (no-slip plant, sys_dVs)
+void ANTI_SLIP_FUZZY_CONTROL::assign_ver_K_matrix(float* ver_K_matrix_in){
+    // ver_K_matrix_in is the pointer of a list of mutidimentional array with size p by (n+q)
+    ver_K_matrix.assign(m_vertex, vector<vector<float> >(p, zeros_nPq) );
+    //
+    for (size_t k = 0; k < m_vertex; ++k){
+        // For each vertex
+        for (size_t i = 0; i < p; ++i){
+            for (size_t j = 0; j < (n+q); ++j){
+                // (ver_K_matrix[k])[i][j] = ver_K_matrix_in[i][j];
+                (ver_K_matrix[k])[i][j] = *ver_K_matrix_in;
+                ver_K_matrix_in++;
+            }
+        }
+        //
+    }
+    //
+}
+
+
+//
+void ANTI_SLIP_FUZZY_CONTROL::set_ver_ratio(float ratio_ft_right, float ratio_ft_left){
+    float one_ratio_ft_right;
+    float one_ratio_ft_left;
+    // Input Saturation
+    ratio_ft_right = SA_r.filter(ratio_ft_right);
+    ratio_ft_left = SA_l.filter(ratio_ft_left);
+    //
+    one_ratio_ft_right = 1.0 - ratio_ft_right;
+    one_ratio_ft_left = 1.0 - ratio_ft_left;
+    //
+    ver_ratio[0] = one_ratio_ft_right   * one_ratio_ft_left;
+    ver_ratio[1] = ratio_ft_right       * one_ratio_ft_left;
+    ver_ratio[2] = one_ratio_ft_right   * ratio_ft_left;
+    ver_ratio[3] = ratio_ft_right       * ratio_ft_left;
+}
+//
+void ANTI_SLIP_FUZZY_CONTROL::fullStateFeedBack_calc(bool enable){
+
+    // Control law
+    if (enable){
+        // Get the state_total
+        get_state_total();
+
+
+        /*
+        // Slower solution, record the outout of each vertex controller
+        sys_inputs = zeros_p;
+        //
+        for (size_t k = 0; k < m_vertex; ++k){
+            ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 );
+            Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // +=
+        }
+        */
+
+
+
+        // Faster solution, no recording the output of each vertex controller
+        sys_inputs = zeros_p;
+        //
+
+        for (size_t k = 0; k < m_vertex; ++k){
+            // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
+            Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
+        }
+
+
+
+        /*
+        for (size_t k = 3; k < 4; ++k){
+            // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
+            Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
+        }
+        */
+
+
+
+        //
+    }else{
+        state_total = zeros_nPq;
+        sys_inputs = zeros_p;
+    }
+
+    // Integral action
+    get_integral(enable);
+}
+
+// Private functions
+// Calculate the sys_outputs
+void ANTI_SLIP_FUZZY_CONTROL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
+    // sys_outputs = E_out*states
+    // Mat_multiply_Vec(sys_outputs, E_out, states);
+    sys_outputs = Mat_multiply_Vec(E_out, states);
+}
+// Calculate the Integral
+void ANTI_SLIP_FUZZY_CONTROL::get_integral(bool enable){ // Calculate the state_int
+    //
+    // Calculate the sys_outputs
+    get_sys_outputs();
+
+    // Integral action
+    // state_int += sys_outputs - command
+    if (enable){
+        Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
+    }else{
+        state_int = zeros_q;
+    }
+}
+// Concatenate the state and state_int
+void ANTI_SLIP_FUZZY_CONTROL::get_state_total(void){ // Total states, [states; state_int]
+    //
+    size_t idx = 0;
+    // states
+    for (size_t i = 0; i < n; ++i){
+        state_total[idx] = states[i];
+        idx++;
+    }
+    // state_int
+    for (size_t i = 0; i < q; ++i){
+        state_total[idx] = state_int[i];
+        idx++;
+    }
+}
+
+// Utilities
+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
+
+    // Size check
+    if (v_out.size() != m_left.size()){
+        v_out.resize(m_left.size());
+    }
+
+    /*
+    // Iterators
+    static vector<float>::iterator it_out;
+    static vector<float>::iterator it_m_row;
+    static vector<float>::iterator it_v;
+
+    //
+    it_out = v_out.begin();
+    for (size_t i = 0; i < m_left.size(); ++i){
+        *it_out = 0.0;
+        it_m_row = vector<vector<float> >(m_left)[i].begin();
+        it_v = vector<float>(v_right).begin();
+        for (size_t j = 0; j < m_left[i].size(); ++j){
+            // *it_out += m_left[i][j] * v_right[j];
+            if (*it_m_row != 0.0 && *it_v != 0.0){
+                (*it_out) += (*it_m_row) * (*it_v);
+            }else{
+                // (*it_out) += 0.0
+            }
+            // (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+    */
+
+    // Indexing
+    for (size_t i = 0; i < m_left.size(); ++i){
+        //
+        v_out[i] = 0.0;
+        //
+        for (size_t j = 0; j < v_right.size(); ++j){
+            if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
+                v_out[i] += m_left[i][j]*v_right[j];
+        }
+    }
+
+}
+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
+    vector<float> v_out;
+    // Size check
+    if (v_out.size() != m_left.size()){
+        v_out.resize(m_left.size());
+    }
+
+    /*
+    // Iterators
+    static vector<float>::iterator it_out;
+    static vector<const float>::iterator it_m_row;
+    static vector<const float>::iterator it_v;
+    //
+    it_out = v_out.begin();
+    for (size_t i = 0; i < m_left.size(); ++i){
+        *it_out = 0.0;
+        it_m_row = m_left[i].begin();
+        it_v = v_right.begin();
+        for (size_t j = 0; j < m_left[i].size(); ++j){
+            // *it_out += m_left[i][j] * v_right[j];
+            if (*it_m_row != 0.0 && *it_v != 0.0){
+                (*it_out) += (*it_m_row) * (*it_v);
+            }else{
+                // (*it_out) += 0.0
+            }
+            // (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+    */
+
+    // Indexing
+    for (size_t i = 0; i < m_left.size(); ++i){
+        //
+        v_out[i] = 0.0;
+        //
+        for (size_t j = 0; j < v_right.size(); ++j){
+            if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
+                v_out[i] += m_left[i][j]*v_right[j];
+        }
+    }
+
+    return v_out;
+}
+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
+{
+    static vector<float> v_c;
+    // Size check
+    if (v_c.size() != v_a.size()){
+        v_c.resize(v_a.size());
+    }
+    //
+    for (size_t i = 0; i < v_a.size(); ++i){
+        if (is_minus){
+            v_c[i] = v_a[i] - v_b[i];
+        }else{
+            v_c[i] = v_a[i] + v_b[i];
+        }
+    }
+    return v_c;
+}
+vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
+{
+    static vector<float> v_c;
+    // Size check
+    if (v_c.size() != v_a.size()){
+        v_c.resize(v_a.size());
+    }
+    // for pure negative
+    if (scale == -1.0){
+        for (size_t i = 0; i < v_a.size(); ++i){
+            v_c[i] = -v_a[i];
+        }
+        return v_c;
+    }
+    // else
+    for (size_t i = 0; i < v_a.size(); ++i){
+        v_c[i] = scale*v_a[i];
+
+    }
+    return v_c;
+}
+// Increment
+void ANTI_SLIP_FUZZY_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
+    // Size check
+    if (v_a.size() != v_b.size()){
+        v_a.resize(v_b.size());
+    }
+    //
+    if (is_minus){ // -=
+        for (size_t i = 0; i < v_b.size(); ++i){
+            v_a[i] -= v_b[i];
+        }
+    }else{ // +=
+        for (size_t i = 0; i < v_b.size(); ++i){
+            v_a[i] += v_b[i];
+        }
+    }
+
+}