Public Member Functions | Private Member Functions | Private Attributes | List of all members
chomp::ChompOptimizer Class Reference

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

Definition at line 54 of file chomp_optimizer.cpp.

chomp::ChompOptimizer::~ChompOptimizer ( )
virtual

Definition at line 272 of file chomp_optimizer.cpp.

Member Function Documentation

void chomp::ChompOptimizer::addIncrementsToTrajectory ( )
private

Definition at line 702 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculateCollisionIncrements ( )
private

Definition at line 607 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculatePseudoInverse ( )
private

Definition at line 684 of file chomp_optimizer.cpp.

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 598 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::calculateTotalIncrements ( )
private

Definition at line 691 of file chomp_optimizer.cpp.

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

Definition at line 776 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::debugCost ( )
private

Definition at line 727 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 750 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

Definition at line 817 of file chomp_optimizer.cpp.

double chomp::ChompOptimizer::getPotential ( double  field_distance,
double  radius,
double  clearence 
)
inlineprivate

Definition at line 86 of file chomp_optimizer.h.

void chomp::ChompOptimizer::getRandomMomentum ( )
private
double chomp::ChompOptimizer::getSmoothnessCost ( )
private

Definition at line 740 of file chomp_optimizer.cpp.

double chomp::ChompOptimizer::getTrajectoryCost ( )
private

Definition at line 735 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::handleJointLimits ( )
private

Definition at line 841 of file chomp_optimizer.cpp.

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 96 of file chomp_optimizer.cpp.

bool chomp::ChompOptimizer::isCollisionFree ( ) const
inline

Definition at line 80 of file chomp_optimizer.h.

bool chomp::ChompOptimizer::isCurrentTrajectoryMeshToMeshCollisionFree ( ) const
private

Definition at line 545 of file chomp_optimizer.cpp.

bool chomp::ChompOptimizer::isInitialized ( ) const
inline

Definition at line 75 of file chomp_optimizer.h.

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

Definition at line 189 of file chomp_optimizer.h.

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

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

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

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 316 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::performForwardKinematics ( )
private

Definition at line 922 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::perturbTrajectory ( )
private

Definition at line 1034 of file chomp_optimizer.cpp.

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

Definition at line 277 of file chomp_optimizer.cpp.

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

Definition at line 1020 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::updateFullTrajectory ( )
private

Definition at line 722 of file chomp_optimizer.cpp.

void chomp::ChompOptimizer::updateMomentum ( )
private
void chomp::ChompOptimizer::updatePositionFromMomentum ( )
private

Member Data Documentation

Eigen::MatrixXd chomp::ChompOptimizer::best_group_trajectory_
private

Definition at line 156 of file chomp_optimizer.h.

double chomp::ChompOptimizer::best_group_trajectory_cost_
private

Definition at line 157 of file chomp_optimizer.h.

unsigned int chomp::ChompOptimizer::collision_free_iteration_
private

Definition at line 128 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::collision_increments_
private

Definition at line 175 of file chomp_optimizer.h.

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

Definition at line 149 of file chomp_optimizer.h.

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

Definition at line 146 of file chomp_optimizer.h.

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

Definition at line 147 of file chomp_optimizer.h.

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

Definition at line 150 of file chomp_optimizer.h.

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

Definition at line 152 of file chomp_optimizer.h.

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

Definition at line 148 of file chomp_optimizer.h.

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

Definition at line 151 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::final_increments_
private

Definition at line 176 of file chomp_optimizer.h.

int chomp::ChompOptimizer::free_vars_end_
private

Definition at line 126 of file chomp_optimizer.h.

int chomp::ChompOptimizer::free_vars_start_
private

Definition at line 125 of file chomp_optimizer.h.

ChompTrajectory* chomp::ChompOptimizer::full_trajectory_
private

Definition at line 130 of file chomp_optimizer.h.

ChompTrajectory chomp::ChompOptimizer::group_trajectory_
private

Definition at line 134 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::group_trajectory_backup_
private

Definition at line 155 of file chomp_optimizer.h.

