37 #include <boost/filesystem/fstream.hpp> 51 for (
unsigned int i = 0; i < entry1->first.size(); ++i)
52 dist += entry1->first[i].distance(entry2->first[i]);
64 const std::string& cache_name,
const unsigned int num_joints,
Options opts)
75 std::lock_guard<std::mutex> slock(
lock_);
77 boost::filesystem::path prefix(!cached_ik_path.empty() ? cached_ik_path : boost::filesystem::current_path());
79 boost::filesystem::create_directories(prefix);
81 cache_file_name_ = prefix / (robot_description + group_name +
"_" + cache_name +
"_" +
91 boost::filesystem::ifstream cache_file(
cache_file_name_, std::ios_base::binary | std::ios_base::in);
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));
98 ROS_INFO_NAMED(
"cached_ik",
"Found %d IK solutions for a %d-dof system with %d end effectors in %s",
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];
109 entry.first.resize(num_tips);
110 entry.second.resize(num_dofs);
116 cache_file.read(buffer, bufsize);
117 for (
auto& pose : entry.first)
119 memcpy(&pose.position[0], buffer + j * pose_size, position_size);
120 memcpy(&pose.orientation[0], buffer + j * pose_size + position_size, orientation_size);
123 memcpy(&entry.second[0], buffer + offset_conf, config_size);
129 std::vector<IKEntry*> ik_entry_ptrs(last_saved_cache_size_);
142 double dist = 0.,
diff;
143 for (
unsigned int i = 0; i < config1.size(); ++i)
145 diff = config1[i] - config2[i];
155 static IKEntry dummy = std::make_pair(std::vector<Pose>(1, pose), std::vector<double>(
num_joints_, 0.));
158 IKEntry query = std::make_pair(std::vector<Pose>(1, pose), std::vector<double>());
169 IKEntry query = std::make_pair(poses, std::vector<double>());
178 std::lock_guard<std::mutex> slock(
lock_);
179 ik_cache_.emplace_back(std::vector<Pose>(1u, pose), config);
187 const std::vector<double>& config)
const 195 for (
unsigned int i = 0; i < poses.size(); ++i)
197 dist += nearest.first[i].distance(poses[i]);
207 std::lock_guard<std::mutex> slock(
lock_);
219 ROS_ERROR_NAMED(
"cached_ik",
"can't save cache before initialization");
223 boost::filesystem::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];
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));
241 for (
unsigned int i = 0; i < num_tips; ++i)
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);
246 memcpy(buffer + offset_conf, &entry.second[0], config_size);
247 cache_file.write(buffer, bufsize);
255 std::vector<geometry_msgs::Pose> poses(tip_names.size());
256 double error, max_error = 0.;
262 for (
unsigned int i = 0; i < poses.size(); ++i)
263 error += entry.first[i].distance(poses[i]);
265 error /= (
double)poses.size();
266 if (error > max_error)
269 ROS_ERROR_NAMED(
"cached_ik",
"Cache entry is invalid, error = %g", error);
271 ROS_INFO_NAMED(
"cached_ik",
"Max. error in cache entries is %g", max_error);
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);
284 return (position - pose.
position).length() + (orientation.angleShortestPath(pose.
orientation));
288 : robot_description_(robot_description), group_name_(group_name),
num_joints_(num_joints)
294 for (
auto& cache : *
this)
299 const std::vector<std::string>& active,
300 const std::vector<Pose>& poses)
const 302 auto key(
getKey(fixed, active));
305 return it->second->getBestApproximateIKSolution(poses);
314 const std::vector<std::string>& active,
const std::vector<Pose>& poses,
315 const std::vector<double>& config)
317 auto key(
getKey(fixed, active));
320 it->second->updateCache(nearest, poses, config);
323 value_type val = std::make_pair(key,
nullptr);
324 auto it = insert(val).first;
330 std::string
IKCacheMap::getKey(
const std::vector<std::string>& fixed,
const std::vector<std::string>& active)
const 333 std::accumulate(fixed.begin(), fixed.end(), key);
335 std::accumulate(active.begin(), active.end(), key);
void clear() override
Clear the datastructure.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void add(const _T &data) override
Add an element to the datastructure.
std::pair< std::vector< Pose >, std::vector< double >> IKEntry
#define ROS_INFO_NAMED(name,...)
const IKEntry & getBestApproximateIKSolution(const Pose &pose) const
double configDistance2(const std::vector< double > &config1, const std::vector< double > &config2) const
NearestNeighborsGNAT< IKEntry * > ik_nn_
double min_config_distance2_
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double distance(const Pose &pose) const
unsigned int max_cache_size_
std::string getKey(const std::vector< std::string > &fixed, const std::vector< std::string > &active) const
unsigned int max_cache_size
A cache of inverse kinematic solutions.
std::string cached_ik_path
virtual const std::vector< std::string > & getTipFrames() const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
Specific implementation of kinematics using KDL. This version can be used with any robot...
class to represent end effector pose
void updateCache(const IKEntry &nearest, const Pose &pose, const std::vector< double > &config) const
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)
unsigned int last_saved_cache_size_
std::string robot_description_
const IKEntry & getBestApproximateIKSolution(const std::vector< std::string > &fixed, const std::vector< std::string > &active, const std::vector< Pose > &poses) const
void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin &fk) const
double min_joint_config_distance
#define ROS_ERROR_NAMED(name,...)
boost::filesystem::path cache_file_name_
std::vector< IKEntry > ik_cache_
double min_pose_distance_
tf2::Quaternion orientation
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
IKCacheMap(const std::string &robot_description, const std::string &group_name, unsigned int num_joints)
void initializeCache(const std::string &robot_description, const std::string &group_name, const std::string &cache_name, const unsigned int num_joints, Options opts=Options())