cached_ik_kinematics_plugin.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Rice University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Rice University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mark Moll */
36 
37 #ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_
38 #define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_
39 
42 #include <tf2/LinearMath/Vector3.h>
45 #include <boost/filesystem.hpp>
46 #include <unordered_map>
47 #include <mutex>
48 #include <utility>
49 
51 {
53 class IKCache
54 {
55 public:
56  struct Options
57  {
59  {
60  }
61  unsigned int max_cache_size;
64  std::string cached_ik_path;
65  };
66 
74  struct Pose
75  {
76  Pose() = default;
77  Pose(const geometry_msgs::Pose& pose);
78  tf2::Vector3 position;
81  double distance(const Pose& pose) const;
82  };
83 
89  using IKEntry = std::pair<std::vector<Pose>, std::vector<double>>;
90 
91  IKCache();
92  ~IKCache();
93  IKCache(const IKCache&) = default;
94 
96  const IKEntry& getBestApproximateIKSolution(const Pose& pose) const;
98  const IKEntry& getBestApproximateIKSolution(const std::vector<Pose>& poses) const;
100  void initializeCache(const std::string& robot_description, const std::string& group_name,
101  const std::string& cache_name, const unsigned int num_joints, Options opts = Options());
106  void updateCache(const IKEntry& nearest, const Pose& pose, const std::vector<double>& config) const;
111  void updateCache(const IKEntry& nearest, const std::vector<Pose>& poses, const std::vector<double>& config) const;
114 
115 protected:
117  double configDistance2(const std::vector<double>& config1, const std::vector<double>& config2) const;
119  void saveCache() const;
120 
122  unsigned int num_joints_;
123 
129  unsigned int max_cache_size_;
131  boost::filesystem::path cache_file_name_;
132 
139  mutable std::vector<IKEntry> ik_cache_;
143  mutable unsigned int last_saved_cache_size_{ 0 };
145  mutable std::mutex lock_;
146 };
147 
149 class IKCacheMap : public std::unordered_map<std::string, IKCache*>
150 {
151 public:
154 
155  IKCacheMap(const std::string& robot_description, const std::string& group_name, unsigned int num_joints);
156  ~IKCacheMap();
161  const IKEntry& getBestApproximateIKSolution(const std::vector<std::string>& fixed,
162  const std::vector<std::string>& active,
163  const std::vector<Pose>& poses) const;
168  void updateCache(const IKEntry& nearest, const std::vector<std::string>& fixed,
169  const std::vector<std::string>& active, const std::vector<Pose>& poses,
170  const std::vector<double>& config);
171 
172 protected:
173  std::string getKey(const std::vector<std::string>& fixed, const std::vector<std::string>& active) const;
174  std::string robot_description_;
175  std::string group_name_;
176  unsigned int num_joints_;
177 };
178 
180 template <class KinematicsPlugin>
181 class CachedIKKinematicsPlugin : public KinematicsPlugin
182 {
183 public:
188 
190 
192 
193  // virtual methods that need to be wrapped:
194 
195  bool getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
196  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
197  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
198 
199  bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_frame,
200  const std::string& tip_frame, double search_discretization) override;
201 
202  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
203  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
204  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
205 
206  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
207  const std::vector<double>& consistency_limits, std::vector<double>& solution,
208  moveit_msgs::MoveItErrorCodes& error_code,
209  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
210 
211  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
212  std::vector<double>& solution, const IKCallbackFn& solution_callback,
213  moveit_msgs::MoveItErrorCodes& error_code,
214  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
215 
216  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
217  const std::vector<double>& consistency_limits, std::vector<double>& solution,
218  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
219  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
220 
221 protected:
223 };
224 
236 template <class KinematicsPlugin>
238 {
239 public:
244 
245  bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_frame,
246  const std::vector<std::string>& tip_frames, double search_discretization) override;
247 
248  bool searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
249  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
250  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
252  const moveit::core::RobotState* context_state = nullptr) const override;
253 };
254 }
255 
257 #endif
std::pair< std::vector< Pose >, std::vector< double >> IKEntry
const IKEntry & getBestApproximateIKSolution(const Pose &pose) const
Definition: ik_cache.cpp:151
double configDistance2(const std::vector< double > &config1, const std::vector< double > &config2) const
Definition: ik_cache.cpp:140
NearestNeighborsGNAT< IKEntry * > ik_nn_
ROSCONSOLE_DECL void initialize()
A cache of inverse kinematic solutions.
Specific implementation of kinematics using KDL. This version can be used with any robot...
void updateCache(const IKEntry &nearest, const Pose &pose, const std::vector< double > &config) const
Definition: ik_cache.cpp:173
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin &fk) const
Definition: ik_cache.cpp:252
double distance(const urdf::Pose &transform)
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())
Definition: ik_cache.cpp:63


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:41