stomp_core examples

Optimize with STOMP

The full example program can be found in file stomp_example.cpp

Define a Simple Task Implementation

Create a class that implements the stomp_core::Task as shown below

class SimpleOptimizationTask: public stomp_core::Task
{
public:
  SimpleOptimizationTask(const Eigen::MatrixXd& parameters_bias,
            const std::vector<double>& bias_thresholds,
            const std::vector<double>& std_dev):
              parameters_bias_(parameters_bias),
              bias_thresholds_(bias_thresholds),
              std_dev_(std_dev)
  {

    // generate smoothing matrix
    int num_timesteps = parameters_bias.cols();
    stomp_core::generateSmoothingMatrix(num_timesteps,1.0,smoothing_M_);
    srand(time(0));

  }

For full details look at stomp_core_examples::SimpleOptimizationTask

Instantiate a Task Object

Create an instance of the SimpleOptimizationTask defined in the previous step

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

Create a STOMP config object

Populate the stomp_core::StompConfiguration object as shown below:

  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;

Create an STOMP instance

Create an instance of STOMP using the Task and Configuration objects created in previous steps

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

Call the Solve Method

Call the solve method and check the result

  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;
  }