wls_alloc.c

Go to the documentation of this file.

/*
 * Copyright (C) Anton Naruta && Daniel Hoppener
 * MAVLab Delft University of Technology
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */
 
#include "wls_alloc.h"
#include "std.h"
 
#include <string.h>
#include <math.h>
#include <float.h>
#include "math/qr_solve/qr_solve.h"
#include "math/qr_solve/r8lib_min.h"
 
// provide loop feedback
#ifndef WLS_VERBOSE
#define WLS_VERBOSE FALSE
#endif
 
#if WLS_VERBOSE
#include <stdio.h>
static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
#endif
 
 
#define WLS_N_C ((WLS_N_U)+(WLS_N_V))
 
static void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
  float in[m * n];
  // convert A to 1d array
  int k = 0;
  for (int j = 0; j < n; j++) {
    for (int i = 0; i < m; i++) {
      in[k++] = A[i][j];
    }
  }
  // use solver
  qr_solve(m, n, in, b, x);
}
 
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
    float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
    float gamma_sq, int imax,  int n_u, int n_v) {
  // allocate variables, use defaults where parameters are set to 0
  if(!gamma_sq) gamma_sq = 100000;
  if(!imax) imax = 100;
 
  int n_c = n_u + n_v;
 
  float A[WLS_N_C][WLS_N_U];
  float A_free[WLS_N_C][WLS_N_U];
 
  // Create a pointer array to the rows of A_free
  // such that we can pass it to a function
  float * A_free_ptr[WLS_N_C];
  for(int i = 0; i < n_c; i++)
    A_free_ptr[i] = A_free[i];
 
  float b[WLS_N_C];
  float d[WLS_N_C];
 
  int free_index[WLS_N_U];
  int free_index_lookup[WLS_N_U];
  int n_free = 0;
  int free_chk = -1;
 
  int iter = 0;
  float p_free[WLS_N_U];
  float p[WLS_N_U];
  float u_opt[WLS_N_U];
  int infeasible_index[WLS_N_U] UNUSED;
  int n_infeasible = 0;
  float lambda[WLS_N_U];
  float W[WLS_N_U];
 
  // Initialize u and the working set, if provided from input
  if (!u_guess) {
    for (int i = 0; i < n_u; i++) {
      u[i] = (umax[i] + umin[i]) * 0.5;
    }
  } else {
    for (int i = 0; i < n_u; i++) {
      u[i] = u_guess[i];
    }
  }
  W_init ? memcpy(W, W_init, n_u * sizeof(float))
    : memset(W, 0, n_u * sizeof(float));
 
  memset(free_index_lookup, -1, n_u * sizeof(float));
 
  // find free indices
  for (int i = 0; i < n_u; i++) {
    if (W[i] == 0) {
      free_index_lookup[i] = n_free;
      free_index[n_free++] = i;
    }
  }
 
  // fill up A, A_free, b and d
  for (int i = 0; i < n_v; i++) {
    // If Wv is a NULL pointer, use Wv = identity
    b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
    d[i] = b[i];
    for (int j = 0; j < n_u; j++) {
      // If Wv is a NULL pointer, use Wv = identity
      A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
      d[i] -= A[i][j] * u[j];
    }
  }
  for (int i = n_v; i < n_c; i++) {
    memset(A[i], 0, n_u * sizeof(float));
    A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
    b[i] = up ? (Wu ? Wu[i-n_v] * up[i-n_v] : up[i-n_v]) : 0;
    d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
  }
 
  // -------------- Start loop ------------
  while (iter++ < imax) {
    // clear p, copy u to u_opt
    memset(p, 0, n_u * sizeof(float));
    memcpy(u_opt, u, n_u * sizeof(float));
 
    // Construct a matrix with the free columns of A
    if (free_chk != n_free) {
      for (int i = 0; i < n_c; i++) {
        for (int j = 0; j < n_free; j++) {
          A_free[i][j] = A[i][free_index[j]];
        }
      }
      free_chk = n_free;
    }
 
 
    // Count the infeasible free actuators
    n_infeasible = 0;
 
    if (n_free > 0) {
      // Still free variables left, calculate corresponding solution
 
      // use a solver to find the solution to A_free*p_free = d
      qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
 
      //print results current step
#if WLS_VERBOSE
      print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
#endif
 
      // Set the nonzero values of p and add to u_opt
      for (int i = 0; i < n_free; i++) {
        p[free_index[i]] = p_free[i];
        u_opt[free_index[i]] += p_free[i];
 
        // check limits
        if ( (u_opt[free_index[i]] > umax[free_index[i]] || u_opt[free_index[i]] < umin[free_index[i]])) {
          infeasible_index[n_infeasible++] = free_index[i];
        }
      }
    }
 
    // Check feasibility of the solution
    if (n_infeasible == 0) {
      // all variables are within limits
      memcpy(u, u_opt, n_u * sizeof(float));
      memset(lambda, 0, n_u * sizeof(float));
 
      // d = d + A_free*p_free; lambda = A*d;
      for (int i = 0; i < n_c; i++) {
        for (int k = 0; k < n_free; k++) {
          d[i] -= A_free[i][k] * p_free[k];
        }
        for (int k = 0; k < n_u; k++) {
          lambda[k] += A[i][k] * d[i];
        }
      }
      bool break_flag = true;
 
      // lambda = lambda x W;
      for (int i = 0; i < n_u; i++) {
        lambda[i] *= W[i];
        // if any lambdas are negative, keep looking for solution
        if (lambda[i] < -FLT_EPSILON) {
          break_flag = false;
          W[i] = 0;
          // add a free index
          if (free_index_lookup[i] < 0) {
            free_index_lookup[i] = n_free;
            free_index[n_free++] = i;
          }
        }
      }
      if (break_flag) {
 
#if WLS_VERBOSE
        print_final_values(n_u, n_v, u, B, v, umin, umax);
#endif
 
        // if solution is found, return number of iterations
        return iter;
      }
    } else {
      // scaling back actuator command (0-1)
      float alpha = 1.0;
      float alpha_tmp;
      int id_alpha = free_index[0];
 
      // find the lowest distance from the limit among the free variables
      for (int i = 0; i < n_free; i++) {
        int id = free_index[i];
 
        alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
          : (umax[id] - u[id]) / p[id];
 
        if (isnan(alpha_tmp) || alpha_tmp < 0.f) {
          alpha_tmp = 1.0f;
        }
        if (alpha_tmp < alpha) {
          alpha = alpha_tmp;
          id_alpha = id;
        }
      }
 
      // update input u = u + alpha*p
      for (int i = 0; i < n_u; i++) {
        u[i] += alpha * p[i];
        Bound(u[i],umin[i],umax[i]);
      }
      // update d = d-alpha*A*p_free
      for (int i = 0; i < n_c; i++) {
        for (int k = 0; k < n_free; k++) {
          d[i] -= A_free[i][k] * alpha * p_free[k];
        }
      }
      // get rid of a free index
      W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
 
      free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
      free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
        free_index_lookup[id_alpha];
      free_index_lookup[id_alpha] = -1;
    }
  }
  return iter;
}
 
