Interface class which applies filtering methods to the update parameters before it is added onto the optimized trajectory. More...
#include <stomp_update_filter.h>
Public Member Functions | |
virtual bool | configure (const XmlRpc::XmlRpcValue &config)=0 |
Sets internal members of the plugin from the configuration data. | |
virtual void | done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd ¶meters) |
Called by the Stomp at the end of the optimization process. | |
virtual bool | filter (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, const Eigen::MatrixXd ¶meters, Eigen::MatrixXd &updates, bool &filtered)=0 |
Filters the parameter updates and may change its values. | |
virtual std::string | getGroupName () const |
virtual std::string | getName () const |
virtual bool | initialize (moveit::core::RobotModelConstPtr robot_model_ptr, const std::string &group_name, const XmlRpc::XmlRpcValue &config)=0 |
Initializes and configures. | |
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 | setMotionPlanRequest (const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::MotionPlanRequest &req, const stomp_core::StompConfiguration &config, moveit_msgs::MoveItErrorCodes &error_code)=0 |
Stores the planning details. | |
StompUpdateFilter () | |
virtual | ~StompUpdateFilter () |
Interface class which applies filtering methods to the update parameters before it is added onto the optimized trajectory.
Definition at line 55 of file stomp_update_filter.h.
stomp_moveit::update_filters::StompUpdateFilter::StompUpdateFilter | ( | ) | [inline] |
Definition at line 58 of file stomp_update_filter.h.
virtual stomp_moveit::update_filters::StompUpdateFilter::~StompUpdateFilter | ( | ) | [inline, virtual] |
Definition at line 59 of file stomp_update_filter.h.
virtual bool stomp_moveit::update_filters::StompUpdateFilter::configure | ( | const XmlRpc::XmlRpcValue & | config | ) | [pure virtual] |
Sets internal members of the plugin from the configuration data.
config | The configuration data. Usually loaded from the ros parameter server |
Implemented in stomp_moveit::update_filters::PolynomialSmoother, stomp_moveit::update_filters::TrajectoryVisualization, stomp_moveit::update_filters::ControlCostProjection, and stomp_moveit::update_filters::UpdateLogger.
virtual void stomp_moveit::update_filters::StompUpdateFilter::done | ( | bool | success, |
int | total_iterations, | ||
double | final_cost, | ||
const Eigen::MatrixXd & | parameters | ||
) | [inline, virtual] |
Called by the Stomp at the end of the optimization process.
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 current iteration [num_dimensions x num_timesteps] |
Reimplemented in stomp_moveit::update_filters::TrajectoryVisualization, and stomp_moveit::update_filters::UpdateLogger.
Definition at line 128 of file stomp_update_filter.h.
virtual bool stomp_moveit::update_filters::StompUpdateFilter::filter | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
const Eigen::MatrixXd & | parameters, | ||
Eigen::MatrixXd & | updates, | ||
bool & | filtered | ||
) | [pure virtual] |
Filters the parameter updates and may change its values.
start_timestep | start index into the 'parameters' array, usually 0. |
num_timesteps | number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop |
parameters | The parameters generated in the previous iteration [num_dimensions x num_timesteps] |
updates | Output argument which contains the updates to be applied to the parameters [num_dimensions x num_timesteps] |
filtered | Output argument which is set to 'true' if the updates were modified. |
Implemented in stomp_moveit::update_filters::PolynomialSmoother, stomp_moveit::update_filters::TrajectoryVisualization, stomp_moveit::update_filters::ControlCostProjection, and stomp_moveit::update_filters::UpdateLogger.
virtual std::string stomp_moveit::update_filters::StompUpdateFilter::getGroupName | ( | ) | const [inline, virtual] |
Reimplemented in stomp_moveit::update_filters::TrajectoryVisualization, stomp_moveit::update_filters::PolynomialSmoother, stomp_moveit::update_filters::ControlCostProjection, and stomp_moveit::update_filters::UpdateLogger.
Definition at line 137 of file stomp_update_filter.h.
virtual std::string stomp_moveit::update_filters::StompUpdateFilter::getName | ( | ) | const [inline, virtual] |
Reimplemented in stomp_moveit::update_filters::PolynomialSmoother, stomp_moveit::update_filters::TrajectoryVisualization, stomp_moveit::update_filters::ControlCostProjection, and stomp_moveit::update_filters::UpdateLogger.
Definition at line 131 of file stomp_update_filter.h.
virtual bool stomp_moveit::update_filters::StompUpdateFilter::initialize | ( | moveit::core::RobotModelConstPtr | robot_model_ptr, |
const std::string & | group_name, | ||
const XmlRpc::XmlRpcValue & | config | ||
) | [pure virtual] |
Initializes and configures.
robot_model_ptr | A pointer to the robot model. |
group_name | The designated planning group. |
config | The configuration data. Usually loaded from the ros parameter server |
Implemented in stomp_moveit::update_filters::PolynomialSmoother, stomp_moveit::update_filters::TrajectoryVisualization, stomp_moveit::update_filters::ControlCostProjection, and stomp_moveit::update_filters::UpdateLogger.
virtual void stomp_moveit::update_filters::StompUpdateFilter::postIteration | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
double | cost, | ||
const Eigen::MatrixXd & | parameters | ||
) | [inline, virtual] |
Called by STOMP at the end of each iteration.
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]. |
Definition at line 117 of file stomp_update_filter.h.
virtual bool stomp_moveit::update_filters::StompUpdateFilter::setMotionPlanRequest | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const moveit_msgs::MotionPlanRequest & | req, | ||
const stomp_core::StompConfiguration & | config, | ||
moveit_msgs::MoveItErrorCodes & | error_code | ||
) | [pure virtual] |
Stores the planning details.
planning_scene | A smart pointer to the planning scene |
req | The motion planning request |
config | The Stomp configuration. |
error_code | Moveit error code. |
Implemented in stomp_moveit::update_filters::PolynomialSmoother, stomp_moveit::update_filters::TrajectoryVisualization, stomp_moveit::update_filters::ControlCostProjection, and stomp_moveit::update_filters::UpdateLogger.