00001 /* 00002 * Copyright (c) 2010, Maxim Likhachev 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the University of Pennsylvania nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #ifndef __SBPL_ARM_PLANNER_PARAMS_H_ 00031 #define __SBPL_ARM_PLANNER_PARAMS_H_ 00032 00033 #include <iostream> 00034 #include <string> 00035 #include <vector> 00036 #include <iterator> 00037 #include <ros/ros.h> 00038 #include <angles/angles.h> 00039 #include <sbpl/config.h> 00040 00041 #define DEG2RAD(d) ((d)*(M_PI/180.0)) 00042 #define RAD2DEG(r) ((r)*(180.0/M_PI)) 00043 00044 using namespace std; 00045 00046 class SBPLArmPlannerParams 00047 { 00048 00049 public: 00050 00052 SBPLArmPlannerParams(); 00053 00055 ~SBPLArmPlannerParams(){}; 00056 00058 void initFromParamServer(); 00059 00061 bool initFromParamFile(std::string param_file); 00062 00064 bool initMotionPrimsFromFile(FILE* fCfg); 00065 00067 bool initFromParamFile(FILE* fCfg); 00068 00070 bool initFromResource(std::string url); 00071 00073 void printParams(FILE* fOut); 00074 00077 void precomputeSmoothingCosts(); 00078 00080 void addMotionPrim(std::vector<double> mprim, bool add_converse, bool short_dist_mprim); 00081 00083 void setCellCost(int cost_per_cell); 00084 00086 void printMotionPrims(FILE* fOut); 00087 00088 double getSmallestShoulderPanMotion(); 00089 00091 double getLargestMotionPrimOffset(); 00092 00094 void printSmoothingCosts(FILE* fOut); 00095 00098 double epsilon_; 00099 00101 bool use_multires_mprims_; 00102 00104 bool use_dijkstra_heuristic_; 00105 00107 bool use_orientation_solver_; 00108 00110 bool use_ik_; 00111 00113 bool use_smoothing_; 00114 00116 bool use_6d_pose_goal_; 00117 00119 bool sum_heuristics_; 00120 00123 bool use_uniform_cost_; 00124 00126 bool verbose_; 00127 00129 bool verbose_heuristics_; 00130 00132 bool verbose_collisions_; 00133 00135 int angle_delta_; 00136 00138 std::vector<std::vector<double> > mprims_; 00139 00141 int num_mprims_; 00142 00144 int num_long_dist_mprims_; 00145 00147 int num_short_dist_mprims_; 00148 00151 int short_dist_mprims_thresh_c_; 00152 00155 double short_dist_mprims_thresh_m_; 00156 00157 std::string planner_name_; 00158 00161 std::vector<std::vector<int> > smoothing_cost_; 00162 00164 int cost_multiplier_; 00165 00166 int range1_cost_; 00167 int range2_cost_; 00168 int range3_cost_; 00169 00171 int cost_per_cell_; 00172 00175 int cost_per_meter_; 00176 00178 int is_goal_function_; 00179 00180 bool two_calls_to_op_; 00181 00182 bool use_research_heuristic_; 00183 00184 double max_mprim_offset_; 00185 00187 double sizeX_; 00188 double sizeY_; 00189 double sizeZ_; 00190 00192 double resolution_; 00193 00195 double originX_; 00196 double originY_; 00197 double originZ_; 00198 00199 int solve_for_ik_thresh_; 00200 double solve_for_ik_thresh_m_; 00201 }; 00202 00203 #endif 00204