38 #ifndef KINEMATICS_CACHE_H_ 39 #define KINEMATICS_CACHE_H_ 41 #include <moveit/kinematics_base/kinematics_base.h> 42 #include <moveit/robot_model/robot_model.h> 43 #include <moveit/robot_state/robot_state.h> 45 #include <boost/shared_ptr.hpp> 54 geometry_msgs::Point
origin;
79 bool getSolution(
const geometry_msgs::Pose& pose,
unsigned int solution_index, std::vector<double>& solution)
const;
90 bool getSolutions(
const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solutions)
const;
98 bool getNumSolutions(
const geometry_msgs::Pose& pose,
unsigned int& num_solutions)
const;
106 bool initialize(kinematics::KinematicsBaseConstPtr& solver,
const robot_model::RobotModelConstPtr& kinematic_model,
107 const KinematicsCache::Options& opt);
134 bool addToCache(
const geometry_msgs::Pose& pose,
const std::vector<double>& joint_values,
bool overwrite =
false);
144 unsigned int getSolutionLocation(
unsigned int& grid_index,
unsigned int& solution_index)
const;
147 std::vector<double>
getSolution(
unsigned int grid_index,
unsigned int solution_index)
const;
150 bool getGridIndex(
const geometry_msgs::Pose& pose,
unsigned int& grid_index)
const;
void updateDistances(const geometry_msgs::Pose &pose)
const kinematics::KinematicsBaseConstPtr & getSolverInstance() const
Return the instance of the kinematics solver.
void setup(const KinematicsCache::Options &opt)
Setup the cache.
bool initialize(kinematics::KinematicsBaseConstPtr &solver, const planning_models::RobotModelConstPtr &kinematic_model, const KinematicsCache::Options &opt)
Initialize the cache class.
double cache_resolution_z_
boost::shared_ptr< robot_state::JointStateGroup > joint_state_group_
double min_squared_distance_
const robot_model::RobotModelConstPtr & getModelInstance() const
Return the instance of the kinematics model.
geometry_msgs::Point origin
bool addToCache(const geometry_msgs::Pose &pose, const std::vector< double > &joint_values, bool overwrite=false)
Add a new solution to the cache at the given location.
geometry_msgs::Point cache_origin_
bool generateCacheMap(double timeout)
Generate the cache map spending timeout (seconds) on the generation process)
const robot_model::JointModelGroup * joint_model_group_
kinematics::KinematicsBaseConstPtr kinematics_solver_
const std::string & getGroupName() const
unsigned int getSolutionLocation(unsigned int &grid_index, unsigned int &solution_index) const
Get the location of a solution given the grid index and solution index.
unsigned int max_solutions_per_grid_location
KinematicsCache::Options options_
bool readFromFile(const std::string &filename)
unsigned int cache_size_y_
double max_squared_distance_
double cache_resolution_x_
boost::shared_ptr< KinematicsCache > KinematicsCachePtr
robot_state::RobotStatePtr kinematic_state_
unsigned int kinematics_cache_size_
const Options & getCacheParameters() const
Return the cache parameters used for cache construction.
bool getGridIndex(const geometry_msgs::Pose &pose, unsigned int &grid_index) const
Get the grid index for a given pose.
unsigned int max_solutions_per_grid_location_
bool writeToFile(const std::string &filename)
bool getSolution(const geometry_msgs::Pose &pose, unsigned int solution_index, std::vector< double > &solution) const
Get a candidate solution for a particular pose. Note that the pose will be projected onto the grid us...
robot_model::RobotModelConstPtr kinematic_model_
boost::array< double, 3 > resolution
std::vector< double > kinematics_cache_vector_
double cache_resolution_y_
bool getNumSolutions(const geometry_msgs::Pose &pose, unsigned int &num_solutions) const
Get number of candidate solutions for a particular pose.
unsigned int solution_dimension_
planning_models::RobotModelConstPtr kinematic_model_
boost::array< double, 3 > workspace_size
unsigned int cache_size_z_
unsigned int kinematics_cache_points_with_solution_
unsigned int cache_size_x_
bool getSolutions(const geometry_msgs::Pose &pose, std::vector< std::vector< double > > &solutions) const
Get all candidate solutions for a particular pose. Note that the pose will be projected onto the grid...
unsigned int size_grid_node_
std::vector< unsigned int > num_solutions_vector_
std::pair< double, double > getMinMaxSquaredDistance()