Loads and manages the STOMP plugins during the planning process. More...

#include <stomp_optimization_task.h>

Inheritance diagram for stomp_moveit::StompOptimizationTask:

Public Member Functions
virtual bool	computeCosts (const Eigen::MatrixXd &parameters, 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. It does this by calling the loaded Cost Function plugins
virtual bool	computeNoisyCosts (const Eigen::MatrixXd &parameters, 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. It does this by calling the loaded Cost Function plugins
virtual void	done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd &parameters) override
	Called by Stomp at the end of the optimization process.
virtual bool	filterNoisyParameters (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd &parameters, bool &filtered) override
	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. It accomplishes this by calling the loaded Noisy Filter plugins.
virtual bool	filterParameterUpdates (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, const Eigen::MatrixXd &parameters, Eigen::MatrixXd &updates) override
	Filters the given parameters which is applied after calculating the update. It could be used for clipping of joint limits or projecting the goal pose into the null space of the Jacobian. It accomplishes this by calling the loaded Update Filter plugins.
virtual bool	generateNoisyParameters (const Eigen::MatrixXd &parameters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd &parameters_noise, Eigen::MatrixXd &noise) override
	Generates a noisy trajectory from the parameters by calling the active Noise Generator plugin.
virtual void	postIteration (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, double cost, const Eigen::MatrixXd &parameters)
	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)
	Passes the planning details down to each loaded plugin.
	StompOptimizationTask (moveit::core::RobotModelConstPtr robot_model_ptr, std::string group_name, const XmlRpc::XmlRpcValue &config)
	Constructor.
virtual	~StompOptimizationTask ()
Protected Attributes
CostFuctionLoaderPtr	cost_function_loader_
std::vector < cost_functions::StompCostFunctionPtr >	cost_functions_
std::string	group_name_
NoiseGeneratorLoaderPtr	noise_generator_loader_
std::vector < noise_generators::StompNoiseGeneratorPtr >	noise_generators_
NoisyFilterLoaderPtr	noisy_filter_loader_
std::vector < noisy_filters::StompNoisyFilterPtr >	noisy_filters_
planning_scene::PlanningSceneConstPtr	planning_scene_ptr_
moveit::core::RobotModelConstPtr	robot_model_ptr_
UpdateFilterLoaderPtr	update_filter_loader_
std::vector < update_filters::StompUpdateFilterPtr >	update_filters_

Detailed Description

Loads and manages the STOMP plugins during the planning process.

Examples:: All examples are located here stomp_core examples

Definition at line 62 of file stomp_optimization_task.h.

Constructor & Destructor Documentation

stomp_moveit::StompOptimizationTask::StompOptimizationTask	(	moveit::core::RobotModelConstPtr	robot_model_ptr,
		std::string	group_name,
		const XmlRpc::XmlRpcValue &	config
	)

Constructor.

Parameters:

robot_model_ptr	A pointer to the robot model
group_name	The planning group name
config	The configuration parameter data

Definition at line 180 of file stomp_optimization_task.cpp.

stomp_moveit::StompOptimizationTask::~StompOptimizationTask ( ) [virtual]

Definition at line 243 of file stomp_optimization_task.cpp.

Member Function Documentation

bool stomp_moveit::StompOptimizationTask::computeCosts	(	const Eigen::MatrixXd &	parameters,
		std::size_t	start_timestep,
		std::size_t	num_timesteps,
		int	iteration_number,
		Eigen::VectorXd &	costs,
		bool &	validity
	)		`[override, virtual]`

computes the state costs as a function of the optimized parameters for each time step. It does this by calling the loaded Cost Function plugins

Parameters:

parameters	[num_dimensions] num_parameters - policy parameters to execute
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
costs	vector containing the state costs per timestep.
validity	whether or not the trajectory is valid

Returns:: false if there was an irrecoverable failure, true otherwise.

Implements Task.

Definition at line 289 of file stomp_optimization_task.cpp.

bool stomp_moveit::StompOptimizationTask::computeNoisyCosts	(	const Eigen::MatrixXd &	parameters,
		std::size_t	start_timestep,
		std::size_t	num_timesteps,
		int	iteration_number,
		int	rollout_number,
		Eigen::VectorXd &	costs,
		bool &	validity
	)		`[override, virtual]`

