moveit2
The MoveIt Motion Planning Framework for ROS 2.
kinematics_cache.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2012, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Sachin Chitta
36  *********************************************************************/
37 
38 #pragma once
39 
43 
44 #include <memory>
45 
46 namespace kinematics_cache
47 {
48 class KinematicsCache
49 {
50 public:
51  struct Options
52  {
53  geometry_msgs::Point origin;
54  std::array<double, 3> workspace_size;
55  std::array<double, 3> resolution;
57  };
58 
64 
69  bool generateCacheMap(double timeout);
70 
78  bool getSolution(const geometry_msgs::Pose& pose, unsigned int solution_index, std::vector<double>& solution) const;
79 
89  bool getSolutions(const geometry_msgs::Pose& pose, std::vector<std::vector<double> >& solutions) const;
90 
97  bool getNumSolutions(const geometry_msgs::Pose& pose, unsigned int& num_solutions) const;
98 
105  bool initialize(kinematics::KinematicsBaseConstPtr& solver, const moveit::core::RobotModelConstPtr& kinematic_model,
107 
109  const kinematics::KinematicsBaseConstPtr& getSolverInstance() const
110  {
111  return kinematics_solver_;
112  }
113 
115  const moveit::core::RobotModelConstPtr& getModelInstance() const
116  {
117  return kinematic_model_;
118  }
119 
122  {
123  return options_;
124  }
125 
126  const std::string& getGroupName() const
127  {
128  return kinematics_solver_->getGroupName();
129  }
130 
133  bool addToCache(const geometry_msgs::Pose& pose, const std::vector<double>& joint_values, bool overwrite = false);
134 
135  bool writeToFile(const std::string& filename);
136 
137  bool readFromFile(const std::string& filename);
138 
139  std::pair<double, double> getMinMaxSquaredDistance();
140 
141 private:
143  unsigned int getSolutionLocation(unsigned int& grid_index, unsigned int& solution_index) const;
144 
146  std::vector<double> getSolution(unsigned int grid_index, unsigned int solution_index) const;
147 
149  bool getGridIndex(const geometry_msgs::Pose& pose, unsigned int& grid_index) const;
150 
152  void setup(const KinematicsCache::Options& opt);
153 
154  void updateDistances(const geometry_msgs::Pose& pose);
155 
156  KinematicsCache::Options options_;
157  geometry_msgs::Point cache_origin_;
158  double cache_resolution_x_, cache_resolution_y_, cache_resolution_z_;
159  unsigned int cache_size_x_, cache_size_y_, cache_size_z_;
160  unsigned int max_solutions_per_grid_location_;
161  unsigned int solution_dimension_;
162  unsigned int size_grid_node_;
163  unsigned int kinematics_cache_points_with_solution_;
164  unsigned int kinematics_cache_size_;
166  std::vector<double> kinematics_cache_vector_;
167  std::vector<unsigned int> num_solutions_vector_;
169  kinematics::KinematicsBaseConstPtr kinematics_solver_;
171  moveit::core::RobotModelConstPtr kinematic_model_;
172  moveit::core::RobotStatePtr kinematic_state_;
174  const moveit::core::JointModelGroup* joint_model_group_;
175  std::shared_ptr<moveit::core::JointStateGroup> joint_state_group_;
177  // mutable std::vector<double> solution_local_; /** Local pre-allocated storage */
178 
179  double min_squared_distance_, max_squared_distance_;
180 };
181 
182 typedef std::shared_ptr<KinematicsCache> KinematicsCachePtr;
183 } // namespace kinematics_cache
bool readFromFile(const std::string &filename)
std::pair< double, double > getMinMaxSquaredDistance()
const std::string & getGroupName() const
const kinematics::KinematicsBaseConstPtr & getSolverInstance() const
Return the instance of the kinematics solver.
bool initialize(kinematics::KinematicsBaseConstPtr &solver, const planning_models::RobotModelConstPtr &kinematic_model, const KinematicsCache::Options &opt)
Initialize the cache class.
bool generateCacheMap(double timeout)
Generate the cache map spending timeout (seconds) on the generation process)
const Options & getCacheParameters() const
Return the cache parameters used for cache construction.
bool writeToFile(const std::string &filename)
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.
bool getNumSolutions(const geometry_msgs::Pose &pose, unsigned int &num_solutions) const
Get number of candidate solutions for a particular pose.
const moveit::core::RobotModelConstPtr & getModelInstance() const
Return the instance of the kinematics model.
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...
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...
std::shared_ptr< KinematicsCache > KinematicsCachePtr
Definition: setup.py:1