Evaluates the cost of the goal pose by determining how far the Cartesian tool pose is from the desired under-constrained task manifold. More...

#include <tool_goal_pose.h>

Inheritance diagram for stomp_moveit::cost_functions::ToolGoalPose:

[legend]

Public Member Functions
virtual bool	computeCosts (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 goal state costs as a function of the distance from the desired task manifold. More...

virtual bool	configure (const XmlRpc::XmlRpcValue &config) override
	Sets internal members of the plugin from the configuration data. More...

virtual void	done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd &parameters) override
	Called by the Stomp Task at the end of the optimization process. More...

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, XmlRpc::XmlRpcValue &config) override
	Initializes and configures the Cost Function. 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) override
	Stores the planning details which will be used during the costs calculations. More...

	ToolGoalPose ()

virtual	~ToolGoalPose ()

Public Member Functions inherited from stomp_moveit::cost_functions::StompCostFunction
virtual int	getOptimizedIndex () const

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

Protected Attributes
std::string	group_name_

Eigen::VectorXd	last_joint_pose_

Eigen::Affine3d	last_tool_pose_

Eigen::VectorXd	max_twist_error_

Eigen::VectorXd	min_twist_error_

std::string	name_

double	orientation_cost_weight_
	factor multiplied to the scaled orientation error More...

moveit_msgs::MotionPlanRequest	plan_request_

planning_scene::PlanningSceneConstPtr	planning_scene_

double	position_cost_weight_
	factor multiplied to the scaled position error More...

moveit::core::RobotModelConstPtr	robot_model_

moveit::core::RobotStatePtr	state_

Eigen::Affine3d	tool_goal_pose_
	The desired goal pose for the active plan request. More...

Eigen::VectorXd	tool_goal_tolerance_

std::string	tool_link_

Eigen::VectorXd	tool_twist_error_

Detailed Description

Evaluates the cost of the goal pose by determining how far the Cartesian tool pose is from the desired under-constrained task manifold.

Examples:: All examples are located here STOMP Plugins Examples

Definition at line 47 of file tool_goal_pose.h.

Constructor & Destructor Documentation

stomp_moveit::cost_functions::ToolGoalPose::ToolGoalPose ( )

Definition at line 45 of file tool_goal_pose.cpp.

stomp_moveit::cost_functions::ToolGoalPose::~ToolGoalPose ( )

virtual

Definition at line 52 of file tool_goal_pose.cpp.

Member Function Documentation

