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_

Detailed Description

A cache of inverse kinematic solutions.

Definition at line 53 of file cached_ik_kinematics_plugin.h.

Member Typedef Documentation

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.

Constructor & Destructor Documentation

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.

cached_ik_kinematics_plugin::IKCache::IKCache ( const IKCache & )

default

Member Function Documentation

double cached_ik_kinematics_plugin::IKCache::configDistance2	(	const std::vector< double > &	config1,
		const std::vector< double > &	config2
	)		const

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.

void cached_ik_kinematics_plugin::IKCache::saveCache ( ) const

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.

Member Data Documentation

boost::filesystem::path cached_ik_kinematics_plugin::IKCache::cache_file_name_

protected

file name for loading / saving cache

Definition at line 131 of file cached_ik_kinematics_plugin.h.

std::vector<IKEntry> cached_ik_kinematics_plugin::IKCache::ik_cache_

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.

NearestNeighborsGNAT<IKEntry*> cached_ik_kinematics_plugin::IKCache::ik_nn_

mutableprotected

nearest neighbor data structure over IK cache entries

Definition at line 141 of file cached_ik_kinematics_plugin.h.

unsigned int cached_ik_kinematics_plugin::IKCache::last_saved_cache_size_ { 0 }

mutableprotected

size of the cache when it was last saved

Definition at line 143 of file cached_ik_kinematics_plugin.h.

std::mutex cached_ik_kinematics_plugin::IKCache::lock_

mutableprotected

mutex for changing IK cache