stomp_example.cpp

#include <iostream>
#include <Eigen/Dense>
#include "stomp_core/stomp.h"
#include "simple_optimization_task.h"

using Trajectory = Eigen::MatrixXd;                              
static const std::size_t NUM_DIMENSIONS = 3;                            
static const std::size_t NUM_TIMESTEPS = 20;                            
static const double DELTA_T = 0.1;                                      
static const std::vector<double> START_POS = {1.4, 1.4, 0.5};           
static const std::vector<double> END_POS = {-1.25, 1.0, -0.26};         
static const std::vector<double> BIAS_THRESHOLD = {0.050,0.050,0.050};  
static const std::vector<double> STD_DEV = {1.0, 1.0, 1.0};             
stomp_core::StompConfiguration create3DOFConfiguration()
{
  using namespace stomp_core;

  StompConfiguration c;
  c.num_timesteps = NUM_TIMESTEPS;
  c.num_iterations = 40;
  c.num_dimensions = NUM_DIMENSIONS;
  c.delta_t = DELTA_T;
  c.control_cost_weight = 0.0;
  c.initialization_method = TrajectoryInitializations::LINEAR_INTERPOLATION;
  c.num_iterations_after_valid = 0;
  c.num_rollouts = 20;
  c.max_rollouts = 20;

  return c;
}

bool compareDiff(const Trajectory& optimized, const Trajectory& desired,
                 const std::vector<double>& thresholds)
{
  auto num_dimensions = optimized.rows();
  Trajectory diff = Trajectory::Zero(num_dimensions,optimized.cols());
  for(auto d = 0u;d < num_dimensions ; d++)
  {
    diff.row(d) = optimized.row(d)- desired.row(d);
    diff.row(d).cwiseAbs();
    if((diff.row(d).array() > thresholds[d] ).any() )
    {
      return false;
    }
  }

  return true;
}

void interpolate(const std::vector<double>& start, const std::vector<double>& end,
                 std::size_t num_timesteps, Trajectory& traj)
{
  auto dimensions = start.size();
  traj = Eigen::MatrixXd::Zero(dimensions,num_timesteps);
  for(auto d = 0u; d < dimensions; d++)
  {
    double delta = (end[d] - start[d])/(num_timesteps - 1);
    for(auto t = 0u; t < num_timesteps; t++)
    {
      traj(d,t) = start[d] + t*delta;
    }
  }
}

int main(int argc,char** argv)
{
  using namespace stomp_core_examples;
  using namespace stomp_core;

  Trajectory trajectory_bias;
  interpolate(START_POS,END_POS,NUM_TIMESTEPS,trajectory_bias);

  TaskPtr task(new SimpleOptimizationTask(trajectory_bias,BIAS_THRESHOLD,STD_DEV));


  StompConfiguration config = create3DOFConfiguration();
  Stomp stomp(config,task);


  Trajectory optimized;
  if(stomp.solve(START_POS,END_POS,optimized))
  {
    std::cout<<"STOMP succeeded"<<std::endl;
  }
  else
  {
    std::cout<<"A valid solution was not found"<<std::endl;
    return -1;
  }

  if(compareDiff(optimized,trajectory_bias,BIAS_THRESHOLD))
  {
    std::cout<<"The solution is within the expected thresholds"<<std::endl;
  }
  else
  {
    std::cout<<"The solution exceeded the required thresholds"<<std::endl;
    return -1;
  }

  return 0;

}