#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 moveit::core::RobotState &start_state)

void	destroy ()

bool	isCollisionFree () const

bool	isInitialized () const

bool	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 clearance)

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 moveit::core::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< EigenSTL::vector_Vector3d >	collision_point_acc_eigen_

std::vector< std::vector< std::string > >	collision_point_joint_names_

std::vector< EigenSTL::vector_Vector3d >	collision_point_pos_eigen_

std::vector< std::vector< double > >	collision_point_potential_

std::vector< EigenSTL::vector_Vector3d >	collision_point_potential_gradient_

std::vector< EigenSTL::vector_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_

collision_detection::GroupStateRepresentationPtr	gsr_

const collision_detection::CollisionEnvHybrid *	hy_env_

bool	initialized_

bool	is_collision_free_

int	iteration_

Eigen::MatrixXd	jacobian_

Eigen::MatrixXd	jacobian_jacobian_tranpose_

Eigen::MatrixXd	jacobian_pseudo_inverse_

std::vector< EigenSTL::vector_Vector3d >	joint_axes_

std::vector< ChompCost >	joint_costs_

const moveit::core::JointModelGroup *	joint_model_group_

std::vector< std::string >	joint_names_

std::map< std::string, std::map< std::string, bool > >	joint_parent_map_

std::vector< EigenSTL::vector_Vector3d >	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_

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_

const moveit::core::RobotModelConstPtr &	robot_model_

Eigen::VectorXd	smoothness_derivative_

Eigen::MatrixXd	smoothness_increments_

moveit::core::RobotState	start_state_

moveit::core::RobotState	state_

std::vector< int >	state_is_in_collision_

double	stochasticity_factor_

double	worst_collision_cost_state_

Detailed Description

Definition at line 85 of file chomp_optimizer.h.

Constructor & Destructor Documentation

◆ ChompOptimizer()

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

Definition at line 89 of file chomp_optimizer.cpp.

◆ ~ChompOptimizer()

chomp::ChompOptimizer::~ChompOptimizer ( )

virtual

Definition at line 281 of file chomp_optimizer.cpp.

Member Function Documentation

◆ addIncrementsToTrajectory()

void chomp::ChompOptimizer::addIncrementsToTrajectory ( )

private

Definition at line 709 of file chomp_optimizer.cpp.

◆ calculateCollisionIncrements()

void chomp::ChompOptimizer::calculateCollisionIncrements ( )

private

Definition at line 614 of file chomp_optimizer.cpp.

◆ calculatePseudoInverse()

void chomp::ChompOptimizer::calculatePseudoInverse ( )

private

Definition at line 691 of file chomp_optimizer.cpp.

◆ calculateSmoothnessIncrements()

void chomp::ChompOptimizer::calculateSmoothnessIncrements ( )

private

TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters values in the chomp_planning.yaml file so that CHOMP can find optimal paths

Definition at line 605 of file chomp_optimizer.cpp.

◆ calculateTotalIncrements()

void chomp::ChompOptimizer::calculateTotalIncrements ( )

private

Definition at line 698 of file chomp_optimizer.cpp.

◆ computeJointProperties()

void chomp::ChompOptimizer::computeJointProperties ( int trajectoryPoint )

private

Definition at line 783 of file chomp_optimizer.cpp.

◆ debugCost()

void chomp::ChompOptimizer::debugCost ( )

private

Definition at line 734 of file chomp_optimizer.cpp.

◆ destroy()

void chomp::ChompOptimizer::destroy ( )

inline

Definition at line 132 of file chomp_optimizer.h.

◆ getCollisionCost()

double chomp::ChompOptimizer::getCollisionCost ( )

private

Definition at line 757 of file chomp_optimizer.cpp.

◆ getJacobian()

template<typename Derived >

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

private

Definition at line 823 of file chomp_optimizer.cpp.

◆ getPotential()

double chomp::ChompOptimizer::getPotential	(	double	field_distance,
		double	radius,
		double	clearance
	)

inlineprivate

Definition at line 148 of file chomp_optimizer.h.

◆ getRandomMomentum()

void chomp::ChompOptimizer::getRandomMomentum ( )

private

◆ getSmoothnessCost()

double chomp::ChompOptimizer::getSmoothnessCost ( )

private

Definition at line 747 of file chomp_optimizer.cpp.

◆ getTrajectoryCost()

double chomp::ChompOptimizer::getTrajectoryCost ( )

private

Definition at line 742 of file chomp_optimizer.cpp.

◆ handleJointLimits()

void chomp::ChompOptimizer::handleJointLimits ( )

private

Definition at line 847 of file chomp_optimizer.cpp.

◆ initialize()

void chomp::ChompOptimizer::initialize ( )

private

TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters values in the chomp_planning.yaml file so that CHOMP can find optimal paths

Definition at line 124 of file chomp_optimizer.cpp.

◆ isCollisionFree()

bool chomp::ChompOptimizer::isCollisionFree ( ) const

inline

Definition at line 142 of file chomp_optimizer.h.

◆ isCurrentTrajectoryMeshToMeshCollisionFree()

bool chomp::ChompOptimizer::isCurrentTrajectoryMeshToMeshCollisionFree ( ) const

private

Definition at line 552 of file chomp_optimizer.cpp.

◆ isInitialized()

bool chomp::ChompOptimizer::isInitialized ( ) const

inline

Definition at line 137 of file chomp_optimizer.h.

◆ isParent()

bool chomp::ChompOptimizer::isParent	(	const std::string &	childLink,
		const std::string &	parentLink
	)		const

inlineprivate

Definition at line 246 of file chomp_optimizer.h.

◆ optimize()

bool chomp::ChompOptimizer::optimize ( )

Optimizes the CHOMP cost function and tries to find an optimal path

Returns: true if an optimal collision free path is found else returns false

TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters values in the chomp_planning.yaml file so that CHOMP can find optimal paths

Definition at line 325 of file chomp_optimizer.cpp.

◆ performForwardKinematics()

void chomp::ChompOptimizer::performForwardKinematics ( )

private

Definition at line 928 of file chomp_optimizer.cpp.

◆ perturbTrajectory()

void chomp::ChompOptimizer::perturbTrajectory ( )

private

Definition at line 1037 of file chomp_optimizer.cpp.

◆ registerParents()

void chomp::ChompOptimizer::registerParents ( const moveit::core::JointModel * model )

private

Definition at line 286 of file chomp_optimizer.cpp.

◆ setRobotStateFromPoint()

void chomp::ChompOptimizer::setRobotStateFromPoint	(	ChompTrajectory &	group_trajectory,
		int	i
	)

private

Definition at line 1024 of file chomp_optimizer.cpp.

◆ updateFullTrajectory()

void chomp::ChompOptimizer::updateFullTrajectory ( )

private

Definition at line 729 of file chomp_optimizer.cpp.

◆ updateMomentum()

void chomp::ChompOptimizer::updateMomentum ( )

private

◆ updatePositionFromMomentum()

void chomp::ChompOptimizer::updatePositionFromMomentum ( )

private

Member Data Documentation

◆ best_group_trajectory_

Eigen::MatrixXd chomp::ChompOptimizer::best_group_trajectory_

private

Definition at line 213 of file chomp_optimizer.h.

◆ best_group_trajectory_cost_

double chomp::ChompOptimizer::best_group_trajectory_cost_

private

Definition at line 214 of file chomp_optimizer.h.

◆ collision_free_iteration_

unsigned int chomp::ChompOptimizer::collision_free_iteration_

private

Definition at line 186 of file chomp_optimizer.h.

◆ collision_increments_

Eigen::MatrixXd chomp::ChompOptimizer::collision_increments_

private

Definition at line 232 of file chomp_optimizer.h.

◆ collision_point_acc_eigen_

std::vector<EigenSTL::vector_Vector3d> chomp::ChompOptimizer::collision_point_acc_eigen_

private

Definition at line 206 of file chomp_optimizer.h.

◆ collision_point_joint_names_

std::vector<std::vector<std::string> > chomp::ChompOptimizer::collision_point_joint_names_

private

Definition at line 203 of file chomp_optimizer.h.

◆ collision_point_pos_eigen_

std::vector<EigenSTL::vector_Vector3d> chomp::ChompOptimizer::collision_point_pos_eigen_

private

Definition at line 204 of file chomp_optimizer.h.

◆ collision_point_potential_

std::vector<std::vector<double> > chomp::ChompOptimizer::collision_point_potential_

private

Definition at line 207 of file chomp_optimizer.h.

◆ collision_point_potential_gradient_

std::vector<EigenSTL::vector_Vector3d> chomp::ChompOptimizer::collision_point_potential_gradient_

private

Definition at line 209 of file chomp_optimizer.h.

◆ collision_point_vel_eigen_

std::vector<EigenSTL::vector_Vector3d> chomp::ChompOptimizer::collision_point_vel_eigen_

private

Definition at line 205 of file chomp_optimizer.h.

◆ collision_point_vel_mag_

std::vector<std::vector<double> > chomp::ChompOptimizer::collision_point_vel_mag_

private

Definition at line 208 of file chomp_optimizer.h.

◆ final_increments_

Eigen::MatrixXd chomp::ChompOptimizer::final_increments_

private

Definition at line 233 of file chomp_optimizer.h.

