A dummy task for testing STOMP. More...
Public Member Functions | |
bool | computeCosts (const Trajectory ¶meters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, Eigen::VectorXd &costs, bool &validity) override |
computes the state costs as a function of the optimized parameters for each time step. More... | |
bool | computeNoisyCosts (const Trajectory ¶meters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::VectorXd &costs, bool &validity) override |
computes the state costs as a function of the noisy parameters for each time step. More... | |
DummyTask (const Trajectory ¶meters_bias, const std::vector< double > &bias_thresholds, const std::vector< double > &std_dev) | |
A dummy task for testing Stomp. More... | |
bool | filterParameterUpdates (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, 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. More... | |
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) override |
See base clase for documentation. | |
Public Member Functions inherited from stomp_core::Task | |
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. More... | |
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. More... | |
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. More... | |
Protected Member Functions | |
bool | smoothParameterUpdates (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, Eigen::MatrixXd &updates) |
Perform a smooth update given a noisy update. More... | |
Protected Attributes | |
std::vector< double > | bias_thresholds_ |
Trajectory | parameters_bias_ |
Eigen::MatrixXd | smoothing_M_ |
std::vector< double > | std_dev_ |
A dummy task for testing STOMP.
Definition at line 46 of file stomp_3dof.cpp.
|
inline |
A dummy task for testing Stomp.
parameters_bias | default parameter bias used for computing cost for the test |
bias_thresholds | threshold to determine whether two trajectories are equal |
std_dev | standard deviation used for generating noisy parameters |
Definition at line 55 of file stomp_3dof.cpp.
|
inlineoverridevirtual |
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 |
Implements stomp_core::Task.
Definition at line 95 of file stomp_3dof.cpp.
|
inlineoverridevirtual |
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 |
Implements stomp_core::Task.
Definition at line 105 of file stomp_3dof.cpp.
|
inlineoverridevirtual |
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 from stomp_core::Task.
Definition at line 139 of file stomp_3dof.cpp.
|
inlineprotected |
Perform a smooth update given a noisy update.
start_timestep | starting timestep |
num_timesteps | number of timesteps |
iteration_number | number of interations allowed |
updates | returned smooth update |
Definition at line 159 of file stomp_3dof.cpp.
|
protected |
Threshold to determine whether two trajectories are equal
Definition at line 176 of file stomp_3dof.cpp.
|
protected |
Parameter bias used for computing cost for the test
Definition at line 175 of file stomp_3dof.cpp.
|
protected |
Matrix used for smoothing the trajectory
Definition at line 178 of file stomp_3dof.cpp.
|
protected |
Standard deviation used for generating noisy parameters
Definition at line 177 of file stomp_3dof.cpp.