kinematics_cache.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2012, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Sachin Chitta
00036 *********************************************************************/
00037 
00038 #include <kinematics_cache/kinematics_cache.h>
00039 #include <fstream>
00040 #include <iostream>
00041 
00042 namespace kinematics_cache
00043 {
00044 KinematicsCache::KinematicsCache() : min_squared_distance_(1e6), max_squared_distance_(0.0)
00045 {
00046 }
00047 
00048 bool KinematicsCache::initialize(kinematics::KinematicsBaseConstPtr& kinematics_solver,
00049                                  const planning_models::RobotModelConstPtr& kinematic_model,
00050                                  const KinematicsCache::Options& opt)
00051 {
00052   options_ = opt;
00053   kinematics_solver_ = kinematics_solver;
00054   kinematic_model_ = kinematic_model;
00055   joint_model_group_ = kinematic_model_->getJointModelGroup(kinematics_solver_->getGroupName());
00056   kinematic_state_.reset(new planning_models::RobotState(kinematic_model));
00057   joint_state_group_.reset(
00058       new planning_models::RobotState* ::JointStateGroup(kinematic_state_.get(), joint_model_group_));
00059 
00060   setup(opt);
00061   return true;
00062 }
00063 
00064 void KinematicsCache::setup(const KinematicsCache::Options& opt)
00065 {
00066   cache_origin_ = opt.origin;
00067   cache_resolution_x_ = opt.resolution[0];
00068   cache_resolution_y_ = opt.resolution[1];
00069   cache_resolution_z_ = opt.resolution[2];
00070 
00071   cache_size_x_ = (unsigned int)(opt.workspace_size[0] / opt.resolution[0]);
00072   cache_size_y_ = (unsigned int)(opt.workspace_size[1] / opt.resolution[1]);
00073   cache_size_z_ = (unsigned int)(opt.workspace_size[2] / opt.resolution[2]);
00074   max_solutions_per_grid_location_ = std::max((unsigned int)1, opt.max_solutions_per_grid_location);
00075   solution_dimension_ = joint_model_group_->getVariableCount();
00076   size_grid_node_ = max_solutions_per_grid_location_ * solution_dimension_;
00077   kinematics_cache_size_ = cache_size_x_ * cache_size_y_ * cache_size_z_;
00078   kinematics_cache_points_with_solution_ = 0;
00079   kinematics_cache_vector_.resize(kinematics_cache_size_ * size_grid_node_, 0.0);
00080   num_solutions_vector_.resize(kinematics_cache_size_, 0);
00081   ROS_DEBUG("Origin: %f %f %f", cache_origin_.x, cache_origin_.y, cache_origin_.z);
00082   ROS_DEBUG("Cache size (num points x,y,z): %d %d %d", cache_size_x_, cache_size_y_, cache_size_z_);
00083   ROS_DEBUG("Cache resolution: %f %f %f", cache_resolution_x_, cache_resolution_y_, cache_resolution_z_);
00084   ROS_DEBUG("Solutions per grid location: %d", (int)max_solutions_per_grid_location_);
00085   ROS_DEBUG("Solution dimension: %d", (int)solution_dimension_);
00086 }
00087 
00088 bool KinematicsCache::generateCacheMap(double timeout)
00089 {
00090   ros::WallTime start_time = ros::WallTime::now();
00091   std::vector<std::string> fk_names;
00092   std::vector<double> fk_values;
00093   std::vector<geometry_msgs::Pose> poses;
00094 
00095   fk_names.push_back(kinematics_solver_->getTipFrame());
00096   fk_values.resize(kinematics_solver_->getJointNames().size(), 0.0);
00097   poses.resize(1);
00098 
00099   while ((ros::WallTime::now() - start_time).toSec() <= timeout &&
00100          kinematics_cache_points_with_solution_ <= kinematics_cache_size_)
00101   {
00102     joint_state_group_->setToRandomValues();
00103     joint_state_group_->getGroupStateValues(fk_values);
00104     if (!kinematics_solver_->getPositionFK(fk_names, fk_values, poses))
00105     {
00106       ROS_ERROR("Fk failed");
00107       return false;
00108     }
00109     if (!addToCache(poses[0], fk_values))
00110     {
00111       ROS_DEBUG("Adding to cache failed for: %f %f %f", poses[0].position.x, poses[0].position.y, poses[0].position.z);
00112     }
00113     ROS_DEBUG("Adding: %d", kinematics_cache_points_with_solution_);
00114   }
00115   ROS_INFO("Cache map generated with %d valid points", kinematics_cache_points_with_solution_);
00116   return true;
00117 }
00118 
00119 bool KinematicsCache::addToCache(const geometry_msgs::Pose& pose, const std::vector<double>& joint_values,
00120                                  bool overwrite)
00121 {
00122   unsigned int grid_index;
00123   if (!getGridIndex(pose, grid_index))
00124   {
00125     ROS_DEBUG("Failed to get grid index");
00126     return false;
00127   }
00128   unsigned int num_solutions = num_solutions_vector_[grid_index];
00129   if (!overwrite && num_solutions >= max_solutions_per_grid_location_)
00130   {
00131     ROS_DEBUG("Pose already has max number of solutions");
00132     return true;
00133   }
00134   if (num_solutions == 0)
00135     kinematics_cache_points_with_solution_++;
00136   if (overwrite && num_solutions >= max_solutions_per_grid_location_)
00137     num_solutions = 0;
00138   unsigned int start_index = getSolutionLocation(grid_index, num_solutions);
00139   for (unsigned int i = 0; i < joint_values.size(); ++i)
00140   {
00141     //    ROS_INFO("Joint value[%d]: %f, localtion: %d",i,joint_values[i],start_index+i);
00142     kinematics_cache_vector_[start_index + i] = joint_values[i];
00143   }
00144   if (num_solutions_vector_[grid_index] < max_solutions_per_grid_location_)
00145     num_solutions_vector_[grid_index]++;
00146   updateDistances(pose);
00147   return true;
00148 }
00149 
00150 bool KinematicsCache::getGridIndex(const geometry_msgs::Pose& pose, unsigned int& grid_index) const
00151 {
00152   int x_index = (int)((pose.position.x - cache_origin_.x) / cache_resolution_x_);
00153   int y_index = (int)((pose.position.y - cache_origin_.y) / cache_resolution_y_);
00154   int z_index = (int)((pose.position.z - cache_origin_.z) / cache_resolution_z_);
00155 
00156   if (x_index >= (int)cache_size_x_ || x_index < 0)
00157   {
00158     ROS_DEBUG("X position %f,%d lies outside grid: %d %d", pose.position.x, x_index, 0, cache_size_x_);
00159     return false;
00160   }
00161   if (y_index >= (int)cache_size_y_ || y_index < 0)
00162   {
00163     ROS_DEBUG("Y position %f,%d lies outside grid: %d %d", pose.position.y, y_index, 0, cache_size_y_);
00164     return false;
00165   }
00166   if (z_index >= (int)cache_size_z_ || z_index < 0)
00167   {
00168     ROS_DEBUG("Z position %f,%d lies outside grid: %d %d", pose.position.z, z_index, 0, cache_size_z_);
00169     return false;
00170   }
00171   ROS_DEBUG("Grid indices: %d %d %d", x_index, y_index, z_index);
00172   grid_index = (x_index + y_index * cache_size_x_ + z_index * cache_size_x_ * cache_size_y_);
00173   return true;
00174 }
00175 
00176 bool KinematicsCache::getNumSolutions(const geometry_msgs::Pose& pose, unsigned int& num_solutions) const
00177 {
00178   unsigned int grid_index;
00179   if (!getGridIndex(pose, grid_index))
00180     return false;
00181 
00182   num_solutions = num_solutions_vector_[grid_index];
00183   return true;
00184 }
00185 
00186 unsigned int KinematicsCache::getSolutionLocation(unsigned int& grid_index, unsigned int& solution_index) const
00187 {
00188   ROS_DEBUG("[Grid Index, Solution number location]: %d, %d", grid_index,
00189             grid_index * size_grid_node_ + solution_index * solution_dimension_);
00190   return (grid_index * size_grid_node_ + solution_index * solution_dimension_);
00191 }
00192 
00193 bool KinematicsCache::getSolution(const geometry_msgs::Pose& pose, unsigned int solution_index,
00194                                   std::vector<double>& solution) const
00195 {
00196   unsigned int grid_index;
00197   if (!getGridIndex(pose, grid_index))
00198     return false;
00199   if (solution_index >= max_solutions_per_grid_location_)
00200     return false;
00201   if (solution.size() < solution_dimension_)
00202     return false;
00203 
00204   solution = getSolution(grid_index, solution_index);
00205   return true;
00206 }
00207 
00208 bool KinematicsCache::getSolutions(const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solution) const
00209 {
00210   unsigned int grid_index;
00211   if (!getGridIndex(pose, grid_index))
00212     return false;
00213   if (solution.size() != num_solutions_vector_[grid_index])
00214     return false;
00215   for (unsigned int i = 0; i < solution.size(); i++)
00216   {
00217     if (solution[i].size() != solution_dimension_)
00218       return false;
00219     solution[i] = getSolution(grid_index, i);
00220   }
00221   return true;
00222 }
00223 
00224 std::vector<double> KinematicsCache::getSolution(unsigned int grid_index, unsigned int solution_index) const
00225 {
00226   std::vector<double> solution_local(solution_dimension_);
00227   unsigned int solution_location = getSolutionLocation(grid_index, solution_index);
00228   for (unsigned int i = 0; i < solution_dimension_; ++i)
00229     solution_local[i] = kinematics_cache_vector_[solution_location + i];
00230   return solution_local;
00231 }
00232 
00233 bool KinematicsCache::readFromFile(const std::string& filename)
00234 {
00235   std::ifstream file(filename.c_str());
00236   if (!file.is_open())
00237   {
00238     ROS_WARN("Could not open file: %s", filename.c_str());
00239     return false;
00240   }
00241   std::string group_name;
00242   std::getline(file, group_name);
00243   if (group_name.empty())
00244   {
00245     ROS_ERROR("Could not find group_name in file: %s", group_name.c_str());
00246     file.close();
00247     return false;
00248   }
00249   if (group_name != kinematics_solver_->getGroupName())
00250   {
00251     ROS_ERROR("Input file group name %s does not match solver group name %s", group_name.c_str(),
00252               kinematics_solver_->getGroupName().c_str());
00253     file.close();
00254     return false;
00255   }
00256 
00257   std::string line_string;
00258   std::getline(file, line_string);
00259   std::stringstream line_stream(line_string);
00260   line_stream >> options_.origin.x >> options_.origin.y >> options_.origin.z;
00261 
00262   std::getline(file, line_string);
00263   line_stream.clear();
00264   line_stream.str("");
00265   line_stream << line_string;
00266   line_stream >> options_.workspace_size[0] >> options_.workspace_size[1] >> options_.workspace_size[2];
00267 
00268   std::getline(file, line_string);
00269   line_stream.clear();
00270   line_stream.str("");
00271   line_stream << line_string;
00272   line_stream >> options_.resolution[0] >> options_.resolution[1] >> options_.resolution[2];
00273 
00274   std::getline(file, line_string);
00275   line_stream.clear();
00276   line_stream.str("");
00277   line_stream << line_string;
00278   line_stream >> options_.max_solutions_per_grid_location;
00279 
00280   setup(options_);
00281 
00282   std::getline(file, line_string);
00283   line_stream.clear();
00284   line_stream.str("");
00285   line_stream << line_string;
00286   line_stream >> min_squared_distance_;
00287 
00288   std::getline(file, line_string);
00289   line_stream.clear();
00290   line_stream.str("");
00291   line_stream << line_string;
00292   line_stream >> max_squared_distance_;
00293 
00294   std::vector<double> kinematics_cache_vector;
00295   std::vector<unsigned int> num_solutions_vector;
00296   std::getline(file, line_string);
00297   std::getline(file, line_string);
00298   line_stream.clear();
00299   line_stream.str("");
00300   line_stream << line_string;
00301   double d;
00302   while (line_stream >> d)
00303   {
00304     kinematics_cache_vector.push_back(d);
00305   }
00306 
00307   std::getline(file, line_string);
00308   std::getline(file, line_string);
00309   line_stream.clear();
00310   line_stream.str("");
00311   line_stream << line_string;
00312   unsigned int index;
00313   while (line_stream >> index)
00314   {
00315     num_solutions_vector.push_back(index);
00316   }
00317 
00318   file.close();
00319 
00320   kinematics_cache_vector_ = kinematics_cache_vector;
00321   num_solutions_vector_ = num_solutions_vector;
00322   ROS_DEBUG("Read %d total points from file: %s", (int)num_solutions_vector_.size(), filename.c_str());
00323   return true;
00324 }
00325 
00326 bool KinematicsCache::writeToFile(const std::string& filename)
00327 {
00328   ROS_DEBUG("Writing %d total points to file: %s", (int)num_solutions_vector_.size(), filename.c_str());
00329   std::ofstream file;
00330   file.open(filename.c_str());
00331   if (!file.is_open())
00332   {
00333     ROS_WARN("Could not open file: %s", filename.c_str());
00334     return false;
00335   }
00336   if (file.good())
00337   {
00338     std::string group_name = kinematics_solver_->getGroupName();
00339     file << group_name << std::endl;
00340 
00341     file << options_.origin.x << " " << options_.origin.y << " " << options_.origin.z << std::endl;
00342     file << options_.workspace_size[0] << " " << options_.workspace_size[1] << " " << options_.workspace_size[2]
00343          << std::endl;
00344     file << options_.resolution[0] << " " << options_.resolution[1] << " " << options_.resolution[2] << std::endl;
00345     file << options_.max_solutions_per_grid_location << std::endl;
00346     file << min_squared_distance_ << std::endl;
00347     file << max_squared_distance_ << std::endl;
00348     file << kinematics_cache_vector_.size() << std::endl;
00349     std::copy(kinematics_cache_vector_.begin(), kinematics_cache_vector_.end(),
00350               std::ostream_iterator<double>(file, " "));
00351     file << std::endl;
00352 
00353     file << num_solutions_vector_.size() << std::endl;
00354     std::copy(num_solutions_vector_.begin(), num_solutions_vector_.end(),
00355               std::ostream_iterator<unsigned int>(file, " "));
00356     file << std::endl;
00357   }
00358 
00359   file.close();
00360   return true;
00361 }
00362 
00363 std::pair<double, double> KinematicsCache::getMinMaxSquaredDistance()
00364 {
00365   return std::pair<double, double>(min_squared_distance_, max_squared_distance_);
00366 }
00367 
00368 void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose)
00369 {
00370   double distance_squared =
00371       (pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z);
00372   min_squared_distance_ = std::min<double>(distance_squared, min_squared_distance_);
00373   max_squared_distance_ = std::max<double>(distance_squared, max_squared_distance_);
00374 }
00375 }


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Mon Jul 24 2017 02:21:02