Program Listing for File stomp.h

↰ Return to documentation for file (include/stomp/stomp.h)

#ifndef INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_H_
#define INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_H_

#include <atomic>
#include <stomp/utils.h>
#include <stomp/task.h>

namespace stomp
{
class Stomp
{
public:
  Stomp(const StompConfiguration& config, TaskPtr task);

  bool solve(const std::vector<double>& first, const std::vector<double>& last, Eigen::MatrixXd& parameters_optimized);

  bool solve(const Eigen::VectorXd& first, const Eigen::VectorXd& last, Eigen::MatrixXd& parameters_optimized);

  bool solve(const Eigen::MatrixXd& initial_parameters, Eigen::MatrixXd& parameters_optimized);

  void setConfig(const StompConfiguration& config);

  bool cancel();

  bool clear();

protected:
  // initialization methods
  bool resetVariables();

  bool computeInitialTrajectory(const std::vector<double>& first, const std::vector<double>& last);

  // optimization steps
  bool runSingleIteration();

  bool generateNoisyRollouts();

  bool filterNoisyRollouts();

  bool computeNoisyRolloutsCosts();

  bool computeRolloutsStateCosts();

  bool computeRolloutsControlCosts();

  bool computeProbabilities();

  bool updateParameters();

  bool computeOptimizedCost();

protected:
  // process control
  std::atomic<bool> proceed_;
  TaskPtr task_;
  StompConfiguration config_;
  unsigned int current_iteration_;
  // optimized parameters
  bool parameters_valid_;
  bool parameters_valid_prev_;
  double parameters_total_cost_;
  double current_lowest_cost_;
  Eigen::MatrixXd parameters_optimized_;
  Eigen::MatrixXd parameters_updates_;
  Eigen::VectorXd parameters_state_costs_;
  Eigen::MatrixXd parameters_control_costs_;
  // rollouts
  std::vector<Rollout> noisy_rollouts_;
  std::vector<Rollout> reused_rollouts_;
  int num_active_rollouts_;
  // finite difference and optimization matrices
  int num_timesteps_padded_;
  int start_index_padded_;
  Eigen::MatrixXd finite_diff_matrix_A_padded_;
  Eigen::MatrixXd control_cost_matrix_R_padded_;
  Eigen::MatrixXd control_cost_matrix_R_;
  Eigen::MatrixXd inv_control_cost_matrix_R_;
};

} /* namespace stomp */

#endif /* INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_H_ */