Saves the update values into a file for post analysis. The file is compatible with the python numpy library and can be loaded into a numpy array by running. 'numpy.loadtxt(file_name)'. More...
#include <update_logger.h>
Public Member Functions | |
virtual bool | configure (const XmlRpc::XmlRpcValue &config) |
see base class for documentation | |
virtual void | done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd ¶meters) override |
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) override |
Store the updates values into a file that can be loaded into a numpy array. | |
virtual std::string | getGroupName () const override |
virtual std::string | getName () const override |
virtual bool | initialize (moveit::core::RobotModelConstPtr robot_model_ptr, const std::string &group_name, const XmlRpc::XmlRpcValue &config) |
see base class for documentation | |
virtual bool | setMotionPlanRequest (const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::MotionPlanRequest &req, const stomp_core::StompConfiguration &config, moveit_msgs::MoveItErrorCodes &error_code) |
see base class for documentation | |
UpdateLogger () | |
virtual | ~UpdateLogger () |
Protected Attributes | |
std::string | directory_ |
std::ofstream | file_stream_ |
std::string | filename_ |
Eigen::IOFormat | format_ |
std::string | full_file_name_ |
std::string | group_name_ |
std::string | name_ |
std::string | package_ |
stomp_core::StompConfiguration | stomp_config_ |
std::stringstream | stream_ |
Saves the update values into a file for post analysis. The file is compatible with the python numpy library and can be loaded into a numpy array by running. 'numpy.loadtxt(file_name)'.
Definition at line 47 of file update_logger.h.
stomp_moveit::update_filters::UpdateLogger::UpdateLogger | ( | ) |
Definition at line 41 of file update_logger.cpp.
stomp_moveit::update_filters::UpdateLogger::~UpdateLogger | ( | ) | [virtual] |
Definition at line 47 of file update_logger.cpp.
bool stomp_moveit::update_filters::UpdateLogger::configure | ( | const XmlRpc::XmlRpcValue & | config | ) | [virtual] |
see base class for documentation
Implements stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 60 of file update_logger.cpp.
void stomp_moveit::update_filters::UpdateLogger::done | ( | bool | success, |
int | total_iterations, | ||
double | final_cost, | ||
const Eigen::MatrixXd & | parameters | ||
) | [override, 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 from stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 137 of file update_logger.cpp.
bool stomp_moveit::update_filters::UpdateLogger::filter | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
const Eigen::MatrixXd & | parameters, | ||
Eigen::MatrixXd & | updates, | ||
bool & | filtered | ||
) | [override, virtual] |
Store the updates values into a file that can be loaded into a numpy array.
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 | The updates to be applied to the parameters [num_dimensions x num_timesteps] |
filtered | Always false as this filter never changes the updates values. |
Implements stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 126 of file update_logger.cpp.
virtual std::string stomp_moveit::update_filters::UpdateLogger::getGroupName | ( | ) | const [inline, override, virtual] |
Reimplemented from stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 84 of file update_logger.h.
virtual std::string stomp_moveit::update_filters::UpdateLogger::getName | ( | ) | const [inline, override, virtual] |
Reimplemented from stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 89 of file update_logger.h.
bool stomp_moveit::update_filters::UpdateLogger::initialize | ( | moveit::core::RobotModelConstPtr | robot_model_ptr, |
const std::string & | group_name, | ||
const XmlRpc::XmlRpcValue & | config | ||
) | [virtual] |
see base class for documentation
Implements stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 52 of file update_logger.cpp.
bool stomp_moveit::update_filters::UpdateLogger::setMotionPlanRequest | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const moveit_msgs::MotionPlanRequest & | req, | ||
const stomp_core::StompConfiguration & | config, | ||
moveit_msgs::MoveItErrorCodes & | error_code | ||
) | [virtual] |
see base class for documentation
Implements stomp_moveit::update_filters::StompUpdateFilter.
Definition at line 89 of file update_logger.cpp.
std::string stomp_moveit::update_filters::UpdateLogger::directory_ [protected] |
Definition at line 104 of file update_logger.h.
std::ofstream stomp_moveit::update_filters::UpdateLogger::file_stream_ [protected] |
Definition at line 112 of file update_logger.h.
std::string stomp_moveit::update_filters::UpdateLogger::filename_ [protected] |
Definition at line 102 of file update_logger.h.
Eigen::IOFormat stomp_moveit::update_filters::UpdateLogger::format_ [protected] |
Definition at line 113 of file update_logger.h.
std::string stomp_moveit::update_filters::UpdateLogger::full_file_name_ [protected] |
Definition at line 111 of file update_logger.h.
std::string stomp_moveit::update_filters::UpdateLogger::group_name_ [protected] |
Definition at line 99 of file update_logger.h.
std::string stomp_moveit::update_filters::UpdateLogger::name_ [protected] |
Definition at line 98 of file update_logger.h.
std::string stomp_moveit::update_filters::UpdateLogger::package_ [protected] |
Definition at line 103 of file update_logger.h.
stomp_core::StompConfiguration stomp_moveit::update_filters::UpdateLogger::stomp_config_ [protected] |
Definition at line 107 of file update_logger.h.
std::stringstream stomp_moveit::update_filters::UpdateLogger::stream_ [protected] |
Definition at line 110 of file update_logger.h.