computes the state costs as a function of the noisy parameters for each time step. It does this by calling the loaded Cost Function plugins

Parameters:

parameters	[num_dimensions] num_parameters - policy parameters to execute
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
rollout_number	index of the noisy trajectory whose cost is being evaluated.
costs	vector containing the state costs per timestep.
validity	whether or not the trajectory is valid

Returns:: false if there was an irrecoverable failure, true otherwise.

Implements Task.

Definition at line 260 of file stomp_optimization_task.cpp.

void stomp_moveit::StompOptimizationTask::done	(	bool	success,
		int	total_iterations,
		double	final_cost,
		const Eigen::MatrixXd &	parameters
	)		`[override, virtual]`

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 current iteration[num_dimensions x num_timesteps]

Reimplemented from Task.

Definition at line 429 of file stomp_optimization_task.cpp.

bool stomp_moveit::StompOptimizationTask::filterNoisyParameters	(	std::size_t	start_timestep,
		std::size_t	num_timesteps,
		int	iteration_number,
		int	rollout_number,
		Eigen::MatrixXd &	parameters,
		bool &	filtered
	)		`[override, virtual]`

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. It accomplishes this by calling the loaded Noisy Filter plugins.

Parameters:

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
rollout_number	The rollout index for this noisy parameter set
parameters	The noisy parameters *
filtered	False if no filtering was done

Returns:: false if there was an irrecoverable failure, true otherwise.

Reimplemented from Task.

Definition at line 361 of file stomp_optimization_task.cpp.

bool stomp_moveit::StompOptimizationTask::filterParameterUpdates	(	std::size_t	start_timestep,
		std::size_t	num_timesteps,
		int	iteration_number,
		const Eigen::MatrixXd &	parameters,
		Eigen::MatrixXd &	updates
	)		`[override, virtual]`

Filters the given parameters which is applied after calculating the update. It could be used for clipping of joint limits or projecting the goal pose into the null space of the Jacobian. It accomplishes this by calling the loaded Update Filter plugins.

Parameters:

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 optimized parameters
updates	The updates to the parameters

Returns:: false if there was an irrecoverable failure, true otherwise.

Reimplemented from Task.

Definition at line 383 of file stomp_optimization_task.cpp.

bool stomp_moveit::StompOptimizationTask::generateNoisyParameters	(	const Eigen::MatrixXd &	parameters,
		std::size_t	start_timestep,
		std::size_t	num_timesteps,
		int	iteration_number,
		int	rollout_number,
		Eigen::MatrixXd &	parameters_noise,
		Eigen::MatrixXd &	noise
	)		`[override, virtual]`

Generates a noisy trajectory from the parameters by calling the active Noise Generator plugin.

Parameters:

parameters	[num_dimensions] x [num_parameters] the current value of the optimized parameters
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
rollout_number	index of the noisy trajectory.
parameters_noise	the parameters + noise
noise	the noise applied to the parameters

Returns:: false if there was an irrecoverable failure, true otherwise.

Implements Task.

Definition at line 248 of file stomp_optimization_task.cpp.

void stomp_moveit::StompOptimizationTask::postIteration	(	std::size_t	start_timestep,
		std::size_t	num_timesteps,
		int	iteration_number,
		double	cost,
		const Eigen::MatrixXd &	parameters
	)		`[virtual]`

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].

Reimplemented from Task.

Definition at line 405 of file stomp_optimization_task.cpp.

bool stomp_moveit::StompOptimizationTask::setMotionPlanRequest	(	const planning_scene::PlanningSceneConstPtr &	planning_scene,
		const moveit_msgs::MotionPlanRequest &	req,
		const stomp_core::StompConfiguration &	config,
		moveit_msgs::MoveItErrorCodes &	error_code
	)		`[virtual]`

Passes the planning details down to each loaded plugin.

Parameters:

planning_scene	A smart pointer to the planning scene
req	The motion planning request
config	The Stomp configuration, usually loaded from the ros parameter server
error_code	Moveit error code