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

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

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

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

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

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

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

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

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

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 MoveIt! 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.

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
	)

overridevirtual

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 stomp_core::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
	)

overridevirtual

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 stomp_core::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
	)

overridevirtual

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 stomp_core::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
	)

overridevirtual

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 stomp_core::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
	)

overridevirtual

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 stomp_core::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
	)

overridevirtual

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 stomp_core::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 stomp_core::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

Returns: true if succeeded,false otherwise.

Definition at line 317 of file stomp_optimization_task.cpp.

Member Data Documentation

NoiseGeneratorLoaderPtr stomp_moveit::StompOptimizationTask::noise_generator_loader_

protected

Arrays containing the loaded plugins >

Definition at line 211 of file stomp_optimization_task.h.

planning_scene::PlanningSceneConstPtr stomp_moveit::StompOptimizationTask::planning_scene_ptr_

protected

The plugin loaders for each type of plugin supported>

Definition at line 205 of file stomp_optimization_task.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation