#include <collision_proximity_planner.h>

Public Member Functions
void	clear ()
	CollisionProximityPlanner (const std::string &robot_description_name)
void	fillInGroupState (arm_navigation_msgs::RobotState &robot_state, const arm_navigation_msgs::RobotState &group_state)
bool	findPathToFreeState (const arm_navigation_msgs::RobotState &robot_state, arm_navigation_msgs::RobotTrajectory &robot_trajectory)
bool	getStateGradient (const arm_navigation_msgs::RobotState &group_state, double &distance, arm_navigation_msgs::RobotState &gradient)
	Given a robot state, get the gradient direction to be moved in.
bool	refineState (const arm_navigation_msgs::RobotState &group_state, arm_navigation_msgs::RobotTrajectory &robot_trajectory)
bool	setGroupState (const arm_navigation_msgs::RobotState &group_state)
	Set a group state. This must be called before you can make multiple queries to refineState below. This function can be used to define a mapping between the group state specified in the argument and the internal group state for more efficiency.
bool	setRobotState (const arm_navigation_msgs::RobotState &robot_state)
	Set the robot state that you want to check. Note that this locks the collision space and you will have to call clear() to unlock the space.
virtual	~CollisionProximityPlanner ()
Private Member Functions
bool	calculateCollisionIncrements (Eigen::MatrixXd &collision_increments, double &distance)
void	fillInGroupArray (const KDL::JntArray &jnt_array_group, const std::vector< int > &group_joint_to_kdl_joint_index, KDL::JntArray &jnt_array)
bool	getFreePath (arm_navigation_msgs::GetMotionPlan::Request &req, arm_navigation_msgs::GetMotionPlan::Response &res)
void	getGroupArray (const KDL::JntArray &jnt_array, const std::vector< int > &group_joint_to_kdl_joint_index, KDL::JntArray &jnt_array_group)
template<typename Derived >
void	getJacobian (const int &link_index, std::vector< Eigen::Map< Eigen::Vector3d > > &joint_pos, std::vector< Eigen::Map< Eigen::Vector3d > > &joint_axis, Eigen::Vector3d &collision_point_pos, Eigen::MatrixBase< Derived > &jacobian, const std::vector< int > &group_joint_to_kdl_joint_index) const
bool	initializeForGroup (const std::string &group_name)
void	isParentJoint (const int &link_index, const int &joint_index)
bool	isParentJoint (const int &link_index, const int &joint_index) const
void	kdlJointTrajectoryToRobotTrajectory (std::vector< KDL::JntArray > &jnt_trajectory, arm_navigation_msgs::RobotTrajectory &robot_trajectory)
bool	mapGroupState (const arm_navigation_msgs::RobotState &group_state, const std::vector< int > &mapping)
void	performForwardKinematics (KDL::JntArray &jnt_array, const bool &full)
void	setPlanningMonitorToCurrentState ()
void	updateCollisionProximitySpace (const arm_navigation_msgs::RobotState &group_state)
void	updateGroupRobotState (const KDL::JntArray &jnt_array)
void	updateJointState (KDL::JntArray &jnt_array, Eigen::MatrixXd &collision_increments)
Private Attributes
std::vector< std::vector< int > >	active_joints_
chomp::ChompRobotModel	chomp_robot_model_
Eigen::MatrixXd	collision_increments_
std::vector< KDL::Vector >	collision_point_pos_
std::vector< Eigen::Map < Eigen::Vector3d > >	collision_point_pos_eigen_
std::vector< Eigen::Vector3d >	collision_point_potential_gradient_
std::vector< double >	collision_point_vel_mag_
collision_proximity::CollisionProximitySpace *	cps_
std::vector< std::string >	current_attached_body_names_
std::vector< std::string >	current_link_names_
ros::Publisher	display_trajectory_publisher_
std::vector< int >	group_joint_to_kdl_joint_index_
std::string	group_name_cps_
std::vector< int >	group_state_joint_array_group_mapping_
Eigen::MatrixXd	jacobian_
Eigen::MatrixXd	jacobian_jacobian_tranpose_
Eigen::MatrixXd	jacobian_pseudo_inverse_
KDL::JntArray	jnt_array_
KDL::JntArray	jnt_array_group_
std::vector< int >	joint_array_group_group_state_mapping_
std::vector< KDL::Vector >	joint_axis_
std::vector< Eigen::Map < Eigen::Vector3d > >	joint_axis_eigen_
std::vector< KDL::Vector >	joint_pos_
std::vector< Eigen::Map < Eigen::Vector3d > >	joint_pos_eigen_
int	max_iterations_
double	max_joint_update_
int	num_joints_
const chomp::ChompRobotModel::ChompPlanningGroup *	planning_group_
ros::ServiceServer	planning_service_
ros::NodeHandle	private_handle_
std::string	reference_frame_
arm_navigation_msgs::RobotState	robot_state_group_
ros::NodeHandle	root_handle_
std::vector< KDL::Frame >	segment_frames_
bool	use_pseudo_inverse_
ros::Publisher	vis_marker_array_publisher_
ros::Publisher	vis_marker_publisher_

Detailed Description

Definition at line 72 of file collision_proximity_planner.h.

Constructor & Destructor Documentation

collision_proximity_planner::CollisionProximityPlanner::CollisionProximityPlanner ( const std::string & robot_description_name )

Definition at line 46 of file collision_proximity_planner.cpp.

