A cache of inverse kinematic solutions. More...
#include <cached_ik_kinematics_plugin.h>
Classes | |
struct | Options |
struct | Pose |
class to represent end effector pose More... | |
Public Types | |
using | IKEntry = std::pair< std::vector< Pose >, std::vector< double >> |
Public Member Functions | |
const IKEntry & | getBestApproximateIKSolution (const Pose &pose) const |
const IKEntry & | getBestApproximateIKSolution (const std::vector< Pose > &poses) const |
IKCache () | |
IKCache (const IKCache &)=default | |
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()) |
void | updateCache (const IKEntry &nearest, const Pose &pose, const std::vector< double > &config) const |
void | updateCache (const IKEntry &nearest, const std::vector< Pose > &poses, const std::vector< double > &config) const |
void | verifyCache (kdl_kinematics_plugin::KDLKinematicsPlugin &fk) const |
~IKCache () | |
Protected Member Functions | |
double | configDistance2 (const std::vector< double > &config1, const std::vector< double > &config2) const |
void | saveCache () const |
Protected Attributes | |
boost::filesystem::path | cache_file_name_ |
std::vector< IKEntry > | ik_cache_ |
NearestNeighborsGNAT< IKEntry * > | ik_nn_ |
unsigned int | last_saved_cache_size_ { 0 } |
std::mutex | lock_ |
unsigned int | max_cache_size_ |
double | min_config_distance2_ |
double | min_pose_distance_ |
unsigned int | num_joints_ |
A cache of inverse kinematic solutions.
Definition at line 53 of file cached_ik_kinematics_plugin.h.
using cached_ik_kinematics_plugin::IKCache::IKEntry = std::pair<std::vector<Pose>, std::vector<double>> |
the IK cache entries are simply a pair formed by a vector of poses (one for each end effector) and a configuration that achieves those poses
Definition at line 89 of file cached_ik_kinematics_plugin.h.
cached_ik_kinematics_plugin::IKCache::IKCache | ( | ) |
Definition at line 46 of file ik_cache.cpp.
cached_ik_kinematics_plugin::IKCache::~IKCache | ( | ) |
Definition at line 57 of file ik_cache.cpp.
|
default |
|
protected |
compute the distance between two joint configurations
Definition at line 140 of file ik_cache.cpp.
const IKCache::IKEntry & cached_ik_kinematics_plugin::IKCache::getBestApproximateIKSolution | ( | const Pose & | pose | ) | const |
get the entry from the IK cache that best matches a given pose
Definition at line 151 of file ik_cache.cpp.
const IKCache::IKEntry & cached_ik_kinematics_plugin::IKCache::getBestApproximateIKSolution | ( | const std::vector< Pose > & | poses | ) | const |
get the entry from the IK cache that best matches a given vector of poses
Definition at line 162 of file ik_cache.cpp.
void cached_ik_kinematics_plugin::IKCache::initializeCache | ( | const std::string & | robot_description, |
const std::string & | group_name, | ||
const std::string & | cache_name, | ||
const unsigned int | num_joints, | ||
Options | opts = Options() |
||
) |
initialize cache, read from disk if found
Definition at line 63 of file ik_cache.cpp.
|
protected |
save current state of cache to disk
Definition at line 216 of file ik_cache.cpp.
void cached_ik_kinematics_plugin::IKCache::updateCache | ( | const IKEntry & | nearest, |
const Pose & | pose, | ||
const std::vector< double > & | config | ||
) | const |
insert (pose,config) as an entry if it's different enough from the most similar cache entry
Definition at line 173 of file ik_cache.cpp.
void cached_ik_kinematics_plugin::IKCache::updateCache | ( | const IKEntry & | nearest, |
const std::vector< Pose > & | poses, | ||
const std::vector< double > & | config | ||
) | const |
insert (pose,config) as an entry if it's different enough from the most similar cache entry
Definition at line 186 of file ik_cache.cpp.
void cached_ik_kinematics_plugin::IKCache::verifyCache | ( | kdl_kinematics_plugin::KDLKinematicsPlugin & | fk | ) | const |
verify with forward kinematics that that the cache entries are correct
Definition at line 252 of file ik_cache.cpp.
|
protected |
file name for loading / saving cache
Definition at line 131 of file cached_ik_kinematics_plugin.h.
|
mutableprotected |
the IK methods are declared const in the base class, but the wrapped methods need to modify the cache, so the next four members are mutable cache of IK solutions
Definition at line 139 of file cached_ik_kinematics_plugin.h.
|
mutableprotected |
nearest neighbor data structure over IK cache entries
Definition at line 141 of file cached_ik_kinematics_plugin.h.
|
mutableprotected |
size of the cache when it was last saved
Definition at line 143 of file cached_ik_kinematics_plugin.h.
|
mutableprotected |
mutex for changing IK cache
Definition at line 145 of file cached_ik_kinematics_plugin.h.
|
protected |
maximum size of the cache
Definition at line 129 of file cached_ik_kinematics_plugin.h.
|
protected |
... or the configurations are at least minConfigDistance2_^.5 apart.
Definition at line 127 of file cached_ik_kinematics_plugin.h.
|
protected |
for all cache entries, the poses are at least minPoseDistance_ apart ...
Definition at line 125 of file cached_ik_kinematics_plugin.h.
|
protected |
number of joints in the system
Definition at line 122 of file cached_ik_kinematics_plugin.h.