bool stomp_moveit::cost_functions::ToolGoalPose::computeCosts	(	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 goal state costs as a function of the distance from the desired task manifold.

Parameters

parameters	The parameter values to evaluate for state costs [num_dimensions x num_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 whose cost is being evaluated. *
costs	vector containing the state costs per timestep. Only the array's last entry is set. [num_parameters x 1]
validity	whether or not the trajectory is valid

Returns: true if cost were properly computed

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 175 of file tool_goal_pose.cpp.

bool stomp_moveit::cost_functions::ToolGoalPose::configure ( const XmlRpc::XmlRpcValue & config )

overridevirtual

Sets internal members of the plugin from the configuration data.

Parameters

config The configuration data . Usually loaded from the ros parameter server

Returns: true if succeeded, false otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 66 of file tool_goal_pose.cpp.

void stomp_moveit::cost_functions::ToolGoalPose::done	(	bool	success,
		int	total_iterations,
		double	final_cost,
		const Eigen::MatrixXd &	parameters
	)

overridevirtual

Called by the Stomp Task 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_moveit::cost_functions::StompCostFunction.

Definition at line 230 of file tool_goal_pose.cpp.

virtual std::string stomp_moveit::cost_functions::ToolGoalPose::getGroupName ( ) const

inlineoverridevirtual

Definition at line 103 of file tool_goal_pose.h.

virtual std::string stomp_moveit::cost_functions::ToolGoalPose::getName ( ) const

inlineoverridevirtual

Definition at line 109 of file tool_goal_pose.h.

bool stomp_moveit::cost_functions::ToolGoalPose::initialize	(	moveit::core::RobotModelConstPtr	robot_model_ptr,
		const std::string &	group_name,
		XmlRpc::XmlRpcValue &	config
	)

overridevirtual

Initializes and configures the Cost Function.

Parameters

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

Returns: true if succeeded, false otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 57 of file tool_goal_pose.cpp.

bool stomp_moveit::cost_functions::ToolGoalPose::setMotionPlanRequest	(	const planning_scene::PlanningSceneConstPtr &	planning_scene,
		const moveit_msgs::MotionPlanRequest &	req,
		const stomp_core::StompConfiguration &	config,
		moveit_msgs::MoveItErrorCodes &	error_code
	)

overridevirtual

Stores the planning details which will be used during the costs calculations.

Parameters

planning_scene	A smart pointer to the planning scene
req	The motion planning request
config	The Stomp configuration.
error_code	Moveit error code.

Returns: true if succeeded, false otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 90 of file tool_goal_pose.cpp.

Member Data Documentation

std::string stomp_moveit::cost_functions::ToolGoalPose::group_name_

protected

Definition at line 129 of file tool_goal_pose.h.

Eigen::VectorXd stomp_moveit::cost_functions::ToolGoalPose::last_joint_pose_

protected

Definition at line 149 of file tool_goal_pose.h.

Eigen::Affine3d stomp_moveit::cost_functions::ToolGoalPose::last_tool_pose_

protected

Definition at line 150 of file tool_goal_pose.h.

Eigen::VectorXd stomp_moveit::cost_functions::ToolGoalPose::max_twist_error_

protected

Definition at line 142 of file tool_goal_pose.h.

Eigen::VectorXd stomp_moveit::cost_functions::ToolGoalPose::min_twist_error_

protected

Definition at line 141 of file tool_goal_pose.h.

std::string stomp_moveit::cost_functions::ToolGoalPose::name_

protected

Definition at line 126 of file tool_goal_pose.h.

double stomp_moveit::cost_functions::ToolGoalPose::orientation_cost_weight_

protected

factor multiplied to the scaled orientation error

Definition at line 146 of file tool_goal_pose.h.

moveit_msgs::MotionPlanRequest stomp_moveit::cost_functions::ToolGoalPose::plan_request_

protected

Definition at line 136 of file tool_goal_pose.h.

planning_scene::PlanningSceneConstPtr stomp_moveit::cost_functions::ToolGoalPose::planning_scene_

protected

Definition at line 135 of file tool_goal_pose.h.

double stomp_moveit::cost_functions::ToolGoalPose::position_cost_weight_

protected

factor multiplied to the scaled position error

Definition at line 145 of file tool_goal_pose.h.

moveit::core::RobotModelConstPtr stomp_moveit::cost_functions::ToolGoalPose::robot_model_

protected

Definition at line 131 of file tool_goal_pose.h.

moveit::core::RobotStatePtr stomp_moveit::cost_functions::ToolGoalPose::state_

protected

Definition at line 132 of file tool_goal_pose.h.

Eigen::Affine3d stomp_moveit::cost_functions::ToolGoalPose::tool_goal_pose_

protected

The desired goal pose for the active plan request.

Definition at line 139 of file tool_goal_pose.h.

Eigen::VectorXd stomp_moveit::cost_functions::ToolGoalPose::tool_goal_tolerance_

protected

Definition at line 140 of file tool_goal_pose.h.

std::string stomp_moveit::cost_functions::ToolGoalPose::tool_link_

protected

Definition at line 130 of file tool_goal_pose.h.

Eigen::VectorXd stomp_moveit::cost_functions::ToolGoalPose::tool_twist_error_

protected

Definition at line 151 of file tool_goal_pose.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