#if WLS_VERBOSE
static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
 
  printf("n_c = %d n_free = %d\n", n_c, n_free);
 
  printf("A_free =\n");
  for(int i = 0; i < n_c; i++) {
    for (int j = 0; j < n_free; j++) {
      printf("%f ", A_free_ptr[i][j]);
    }
    printf("\n");
  }
 
  printf("d = ");
  for (int j = 0; j < n_c; j++) {
    printf("%f ", d[j]);
  }
 
  printf("\noutput = ");
  for (int j = 0; j < n_free; j++) {
    printf("%f ", p_free[j]);
  }
  printf("\n\n");
}
 
static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
  printf("n_u = %d n_v = %d\n", n_u, n_v);
 
  printf("B =\n");
  for(int i = 0; i < n_v; i++) {
    for (int j = 0; j < n_u; j++) {
      printf("%f ", B[i][j]);
    }
    printf("\n");
  }
 
  printf("v = ");
  for (int j = 0; j < n_v; j++) {
    printf("%f ", v[j]);
  }
 
  printf("\nu = ");
  for (int j = 0; j < n_u; j++) {
    printf("%f ", u[j]);
  }
  printf("\n");
 
  printf("\numin = ");
  for (int j = 0; j < n_u; j++) {
    printf("%f ", umin[j]);
  }
  printf("\n");
 
  printf("\numax = ");
  for (int j = 0; j < n_u; j++) {
    printf("%f ", umax[j]);
  }
  printf("\n\n");
 
}
#endif