◆ free_vars_end_

int chomp::ChompOptimizer::free_vars_end_

private

Definition at line 184 of file chomp_optimizer.h.

◆ free_vars_start_

int chomp::ChompOptimizer::free_vars_start_

private

Definition at line 183 of file chomp_optimizer.h.

◆ full_trajectory_

ChompTrajectory* chomp::ChompOptimizer::full_trajectory_

private

Definition at line 188 of file chomp_optimizer.h.

◆ group_trajectory_

ChompTrajectory chomp::ChompOptimizer::group_trajectory_

private

Definition at line 192 of file chomp_optimizer.h.

◆ group_trajectory_backup_

Eigen::MatrixXd chomp::ChompOptimizer::group_trajectory_backup_

private

Definition at line 212 of file chomp_optimizer.h.

◆ gsr_

collision_detection::GroupStateRepresentationPtr chomp::ChompOptimizer::gsr_

private

Definition at line 200 of file chomp_optimizer.h.

◆ hy_env_

const collision_detection::CollisionEnvHybrid* chomp::ChompOptimizer::hy_env_

private

Definition at line 197 of file chomp_optimizer.h.

◆ initialized_

bool chomp::ChompOptimizer::initialized_

private

Definition at line 201 of file chomp_optimizer.h.

◆ is_collision_free_

bool chomp::ChompOptimizer::is_collision_free_

private

Definition at line 228 of file chomp_optimizer.h.

◆ iteration_

int chomp::ChompOptimizer::iteration_

private

Definition at line 185 of file chomp_optimizer.h.

◆ jacobian_

Eigen::MatrixXd chomp::ChompOptimizer::jacobian_

private

Definition at line 237 of file chomp_optimizer.h.

◆ jacobian_jacobian_tranpose_

Eigen::MatrixXd chomp::ChompOptimizer::jacobian_jacobian_tranpose_

private

Definition at line 239 of file chomp_optimizer.h.

◆ jacobian_pseudo_inverse_

Eigen::MatrixXd chomp::ChompOptimizer::jacobian_pseudo_inverse_

private

Definition at line 238 of file chomp_optimizer.h.

◆ joint_axes_

std::vector<EigenSTL::vector_Vector3d> chomp::ChompOptimizer::joint_axes_

private

Definition at line 210 of file chomp_optimizer.h.

◆ joint_costs_

std::vector<ChompCost> chomp::ChompOptimizer::joint_costs_

private

Definition at line 199 of file chomp_optimizer.h.

◆ joint_model_group_

const moveit::core::JointModelGroup* chomp::ChompOptimizer::joint_model_group_

private

Definition at line 196 of file chomp_optimizer.h.

◆ joint_names_

std::vector<std::string> chomp::ChompOptimizer::joint_names_

private

Definition at line 243 of file chomp_optimizer.h.

◆ joint_parent_map_

std::map<std::string, std::map<std::string, bool> > chomp::ChompOptimizer::joint_parent_map_

private

Definition at line 244 of file chomp_optimizer.h.

◆ joint_positions_

std::vector<EigenSTL::vector_Vector3d> chomp::ChompOptimizer::joint_positions_

private

Definition at line 211 of file chomp_optimizer.h.

◆ joint_state_velocities_

Eigen::VectorXd chomp::ChompOptimizer::joint_state_velocities_

private

Definition at line 241 of file chomp_optimizer.h.

◆ last_improvement_iteration_

int chomp::ChompOptimizer::last_improvement_iteration_

private

Definition at line 215 of file chomp_optimizer.h.

◆ momentum_

Eigen::MatrixXd chomp::ChompOptimizer::momentum_

private

Definition at line 219 of file chomp_optimizer.h.

◆ multivariate_gaussian_

std::vector<MultivariateGaussian> chomp::ChompOptimizer::multivariate_gaussian_

private

Definition at line 222 of file chomp_optimizer.h.

◆ num_collision_free_iterations_

unsigned int chomp::ChompOptimizer::num_collision_free_iterations_

private

Definition at line 216 of file chomp_optimizer.h.

◆ num_collision_points_

int chomp::ChompOptimizer::num_collision_points_

private

Definition at line 182 of file chomp_optimizer.h.

◆ num_joints_

int chomp::ChompOptimizer::num_joints_

private

Definition at line 179 of file chomp_optimizer.h.

◆ num_vars_all_

int chomp::ChompOptimizer::num_vars_all_

private

Definition at line 181 of file chomp_optimizer.h.

◆ num_vars_free_

int chomp::ChompOptimizer::num_vars_free_

private

Definition at line 180 of file chomp_optimizer.h.

◆ parameters_

const ChompParameters* chomp::ChompOptimizer::parameters_

