37 #include <boost/filesystem/fstream.hpp>
49 for (
unsigned int i = 0; i < entry1->first.size(); ++i)
50 dist += entry1->first[i].distance(entry2->first[i]);
61 void IKCache::initializeCache(
const std::string& robot_id,
const std::string& group_name,
const std::string& cache_name,
62 const unsigned int num_joints,
const Options& opts)
70 std::string cached_ik_path = opts.cached_ik_path;
73 std::lock_guard<std::mutex> slock(
lock_);
75 boost::filesystem::path prefix(!cached_ik_path.empty() ? cached_ik_path : boost::filesystem::current_path());
77 boost::filesystem::create_directories(prefix);
89 boost::filesystem::ifstream cache_file(
cache_file_name_, std::ios_base::binary | std::ios_base::in);
91 unsigned int num_dofs;
92 cache_file.read((
char*)&num_dofs,
sizeof(
unsigned int));
93 unsigned int num_tips;
94 cache_file.read((
char*)&num_tips,
sizeof(
unsigned int));
96 ROS_INFO_NAMED(
"cached_ik",
"Found %d IK solutions for a %d-dof system with %d end effectors in %s",
99 unsigned int position_size = 3 *
sizeof(
tf2Scalar);
100 unsigned int orientation_size = 4 *
sizeof(
tf2Scalar);
101 unsigned int pose_size = position_size + orientation_size;
102 unsigned int config_size = num_dofs *
sizeof(double);
103 unsigned int offset_conf = pose_size * num_tips;
104 unsigned int bufsize = offset_conf + config_size;
105 char* buffer =
new char[bufsize];
107 entry.first.resize(num_tips);
108 entry.second.resize(num_dofs);
114 cache_file.read(buffer, bufsize);
115 for (
auto& pose : entry.first)
117 memcpy(&pose.position[0], buffer + j * pose_size, position_size);
118 memcpy(&pose.orientation[0], buffer + j * pose_size + position_size, orientation_size);
121 memcpy(&entry.second[0], buffer + offset_conf, config_size);
140 double dist = 0., diff;
141 for (
unsigned int i = 0; i < config1.size(); ++i)
143 diff = config1[i] - config2[i];
153 static IKEntry dummy = std::make_pair(std::vector<Pose>(1, pose), std::vector<double>(
num_joints_, 0.));
156 IKEntry query = std::make_pair(std::vector<Pose>(1, pose), std::vector<double>());
167 IKEntry query = std::make_pair(poses, std::vector<double>());
176 std::lock_guard<std::mutex> slock(
lock_);
177 ik_cache_.emplace_back(std::vector<Pose>(1u, pose), config);
185 const std::vector<double>& config)
const
193 for (
unsigned int i = 0; i < poses.size(); ++i)
195 dist += nearest.first[i].distance(poses[i]);
205 std::lock_guard<std::mutex> slock(
lock_);
217 ROS_ERROR_NAMED(
"cached_ik",
"can't save cache before initialization");
221 boost::filesystem::ofstream cache_file(
cache_file_name_, std::ios_base::binary | std::ios_base::out);
222 unsigned int position_size = 3 *
sizeof(
tf2Scalar);
223 unsigned int orientation_size = 4 *
sizeof(
tf2Scalar);
224 unsigned int pose_size = position_size + orientation_size;
225 unsigned int num_tips =
ik_cache_[0].first.size();
226 unsigned int config_size =
ik_cache_[0].second.size() *
sizeof(double);
227 unsigned int offset_conf = num_tips * pose_size;
228 unsigned int bufsize = offset_conf + config_size;
229 char* buffer =
new char[bufsize];
234 unsigned int sz =
ik_cache_[0].second.size();
235 cache_file.write((
char*)&sz,
sizeof(
unsigned int));
236 cache_file.write((
char*)&num_tips,
sizeof(
unsigned int));
239 for (
unsigned int i = 0; i < num_tips; ++i)
241 memcpy(buffer + i * pose_size, &entry.first[i].position[0], position_size);
242 memcpy(buffer + i * pose_size + position_size, &entry.first[i].orientation[0], orientation_size);
244 memcpy(buffer + offset_conf, &entry.second[0], config_size);
245 cache_file.write(buffer, bufsize);
252 const std::vector<std::string>& tip_names(fk.
getTipFrames());
253 std::vector<geometry_msgs::Pose> poses(tip_names.size());
254 double error, max_error = 0.;
260 for (
unsigned int i = 0; i < poses.size(); ++i)
261 error += entry.first[i].distance(poses[i]);
263 error /= (double)poses.size();
264 if (error > max_error)
267 ROS_ERROR_NAMED(
"cached_ik",
"Cache entry is invalid, error = %g", error);
269 ROS_INFO_NAMED(
"cached_ik",
"Max. error in cache entries is %g", max_error);
282 return (position - pose.position).length() + (orientation.angleShortestPath(pose.orientation));
285 IKCacheMap::IKCacheMap(
const std::string& robot_description,
const std::string& group_name,
unsigned int num_joints)
286 : robot_description_(robot_description), group_name_(group_name),
num_joints_(num_joints)
292 for (
auto& cache : *
this)
297 const std::vector<std::string>& active,
298 const std::vector<Pose>& poses)
const
300 auto key(
getKey(fixed, active));
303 return it->second->getBestApproximateIKSolution(poses);
312 const std::vector<std::string>& active,
const std::vector<Pose>& poses,
313 const std::vector<double>& config)
315 auto key(
getKey(fixed, active));
318 it->second->updateCache(nearest, poses, config);
321 value_type val = std::make_pair(key,
nullptr);
322 auto it = insert(val).first;
328 std::string
IKCacheMap::getKey(
const std::vector<std::string>& fixed,
const std::vector<std::string>& active)
const
331 std::accumulate(fixed.begin(), fixed.end(), key);
333 std::accumulate(active.begin(), active.end(), key);