Class ComposableTask
Defined in File stomp_moveit_task.hpp
Inheritance Relationships
Base Type
public stomp::Task
Class Documentation
-
class ComposableTask : public stomp::Task
Public Functions
-
inline ComposableTask(NoiseGeneratorFn noise_generator_fn, CostFn cost_fn, FilterFn filter_fn, PostIterationFn post_iteration_fn, DoneFn done_fn)
-
~ComposableTask() = default
-
inline bool generateNoisyParameters(const Eigen::MatrixXd ¶meters, std::size_t, std::size_t, int, int, Eigen::MatrixXd ¶meters_noise, Eigen::MatrixXd &noise) override
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
-
inline bool computeCosts(const Eigen::MatrixXd ¶meters, std::size_t, std::size_t, int, Eigen::VectorXd &costs, bool &validity) override
computes the state costs as a function of the distance from the bias parameters
- 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 bool computeNoisyCosts(const Eigen::MatrixXd ¶meters, std::size_t, std::size_t, int, int, Eigen::VectorXd &costs, bool &validity) override
computes the state costs as a function of the distance from the bias parameters
- 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.
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 bool filterParameterUpdates(std::size_t, std::size_t, int, const Eigen::MatrixXd ¶meters, Eigen::MatrixXd &updates) override
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 void postIteration(std::size_t, std::size_t, int iteration_number, double cost, const Eigen::MatrixXd ¶meters) override
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 void done(bool success, int total_iterations, double final_cost, const Eigen::MatrixXd ¶meters) override
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]
-
inline ComposableTask(NoiseGeneratorFn noise_generator_fn, CostFn cost_fn, FilterFn filter_fn, PostIterationFn post_iteration_fn, DoneFn done_fn)