private

Definition at line 191 of file chomp_optimizer.h.

◆ planning_group_

std::string chomp::ChompOptimizer::planning_group_

private

Definition at line 190 of file chomp_optimizer.h.

◆ planning_scene_

planning_scene::PlanningSceneConstPtr chomp::ChompOptimizer::planning_scene_

private

Definition at line 193 of file chomp_optimizer.h.

◆ point_is_in_collision_

std::vector<std::vector<int> > chomp::ChompOptimizer::point_is_in_collision_

private

Definition at line 227 of file chomp_optimizer.h.

◆ random_joint_momentum_

Eigen::VectorXd chomp::ChompOptimizer::random_joint_momentum_

private

Definition at line 221 of file chomp_optimizer.h.

◆ random_momentum_

Eigen::MatrixXd chomp::ChompOptimizer::random_momentum_

private

Definition at line 220 of file chomp_optimizer.h.

◆ random_state_

Eigen::VectorXd chomp::ChompOptimizer::random_state_

private

Definition at line 240 of file chomp_optimizer.h.

◆ robot_model_

const moveit::core::RobotModelConstPtr& chomp::ChompOptimizer::robot_model_

private

Definition at line 189 of file chomp_optimizer.h.

◆ smoothness_derivative_

Eigen::VectorXd chomp::ChompOptimizer::smoothness_derivative_

private

Definition at line 236 of file chomp_optimizer.h.

◆ smoothness_increments_

Eigen::MatrixXd chomp::ChompOptimizer::smoothness_increments_

private

Definition at line 231 of file chomp_optimizer.h.

◆ start_state_

moveit::core::RobotState chomp::ChompOptimizer::start_state_

private

Definition at line 195 of file chomp_optimizer.h.

◆ state_

moveit::core::RobotState chomp::ChompOptimizer::state_

private

Definition at line 194 of file chomp_optimizer.h.

◆ state_is_in_collision_

std::vector<int> chomp::ChompOptimizer::state_is_in_collision_

private

Array containing a boolean about collision info for each point in the trajectory

Definition at line 225 of file chomp_optimizer.h.

◆ stochasticity_factor_

double chomp::ChompOptimizer::stochasticity_factor_

private

Definition at line 223 of file chomp_optimizer.h.

◆ worst_collision_cost_state_

double chomp::ChompOptimizer::worst_collision_cost_state_

private

Definition at line 229 of file chomp_optimizer.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ChompOptimizer()

◆ ~ChompOptimizer()

Member Function Documentation

◆ addIncrementsToTrajectory()

◆ calculateCollisionIncrements()

◆ calculatePseudoInverse()

◆ calculateSmoothnessIncrements()

◆ calculateTotalIncrements()

◆ computeJointProperties()

◆ debugCost()

◆ destroy()

◆ getCollisionCost()

◆ getJacobian()

◆ getPotential()

◆ getRandomMomentum()

◆ getSmoothnessCost()

◆ getTrajectoryCost()

◆ handleJointLimits()

◆ initialize()

◆ isCollisionFree()

◆ isCurrentTrajectoryMeshToMeshCollisionFree()

◆ isInitialized()

◆ isParent()

◆ optimize()

◆ performForwardKinematics()

◆ perturbTrajectory()

◆ registerParents()

◆ setRobotStateFromPoint()

◆ updateFullTrajectory()

◆ updateMomentum()

◆ updatePositionFromMomentum()

Member Data Documentation

◆ best_group_trajectory_

◆ best_group_trajectory_cost_

◆ collision_free_iteration_

◆ collision_increments_

◆ collision_point_acc_eigen_

◆ collision_point_joint_names_

◆ collision_point_pos_eigen_

◆ collision_point_potential_

◆ collision_point_potential_gradient_

◆ collision_point_vel_eigen_

◆ collision_point_vel_mag_

◆ final_increments_

◆ free_vars_end_

◆ free_vars_start_

◆ full_trajectory_

◆ group_trajectory_

◆ group_trajectory_backup_

◆ gsr_

◆ hy_env_

◆ initialized_

◆ is_collision_free_

◆ iteration_

◆ jacobian_

◆ jacobian_jacobian_tranpose_

◆ jacobian_pseudo_inverse_

◆ joint_axes_

◆ joint_costs_

◆ joint_model_group_

◆ joint_names_

◆ joint_parent_map_

◆ joint_positions_

◆ joint_state_velocities_

◆ last_improvement_iteration_

◆ momentum_

◆ multivariate_gaussian_

◆ num_collision_free_iterations_

◆ num_collision_points_

◆ num_joints_

◆ num_vars_all_

◆ num_vars_free_

◆ parameters_

◆ planning_group_

◆ planning_scene_