Class Task

Class Documentation

class Task

Defines the STOMP improvement policy.

Public Functions

inline Task()
virtual bool 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) = 0

Generates a noisy trajectory from the parameters.

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

Returns:

True if cost were properly computed, otherwise false

virtual bool 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) = 0

computes the state costs as a function of the noisy parameters for each time step.

Parameters:
  • 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

Returns:

True if cost were properly computed, otherwise false

virtual bool computeCosts(const Eigen::MatrixXd &parameters, 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.

Parameters:
  • 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

Returns:

True if cost were properly computed, otherwise false

inline virtual bool filterNoisyParameters(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd &parameters, 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.

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 rollout index for this noisy parameter set

  • parameters – The noisy parameters *

  • filtered – False if no filtering was done

Returns:

False if no filtering was done, otherwise true

inline virtual bool filterParameterUpdates(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, const Eigen::MatrixXd &parameters, 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.

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

  • parameters – The optimized parameters

  • updates – The updates to the parameters

Returns:

True if successful, otherwise false

inline virtual void postIteration(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, double cost, const Eigen::MatrixXd &parameters)

Called by STOMP at the end of each iteration.

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

  • 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].

inline virtual void done(bool success, int total_iterations, double final_cost, const Eigen::MatrixXd &parameters)

Called by Stomp at the end of the optimization process.

Parameters:
  • 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]