#include <chomp_optimizer.h>

Public Member Functions
	ChompOptimizer (ChompTrajectory trajectory, planning_models::KinematicModel robot_model, const std::string &planning_group, const ChompParameters parameters, const ros::Publisher &vis_marker_array_publisher, const ros::Publisher &vis_marker_publisher, collision_proximity::CollisionProximitySpace collision_space)
void	destroy ()
template<typename Derived >
void	getJacobian (int trajectoryPoint, Vector3d &collision_point_pos, string &jointName, MatrixBase< Derived > &jacobian) const
void	optimize ()
virtual	~ChompOptimizer ()
Private Member Functions
void	addIncrementsToTrajectory ()
void	animateEndeffector ()
void	animatePath ()
void	calculateCollisionIncrements ()
void	calculatePseudoInverse ()
void	calculateSmoothnessIncrements ()
void	calculateTotalIncrements ()
collision_proximity::CollisionProximitySpace::TrajectorySafety	checkCurrentIterValidity ()
void	computeJointProperties (int trajectoryPoint)
void	debugCost ()
double	getCollisionCost ()
template<typename Derived >
void	getJacobian (int trajectoryPoint, Eigen::Vector3d &collision_point_pos, std::string &jointName, Eigen::MatrixBase< Derived > &jacobian) const
double	getPotential (double field_distance, double radius, double clearence)
void	getRandomMomentum ()
void	getRandomState (const planning_models::KinematicState *currentState, const std::string &groupName, Eigen::VectorXd &state_vec)
double	getSmoothnessCost ()
double	getTrajectoryCost ()
void	handleJointLimits ()
void	initialize ()
bool	isParent (const std::string &childLink, const std::string &parentLink) const
void	performForwardKinematics ()
void	perturbTrajectory ()
void	registerParents (const planning_models::KinematicModel::JointModel *model)
void	setRobotStateFromPoint (ChompTrajectory &group_trajectory, int i)
void	updateFullTrajectory ()
void	updateMomentum ()
void	updatePositionFromMomentum ()
void	visualizeState (int index)
Private Attributes
Eigen::MatrixXd	best_group_trajectory_
double	best_group_trajectory_cost_
unsigned int	collision_free_iteration_
Eigen::MatrixXd	collision_increments_
std::vector< std::vector < Eigen::Vector3d > >	collision_point_acc_eigen_
std::vector< std::vector < std::string > >	collision_point_joint_names_
std::vector< std::vector < Eigen::Vector3d > >	collision_point_pos_eigen_
std::vector< std::vector < double > >	collision_point_potential_
std::vector< std::vector < Eigen::Vector3d > >	collision_point_potential_gradient_
std::vector< std::vector < Eigen::Vector3d > >	collision_point_vel_eigen_
std::vector< std::vector < double > >	collision_point_vel_mag_
collision_proximity::CollisionProximitySpace *	collision_space_
Eigen::MatrixXd	final_increments_
int	free_vars_end_
int	free_vars_start_
ChompTrajectory *	full_trajectory_
ChompTrajectory	group_trajectory_
Eigen::MatrixXd	group_trajectory_backup_
bool	is_collision_free_
int	iteration_
Eigen::MatrixXd	jacobian_
Eigen::MatrixXd	jacobian_jacobian_tranpose_
Eigen::MatrixXd	jacobian_pseudo_inverse_
std::vector< std::vector < tf::Vector3 > >	joint_axes_
std::vector< ChompCost >	joint_costs_
std::vector< std::string >	joint_names_
std::map< std::string, std::map< std::string, bool > >	joint_parent_map_
std::vector< std::vector < tf::Vector3 > >	joint_positions_
Eigen::VectorXd	joint_state_velocities_
int	last_improvement_iteration_
Eigen::MatrixXd	momentum_
std::vector< MultivariateGaussian >	multivariate_gaussian_
unsigned int	num_collision_free_iterations_
int	num_collision_points_
int	num_joints_
int	num_vars_all_
int	num_vars_free_
const ChompParameters *	parameters_
const std::string &	planning_group_
std::vector< std::vector< int > >	point_is_in_collision_
Eigen::VectorXd	random_joint_momentum_
Eigen::MatrixXd	random_momentum_
Eigen::VectorXd	random_state_
planning_models::KinematicModel *	robot_model_
planning_models::KinematicState *	robot_state_
Eigen::VectorXd	smoothness_derivative_
Eigen::MatrixXd	smoothness_increments_
std::vector< int >	state_is_in_collision_
double	stochasticity_factor_
ros::Publisher	vis_marker_array_pub_
ros::Publisher	vis_marker_pub_
double	worst_collision_cost_state_

Detailed Description

Definition at line 54 of file chomp_optimizer.h.

Constructor & Destructor Documentation

chomp::ChompOptimizer::ChompOptimizer	(	ChompTrajectory *	trajectory,
		planning_models::KinematicModel *	robot_model,
		const std::string &	planning_group,
		const ChompParameters *	parameters,
		const ros::Publisher &	vis_marker_array_publisher,
		const ros::Publisher &	vis_marker_publisher,
		collision_proximity::CollisionProximitySpace *	collision_space
	)

Definition at line 59 of file chomp_optimizer.cpp.

chomp::ChompOptimizer::~ChompOptimizer ( ) [virtual]

Definition at line 254 of file chomp_optimizer.cpp.

Member Function Documentation

void chomp::ChompOptimizer::addIncrementsToTrajectory ( ) [private]

Definition at line 641 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::animateEndeffector ( ) [private]

Definition at line 1068 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::animatePath ( ) [private]

Definition at line 1058 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculateCollisionIncrements ( ) [private]

Definition at line 547 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculatePseudoInverse ( ) [private]

Definition at line 623 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculateSmoothnessIncrements ( ) [private]

Definition at line 537 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculateTotalIncrements ( ) [private]

Definition at line 630 of file chomp_optimizer.cpp.

CollisionProximitySpace::TrajectorySafety chomp::ChompOptimizer::checkCurrentIterValidity ( ) [private]

Definition at line 506 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::computeJointProperties ( int trajectoryPoint ) [private]

Definition at line 719 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::debugCost ( ) [private]

Definition at line 670 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::destroy ( ) [inline]

Definition at line 66 of file chomp_optimizer.h.

double chomp::ChompOptimizer::getCollisionCost ( ) [private]

Definition at line 693 of file chomp_optimizer.cpp.

template<typename Derived >

void chomp::ChompOptimizer::getJacobian	(	int	trajectoryPoint,
		Eigen::Vector3d &	collision_point_pos,
		std::string &	jointName,
		Eigen::MatrixBase< Derived > &	jacobian
	)		const `[private]`

template<typename Derived >

void chomp::ChompOptimizer::getJacobian	(	int	trajectoryPoint,
		Vector3d &	collision_point_pos,
		string &	jointName,
		MatrixBase< Derived > &	jacobian
	)		const

Definition at line 759 of file chomp_optimizer.cpp.

double chomp::ChompOptimizer::getPotential	(	double	field_distance,
		double	radius,
		double	clearence
	)		`[inline, private]`

Definition at line 72 of file chomp_optimizer.h.

void chomp::ChompOptimizer::getRandomMomentum ( ) [private]

Definition at line 1029 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::getRandomState	(	const planning_models::KinematicState *	currentState,
		const std::string &	groupName,
		Eigen::VectorXd &	state_vec
	)		`[private]`