00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00037 #ifndef CHOMP_PARAMETERS_H_ 00038 #define CHOMP_PARAMETERS_H_ 00039 00040 #include <ros/ros.h> 00041 00042 namespace chomp 00043 { 00044 00045 class ChompParameters 00046 { 00047 public: 00048 ChompParameters(); 00049 virtual ~ChompParameters(); 00050 00051 void initFromNodeHandle(); 00052 00053 double getPlanningTimeLimit() const; 00054 void setPlanningTimeLimit(double planning_time_limit); 00055 int getMaxIterations() const; 00056 int getMaxIterationsAfterCollisionFree() const; 00057 double getSmoothnessCostWeight() const; 00058 double getObstacleCostWeight() const; 00059 bool getAnimatePath() const; 00060 double getLearningRate() const; 00061 double getSmoothnessCostVelocity() const; 00062 double getSmoothnessCostAcceleration() const; 00063 double getSmoothnessCostJerk() const; 00064 bool getAddRandomness() const; 00065 bool getUseHamiltonianMonteCarlo() const; 00066 double getHmcDiscretization() const; 00067 double getHmcStochasticity() const; 00068 double getHmcAnnealingFactor() const; 00069 double getRidgeFactor() const; 00070 bool getUsePseudoInverse() const; 00071 double getPseudoInverseRidgeFactor() const; 00072 bool getAnimateEndeffector() const; 00073 std::string getAnimateEndeffectorSegment() const; 00074 00075 private: 00076 double planning_time_limit_; 00077 int max_iterations_; 00078 int max_iterations_after_collision_free_; 00079 double smoothness_cost_weight_; 00080 double obstacle_cost_weight_; 00081 double learning_rate_; 00082 bool animate_path_; 00083 double smoothness_cost_velocity_; 00084 double smoothness_cost_acceleration_; 00085 double smoothness_cost_jerk_; 00086 bool add_randomness_; 00087 bool use_hamiltonian_monte_carlo_; 00088 double hmc_stochasticity_; 00089 double hmc_discretization_; 00090 double hmc_annealing_factor_; 00091 double ridge_factor_; 00092 bool use_pseudo_inverse_; 00093 double pseudo_inverse_ridge_factor_; 00094 bool animate_endeffector_; 00095 std::string animate_endeffector_segment_; 00096 00097 }; 00098 00100 00101 inline double ChompParameters::getPlanningTimeLimit() const 00102 { 00103 return planning_time_limit_; 00104 } 00105 00106 inline void ChompParameters::setPlanningTimeLimit(double planning_time_limit) 00107 { 00108 planning_time_limit_ = planning_time_limit; 00109 } 00110 00111 inline int ChompParameters::getMaxIterations() const 00112 { 00113 return max_iterations_; 00114 } 00115 00116 inline int ChompParameters::getMaxIterationsAfterCollisionFree() const 00117 { 00118 return max_iterations_after_collision_free_; 00119 } 00120 00121 inline double ChompParameters::getSmoothnessCostWeight() const 00122 { 00123 return smoothness_cost_weight_; 00124 } 00125 00126 inline double ChompParameters::getObstacleCostWeight() const 00127 { 00128 return obstacle_cost_weight_; 00129 } 00130 00131 inline double ChompParameters::getLearningRate() const 00132 { 00133 return learning_rate_; 00134 } 00135 00136 inline bool ChompParameters::getAnimatePath() const 00137 { 00138 return animate_path_; 00139 } 00140 00141 inline bool ChompParameters::getAddRandomness() const 00142 { 00143 return add_randomness_; 00144 } 00145 00146 inline double ChompParameters::getSmoothnessCostVelocity() const 00147 { 00148 return smoothness_cost_velocity_; 00149 } 00150 00151 inline double ChompParameters::getSmoothnessCostAcceleration() const 00152 { 00153 return smoothness_cost_acceleration_; 00154 } 00155 00156 inline double ChompParameters::getSmoothnessCostJerk() const 00157 { 00158 return smoothness_cost_jerk_; 00159 } 00160 00161 inline double ChompParameters::getHmcDiscretization() const 00162 { 00163 return hmc_discretization_; 00164 } 00165 00166 inline double ChompParameters::getHmcStochasticity() const 00167 { 00168 return hmc_stochasticity_; 00169 } 00170 00171 inline double ChompParameters::getHmcAnnealingFactor() const 00172 { 00173 return hmc_annealing_factor_; 00174 } 00175 00176 inline bool ChompParameters::getUseHamiltonianMonteCarlo() const 00177 { 00178 return use_hamiltonian_monte_carlo_; 00179 } 00180 00181 inline double ChompParameters::getRidgeFactor() const 00182 { 00183 return ridge_factor_; 00184 } 00185 00186 inline bool ChompParameters::getUsePseudoInverse() const 00187 { 00188 return use_pseudo_inverse_; 00189 } 00190 00191 inline double ChompParameters::getPseudoInverseRidgeFactor() const 00192 { 00193 return pseudo_inverse_ridge_factor_; 00194 } 00195 00196 inline bool ChompParameters::getAnimateEndeffector() const 00197 { 00198 return animate_endeffector_; 00199 } 00200 00201 inline std::string ChompParameters::getAnimateEndeffectorSegment() const 00202 { 00203 return animate_endeffector_segment_; 00204 } 00205 00206 } // namespace chomp 00207 00208 #endif /* CHOMP_PARAMETERS_H_ */