collision_proximity_planner::CollisionProximityPlanner::~CollisionProximityPlanner ( ) [virtual]

Definition at line 72 of file collision_proximity_planner.cpp.

Member Function Documentation

bool collision_proximity_planner::CollisionProximityPlanner::calculateCollisionIncrements	(	Eigen::MatrixXd &	collision_increments,
		double &	distance
	)		`[private]`

Definition at line 341 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::clear ( void )

Definition at line 139 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::fillInGroupArray	(	const KDL::JntArray &	jnt_array_group,
		const std::vector< int > &	group_joint_to_kdl_joint_index,
		KDL::JntArray &	jnt_array
	)		`[private]`

Definition at line 170 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::fillInGroupState	(	arm_navigation_msgs::RobotState &	robot_state,
		const arm_navigation_msgs::RobotState &	group_state
	)

Definition at line 146 of file collision_proximity_planner.cpp.

bool collision_proximity_planner::CollisionProximityPlanner::findPathToFreeState	(	const arm_navigation_msgs::RobotState &	robot_state,
		arm_navigation_msgs::RobotTrajectory &	robot_trajectory
	)

Definition at line 178 of file collision_proximity_planner.cpp.

bool collision_proximity_planner::CollisionProximityPlanner::getFreePath	(	arm_navigation_msgs::GetMotionPlan::Request &	req,
		arm_navigation_msgs::GetMotionPlan::Response &	res
	)		`[private]`

Definition at line 108 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::getGroupArray	(	const KDL::JntArray &	jnt_array,
		const std::vector< int > &	group_joint_to_kdl_joint_index,
		KDL::JntArray &	jnt_array_group
	)		`[private]`

Definition at line 162 of file collision_proximity_planner.cpp.

template<typename Derived >

void collision_proximity_planner::CollisionProximityPlanner::getJacobian	(	const int &	link_index,
		std::vector< Eigen::Map< Eigen::Vector3d > > &	joint_pos,
		std::vector< Eigen::Map< Eigen::Vector3d > > &	joint_axis,
		Eigen::Vector3d &	collision_point_pos,
		Eigen::MatrixBase< Derived > &	jacobian,
		const std::vector< int > &	group_joint_to_kdl_joint_index
	)		const `[private]`

Definition at line 193 of file collision_proximity_planner.h.

bool collision_proximity_planner::CollisionProximityPlanner::getStateGradient	(	const arm_navigation_msgs::RobotState &	group_state,
		double &	distance,
		arm_navigation_msgs::RobotState &	gradient
	)

Given a robot state, get the gradient direction to be moved in.

Parameters:

joint_state_group The group state that needs to be refined

Returns:: True if a valid refinement was found, false otherwise

Definition at line 258 of file collision_proximity_planner.cpp.

bool collision_proximity_planner::CollisionProximityPlanner::initializeForGroup ( const std::string & group_name ) [private]

Definition at line 76 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::isParentJoint	(	const int &	link_index,
		const int &	joint_index
	)		`[private]`

bool collision_proximity_planner::CollisionProximityPlanner::isParentJoint	(	const int &	link_index,
		const int &	joint_index
	)		const `[inline, private]`

Definition at line 215 of file collision_proximity_planner.h.

void collision_proximity_planner::CollisionProximityPlanner::kdlJointTrajectoryToRobotTrajectory	(	std::vector< KDL::JntArray > &	jnt_trajectory,
		arm_navigation_msgs::RobotTrajectory &	robot_trajectory
	)		`[private]`

Definition at line 314 of file collision_proximity_planner.cpp.

bool collision_proximity_planner::CollisionProximityPlanner::mapGroupState	(	const arm_navigation_msgs::RobotState &	group_state,
		const std::vector< int > &	mapping
	)		`[private]`

Definition at line 245 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::performForwardKinematics	(	KDL::JntArray &	jnt_array,
		const bool &	full
	)		`[private]`

Definition at line 398 of file collision_proximity_planner.cpp.

bool collision_proximity_planner::CollisionProximityPlanner::refineState	(	const arm_navigation_msgs::RobotState &	group_state,
		arm_navigation_msgs::RobotTrajectory &	robot_trajectory
	)

Definition at line 276 of file collision_proximity_planner.cpp.

bool collision_proximity_planner::CollisionProximityPlanner::setGroupState ( const arm_navigation_msgs::RobotState & group_state )

Set a group state. This must be called before you can make multiple queries to refineState below. This function can be used to define a mapping between the group state specified in the argument and the internal group state for more efficiency.

Parameters:

group_state

Definition at line 224 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::setPlanningMonitorToCurrentState ( ) [private]

bool collision_proximity_planner::CollisionProximityPlanner::setRobotState ( const arm_navigation_msgs::RobotState & robot_state )

Set the robot state that you want to check. Note that this locks the collision space and you will have to call clear() to unlock the space.

Parameters:

robot_state The full robot state.

Returns:: True if setting robot state was successful

void collision_proximity_planner::CollisionProximityPlanner::updateCollisionProximitySpace ( const arm_navigation_msgs::RobotState & group_state ) [private]

Definition at line 335 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::updateGroupRobotState ( const KDL::JntArray & jnt_array ) [private]

Definition at line 330 of file collision_proximity_planner.cpp.

void collision_proximity_planner::CollisionProximityPlanner::updateJointState	(	KDL::JntArray &	jnt_array,
		Eigen::MatrixXd &	collision_increments
	)		`[private]`