Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef KINEMATICS_CACHE_H_
00039 #define KINEMATICS_CACHE_H_
00040
00041 #include <kinematics_base/kinematics_base.h>
00042 #include <planning_models/robot_model.h>
00043 #include <planning_models/kinematic_state.h>
00044
00045 #include <boost/shared_ptr.hpp>
00046
00047 namespace kinematics_cache
00048 {
00049 class KinematicsCache
00050 {
00051 public:
00052 struct Options
00053 {
00054 geometry_msgs::Point origin;
00055 boost::array<double, 3> workspace_size;
00056 boost::array<double, 3> resolution;
00057 unsigned int max_solutions_per_grid_location;
00058 };
00059
00064 KinematicsCache();
00065
00070 bool generateCacheMap(double timeout);
00071
00079 bool getSolution(const geometry_msgs::Pose& pose, unsigned int solution_index, std::vector<double>& solution) const;
00080
00090 bool getSolutions(const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solutions) const;
00091
00098 bool getNumSolutions(const geometry_msgs::Pose& pose, unsigned int& num_solutions) const;
00099
00106 bool initialize(kinematics::KinematicsBaseConstPtr& solver,
00107 const planning_models::RobotModelConstPtr& kinematic_model, const KinematicsCache::Options& opt);
00108
00110 const kinematics::KinematicsBaseConstPtr& getSolverInstance() const
00111 {
00112 return kinematics_solver_;
00113 }
00114
00116 const planning_models::RobotModelConstPtr& getModelInstance() const
00117 {
00118 return kinematic_model_;
00119 }
00120
00122 const Options& getCacheParameters() const
00123 {
00124 return options_;
00125 }
00126
00127 const std::string getGroupName()
00128 {
00129 return kinematics_solver_->getGroupName();
00130 }
00131
00134 bool addToCache(const geometry_msgs::Pose& pose, const std::vector<double>& joint_values, bool overwrite = false);
00135
00136 bool writeToFile(const std::string& filename);
00137
00138 bool readFromFile(const std::string& filename);
00139
00140 std::pair<double, double> getMinMaxSquaredDistance();
00141
00142 private:
00144 unsigned int getSolutionLocation(unsigned int& grid_index, unsigned int& solution_index) const;
00145
00147 std::vector<double> getSolution(unsigned int grid_index, unsigned int solution_index) const;
00148
00150 bool getGridIndex(const geometry_msgs::Pose& pose, unsigned int& grid_index) const;
00151
00153 void setup(const KinematicsCache::Options& opt);
00154
00155 void updateDistances(const geometry_msgs::Pose& pose);
00156
00157 KinematicsCache::Options options_;
00158 geometry_msgs::Point cache_origin_;
00159 double cache_resolution_x_, cache_resolution_y_, cache_resolution_z_;
00160 unsigned int cache_size_x_, cache_size_y_, cache_size_z_;
00161 unsigned int max_solutions_per_grid_location_;
00162 unsigned int solution_dimension_;
00163 unsigned int size_grid_node_;
00164 unsigned int kinematics_cache_points_with_solution_;
00165 unsigned int kinematics_cache_size_;
00167 std::vector<double> kinematics_cache_vector_;
00168 std::vector<unsigned int> num_solutions_vector_;
00170 kinematics::KinematicsBaseConstPtr kinematics_solver_;
00172 planning_models::RobotModelConstPtr kinematic_model_;
00173 planning_models::RobotState* Ptr kinematic_state_;
00175 const planning_models::RobotModel::JointModelGroup* joint_model_group_;
00177 boost::shared_ptr<planning_models::RobotState* ::JointStateGroup> joint_state_group_;
00180
00181
00182 double min_squared_distance_, max_squared_distance_;
00183 };
00184
00185 typedef boost::shared_ptr<KinematicsCache> KinematicsCachePtr;
00186 }
00187
00188 #endif