Defines the STOMP improvement policy. More...
#include <task.h>
Public Member Functions | |
virtual bool | computeCosts (const Eigen::MatrixXd ¶meters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, Eigen::VectorXd &costs, bool &validity)=0 |
computes the state costs as a function of the optimized parameters for each time step. | |
virtual bool | computeNoisyCosts (const Eigen::MatrixXd ¶meters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::VectorXd &costs, bool &validity)=0 |
computes the state costs as a function of the noisy parameters for each time step. | |
virtual void | done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd ¶meters) |
Called by Stomp at the end of the optimization process. | |
virtual bool | filterNoisyParameters (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd ¶meters, bool &filtered) |
Filters the given noisy parameters which is applied after noisy trajectory generation. It could be used for clipping of joint limits or projecting into the null space of the Jacobian. | |
virtual bool | filterParameterUpdates (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, const Eigen::MatrixXd ¶meters, Eigen::MatrixXd &updates) |
Filters the given parameters which is applied after the update. It could be used for clipping of joint limits or projecting into the null space of the Jacobian. | |
virtual bool | generateNoisyParameters (const Eigen::MatrixXd ¶meters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd ¶meters_noise, Eigen::MatrixXd &noise)=0 |
Generates a noisy trajectory from the parameters. | |
virtual void | postIteration (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, double cost, const Eigen::MatrixXd ¶meters) |
Called by STOMP at the end of each iteration. |
virtual bool stomp_core::Task::computeCosts | ( | const Eigen::MatrixXd & | parameters, |
std::size_t | start_timestep, | ||
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
Eigen::VectorXd & | costs, | ||
bool & | validity | ||
) | [pure virtual] |
computes the state costs as a function of the optimized parameters for each time step.
parameters | A matrix [num_dimensions][num_parameters] of the policy parameters to execute |
start_timestep | The start index into the 'parameters' array, usually 0. |
num_timesteps | The number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
costs | A vector containing the state costs per timestep. |
validity | Whether or not the trajectory is valid |
Implemented in stomp_core_examples::SimpleOptimizationTask.
virtual bool stomp_core::Task::computeNoisyCosts | ( | const Eigen::MatrixXd & | parameters, |
std::size_t | start_timestep, | ||
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
int | rollout_number, | ||
Eigen::VectorXd & | costs, | ||
bool & | validity | ||
) | [pure virtual] |
computes the state costs as a function of the noisy parameters for each time step.
parameters | A matrix [num_dimensions][num_parameters] of the policy parameters to execute |
start_timestep | The start index into the 'parameters' array, usually 0. |
num_timesteps | The number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
rollout_number | The index of the noisy trajectory whose cost is being evaluated. |
costs | vector A vector containing the state costs per timestep. |
validity | Whether or not the trajectory is valid |
Implemented in stomp_core_examples::SimpleOptimizationTask.
virtual void stomp_core::Task::done | ( | bool | success, |
int | total_iterations, | ||
double | final_cost, | ||
const Eigen::MatrixXd & | parameters | ||
) | [inline, virtual] |
Called by Stomp at the end of the optimization process.
success | Whether the optimization succeeded |
total_iterations | Number of iterations used |
final_cost | The cost value after optimizing. |
parameters | The parameters generated at the end of the optimization [num_dimensions x num_timesteps] |
virtual bool stomp_core::Task::filterNoisyParameters | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
int | rollout_number, | ||
Eigen::MatrixXd & | parameters, | ||
bool & | filtered | ||
) | [inline, virtual] |
Filters the given noisy parameters which is applied after noisy trajectory generation. It could be used for clipping of joint limits or projecting into the null space of the Jacobian.
start_timestep | The start index into the 'parameters' array, usually 0. |
num_timesteps | The number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
rollout_number | The rollout index for this noisy parameter set |
parameters | The noisy parameters * |
filtered | False if no filtering was done |
virtual bool stomp_core::Task::filterParameterUpdates | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
const Eigen::MatrixXd & | parameters, | ||
Eigen::MatrixXd & | updates | ||
) | [inline, virtual] |
Filters the given parameters which is applied after the update. It could be used for clipping of joint limits or projecting into the null space of the Jacobian.
start_timestep | The start index into the 'parameters' array, usually 0. |
num_timesteps | The number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
parameters | The optimized parameters |
updates | The updates to the parameters |
Reimplemented in stomp_core_examples::SimpleOptimizationTask, and DummyTask.
virtual bool stomp_core::Task::generateNoisyParameters | ( | const Eigen::MatrixXd & | parameters, |
std::size_t | start_timestep, | ||
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
int | rollout_number, | ||
Eigen::MatrixXd & | parameters_noise, | ||
Eigen::MatrixXd & | noise | ||
) | [pure virtual] |
Generates a noisy trajectory from the parameters.
parameters | A matrix [num_dimensions][num_parameters] of the current optimized parameters |
start_timestep | The start index into the 'parameters' array, usually 0. |
num_timesteps | The number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
rollout_number | The index of the noisy trajectory. |
parameters_noise | The parameters + noise |
noise | The noise applied to the parameters |
Implemented in stomp_core_examples::SimpleOptimizationTask, and DummyTask.
virtual void stomp_core::Task::postIteration | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
double | cost, | ||
const Eigen::MatrixXd & | parameters | ||
) | [inline, virtual] |
Called by STOMP at the end of each iteration.
start_timestep | The start index into the 'parameters' array, usually 0. |
num_timesteps | The number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
cost | The cost value for the current parameters. |
parameters | The value of the parameters at the end of the current iteration [num_dimensions x num_timesteps]. |