#include <chomp_optimizer.h>

Public Member Functions
	ChompOptimizer (ChompTrajectory trajectory, const planning_scene::PlanningSceneConstPtr &planning_scene, const std::string &planning_group, const ChompParameters parameters, const planning_models::RobotState &start_state)
void	destroy ()
bool	isInitialized () const
void	optimize ()
virtual	~ChompOptimizer ()
Private Member Functions
void	addIncrementsToTrajectory ()
void	calculateCollisionIncrements ()
void	calculatePseudoInverse ()
void	calculateSmoothnessIncrements ()
void	calculateTotalIncrements ()
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 ()
double	getSmoothnessCost ()
double	getTrajectoryCost ()
void	handleJointLimits ()
void	initialize ()
bool	isCurrentTrajectoryMeshToMeshCollisionFree () const
bool	isParent (const std::string &childLink, const std::string &parentLink) const
void	performForwardKinematics ()
void	perturbTrajectory ()
void	registerParents (const planning_models::RobotModel::JointModel *model)
void	setRobotStateFromPoint (ChompTrajectory &group_trajectory, int i)
void	updateFullTrajectory ()
void	updateMomentum ()
void	updatePositionFromMomentum ()
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_
Eigen::MatrixXd	final_increments_
int	free_vars_end_
int	free_vars_start_
ChompTrajectory *	full_trajectory_
ChompTrajectory	group_trajectory_
Eigen::MatrixXd	group_trajectory_backup_
boost::shared_ptr < collision_detection::GroupStateRepresentation >	gsr_
const collision_detection::CollisionRobotHybrid *	hy_robot_
const collision_detection::CollisionWorldHybrid *	hy_world_
bool	initialized_
bool	is_collision_free_
int	iteration_
Eigen::MatrixXd	jacobian_
Eigen::MatrixXd	jacobian_jacobian_tranpose_
Eigen::MatrixXd	jacobian_pseudo_inverse_
std::vector< std::vector < Eigen::Vector3d > >	joint_axes_
std::vector< ChompCost >	joint_costs_
const planning_models::RobotModel::JointModelGroup *	joint_model_group_
std::vector< std::string >	joint_names_
std::map< std::string, std::map< std::string, bool > >	joint_parent_map_
std::vector< std::vector < Eigen::Vector3d > >	joint_positions_
Eigen::VectorXd	joint_state_velocities_
const planning_models::RobotModelConstPtr &	kmodel_
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_
std::string	planning_group_
planning_scene::PlanningSceneConstPtr	planning_scene_
std::vector< std::vector< int > >	point_is_in_collision_
Eigen::VectorXd	random_joint_momentum_
Eigen::MatrixXd	random_momentum_
Eigen::VectorXd	random_state_
Eigen::VectorXd	smoothness_derivative_
Eigen::MatrixXd	smoothness_increments_
planning_models::RobotState *	start_state_
planning_models::RobotState *	state_
std::vector< int >	state_is_in_collision_
double	stochasticity_factor_
double	worst_collision_cost_state_

Detailed Description

Definition at line 57 of file chomp_optimizer.h.

Constructor & Destructor Documentation

chomp::ChompOptimizer::ChompOptimizer	(	ChompTrajectory *	trajectory,
		const planning_scene::PlanningSceneConstPtr &	planning_scene,
		const std::string &	planning_group,
		const ChompParameters *	parameters,
		const planning_models::RobotState &	start_state
	)

Definition at line 52 of file chomp_optimizer.cpp.

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

Definition at line 269 of file chomp_optimizer.cpp.

Member Function Documentation

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

Definition at line 684 of file chomp_optimizer.cpp.

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

Definition at line 590 of file chomp_optimizer.cpp.

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

Definition at line 666 of file chomp_optimizer.cpp.

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

Definition at line 580 of file chomp_optimizer.cpp.

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

Definition at line 673 of file chomp_optimizer.cpp.

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

Definition at line 758 of file chomp_optimizer.cpp.

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

Definition at line 709 of file chomp_optimizer.cpp.

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

Definition at line 70 of file chomp_optimizer.h.

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

Definition at line 732 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]`