v2/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 
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 robot_model::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 robot_state::RobotState(kinematic_model));
57  joint_state_group_.reset(new robot_state::JointStateGroup(kinematic_state_.get(), joint_model_group_));
58 
59  setup(opt);
60  return true;
61 }
62 
64 {
65  cache_origin_ = opt.origin;
69 
70  cache_size_x_ = (unsigned int)(opt.workspace_size[0] / opt.resolution[0]);
71  cache_size_y_ = (unsigned int)(opt.workspace_size[1] / opt.resolution[1]);
72  cache_size_z_ = (unsigned int)(opt.workspace_size[2] / opt.resolution[2]);
74  solution_dimension_ = joint_model_group_->getVariableCount();
80  ROS_DEBUG_NAMED("kinematics_cache", "Origin: %f %f %f", cache_origin_.x, cache_origin_.y, cache_origin_.z);
81  ROS_DEBUG_NAMED("kinematics_cache", "Cache size (num points x,y,z): %d %d %d", cache_size_x_, cache_size_y_,
82  cache_size_z_);
83  ROS_DEBUG_NAMED("kinematics_cache", "Cache resolution: %f %f %f", cache_resolution_x_, cache_resolution_y_,
85  ROS_DEBUG_NAMED("kinematics_cache", "Solutions per grid location: %d", (int)max_solutions_per_grid_location_);
86  ROS_DEBUG_NAMED("kinematics_cache", "Solution dimension: %d", (int)solution_dimension_);
87 }
88 
89 bool KinematicsCache::generateCacheMap(double timeout)
90 {
91  ros::WallTime start_time = ros::WallTime::now();
92  std::vector<std::string> fk_names;
93  std::vector<double> fk_values;
94  std::vector<geometry_msgs::Pose> poses;
95 
96  fk_names.push_back(kinematics_solver_->getTipFrame());
97  fk_values.resize(kinematics_solver_->getJointNames().size(), 0.0);
98  poses.resize(1);
99 
100  while ((ros::WallTime::now() - start_time).toSec() <= timeout &&
102  {
103  joint_state_group_->setToRandomValues();
104  joint_state_group_->getVariableValues(fk_values);
105  if (!kinematics_solver_->getPositionFK(fk_names, fk_values, poses))
106  {
107  ROS_ERROR_NAMED("kinematics_cache", "Fk failed");
108  return false;
109  }
110  if (!addToCache(poses[0], fk_values))
111  {
112  ROS_DEBUG_NAMED("kinematics_cache", "Adding to cache failed for: %f %f %f", poses[0].position.x,
113  poses[0].position.y, poses[0].position.z);
114  }
115  ROS_DEBUG_NAMED("kinematics_cache", "Adding: %d", kinematics_cache_points_with_solution_);
116  }
117  ROS_DEBUG_NAMED("kinematics_cache", "Cache map generated with %d valid points",
119  return true;
120 }
121 
122 bool KinematicsCache::addToCache(const geometry_msgs::Pose& pose, const std::vector<double>& joint_values,
123  bool overwrite)
124 {
125  unsigned int grid_index;
126  if (!getGridIndex(pose, grid_index))
127  {
128  ROS_DEBUG_NAMED("kinematics_cache", "Failed to get grid index");
129  return false;
130  }
131  unsigned int num_solutions = num_solutions_vector_[grid_index];
132  if (!overwrite && num_solutions >= max_solutions_per_grid_location_)
133  {
134  ROS_DEBUG_NAMED("kinematics_cache", "Pose already has max number of solutions");
135  return true;
136  }
137  if (num_solutions == 0)
139  if (overwrite && num_solutions >= max_solutions_per_grid_location_)
140  num_solutions = 0;
141  unsigned int start_index = getSolutionLocation(grid_index, num_solutions);
142  for (unsigned int i = 0; i < joint_values.size(); ++i)
143  {
144  // ROS_INFO("Joint value[%d]: %f, localtion: %d",i,joint_values[i],start_index+i);
145  kinematics_cache_vector_[start_index + i] = joint_values[i];
146  }
148  num_solutions_vector_[grid_index]++;
149  updateDistances(pose);
150  return true;
151 }
152 
153 bool KinematicsCache::getGridIndex(const geometry_msgs::Pose& pose, unsigned int& grid_index) const
154 {
155  int x_index = (int)((pose.position.x - cache_origin_.x) / cache_resolution_x_);
156  int y_index = (int)((pose.position.y - cache_origin_.y) / cache_resolution_y_);
157  int z_index = (int)((pose.position.z - cache_origin_.z) / cache_resolution_z_);
158 
159  if (x_index >= (int)cache_size_x_ || x_index < 0)
160  {
161  ROS_DEBUG_NAMED("kinematics_cache", "X position %f,%d lies outside grid: %d %d", pose.position.x, x_index, 0,
162  cache_size_x_);
163  return false;
164  }
165  if (y_index >= (int)cache_size_y_ || y_index < 0)
166  {
167  ROS_DEBUG_NAMED("kinematics_cache", "Y position %f,%d lies outside grid: %d %d", pose.position.y, y_index, 0,
168  cache_size_y_);
169  return false;
170  }
171  if (z_index >= (int)cache_size_z_ || z_index < 0)
172  {
173  ROS_DEBUG_NAMED("kinematics_cache", "Z position %f,%d lies outside grid: %d %d", pose.position.z, z_index, 0,
174  cache_size_z_);
175  return false;
176  }
177  ROS_DEBUG_NAMED("kinematics_cache", "Grid indices: %d %d %d", x_index, y_index, z_index);
178  grid_index = (x_index + y_index * cache_size_x_ + z_index * cache_size_x_ * cache_size_y_);
179  return true;
180 }
181 
182 bool KinematicsCache::getNumSolutions(const geometry_msgs::Pose& pose, unsigned int& num_solutions) const
183 {
184  unsigned int grid_index;
185  if (!getGridIndex(pose, grid_index))
186  return false;
187 
188  num_solutions = num_solutions_vector_[grid_index];
189  return true;
190 }
191 
192 unsigned int KinematicsCache::getSolutionLocation(unsigned int& grid_index, unsigned int& solution_index) const
193 {
194  ROS_DEBUG_NAMED("kinematics_cache", "[Grid Index, Solution number location]: %d, %d", grid_index,
195  grid_index * size_grid_node_ + solution_index * solution_dimension_);
196  return (grid_index * size_grid_node_ + solution_index * solution_dimension_);
197 }
198 
199 bool KinematicsCache::getSolution(const geometry_msgs::Pose& pose, unsigned int solution_index,
200  std::vector<double>& solution) const
201 {
202  unsigned int grid_index;
203  if (!getGridIndex(pose, grid_index))
204  return false;
205  if (solution_index >= max_solutions_per_grid_location_)
206  return false;
207  if (solution.size() < solution_dimension_)
208  return false;
209 
210  solution = getSolution(grid_index, solution_index);
211  return true;
212 }
213 
214 bool KinematicsCache::getSolutions(const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solution) const
215 {
216  unsigned int grid_index;
217  if (!getGridIndex(pose, grid_index))
218  return false;
219  if (solution.size() != num_solutions_vector_[grid_index])
220  return false;
221  for (unsigned int i = 0; i < solution.size(); i++)
222  {
223  if (solution[i].size() != solution_dimension_)
224  return false;
225  solution[i] = getSolution(grid_index, i);
226  }
227  return true;
228 }
229 
230 std::vector<double> KinematicsCache::getSolution(unsigned int grid_index, unsigned int solution_index) const
231 {
232  std::vector<double> solution_local(solution_dimension_);
233  unsigned int solution_location = getSolutionLocation(grid_index, solution_index);
234  for (unsigned int i = 0; i < solution_dimension_; ++i)
235  solution_local[i] = kinematics_cache_vector_[solution_location + i];
236  return solution_local;
237 }
238 
239 bool KinematicsCache::readFromFile(const std::string& filename)
240 {
241  std::ifstream file(filename.c_str());
242  if (!file.is_open())
243  {
244  ROS_DEBUG_NAMED("kinematics_cache", "Could not open file: %s", filename.c_str());
245  return false;
246  }
247  std::string group_name;
248  std::getline(file, group_name);
249  if (group_name.empty())
250  {
251  ROS_ERROR_NAMED("kinematics_cache", "Could not find group_name in file: %s", group_name.c_str());
252  file.close();
253  return false;
254  }
255  if (group_name != kinematics_solver_->getGroupName())
256  {
257  ROS_ERROR_NAMED("kinematics_cache", "Input file group name %s does not match solver group name %s",
258  group_name.c_str(), kinematics_solver_->getGroupName().c_str());
259  file.close();
260  return false;
261  }
262 
263  std::string line_string;
264  std::getline(file, line_string);
265  std::stringstream line_stream(line_string);
266  line_stream >> options_.origin.x >> options_.origin.y >> options_.origin.z;
267 
268  std::getline(file, line_string);
269  line_stream.clear();
270  line_stream.str("");
271  line_stream << line_string;
273 
274  std::getline(file, line_string);
275  line_stream.clear();
276  line_stream.str("");
277  line_stream << line_string;
278  line_stream >> options_.resolution[0] >> options_.resolution[1] >> options_.resolution[2];
279 
280  std::getline(file, line_string);
281  line_stream.clear();
282  line_stream.str("");
283  line_stream << line_string;
285 
286  setup(options_);
287 
288  std::getline(file, line_string);
289  line_stream.clear();
290  line_stream.str("");
291  line_stream << line_string;
292  line_stream >> min_squared_distance_;
293 
294  std::getline(file, line_string);
295  line_stream.clear();
296  line_stream.str("");
297  line_stream << line_string;
298  line_stream >> max_squared_distance_;
299 
300  std::vector<double> kinematics_cache_vector;
301  std::vector<unsigned int> num_solutions_vector;
302  std::getline(file, line_string);
303  std::getline(file, line_string);
304  line_stream.clear();
305  line_stream.str("");
306  line_stream << line_string;
307  double d;
308  while (line_stream >> d)
309  {
310  kinematics_cache_vector.push_back(d);
311  }
312 
313  std::getline(file, line_string);
314  std::getline(file, line_string);
315  line_stream.clear();
316  line_stream.str("");
317  line_stream << line_string;
318  unsigned int index;
319  while (line_stream >> index)
320  {
321  num_solutions_vector.push_back(index);
322  }
323 
324  file.close();
325 
326  kinematics_cache_vector_ = kinematics_cache_vector;
327  num_solutions_vector_ = num_solutions_vector;
328  ROS_DEBUG_NAMED("kinematics_cache", "Read %d total points from file: %s", (int)num_solutions_vector_.size(),
329  filename.c_str());
330  return true;
331 }
332 
333 bool KinematicsCache::writeToFile(const std::string& filename)
334 {
335  ROS_DEBUG_NAMED("kinematics_cache", "Writing %d total points to file: %s", (int)num_solutions_vector_.size(),
336  filename.c_str());
337  std::ofstream file;
338  file.open(filename.c_str());
339  if (!file.is_open())
340  {
341  ROS_DEBUG_NAMED("kinematics_cache", "Could not open file: %s", filename.c_str());
342  return false;
343  }
344  if (file.good())
345  {
346  std::string group_name = kinematics_solver_->getGroupName();
347  file << group_name << std::endl;
348 
349  file << options_.origin.x << " " << options_.origin.y << " " << options_.origin.z << std::endl;
350  file << options_.workspace_size[0] << " " << options_.workspace_size[1] << " " << options_.workspace_size[2]
351  << std::endl;
352  file << options_.resolution[0] << " " << options_.resolution[1] << " " << options_.resolution[2] << std::endl;
353  file << options_.max_solutions_per_grid_location << std::endl;
354  file << min_squared_distance_ << std::endl;
355  file << max_squared_distance_ << std::endl;
356  file << kinematics_cache_vector_.size() << std::endl;
357  std::copy(kinematics_cache_vector_.begin(), kinematics_cache_vector_.end(),
358  std::ostream_iterator<double>(file, " "));
359  file << std::endl;
360 
361  file << num_solutions_vector_.size() << std::endl;
362  std::copy(num_solutions_vector_.begin(), num_solutions_vector_.end(),
363  std::ostream_iterator<unsigned int>(file, " "));
364  file << std::endl;
365  }
366 
367  file.close();
368  return true;
369 }
370 
371 std::pair<double, double> KinematicsCache::getMinMaxSquaredDistance()
372 {
373  return std::pair<double, double>(min_squared_distance_, max_squared_distance_);
374 }
375 
376 void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose)
377 {
378  double distance_squared =
379  (pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z);
380  min_squared_distance_ = std::min<double>(distance_squared, min_squared_distance_);
381  max_squared_distance_ = std::max<double>(distance_squared, max_squared_distance_);
382 }
383 }
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 Wed Jul 10 2019 04:03:02