Demonstrates how to optimize a trajectory using STOMP. More...
#include <iostream>
#include <Eigen/Dense>
#include "stomp_core/stomp.h"
#include "simple_optimization_task.h"
Go to the source code of this file.
Functions | |
bool | compareDiff (const Trajectory &optimized, const Trajectory &desired, const std::vector< double > &thresholds) |
Compares whether two trajectories are close to each other within a threshold. | |
stomp_core::StompConfiguration | create3DOFConfiguration () |
Creates a STOMP configuration object with default parameters. | |
void | interpolate (const std::vector< double > &start, const std::vector< double > &end, std::size_t num_timesteps, Trajectory &traj) |
Compute a linear interpolated trajectory given a start and end state. | |
int | main (int argc, char **argv) |
Variables | |
static const std::vector< double > | BIAS_THRESHOLD = {0.050,0.050,0.050} |
static const double | DELTA_T = 0.1 |
static const std::vector< double > | END_POS = {-1.25, 1.0, -0.26} |
static const std::size_t | NUM_DIMENSIONS = 3 |
static const std::size_t | NUM_TIMESTEPS = 20 |
static const std::vector< double > | START_POS = {1.4, 1.4, 0.5} |
static const std::vector< double > | STD_DEV = {1.0, 1.0, 1.0} |
Demonstrates how to optimize a trajectory using STOMP.
Definition in file stomp_example.cpp.
bool compareDiff | ( | const Trajectory & | optimized, |
const Trajectory & | desired, | ||
const std::vector< double > & | thresholds | ||
) |
Compares whether two trajectories are close to each other within a threshold.
optimized | optimized trajectory |
desired | desired trajectory |
thresholds | used to determine if two values are equal |
Definition at line 74 of file stomp_example.cpp.
Creates a STOMP configuration object with default parameters.
[Create Config]
[Create Config]
Definition at line 47 of file stomp_example.cpp.
void interpolate | ( | const std::vector< double > & | start, |
const std::vector< double > & | end, | ||
std::size_t | num_timesteps, | ||
Trajectory & | traj | ||
) |
Compute a linear interpolated trajectory given a start and end state.
start | start position |
end | last position |
num_timesteps | number of timesteps |
traj | returned linear interpolated trajectory |
Definition at line 99 of file stomp_example.cpp.
int main | ( | int | argc, |
char ** | argv | ||
) |
< Creating a Task with a trajectory bias
[Create Task Object]
[Create Task Object]
[Create STOMP]
< Creating STOMP to find a trajectory close enough to the bias
[Create STOMP]
[Solve]
< Optimizing a trajectory close enough to the bias is produced
[Solve]
< Further verifying the results
Definition at line 114 of file stomp_example.cpp.
const std::vector<double> BIAS_THRESHOLD = {0.050,0.050,0.050} [static] |
Threshold to determine whether two trajectories are equal
Definition at line 40 of file stomp_example.cpp.
const double DELTA_T = 0.1 [static] |
Timestep in seconds
Definition at line 37 of file stomp_example.cpp.
const std::vector<double> END_POS = {-1.25, 1.0, -0.26} [static] |
Trajectory ending posiiton
Definition at line 39 of file stomp_example.cpp.
const std::size_t NUM_DIMENSIONS = 3 [static] |
< Assign Type Trajectory to Eigen::MatrixXd Type < Declaring optimization variables Number of parameters to optimize
Definition at line 35 of file stomp_example.cpp.
const std::size_t NUM_TIMESTEPS = 20 [static] |
Number of timesteps
Definition at line 36 of file stomp_example.cpp.
const std::vector<double> START_POS = {1.4, 1.4, 0.5} [static] |
Trajectory starting position
Definition at line 38 of file stomp_example.cpp.
const std::vector<double> STD_DEV = {1.0, 1.0, 1.0} [static] |
Standard deviation used for generating noisy parameters
Definition at line 41 of file stomp_example.cpp.