collision_detection::GroupStateRepresentationPtr chomp::ChompOptimizer::gsr_
private

Definition at line 143 of file chomp_optimizer.h.

const collision_detection::CollisionRobotHybrid* chomp::ChompOptimizer::hy_robot_
private

Definition at line 140 of file chomp_optimizer.h.

const collision_detection::CollisionWorldHybrid* chomp::ChompOptimizer::hy_world_
private

Definition at line 139 of file chomp_optimizer.h.

bool chomp::ChompOptimizer::initialized_
private

Definition at line 144 of file chomp_optimizer.h.

bool chomp::ChompOptimizer::is_collision_free_
private

Definition at line 171 of file chomp_optimizer.h.

int chomp::ChompOptimizer::iteration_
private

Definition at line 127 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::jacobian_
private

Definition at line 180 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::jacobian_jacobian_tranpose_
private

Definition at line 182 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::jacobian_pseudo_inverse_
private

Definition at line 181 of file chomp_optimizer.h.

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

Definition at line 153 of file chomp_optimizer.h.

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

Definition at line 142 of file chomp_optimizer.h.

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

Definition at line 138 of file chomp_optimizer.h.

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

Definition at line 186 of file chomp_optimizer.h.

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

Definition at line 187 of file chomp_optimizer.h.

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

Definition at line 154 of file chomp_optimizer.h.

Eigen::VectorXd chomp::ChompOptimizer::joint_state_velocities_
private

Definition at line 184 of file chomp_optimizer.h.

const moveit::core::RobotModelConstPtr& chomp::ChompOptimizer::kmodel_
private

Definition at line 131 of file chomp_optimizer.h.

int chomp::ChompOptimizer::last_improvement_iteration_
private

Definition at line 158 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::momentum_
private

Definition at line 162 of file chomp_optimizer.h.

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

Definition at line 165 of file chomp_optimizer.h.

unsigned int chomp::ChompOptimizer::num_collision_free_iterations_
private

Definition at line 159 of file chomp_optimizer.h.

int chomp::ChompOptimizer::num_collision_points_
private

Definition at line 124 of file chomp_optimizer.h.

int chomp::ChompOptimizer::num_joints_
private

Definition at line 121 of file chomp_optimizer.h.

int chomp::ChompOptimizer::num_vars_all_
private

Definition at line 123 of file chomp_optimizer.h.

int chomp::ChompOptimizer::num_vars_free_
private

Definition at line 122 of file chomp_optimizer.h.

const ChompParameters* chomp::ChompOptimizer::parameters_
private

Definition at line 133 of file chomp_optimizer.h.

std::string chomp::ChompOptimizer::planning_group_
private

Definition at line 132 of file chomp_optimizer.h.

planning_scene::PlanningSceneConstPtr chomp::ChompOptimizer::planning_scene_
private

Definition at line 135 of file chomp_optimizer.h.

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

Definition at line 170 of file chomp_optimizer.h.

Eigen::VectorXd chomp::ChompOptimizer::random_joint_momentum_
private

Definition at line 164 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::random_momentum_
private

Definition at line 163 of file chomp_optimizer.h.

Eigen::VectorXd chomp::ChompOptimizer::random_state_
private

Definition at line 183 of file chomp_optimizer.h.

Eigen::VectorXd chomp::ChompOptimizer::smoothness_derivative_
private

Definition at line 179 of file chomp_optimizer.h.

Eigen::MatrixXd chomp::ChompOptimizer::smoothness_increments_
private

Definition at line 174 of file chomp_optimizer.h.

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

Definition at line 137 of file chomp_optimizer.h.

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

Definition at line 136 of file chomp_optimizer.h.

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 168 of file chomp_optimizer.h.

double chomp::ChompOptimizer::stochasticity_factor_
private

Definition at line 166 of file chomp_optimizer.h.

double chomp::ChompOptimizer::worst_collision_cost_state_
private

Definition at line 172 of file chomp_optimizer.h.

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

chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Sun Oct 18 2020 13:17:08