Class Stomp

Defined in File stomp.h

Class Documentation

class Stomp

The Stomp class.

Public Functions

Stomp(const StompConfiguration &config, TaskPtr task)

Stomp Constructor.

Parameters:

config – Stomp configuration parameters
task – The item to be optimized.

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

Find the optimal solution provided a start and end goal.

Parameters:

first – Start state for the task
last – Final state for the task
parameters_optimized – Optimized solution [parameters][timesteps]

Returns:

True if solution was found, otherwise false.

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

Find the optimal solution provided a start and end goal.

Parameters:

first – Start state for the task
last – Final state for the task
parameters_optimized – Optimized solution [Parameters][timesteps]

Returns:

True if solution was found, otherwise false.

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

Find the optimal solution provided an intial guess.

Parameters:

initial_parameters – A matrix [Parameters][timesteps]
parameters_optimized – The optimized solution [Parameters][timesteps]

Returns:

True if solution was found, otherwise false.

void setConfig(const StompConfiguration &config)

Sets the configuration and resets all internal variables.

Parameters:: config – Stomp Configuration struct

bool cancel()

Cancel optimization in progress. (Thread-Safe) This method is thead-safe.

Returns:: True if sucessful, otherwise false.

bool clear()

Resets all internal variables.

Returns:: True if sucessful, otherwise false.

Protected Functions

bool resetVariables()

Reset all internal variables.

Returns:: True if sucessful, otherwise false.

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

Computes an inital guess at a solution given a start and end state.

Parameters:

first – Start state for the task
last – Final state for the task

Returns:

True if sucessful, otherwise false.

bool runSingleIteration()

Run a single iteration of the stomp algorithm.

Returns:: True if it was able to succesfully perform a single iteration. False is returned when an error is encounter at one of the many optimization steps.

bool generateNoisyRollouts()

Generate a set of noisy rollouts.

Returns:: True if sucessful, otherwise false.

bool filterNoisyRollouts()

Applies the optimization task’s filter methods to the noisy trajectories.

Returns:: True if sucessful, otherwise false.

bool computeNoisyRolloutsCosts()

Computes the total cost for each of the noisy rollouts.

Returns:: True if sucessful, otherwise false.

bool computeRolloutsStateCosts()

Computes the cost at every timestep for each noisy rollout.

Returns:: True if sucessful, otherwise false.

bool computeRolloutsControlCosts()

Compute the control cost for each noisy rollout. This is the sum of the acceleration squared, then each noisy rollouts control cost is divided by the maximum control cost found amoung the noisy rollouts.

Returns:: True if sucessful, otherwise false.

bool computeProbabilities()

Computes the probability from the state cost at every timestep for each noisy rollout.

Returns:: True if sucessful, otherwise false.

bool updateParameters()

Computes update from probabilities using convex combination.

Returns:: True if sucessful, otherwise false.

bool computeOptimizedCost()

Computes the optimized trajectory cost [Control Cost + State Cost] If the current cost is not less than the previous cost the parameters are reset to the previous iteration’s parameters.

Returns:

Protected Attributes

std::atomic<bool> proceed_: Used to determine if the optimization has been cancelled.

TaskPtr task_: The task to be optimized.

StompConfiguration config_: Configuration parameters.

unsigned int current_iteration_: Current iteration for the optimization.

bool parameters_valid_: whether or not the optimized parameters are valid

bool parameters_valid_prev_: whether or not the optimized parameters from the previous iteration are valid

double parameters_total_cost_: Total cost of the optimized parameters.

double current_lowest_cost_: Hold the lowest cost of the optimized parameters.

Eigen::MatrixXd parameters_optimized_: A matrix [dimensions][timesteps] of the optimized parameters.

Eigen::MatrixXd parameters_updates_: A matrix [dimensions][timesteps] of the parameter updates.

Eigen::VectorXd parameters_state_costs_: A vector [timesteps] of the parameters state costs.

Eigen::MatrixXd parameters_control_costs_: A matrix [dimensions][timesteps] of the parameters control costs.

std::vector<Rollout> noisy_rollouts_: Holds the noisy rollouts.

std::vector<Rollout> reused_rollouts_: Used for reordering arrays based on cost.

int num_active_rollouts_: Number of active rollouts.

int num_timesteps_padded_: The number of timesteps to pad the optimization with: timesteps + 2*(FINITE_DIFF_RULE_LENGTH - 1)

int start_index_padded_: The index corresponding to the start of the non-paded section in the padded arrays.

Eigen::MatrixXd finite_diff_matrix_A_padded_: The finite difference matrix including padding.

Eigen::MatrixXd control_cost_matrix_R_padded_: The control cost matrix including padding.

Eigen::MatrixXd control_cost_matrix_R_: A matrix [timesteps][timesteps], Referred to as ‘R = A x A_transpose’ in the literature.

Eigen::MatrixXd inv_control_cost_matrix_R_: A matrix [timesteps][timesteps], R^-1 ‘ matrix.