Assigns a cost value to each robot state by evaluating if the robot is in collision. More...

#include <collision_check.h>

Inheritance diagram for stomp_moveit::cost_functions::CollisionCheck:

Public Member Functions
	CollisionCheck ()
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 state costs by checking whether the robot is in collision at each time step.
virtual bool	configure (const XmlRpc::XmlRpcValue &config) override
	Sets internal members of the plugin from the configuration data.
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.
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. Calls the configure method and passes the 'config' value.
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.
virtual	~CollisionCheck ()
Protected Member Functions
bool	checkIntermediateCollisions (const Eigen::VectorXd &start, const Eigen::VectorXd &end, double longest_valid_joint_move)
	Checks for collision between consecutive points by dividing the joint move into sub-moves where the maximum joint motion can not exceed the longest_valid_joint_move value.
Protected Attributes
double	collision_penalty_
	The value assigned to a collision state.
collision_detection::CollisionRequest	collision_request_
collision_detection::CollisionRobotConstPtr	collision_robot_
collision_detection::CollisionWorldConstPtr	collision_world_
std::string	group_name_
std::array < moveit::core::RobotStatePtr, 3 >	intermediate_coll_states_
	Used in checking collisions between to consecutive poses.
Eigen::ArrayXd	intermediate_costs_slots_
double	kernel_window_percentage_
	The value assigned to a collision state.
double	longest_valid_joint_move_
	how far can a joint move in between consecutive trajectory points
std::string	name_
moveit_msgs::MotionPlanRequest	plan_request_
planning_scene::PlanningSceneConstPtr	planning_scene_
Eigen::VectorXd	raw_costs_
moveit::core::RobotModelConstPtr	robot_model_ptr_
moveit::core::RobotStatePtr	robot_state_

Detailed Description

Assigns a cost value to each robot state by evaluating if the robot is in collision.

Examples:: All examples are located here stomp_core examples

Definition at line 45 of file collision_check.h.

Constructor & Destructor Documentation

stomp_moveit::cost_functions::CollisionCheck::CollisionCheck ( )

Definition at line 126 of file collision_check.cpp.

stomp_moveit::cost_functions::CollisionCheck::~CollisionCheck ( ) [virtual]

Definition at line 135 of file collision_check.cpp.

Member Function Documentation

bool stomp_moveit::cost_functions::CollisionCheck::checkIntermediateCollisions	(	const Eigen::VectorXd &	start,
		const Eigen::VectorXd &	end,
		double	longest_valid_joint_move
	)		`[protected]`

Checks for collision between consecutive points by dividing the joint move into sub-moves where the maximum joint motion can not exceed the longest_valid_joint_move value.

Parameters:

start	The start joint pose
end	The end joint pose
longest_valid_joint_move	The maximum distance that the joints are allowed to move before checking for collisions.

Returns:: True if the interval is collision free, false otherwise.

Definition at line 315 of file collision_check.cpp.

bool stomp_moveit::cost_functions::CollisionCheck::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, virtual]`

computes the state costs by checking whether the robot is in collision at each time step.

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. Sets '0' to all collision-free states.
validity	whether or not the trajectory is valid.

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

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 192 of file collision_check.cpp.

bool stomp_moveit::cost_functions::CollisionCheck::configure ( const XmlRpc::XmlRpcValue & config ) [override, virtual]

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 361 of file collision_check.cpp.

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

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 403 of file collision_check.cpp.

virtual std::string stomp_moveit::cost_functions::CollisionCheck::getGroupName ( ) const [inline, override, virtual]

Reimplemented from stomp_moveit::cost_functions::StompCostFunction.

Definition at line 101 of file collision_check.h.

virtual std::string stomp_moveit::cost_functions::CollisionCheck::getName ( ) const [inline, override, virtual]

Reimplemented from stomp_moveit::cost_functions::StompCostFunction.

Definition at line 107 of file collision_check.h.

bool stomp_moveit::cost_functions::CollisionCheck::initialize	(	moveit::core::RobotModelConstPtr	robot_model_ptr,
		const std::string &	group_name,
		XmlRpc::XmlRpcValue &	config
	)		`[override, virtual]`

Initializes and configures the Cost Function. Calls the configure method and passes the 'config' value.

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 140 of file collision_check.cpp.

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