v1/kinematics_cache/src/kinematics_cache.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2012, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Sachin Chitta
36 *********************************************************************/
37 
38 #include <kinematics_cache/kinematics_cache.h>
39 #include <fstream>
40 #include <iostream>
41 
42 namespace kinematics_cache
43 {
44 KinematicsCache::KinematicsCache() : min_squared_distance_(1e6), max_squared_distance_(0.0)
45 {
46 }
47 
48 bool KinematicsCache::initialize(kinematics::KinematicsBaseConstPtr& kinematics_solver,
49  const planning_models::RobotModelConstPtr& kinematic_model,
50  const KinematicsCache::Options& opt)
51 {
52  options_ = opt;
53  kinematics_solver_ = kinematics_solver;
54  kinematic_model_ = kinematic_model;
55  joint_model_group_ = kinematic_model_->getJointModelGroup(kinematics_solver_->getGroupName());
56  kinematic_state_.reset(new planning_models::RobotState(kinematic_model));
57  joint_state_group_.reset(
58  new planning_models::RobotState* ::JointStateGroup(kinematic_state_.get(), joint_model_group_));
59 
60  setup(opt);
61  return true;
62 }
63 
65 {
66  cache_origin_ = opt.origin;
70 
71  cache_size_x_ = (unsigned int)(opt.workspace_size[0] / opt.resolution[0]);
72  cache_size_y_ = (unsigned int)(opt.workspace_size[1] / opt.resolution[1]);
73  cache_size_z_ = (unsigned int)(opt.workspace_size[2] / opt.resolution[2]);
75  solution_dimension_ = joint_model_group_->getVariableCount();
81  ROS_DEBUG("Origin: %f %f %f", cache_origin_.x, cache_origin_.y, cache_origin_.z);
82  ROS_DEBUG("Cache size (num points x,y,z): %d %d %d", cache_size_x_, cache_size_y_, cache_size_z_);
83  ROS_DEBUG("Cache resolution: %f %f %f", cache_resolution_x_, cache_resolution_y_, cache_resolution_z_);
84  ROS_DEBUG("Solutions per grid location: %d", (int)max_solutions_per_grid_location_);
85  ROS_DEBUG("Solution dimension: %d", (int)solution_dimension_);
86 }
87 
89 {
90  ros::WallTime start_time = ros::WallTime::now();
91  std::vector<std::string> fk_names;
92  std::vector<double> fk_values;
93  std::vector<geometry_msgs::Pose> poses;
94 
95  fk_names.push_back(kinematics_solver_->getTipFrame());
96  fk_values.resize(kinematics_solver_->getJointNames().size(), 0.0);
97  poses.resize(1);
98 
99  while ((ros::WallTime::now() - start_time).toSec() <= timeout &&
101  {
102  joint_state_group_->setToRandomValues();
103  joint_state_group_->getGroupStateValues(fk_values);
104  if (!kinematics_solver_->getPositionFK(fk_names, fk_values, poses))
105  {
106  ROS_ERROR("Fk failed");
107  return false;
108  }
109  if (!addToCache(poses[0], fk_values))
110  {
111  ROS_DEBUG("Adding to cache failed for: %f %f %f", poses[0].position.x, poses[0].position.y, poses[0].position.z);
112  }
113  ROS_DEBUG("Adding: %d", kinematics_cache_points_with_solution_);
114  }
115  ROS_INFO("Cache map generated with %d valid points", kinematics_cache_points_with_solution_);
116  return true;
117 }
118 
119 bool KinematicsCache::addToCache(const geometry_msgs::Pose& pose, const std::vector<double>& joint_values,
120  bool overwrite)
121 {
122  unsigned int grid_index;
123  if (!getGridIndex(pose, grid_index))
124  {
125  ROS_DEBUG("Failed to get grid index");
126  return false;
127  }
128  unsigned int num_solutions = num_solutions_vector_[grid_index];
129  if (!overwrite && num_solutions >= max_solutions_per_grid_location_)
130  {
131  ROS_DEBUG("Pose already has max number of solutions");
132  return true;
133  }
134  if (num_solutions == 0)
136  if (overwrite && num_solutions >= max_solutions_per_grid_location_)
137  num_solutions = 0;
138  unsigned int start_index = getSolutionLocation(grid_index, num_solutions);
139  for (unsigned int i = 0; i < joint_values.size(); ++i)
140  {
141  // ROS_INFO("Joint value[%d]: %f, localtion: %d",i,joint_values[i],start_index+i);
142  kinematics_cache_vector_[start_index + i] = joint_values[i];
143  }
145  num_solutions_vector_[grid_index]++;
146  updateDistances(pose);
147  return true;
148 }
149 
150 bool KinematicsCache::getGridIndex(const geometry_msgs::Pose& pose, unsigned int& grid_index) const
151 {
152  int x_index = (int)((pose.position.x - cache_origin_.x) / cache_resolution_x_);
153  int y_index = (int)((pose.position.y - cache_origin_.y) / cache_resolution_y_);
154  int z_index = (int)((pose.position.z - cache_origin_.z) / cache_resolution_z_);
155 
156  if (x_index >= (int)cache_size_x_ || x_index < 0)
157  {
158  ROS_DEBUG("X position %f,%d lies outside grid: %d %d", pose.position.x, x_index, 0, cache_size_x_);
159  return false;
160  }
161  if (y_index >= (int)cache_size_y_ || y_index < 0)
162  {
163  ROS_DEBUG("Y position %f,%d lies outside grid: %d %d", pose.position.y, y_index, 0, cache_size_y_);
164  return false;
165  }
166  if (z_index >= (int)cache_size_z_ || z_index < 0)
167  {
168  ROS_DEBUG("Z position %f,%d lies outside grid: %d %d", pose.position.z, z_index, 0, cache_size_z_);
169  return false;
170  }
171  ROS_DEBUG("Grid indices: %d %d %d", x_index, y_index, z_index);
172  grid_index = (x_index + y_index * cache_size_x_ + z_index * cache_size_x_ * cache_size_y_);
173  return true;
174 }
175 
176 bool KinematicsCache::getNumSolutions(const geometry_msgs::Pose& pose, unsigned int& num_solutions) const
177 {
178  unsigned int grid_index;
179  if (!getGridIndex(pose, grid_index))
180  return false;
181 
182  num_solutions = num_solutions_vector_[grid_index];
183  return true;
184 }
185 
186 unsigned int KinematicsCache::getSolutionLocation(unsigned int& grid_index, unsigned int& solution_index) const
187 {
188  ROS_DEBUG("[Grid Index, Solution number location]: %d, %d", grid_index,
189  grid_index * size_grid_node_ + solution_index * solution_dimension_);
190  return (grid_index * size_grid_node_ + solution_index * solution_dimension_);
191 }
192 
193 bool KinematicsCache::getSolution(const geometry_msgs::Pose& pose, unsigned int solution_index,
194  std::vector<double>& solution) const
195 {
196  unsigned int grid_index;
197  if (!getGridIndex(pose, grid_index))
198  return false;
199  if (solution_index >= max_solutions_per_grid_location_)
200  return false;
201  if (solution.size() < solution_dimension_)
202  return false;
203 
204  solution = getSolution(grid_index, solution_index);
205  return true;
206 }
207 
208 bool KinematicsCache::getSolutions(const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solution) const
209 {
210  unsigned int grid_index;
211  if (!getGridIndex(pose, grid_index))
212  return false;
213  if (solution.size() != num_solutions_vector_[grid_index])
214  return false;
215  for (unsigned int i = 0; i < solution.size(); i++)
216  {
217  if (solution[i].size() != solution_dimension_)
218  return false;
219  solution[i] = getSolution(grid_index, i);
220  }
221  return true;
222 }
223 
224 std::vector<double> KinematicsCache::getSolution(unsigned int grid_index, unsigned int solution_index) const
225 {
226  std::vector<double> solution_local(solution_dimension_);
227  unsigned int solution_location = getSolutionLocation(grid_index, solution_index);
228  for (unsigned int i = 0; i < solution_dimension_; ++i)
229  solution_local[i] = kinematics_cache_vector_[solution_location + i];
230  return solution_local;
231 }
232 
233 bool KinematicsCache::readFromFile(const std::string& filename)
234 {
235  std::ifstream file(filename.c_str());
236  if (!file.is_open())
237  {
238  ROS_WARN("Could not open file: %s", filename.c_str());
239  return false;
240  }
241  std::string group_name;
242  std::getline(file, group_name);
243  if (group_name.empty())
244  {
245  ROS_ERROR("Could not find group_name in file: %s", group_name.c_str());
246  file.close();
247  return false;
248  }
249  if (group_name != kinematics_solver_->getGroupName())
250  {
251  ROS_ERROR("Input file group name %s does not match solver group name %s", group_name.c_str(),
252  kinematics_solver_->getGroupName().c_str());
253  file.close();
254  return false;
255  }
256 
257  std::string line_string;
258  std::getline(file, line_string);
259  std::stringstream line_stream(line_string);
260  line_stream >> options_.origin.x >> options_.origin.y >> options_.origin.z;
261 
262  std::getline(file, line_string);
263  line_stream.clear();
264  line_stream.str("");
265  line_stream << line_string;
267 
268  std::getline(file, line_string);
269  line_stream.clear();
270  line_stream.str("");
271  line_stream << line_string;
272  line_stream >> options_.resolution[0] >> options_.resolution[1] >> options_.resolution[2];
273 
274  std::getline(file, line_string);
275  line_stream.clear();
276  line_stream.str("");
277  line_stream << line_string;
279 
280  setup(options_);
281 
282  std::getline(file, line_string);
283  line_stream.clear();
284  line_stream.str("");
285  line_stream << line_string;
286  line_stream >> min_squared_distance_;
287 
288  std::getline(file, line_string);
289  line_stream.clear();
290  line_stream.str("");
291  line_stream << line_string;
292  line_stream >> max_squared_distance_;
293 
294  std::vector<double> kinematics_cache_vector;
295  std::vector<unsigned int> num_solutions_vector;
296  std::getline(file, line_string);
297  std::getline(file, line_string);
298  line_stream.clear();
299  line_stream.str("");
300  line_stream << line_string;
301  double d;
302  while (line_stream >> d)
303  {
304  kinematics_cache_vector.push_back(d);
305  }
306 
307  std::getline(file, line_string);
308  std::getline(file, line_string);
309  line_stream.clear();
310  line_stream.str("");
311  line_stream << line_string;
312  unsigned int index;
313  while (line_stream >> index)
314  {
315  num_solutions_vector.push_back(index);
316  }
317 
318  file.close();
319 
320  kinematics_cache_vector_ = kinematics_cache_vector;
321  num_solutions_vector_ = num_solutions_vector;
322  ROS_DEBUG("Read %d total points from file: %s", (int)num_solutions_vector_.size(), filename.c_str());
323  return true;
324 }
325 
326 bool KinematicsCache::writeToFile(const std::string& filename)
327 {
328  ROS_DEBUG("Writing %d total points to file: %s", (int)num_solutions_vector_.size(), filename.c_str());
329  std::ofstream file;
330  file.open(filename.c_str());
331  if (!file.is_open())
332  {
333  ROS_WARN("Could not open file: %s", filename.c_str());
334  return false;
335  }
336  if (file.good())
337  {
338  std::string group_name = kinematics_solver_->getGroupName();
339  file << group_name << std::endl;
340 
341  file << options_.origin.x << " " << options_.origin.y << " " << options_.origin.z << std::endl;
342  file << options_.workspace_size[0] << " " << options_.workspace_size[1] << " " << options_.workspace_size[2]
343  << std::endl;
344  file << options_.resolution[0] << " " << options_.resolution[1] << " " << options_.resolution[2] << std::endl;
345  file << options_.max_solutions_per_grid_location << std::endl;
346  file << min_squared_distance_ << std::endl;
347  file << max_squared_distance_ << std::endl;
348  file << kinematics_cache_vector_.size() << std::endl;
349  std::copy(kinematics_cache_vector_.begin(), kinematics_cache_vector_.end(),
350  std::ostream_iterator<double>(file, " "));
351  file << std::endl;
352 
353  file << num_solutions_vector_.size() << std::endl;
354  std::copy(num_solutions_vector_.begin(), num_solutions_vector_.end(),
355  std::ostream_iterator<unsigned int>(file, " "));
356  file << std::endl;
357  }
358 
359  file.close();
360  return true;
361 }
362 
363 std::pair<double, double> KinematicsCache::getMinMaxSquaredDistance()
364 {
365  return std::pair<double, double>(min_squared_distance_, max_squared_distance_);
366 }
367 
368 void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose)
369 {
370  double distance_squared =
371  (pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z);
372  min_squared_distance_ = std::min<double>(distance_squared, min_squared_distance_);
373  max_squared_distance_ = std::max<double>(distance_squared, max_squared_distance_);
374 }
375 }
void setup(const KinematicsCache::Options &opt)
Setup the cache.
bool initialize(kinematics::KinematicsBaseConstPtr &solver, const planning_models::RobotModelConstPtr &kinematic_model, const KinematicsCache::Options &opt)
Initialize the cache class.
bool addToCache(const geometry_msgs::Pose &pose, const std::vector< double > &joint_values, bool overwrite=false)
Add a new solution to the cache at the given location.
bool generateCacheMap(double timeout)
Generate the cache map spending timeout (seconds) on the generation process)
unsigned int getSolutionLocation(unsigned int &grid_index, unsigned int &solution_index) const
Get the location of a solution given the grid index and solution index.
bool getGridIndex(const geometry_msgs::Pose &pose, unsigned int &grid_index) const
Get the grid index for a given pose.
boost::shared_ptr< planning_models::RobotState *::JointStateGroup > joint_state_group_
bool getSolution(const geometry_msgs::Pose &pose, unsigned int solution_index, std::vector< double > &solution) const
Get a candidate solution for a particular pose. Note that the pose will be projected onto the grid us...
bool getNumSolutions(const geometry_msgs::Pose &pose, unsigned int &num_solutions) const
Get number of candidate solutions for a particular pose.
bool getSolutions(const geometry_msgs::Pose &pose, std::vector< std::vector< double > > &solutions) const
Get all candidate solutions for a particular pose. Note that the pose will be projected onto the grid...
const planning_models::RobotModel::JointModelGroup * joint_model_group_


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Sun Oct 18 2020 13:17:18