30 #include <boost/shared_ptr.hpp> 32 #include "stomp_core/utils.h" 38 typedef std::shared_ptr<Task>
TaskPtr;
60 std::size_t start_timestep,
61 std::size_t num_timesteps,
64 Eigen::MatrixXd& parameters_noise,
65 Eigen::MatrixXd& noise) = 0;
79 std::size_t start_timestep,
80 std::size_t num_timesteps,
83 Eigen::VectorXd& costs,
96 virtual bool computeCosts(
const Eigen::MatrixXd& parameters,
97 std::size_t start_timestep,
98 std::size_t num_timesteps,
100 Eigen::VectorXd& costs,
101 bool& validity) = 0 ;
116 std::size_t num_timesteps,
117 int iteration_number,
119 Eigen::MatrixXd& parameters,
138 std::size_t num_timesteps,
139 int iteration_number,
140 const Eigen::MatrixXd& parameters,
141 Eigen::MatrixXd& updates)
155 std::size_t num_timesteps,
int iteration_number,
double cost,
const Eigen::MatrixXd& parameters){}
166 virtual void done(
bool success,
int total_iterations,
double final_cost,
const Eigen::MatrixXd& parameters){}
std::shared_ptr< Task > TaskPtr
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 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 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 join...
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 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 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.
Defines the STOMP improvement policy.