TopiCo Benchmark

topico_example.cpp

// Simple benchmark script for TopiCo

#include "rt_nonfinite.h"
#include "topico_wrapper.h"
#include "topico_wrapper_terminate.h"
#include "topico_wrapper_types.h"
#include "coder_array.h"

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <chrono>


int main(int argc, char **argv) {
  const uint num_dim  = 3;
  const uint num_wayp = 1;

  // Declare Inputs
  coder::array<double, 2U> State_start;
  coder::array<double, 3U> Waypoints;
  coder::array<double, 2U> V_max;
  coder::array<double, 2U> V_min;
  coder::array<double, 2U> A_max;
  coder::array<double, 2U> A_min;
  coder::array<double, 2U> J_max;
  coder::array<double, 2U> J_min;
  coder::array<double, 1U> A_global;
  coder::array<bool, 2U> b_sync_V;
  coder::array<bool, 2U> b_sync_A;
  coder::array<bool, 2U> b_sync_J;
  coder::array<bool, 2U> b_sync_W;
  coder::array<bool, 2U> b_rotate;
  coder::array<bool, 2U> b_hard_V_lim;
  coder::array<bool, 2U> b_catch_up;
  coder::array<signed char, 2U> direction;

  // Declare Outputs
  coder::array<struct0_T, 2U> J_setp_struct;
  coder::array<int, 2U> solution_out;
  coder::array<double, 2U> T_waypoints;
  coder::array<double, 2U> P;
  coder::array<double, 2U> V;
  coder::array<double, 2U> A;
  coder::array<double, 2U> J;
  coder::array<double, 2U> t;

  // Resize Inputs
  State_start.set_size(num_dim,3);
  Waypoints.set_size(num_dim,5,num_wayp);
  V_max.set_size(num_dim,num_wayp);
  V_min.set_size(num_dim,num_wayp);
  A_max.set_size(num_dim,num_wayp);
  A_min.set_size(num_dim,num_wayp);
  J_max.set_size(num_dim,num_wayp);
  J_min.set_size(num_dim,num_wayp);
  A_global.set_size(num_dim);
  b_sync_V.set_size(num_dim,num_wayp);
  b_sync_A.set_size(num_dim,num_wayp);
  b_sync_J.set_size(num_dim,num_wayp);
  b_sync_W.set_size(num_dim,num_wayp);
  b_rotate.set_size(num_dim-1,num_wayp);
  b_hard_V_lim.set_size(num_dim,num_wayp);
  b_catch_up.set_size(num_dim,num_wayp);
  direction.set_size(num_dim,num_wayp);
  
  State_start[(0 + num_dim * 0)] = 0.0; //Initial X Position
  State_start[(1 + num_dim * 0)] = 0.0; //Initial Y Position
  State_start[(2 + num_dim * 0)] = 0.0; //Initial Z Position
  State_start[(0 + num_dim * 1)] = 0.0; //Initial X Velocity
  State_start[(1 + num_dim * 1)] = 0.0; //Initial Y Velocity
  State_start[(2 + num_dim * 1)] = 0.0; //Initial Z Velocity
  State_start[(0 + num_dim * 2)] = 0.0;                       //Initial X Acceleration
  State_start[(1 + num_dim * 2)] = 0.0;                       //Initial Y Acceleration
  State_start[(2 + num_dim * 2)] = 0.0;                       //Initial Z Acceleration

  int idx_wayp = 0;
  Waypoints[(0 + num_dim * 0) + num_dim * 5 * idx_wayp] = 1.0; //Waypoint X Position
  Waypoints[(1 + num_dim * 0) + num_dim * 5 * idx_wayp] = 0.0; //Waypoint Y Position
  Waypoints[(2 + num_dim * 0) + num_dim * 5 * idx_wayp] = 0.0; //Waypoint Z Position
  Waypoints[(0 + num_dim * 1) + num_dim * 5 * idx_wayp] = 0.0; //Waypoint X Velocity
  Waypoints[(1 + num_dim * 1) + num_dim * 5 * idx_wayp] = 0.0; //Waypoint Y Velocity
  Waypoints[(2 + num_dim * 1) + num_dim * 5 * idx_wayp] = 0.0; //Waypoint Z Velocity
  Waypoints[(0 + num_dim * 2) + num_dim * 5 * idx_wayp] = 0.0;                       //Waypoint X Acceleration
  Waypoints[(1 + num_dim * 2) + num_dim * 5 * idx_wayp] = 0.0;                       //Waypoint Y Acceleration
  Waypoints[(2 + num_dim * 2) + num_dim * 5 * idx_wayp] = 0.0;                       //Waypoint Z Acceleration
  Waypoints[(0 + num_dim * 3) + num_dim * 5 * idx_wayp] = 0.0;                       //Waypoint X Movement Velocity
  Waypoints[(1 + num_dim * 3) + num_dim * 5 * idx_wayp] = 0.0;                       //Waypoint Y Movement Velocity
  Waypoints[(2 + num_dim * 3) + num_dim * 5 * idx_wayp] = 0.0;                       //Waypoint Z Movement Velocity
  Waypoints[(0 + num_dim * 4) + num_dim * 5 * idx_wayp] = 0.0;                       //reserved
  Waypoints[(1 + num_dim * 4) + num_dim * 5 * idx_wayp] = 0.0;                       //reserved
  Waypoints[(2 + num_dim * 4) + num_dim * 5 * idx_wayp] = 0.0;                       //reserved

  for (int idx_dim = 0; idx_dim < num_dim; idx_dim++) {  
    A_global[idx_dim] = 0.0; 
    for (int idx_wayp = 0; idx_wayp < num_wayp; idx_wayp++) {
      V_max[idx_dim + num_dim * idx_wayp]        = 1.0;
      V_min[idx_dim + num_dim * idx_wayp]        = -1.0;
      A_max[idx_dim + num_dim * idx_wayp]        = 1.0;
      A_min[idx_dim + num_dim * idx_wayp]        = -1.0;
      J_max[idx_dim + num_dim * idx_wayp]        = 1.0;
      J_min[idx_dim + num_dim * idx_wayp]        = -1.0;
      b_sync_V[idx_dim + num_dim * idx_wayp]     = 1.0;
      b_sync_A[idx_dim + num_dim * idx_wayp]     = 1.0;
      b_sync_J[idx_dim + num_dim * idx_wayp]     = 1.0;
      b_sync_W[idx_dim + num_dim * idx_wayp]     = 0.0;
      b_hard_V_lim[idx_dim + num_dim * idx_wayp] = 1.0;
      b_catch_up[idx_dim + num_dim * idx_wayp]   = 1.0;
      direction[idx_dim + num_dim * idx_wayp]    = 0.0;
    }
  }  

  double ts_rollout = 0.005;

  auto start = std::chrono::high_resolution_clock::now();
 
  topico_wrapper(State_start, Waypoints, V_max, V_min, A_max, A_min, J_max, J_min, A_global, b_sync_V, b_sync_A, b_sync_J, b_sync_W, b_rotate, b_hard_V_lim, b_catch_up, direction, ts_rollout, J_setp_struct,solution_out, T_waypoints, P, V, A, J, t);

  auto stop = std::chrono::high_resolution_clock::now();

  int size_rollout = P.size(1);
  for (int idx = 0; idx < size_rollout; idx++) {
    std::cout << idx * ts_rollout << " " << P[3*idx+0] << " " << P[3*idx+1] << " "  << P[3*idx+2] << std::endl;
  }

  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
  std::cout << "calculation duration: " << (float)duration.count() / 1000 << " [ms]" << std::endl;

  topico_wrapper_terminate();
  return 0;
}