moveit2
The MoveIt Motion Planning Framework for ROS 2.
ik_cache.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Rice University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Rice University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mark Moll */
36 
37 #include <numeric>
38 #include <filesystem>
39 #include <fstream>
40 
42 
44 {
46 {
47  // set distance function for nearest-neighbor queries
48  ik_nn_.setDistanceFunction([](const IKEntry* entry1, const IKEntry* entry2) {
49  double dist = 0.;
50  for (unsigned int i = 0; i < entry1->first.size(); ++i)
51  dist += entry1->first[i].distance(entry2->first[i]);
52  return dist;
53  });
54 }
55 
57 {
58  if (!ik_cache_.empty())
59  saveCache();
60 }
61 
62 void IKCache::initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name,
63  const unsigned int num_joints, const Options& opts)
64 {
65  // read ROS parameters
67  ik_cache_.reserve(max_cache_size_);
71  std::string cached_ik_path = opts.cached_ik_path;
72 
73  // use mutex lock for rest of initialization
74  std::lock_guard<std::mutex> slock(lock_);
75  // determine cache file name
76  std::filesystem::path prefix(!cached_ik_path.empty() ? std::filesystem::path(cached_ik_path) :
77  std::filesystem::current_path());
78  // create cache directory if necessary
79  std::filesystem::create_directories(prefix);
80 
81  cache_file_name_ = prefix / (robot_id + group_name + "_" + cache_name + "_" + std::to_string(max_cache_size_) + "_" +
82  std::to_string(min_pose_distance_) + "_" +
83  std::to_string(std::sqrt(min_config_distance2_)) + ".ikcache");
84 
85  ik_cache_.clear();
86  ik_nn_.clear();
88  if (std::filesystem::exists(cache_file_name_))
89  {
90  // read cache
91  std::ifstream cache_file(cache_file_name_, std::ios_base::binary | std::ios_base::in);
92  cache_file.read((char*)&last_saved_cache_size_, sizeof(unsigned int));
93  unsigned int num_dofs;
94  cache_file.read((char*)&num_dofs, sizeof(unsigned int));
95  unsigned int num_tips;
96  cache_file.read((char*)&num_tips, sizeof(unsigned int));
97 
98  RCLCPP_INFO(LOGGER, "Found %d IK solutions for a %d-dof system with %d end effectors in %s", last_saved_cache_size_,
99  num_dofs, num_tips, cache_file_name_.string().c_str());
100 
101  unsigned int position_size = 3 * sizeof(tf2Scalar);
102  unsigned int orientation_size = 4 * sizeof(tf2Scalar);
103  unsigned int pose_size = position_size + orientation_size;
104  unsigned int config_size = num_dofs * sizeof(double);
105  unsigned int offset_conf = pose_size * num_tips;
106  unsigned int bufsize = offset_conf + config_size;
107  char* buffer = new char[bufsize];
108  IKEntry entry;
109  entry.first.resize(num_tips);
110  entry.second.resize(num_dofs);
112 
113  for (unsigned i = 0; i < last_saved_cache_size_; ++i)
114  {
115  unsigned int j = 0;
116  cache_file.read(buffer, bufsize);
117  for (auto& pose : entry.first)
118  {
119  memcpy(&pose.position[0], buffer + j * pose_size, position_size);
120  memcpy(&pose.orientation[0], buffer + j * pose_size + position_size, orientation_size);
121  ++j;
122  }
123  memcpy(&entry.second[0], buffer + offset_conf, config_size);
124  ik_cache_.push_back(entry);
125  }
126  RCLCPP_INFO(LOGGER, "freeing buffer");
127  delete[] buffer;
128  RCLCPP_INFO(LOGGER, "freed buffer");
129  std::vector<IKEntry*> ik_entry_ptrs(last_saved_cache_size_);
130  for (unsigned int i = 0; i < last_saved_cache_size_; ++i)
131  ik_entry_ptrs[i] = &ik_cache_[i];
132  ik_nn_.add(ik_entry_ptrs);
133  }
134 
135  num_joints_ = num_joints;
136 
137  RCLCPP_INFO(LOGGER, "cache file %s initialized!", cache_file_name_.string().c_str());
138 }
139 
140 double IKCache::configDistance2(const std::vector<double>& config1, const std::vector<double>& config2) const
141 {
142  double dist = 0., diff;
143  for (unsigned int i = 0; i < config1.size(); ++i)
144  {
145  diff = config1[i] - config2[i];
146  dist += diff * diff;
147  }
148  return dist;
149 }
150 
152 {
153  if (ik_cache_.empty())
154  {
155  static IKEntry dummy = std::make_pair(std::vector<Pose>(1, pose), std::vector<double>(num_joints_, 0.));
156  return dummy;
157  }
158  IKEntry query = std::make_pair(std::vector<Pose>(1, pose), std::vector<double>());
159  return *ik_nn_.nearest(&query);
160 }
161 
162 const IKCache::IKEntry& IKCache::getBestApproximateIKSolution(const std::vector<Pose>& poses) const
163 {
164  if (ik_cache_.empty())
165  {
166  static IKEntry dummy = std::make_pair(poses, std::vector<double>(num_joints_, 0.));
167  return dummy;
168  }
169  IKEntry query = std::make_pair(poses, std::vector<double>());
170  return *ik_nn_.nearest(&query);
171 }
172 
173 void IKCache::updateCache(const IKEntry& nearest, const Pose& pose, const std::vector<double>& config) const
174 {
175  if (ik_cache_.size() < ik_cache_.capacity() && (nearest.first[0].distance(pose) > min_pose_distance_ ||
176  configDistance2(nearest.second, config) > min_config_distance2_))
177  {
178  std::lock_guard<std::mutex> slock(lock_);
179  ik_cache_.emplace_back(std::vector<Pose>(1u, pose), config);
180  ik_nn_.add(&ik_cache_.back());
181  if (ik_cache_.size() >= last_saved_cache_size_ + 500u || ik_cache_.size() == max_cache_size_)
182  saveCache();
183  }
184 }
185 
186 void IKCache::updateCache(const IKEntry& nearest, const std::vector<Pose>& poses,
187  const std::vector<double>& config) const
188 {
189  if (ik_cache_.size() < ik_cache_.capacity())
190  {
191  bool add_to_cache = configDistance2(nearest.second, config) > min_config_distance2_;
192  if (!add_to_cache)
193  {
194  double dist = 0.;
195  for (unsigned int i = 0; i < poses.size(); ++i)
196  {
197  dist += nearest.first[i].distance(poses[i]);
198  if (dist > min_pose_distance_)
199  {
200  add_to_cache = true;
201  break;
202  }
203  }
204  }
205  if (add_to_cache)
206  {
207  std::lock_guard<std::mutex> slock(lock_);
208  ik_cache_.emplace_back(poses, config);
209  ik_nn_.add(&ik_cache_.back());
210  if (ik_cache_.size() >= last_saved_cache_size_ + 500u || ik_cache_.size() == max_cache_size_)
211  saveCache();
212  }
213  }
214 }
215 
216 void IKCache::saveCache() const
217 {
218  if (cache_file_name_.empty())
219  RCLCPP_ERROR(LOGGER, "can't save cache before initialization");
220 
221  RCLCPP_INFO(LOGGER, "writing %ld IK solutions to %s", ik_cache_.size(), cache_file_name_.string().c_str());
222 
223  std::ofstream cache_file(cache_file_name_, std::ios_base::binary | std::ios_base::out);
224  unsigned int position_size = 3 * sizeof(tf2Scalar);
225  unsigned int orientation_size = 4 * sizeof(tf2Scalar);
226  unsigned int pose_size = position_size + orientation_size;
227  unsigned int num_tips = ik_cache_[0].first.size();
228  unsigned int config_size = ik_cache_[0].second.size() * sizeof(double);
229  unsigned int offset_conf = num_tips * pose_size;
230  unsigned int bufsize = offset_conf + config_size;
231  char* buffer = new char[bufsize];
232 
233  // write number of IK entries and size of each configuration first
235  cache_file.write((char*)&last_saved_cache_size_, sizeof(unsigned int));
236  unsigned int sz = ik_cache_[0].second.size();
237  cache_file.write((char*)&sz, sizeof(unsigned int));
238  cache_file.write((char*)&num_tips, sizeof(unsigned int));
239  for (const auto& entry : ik_cache_)
240  {
241  for (unsigned int i = 0; i < num_tips; ++i)
242  {
243  memcpy(buffer + i * pose_size, &entry.first[i].position[0], position_size);
244  memcpy(buffer + i * pose_size + position_size, &entry.first[i].orientation[0], orientation_size);
245  }
246  memcpy(buffer + offset_conf, &entry.second[0], config_size);
247  cache_file.write(buffer, bufsize);
248  }
249  delete[] buffer;
250 }
251 
253 {
254  const std::vector<std::string>& tip_names(fk.getTipFrames());
255  std::vector<geometry_msgs::msg::Pose> poses(tip_names.size());
256  double error, max_error = 0.;
257 
258  for (const auto& entry : ik_cache_)
259  {
260  fk.getPositionFK(tip_names, entry.second, poses);
261  error = 0.;
262  for (unsigned int i = 0; i < poses.size(); ++i)
263  error += entry.first[i].distance(poses[i]);
264  if (!poses.empty())
265  error /= (double)poses.size();
266  if (error > max_error)
267  max_error = error;
268  if (error > 1e-4)
269  RCLCPP_ERROR(LOGGER, "Cache entry is invalid, error = %g", error);
270  }
271  RCLCPP_INFO(LOGGER, "Max. error in cache entries is %g", max_error);
272 }
273 
274 IKCache::Pose::Pose(const geometry_msgs::msg::Pose& pose)
275 {
276  position.setX(pose.position.x);
277  position.setY(pose.position.y);
278  position.setZ(pose.position.z);
279  orientation = tf2::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
280 }
281 
282 double IKCache::Pose::distance(const Pose& pose) const
283 {
284  return (position - pose.position).length() + (orientation.angleShortestPath(pose.orientation));
285 }
286 
287 IKCacheMap::IKCacheMap(const std::string& robot_description, const std::string& group_name, unsigned int num_joints)
288  : robot_description_(robot_description), group_name_(group_name), num_joints_(num_joints)
289 {
290 }
291 
293 {
294  for (auto& cache : *this)
295  delete cache.second;
296 }
297 
298 const IKCache::IKEntry& IKCacheMap::getBestApproximateIKSolution(const std::vector<std::string>& fixed,
299  const std::vector<std::string>& active,
300  const std::vector<Pose>& poses) const
301 {
302  auto key(getKey(fixed, active));
303  auto it = find(key);
304  if (it != end())
305  return it->second->getBestApproximateIKSolution(poses);
306  else
307  {
308  static IKEntry dummy = std::make_pair(poses, std::vector<double>(num_joints_, 0.));
309  return dummy;
310  }
311 }
312 
313 void IKCacheMap::updateCache(const IKEntry& nearest, const std::vector<std::string>& fixed,
314  const std::vector<std::string>& active, const std::vector<Pose>& poses,
315  const std::vector<double>& config)
316 {
317  auto key(getKey(fixed, active));
318  auto it = find(key);
319  if (it != end())
320  it->second->updateCache(nearest, poses, config);
321  else
322  {
323  value_type val = std::make_pair(key, nullptr);
324  auto it = insert(val).first;
325  it->second = new IKCache;
327  }
328 }
329 
330 std::string IKCacheMap::getKey(const std::vector<std::string>& fixed, const std::vector<std::string>& active) const
331 {
332  std::string key;
333  std::accumulate(fixed.begin(), fixed.end(), key);
334  key += '_';
335  std::accumulate(active.begin(), active.end(), key);
336  return key;
337 }
338 } // namespace cached_ik_kinematics_plugin
const IKEntry & getBestApproximateIKSolution(const std::vector< std::string > &fixed, const std::vector< std::string > &active, const std::vector< Pose > &poses) const
Definition: ik_cache.cpp:298
std::string getKey(const std::vector< std::string > &fixed, const std::vector< std::string > &active) const
Definition: ik_cache.cpp:330
void updateCache(const IKEntry &nearest, const std::vector< std::string > &fixed, const std::vector< std::string > &active, const std::vector< Pose > &poses, const std::vector< double > &config)
Definition: ik_cache.cpp:313
IKCacheMap(const std::string &robot_description, const std::string &group_name, unsigned int num_joints)
Definition: ik_cache.cpp:287
A cache of inverse kinematic solutions.
const IKEntry & getBestApproximateIKSolution(const Pose &pose) const
Definition: ik_cache.cpp:151
std::pair< std::vector< Pose >, std::vector< double > > IKEntry
void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin &fk) const
Definition: ik_cache.cpp:252
NearestNeighborsGNAT< IKEntry * > ik_nn_
double configDistance2(const std::vector< double > &config1, const std::vector< double > &config2) const
Definition: ik_cache.cpp:140
void initializeCache(const std::string &robot_id, const std::string &group_name, const std::string &cache_name, const unsigned int num_joints, const Options &opts=Options())
Definition: ik_cache.cpp:62
void updateCache(const IKEntry &nearest, const Pose &pose, const std::vector< double > &config) const
Definition: ik_cache.cpp:173
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
void add(const _T &data) override
Add an element to the datastructure.
void clear() override
Clear the datastructure.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
Specific implementation of kinematics using KDL. This version supports any kinematic chain,...
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
virtual const std::vector< std::string > & getTipFrames() const
Return the names of the tip frames of the kinematic tree on which the solver is operating....
double distance(const Pose &pose) const
Definition: ik_cache.cpp:282