Program Listing for File utils.h

↰ Return to documentation for file (/tmp/ws/src/stomp/include/stomp/utils.h)

#ifndef INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_UTILS_H_
#define INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_UTILS_H_

#include <string>
#include <vector>
#include <Eigen/Core>

namespace stomp
{
struct Rollout
{
  Eigen::MatrixXd noise;
  Eigen::MatrixXd parameters_noise;
  Eigen::VectorXd state_costs;
  Eigen::MatrixXd control_costs;
  Eigen::MatrixXd total_costs;
  Eigen::MatrixXd probabilities;
  std::vector<double> full_probabilities;
  std::vector<double> full_costs;
  double importance_weight;
  double total_cost;
};

namespace DerivativeOrders
{
enum DerivativeOrder
{
  STOMP_POSITION = 0,
  STOMP_VELOCITY = 1,
  STOMP_ACCELERATION = 2,
  STOMP_JERK = 3
};
}  // namespace DerivativeOrders

namespace TrajectoryInitializations
{
enum TrajectoryInitialization
{
  LINEAR_INTERPOLATION = 1,
  CUBIC_POLYNOMIAL_INTERPOLATION,
  MININUM_CONTROL_COST
};
}  // namespace TrajectoryInitializations

struct StompConfiguration
{
  // General settings
  int num_iterations;
  int num_iterations_after_valid;
  int num_timesteps;
  int num_dimensions;
  double delta_t;
  int initialization_method;
  // Probability Calculation
  double exponentiated_cost_sensitivity;
  // Noisy trajectory generation
  int num_rollouts;
  int max_rollouts;
  // Cost calculation
  double control_cost_weight;
};

static const int FINITE_DIFF_RULE_LENGTH = 7;

static const double FINITE_CENTRAL_DIFF_COEFFS[FINITE_DIFF_RULE_LENGTH][FINITE_DIFF_RULE_LENGTH] = {
  { 0, 0, 0, 1, 0, 0, 0 },                                                  // position
  { 0, 1.0 / 12.0, -2.0 / 3.0, 0, 2.0 / 3.0, -1.0 / 12.0, 0 },              // velocity
  { 0, -1 / 12.0, 16 / 12.0, -30 / 12.0, 16 / 12.0, -1 / 12.0, 0 },         // acceleration (five point stencil)
  { 0, 1 / 12.0, -17 / 12.0, 46 / 12.0, -46 / 12.0, 17 / 12.0, -1 / 12.0 }  // jerk
};

static const double FINITE_FORWARD_DIFF_COEFFS[FINITE_DIFF_RULE_LENGTH][FINITE_DIFF_RULE_LENGTH] = {
  { 1, 0, 0, 0, 0, 0, 0 },                                                      // position
  { -25.0 / 12.0, 4.0, -3.0, 4.0 / 3.0, -1.0 / 4.0, 0, 0 },                     // velocity
  { 15.0 / 4.0, -77.0 / 6.0, 107.0 / 6.0, -13.0, 61.0 / 12.0, -5.0 / 6.0, 0 },  // acceleration (five point stencil)
  { -49 / 8, 29, -461 / 8, 62, -307 / 8, 13, -15 / 8 }                          // jerk
};

void generateFiniteDifferenceMatrix(int num_time_steps,
                                    DerivativeOrders::DerivativeOrder order,
                                    double dt,
                                    Eigen::MatrixXd& diff_matrix);

void differentiate(const Eigen::VectorXd& parameters,
                   DerivativeOrders::DerivativeOrder order,
                   double dt,
                   Eigen::VectorXd& derivatives);

void generateSmoothingMatrix(int num_time_steps, double dt, Eigen::MatrixXd& projection_matrix_M);

void toVector(const Eigen::MatrixXd& m, std::vector<Eigen::VectorXd>& v);

std::string toString(const std::vector<Eigen::VectorXd>& data);

std::string toString(const Eigen::VectorXd& data);

std::string toString(const Eigen::MatrixXd& data);

} /* namespace stomp */

#endif /* INDUSTRIAL_MOVEIT_STOMP_INCLUDE_STOMP_STOMP_UTILS_H_ */