utils.h
1
26 #ifndef INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_UTILS_H_
27 #define INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_UTILS_H_
28
29 #include <string>
30 #include <vector>
31 #include <Eigen/Core>
32
33 namespace stomp_core
34 {
35
37 struct Rollout
38 {
39  Eigen::MatrixXd noise;
40  Eigen::MatrixXd parameters_noise;
42  Eigen::VectorXd state_costs;
43  Eigen::MatrixXd control_costs;
44  Eigen::MatrixXd total_costs;
45  Eigen::MatrixXd probabilities;
47  std::vector<double> full_probabilities;
48  std::vector<double> full_costs;
52  double total_cost;
54 };
55
56
57 namespace DerivativeOrders
58 {
60 enum DerivativeOrder
61 {
62  STOMP_POSITION = 0,
63  STOMP_VELOCITY = 1,
64  STOMP_ACCELERATION = 2,
65  STOMP_JERK = 3
66 };
67 }
68
69 namespace TrajectoryInitializations
70 {
72 enum TrajectoryInitialization
73 {
74  LINEAR_INTERPOLATION = 1,
75  CUBIC_POLYNOMIAL_INTERPOLATION,
76  MININUM_CONTROL_COST
77 };
78 }
79
82 {
83  // General settings
88  double delta_t;
91  // Probability Calculation
94  // Noisy trajectory generation
98  // Cost calculation
100 };
101
103 static const int FINITE_DIFF_RULE_LENGTH = 7;
104
106 static const double FINITE_CENTRAL_DIFF_COEFFS[FINITE_DIFF_RULE_LENGTH][FINITE_DIFF_RULE_LENGTH] = {
107  {0, 0 , 0 , 1 , 0 , 0 , 0 }, // position
108  {0, 1.0/12.0 , -2.0/3.0 , 0 , 2.0/3.0 , -1.0/12.0 , 0 }, // velocity
109  {0, -1/12.0 , 16/12.0 , -30/12.0 , 16/12.0 , -1/12.0 , 0 }, // acceleration (five point stencil)
110  {0, 1/12.0 , -17/12.0 , 46/12.0 , -46/12.0 , 17/12.0 , -1/12.0} // jerk
111 };
112
114 static const double FINITE_FORWARD_DIFF_COEFFS[FINITE_DIFF_RULE_LENGTH][FINITE_DIFF_RULE_LENGTH] = {
115  {1 , 0 , 0 , 0 , 0 , 0 , 0 }, // position
116  {-25.0/12.0 , 4.0 , -3.0 , 4.0/3.0, -1.0/4.0 , 0 , 0 }, // velocity
117  {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)
118  {-49/8 , 29 , -461/8 , 62 , -307/8 , 13 , -15/8} // jerk
119 };
120
128 void generateFiniteDifferenceMatrix(int num_time_steps, DerivativeOrders::DerivativeOrder order, double dt,
129  Eigen::MatrixXd& diff_matrix);
130
138 void differentiate(const Eigen::VectorXd& parameters, DerivativeOrders::DerivativeOrder order,
139  double dt, Eigen::VectorXd& derivatives );
140
147 void generateSmoothingMatrix(int num_time_steps, double dt, Eigen::MatrixXd& projection_matrix_M);
148
156 void toVector(const Eigen::MatrixXd& m,std::vector<Eigen::VectorXd>& v);
157
163 std::string toString(const std::vector<Eigen::VectorXd>& data);
164
170 std::string toString(const Eigen::VectorXd& data);
171
177 std::string toString(const Eigen::MatrixXd& data);
178
179 } /* namespace stomp */
180
181 #endif /* INDUSTRIAL_MOVEIT_STOMP_CORE_INCLUDE_STOMP_CORE_STOMP_UTILS_H_ */
int num_rollouts
Number of noisy trajectories.
Definition: utils.h:95
The data structure used to store information about a single rollout.
Definition: utils.h:37
Eigen::VectorXd state_costs
A vector [num_time_steps] of the cost at each timestep.
Definition: utils.h:42
int num_iterations
Maximum number of iteration allowed.
Definition: utils.h:84
double control_cost_weight
Percentage of the trajectory accelerations cost to be applied in the total cost calculation > ...
Definition: utils.h:99
int num_dimensions
Parameter dimensionality.
Definition: utils.h:87
double exponentiated_cost_sensitivity
Default exponetiated cost sensitivity coefficient.
Definition: utils.h:92
Eigen::MatrixXd probabilities
A matrix [num_dimensions][num_time_steps] of the probability for each parameter at every timestep...
Definition: utils.h:45
std::vector< double > full_costs
A vector [num_dimensions] of the full coss, state_cost + control_cost for each joint over the entire ...
Definition: utils.h:48
The data structure used to store STOMP configuration parameters.
Definition: utils.h:81
Eigen::MatrixXd parameters_noise
A matrix [num_dimensions][num_time_steps] of the sum of parameters + noise.
Definition: utils.h:40
int initialization_method
TrajectoryInitializations::TrajectoryInitialization.
Definition: utils.h:89
double importance_weight
importance sampling weight
Definition: utils.h:51
double total_cost
combined state + control cost over the entire trajectory for all joints
Definition: utils.h:52
Eigen::MatrixXd noise
A matrix [num_dimensions][num_time_steps] of random noise applied to the parameters.
Definition: utils.h:39
int max_rollouts
The combined number of new and old rollouts during each iteration shouldn&#39;t exceed this value...
Definition: utils.h:96
std::vector< double > full_probabilities
A vector [num_dimensions] of the probabilities for the full trajectory.
Definition: utils.h:47
double delta_t
Time change between consecutive points.
Definition: utils.h:88
Eigen::MatrixXd total_costs
A matrix [num_dimensions][num_time_steps] of the total cost, where total_cost[d] = state_costs_ + con...
Definition: utils.h:44
int num_iterations_after_valid
Stomp will stop optimizing this many iterations after finding a valid solution.
Definition: utils.h:85
Eigen::MatrixXd control_costs
A matrix [num_dimensions][num_time_steps] of the control cost for each parameter at every timestep...
Definition: utils.h:43
int num_timesteps
Number of timesteps.
Definition: utils.h:86

stomp_core
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:43