text: /*********************************************************************
   *
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2012, Willow Garage, Inc.
   *  All rights reserved.
   *
   *  Redistribution and use in source and binary forms, with or without
   *  modification, are permitted provided that the following conditions
   *  are met:
   *
   *   * Redistributions of source code must retain the above copyright
   *     notice, this list of conditions and the following disclaimer.
   *   * Redistributions in binary form must reproduce the above
   *     copyright notice, this list of conditions and the following
   *     disclaimer in the documentation and/or other materials provided
   *     with the distribution.
   *   * Neither the name of Willow Garage, Inc. nor the names of its
   *     contributors may be used to endorse or promote products derived
   *     from this software without specific prior written permission.
   *
   *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
   *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   *  POSSIBILITY OF SUCH DAMAGE.
   *
   * Author: Sachin Chitta
   *********************************************************************/
  
  #include <kinematics_cache/kinematics_cache.h>
  #include <fstream>
  #include <iostream>
  
  namespace kinematics_cache
  {
  KinematicsCache::KinematicsCache() : min_squared_distance_(1e6), max_squared_distance_(0.0)
  {
  }
  
  bool KinematicsCache::initialize(kinematics::KinematicsBaseConstPtr& kinematics_solver,
                                   const planning_models::RobotModelConstPtr& kinematic_model,
                                   const KinematicsCache::Options& opt)
  {
    options_ = opt;
    kinematics_solver_ = kinematics_solver;
    kinematic_model_ = kinematic_model;
    joint_model_group_ = kinematic_model_->getJointModelGroup(kinematics_solver_->getGroupName());
    kinematic_state_ = std::make_shared<planning_models::RobotState>(kinematic_model);
    joint_state_group_ = std::make_shared<planning_models::JointStateGroup>(kinematic_state_.get(), joint_model_group_);
  
    setup(opt);
    return true;
  }
  
  void KinematicsCache::setup(const KinematicsCache::Options& opt)
  {
    cache_origin_ = opt.origin;
    cache_resolution_x_ = opt.resolution[0];
    cache_resolution_y_ = opt.resolution[1];
    cache_resolution_z_ = opt.resolution[2];
  
    cache_size_x_ = (unsigned int)(opt.workspace_size[0] / opt.resolution[0]);
    cache_size_y_ = (unsigned int)(opt.workspace_size[1] / opt.resolution[1]);
    cache_size_z_ = (unsigned int)(opt.workspace_size[2] / opt.resolution[2]);
    max_solutions_per_grid_location_ = std::max((unsigned int)1, opt.max_solutions_per_grid_location);
    solution_dimension_ = joint_model_group_->getVariableCount();
    size_grid_node_ = max_solutions_per_grid_location_ * solution_dimension_;
    kinematics_cache_size_ = cache_size_x_ * cache_size_y_ * cache_size_z_;
    kinematics_cache_points_with_solution_ = 0;
    kinematics_cache_vector_.resize(kinematics_cache_size_ * size_grid_node_, 0.0);
    num_solutions_vector_.resize(kinematics_cache_size_, 0);
    ROS_DEBUG("Origin: %f %f %f", cache_origin_.x, cache_origin_.y, cache_origin_.z);
    ROS_DEBUG("Cache size (num points x,y,z): %d %d %d", cache_size_x_, cache_size_y_, cache_size_z_);
    ROS_DEBUG("Cache resolution: %f %f %f", cache_resolution_x_, cache_resolution_y_, cache_resolution_z_);
    ROS_DEBUG("Solutions per grid location: %d", static_cast<int> max_solutions_per_grid_location_);
    ROS_DEBUG("Solution dimension: %d", static_cast<int> solution_dimension_);
  }
  
  bool KinematicsCache::generateCacheMap(double timeout)
  {
    ros::WallTime start_time = ros::WallTime::now();
    std::vector<std::string> fk_names;
    std::vector<double> fk_values;
    std::vector<geometry_msgs::Pose> poses;
  
    fk_names.push_back(kinematics_solver_->getTipFrame());
    fk_values.resize(kinematics_solver_->getJointNames().size(), 0.0);
    poses.resize(1);
  
    while ((ros::WallTime::now() - start_time).toSec() <= timeout &&
           kinematics_cache_points_with_solution_ <= kinematics_cache_size_)
    {
      joint_state_group_->setToRandomValues();
      joint_state_group_->getGroupStateValues(fk_values);
      if (!kinematics_solver_->getPositionFK(fk_names, fk_values, poses))
      {
        ROS_ERROR("Fk failed");
        return false;
      }
      if (!addToCache(poses[0], fk_values))
      {
        ROS_DEBUG("Adding to cache failed for: %f %f %f", poses[0].position.x, poses[0].position.y, poses[0].position.z);
      }
      ROS_DEBUG("Adding: %d", kinematics_cache_points_with_solution_);
    }
    ROS_INFO("Cache map generated with %d valid points", kinematics_cache_points_with_solution_);
    return true;
  }
  
  bool KinematicsCache::addToCache(const geometry_msgs::Pose& pose, const std::vector<double>& joint_values,
                                   bool overwrite)
  {
    unsigned int grid_index;
    if (!getGridIndex(pose, grid_index))
    {
      ROS_DEBUG("Failed to get grid index");
      return false;
    }
    unsigned int num_solutions = num_solutions_vector_[grid_index];
    if (!overwrite && num_solutions >= max_solutions_per_grid_location_)
    {
      ROS_DEBUG("Pose already has max number of solutions");
      return true;
    }
    if (num_solutions == 0)
      kinematics_cache_points_with_solution_++;
    if (overwrite && num_solutions >= max_solutions_per_grid_location_)
      num_solutions = 0;
    unsigned int start_index = getSolutionLocation(grid_index, num_solutions);
    for (unsigned int i = 0; i < joint_values.size(); ++i)
    {
      //    ROS_INFO("Joint value[%d]: %f, location: %d",i,joint_values[i],start_index+i);
      kinematics_cache_vector_[start_index + i] = joint_values[i];
    }
    if (num_solutions_vector_[grid_index] < max_solutions_per_grid_location_)
      num_solutions_vector_[grid_index]++;
    updateDistances(pose);
    return true;
  }
  
  bool KinematicsCache::getGridIndex(const geometry_msgs::Pose& pose, unsigned int& grid_index) const
  {
    int x_index = static_cast<int>((pose.position.x - cache_origin_.x) / cache_resolution_x_);
    int y_index = static_cast<int>((pose.position.y - cache_origin_.y) / cache_resolution_y_);
    int z_index = static_cast<int>((pose.position.z - cache_origin_.z) / cache_resolution_z_);
  
    if (x_index >= static_cast<int>(cache_size_x_ || x_index < 0))
    {
      ROS_DEBUG("X position %f,%d lies outside grid: %d %d", pose.position.x, x_index, 0, cache_size_x_);
      return false;
    }
    if (y_index >= static_cast<int>(cache_size_y_ || y_index < 0))
    {
      ROS_DEBUG("Y position %f,%d lies outside grid: %d %d", pose.position.y, y_index, 0, cache_size_y_);
      return false;
    }
    if (z_index >= static_cast<int>(cache_size_z_ || z_index < 0))
    {
      ROS_DEBUG("Z position %f,%d lies outside grid: %d %d", pose.position.z, z_index, 0, cache_size_z_);
      return false;
    }
    ROS_DEBUG("Grid indices: %d %d %d", x_index, y_index, z_index);
    grid_index = (x_index + y_index * cache_size_x_ + z_index * cache_size_x_ * cache_size_y_);
    return true;
  }
  
  bool KinematicsCache::getNumSolutions(const geometry_msgs::Pose& pose, unsigned int& num_solutions) const
  {
    unsigned int grid_index;
    if (!getGridIndex(pose, grid_index))
      return false;
  
    num_solutions = num_solutions_vector_[grid_index];
    return true;
  }
  
  unsigned int KinematicsCache::getSolutionLocation(unsigned int& grid_index, unsigned int& solution_index) const
  {
    ROS_DEBUG("[Grid Index, Solution number location]: %d, %d", grid_index,
              grid_index * size_grid_node_ + solution_index * solution_dimension_);
    return (grid_index * size_grid_node_ + solution_index * solution_dimension_);
  }
  
  bool KinematicsCache::getSolution(const geometry_msgs::Pose& pose, unsigned int solution_index,
                                    std::vector<double>& solution) const
  {
    unsigned int grid_index;
    if (!getGridIndex(pose, grid_index))
      return false;
    if (solution_index >= max_solutions_per_grid_location_)
      return false;
    if (solution.size() < solution_dimension_)
      return false;
  
    solution = getSolution(grid_index, solution_index);
    return true;
  }
  
  bool KinematicsCache::getSolutions(const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solution) const
  {
    unsigned int grid_index;
    if (!getGridIndex(pose, grid_index))
      return false;
    if (solution.size() != num_solutions_vector_[grid_index])
      return false;
    for (unsigned int i = 0; i < solution.size(); ++i)
    {
      if (solution[i].size() != solution_dimension_)
        return false;
      solution[i] = getSolution(grid_index, i);
    }
    return true;
  }
  
  std::vector<double> KinematicsCache::getSolution(unsigned int grid_index, unsigned int solution_index) const
  {
    std::vector<double> solution_local(solution_dimension_);
    unsigned int solution_location = getSolutionLocation(grid_index, solution_index);
    for (unsigned int i = 0; i < solution_dimension_; ++i)
      solution_local[i] = kinematics_cache_vector_[solution_location + i];
    return solution_local;
  }
  
  bool KinematicsCache::readFromFile(const std::string& filename)
  {
    std::ifstream file(filename.c_str());
    if (!file.is_open())
    {
      ROS_WARN("Could not open file: %s", filename.c_str());
      return false;
    }
    std::string group_name;
    std::getline(file, group_name);
    if (group_name.empty())
    {
      ROS_ERROR("Could not find group_name in file: %s", group_name.c_str());
      file.close();
      return false;
    }
    if (group_name != kinematics_solver_->getGroupName())
    {
      ROS_ERROR("Input file group name %s does not match solver group name %s", group_name.c_str(),
                kinematics_solver_->getGroupName().c_str());
      file.close();
      return false;
    }
  
    std::string line_string;
    std::getline(file, line_string);
    std::stringstream line_stream(line_string);
    line_stream >> options_.origin.x >> options_.origin.y >> options_.origin.z;
  
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    line_stream >> options_.workspace_size[0] >> options_.workspace_size[1] >> options_.workspace_size[2];
  
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    line_stream >> options_.resolution[0] >> options_.resolution[1] >> options_.resolution[2];
  
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    line_stream >> options_.max_solutions_per_grid_location;
  
    setup(options_);
  
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    line_stream >> min_squared_distance_;
  
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    line_stream >> max_squared_distance_;
  
    std::vector<double> kinematics_cache_vector;
    std::vector<unsigned int> num_solutions_vector;
    std::getline(file, line_string);
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    double d;
    while (line_stream >> d)
    {
      kinematics_cache_vector.push_back(d);
    }
  
    std::getline(file, line_string);
    std::getline(file, line_string);
    line_stream.clear();
    line_stream.str("");
    line_stream << line_string;
    unsigned int index;
    while (line_stream >> index)
    {
      num_solutions_vector.push_back(index);
    }
  
    file.close();
  
    kinematics_cache_vector_ = kinematics_cache_vector;
    num_solutions_vector_ = num_solutions_vector;
    ROS_DEBUG("Read %d total points from file: %s", static_cast<int> num_solutions_vector_.size(), filename.c_str());
    return true;
  }
  
  bool KinematicsCache::writeToFile(const std::string& filename)
  {
    ROS_DEBUG("Writing %d total points to file: %s", static_cast<int> num_solutions_vector_.size(), filename.c_str());
    std::ofstream file;
    file.open(filename.c_str());
    if (!file.is_open())
    {
      ROS_WARN("Could not open file: %s", filename.c_str());
      return false;
    }
    if (file.good())
    {
      std::string group_name = kinematics_solver_->getGroupName();
      file << group_name << '\n';
  
      file << options_.origin.x << " " << options_.origin.y << " " << options_.origin.z << '\n';
      file << options_.workspace_size[0] << " " << options_.workspace_size[1] << " " << options_.workspace_size[2]
           << '\n';
      file << options_.resolution[0] << " " << options_.resolution[1] << " " << options_.resolution[2] << '\n';
      file << options_.max_solutions_per_grid_location << '\n';
      file << min_squared_distance_ << '\n';
      file << max_squared_distance_ << '\n';
      file << kinematics_cache_vector_.size() << '\n';
      std::copy(kinematics_cache_vector_.begin(), kinematics_cache_vector_.end(),
                std::ostream_iterator<double>(file, " "));
      file << '\n';
  
      file << num_solutions_vector_.size() << '\n';
      std::copy(num_solutions_vector_.begin(), num_solutions_vector_.end(),
                std::ostream_iterator<unsigned int>(file, " "));
      file << '\n';
    }
  
    file.close();
    return true;
  }
  
  std::pair<double, double> KinematicsCache::getMinMaxSquaredDistance()
  {
    return std::pair<double, double>(min_squared_distance_, max_squared_distance_);
  }
  
  void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose)
  {
    double distance_squared =
        (pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z);
    min_squared_distance_ = std::min<double>(distance_squared, min_squared_distance_);
    max_squared_distance_ = std::max<double>(distance_squared, max_squared_distance_);
  }
  }  // namespace kinematics_cache
  