moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 #pragma once
38 
43 #include <tf2/LinearMath/Quaternion.h>
44 #include <tf2/LinearMath/Vector3.h>
45 #include <mutex>
46 #include <unordered_map>
47 #include <utility>
48 #include <filesystem>
49 
51 {
52 static const rclcpp::Logger LOGGER =
53  rclcpp::get_logger("moveit_cached_ik_kinematics_plugin.cached_ik_kinematics_plugin");
54 
56 class IKCache
57 {
58 public:
59  struct Options
60  {
62  {
63  }
64  unsigned int max_cache_size;
67  std::string cached_ik_path;
68  };
69 
77  struct Pose
78  {
79  Pose() = default;
80  Pose(const geometry_msgs::msg::Pose& pose);
81  tf2::Vector3 position;
82  tf2::Quaternion orientation;
84  double distance(const Pose& pose) const;
85  };
86 
92  using IKEntry = std::pair<std::vector<Pose>, std::vector<double>>;
93 
94  IKCache();
95  ~IKCache();
96  IKCache(const IKCache&) = delete;
97 
99  const IKEntry& getBestApproximateIKSolution(const Pose& pose) const;
101  const IKEntry& getBestApproximateIKSolution(const std::vector<Pose>& poses) const;
103  void initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name,
104  const unsigned int num_joints, const Options& opts = Options());
109  void updateCache(const IKEntry& nearest, const Pose& pose, const std::vector<double>& config) const;
114  void updateCache(const IKEntry& nearest, const std::vector<Pose>& poses, const std::vector<double>& config) const;
117 
118 protected:
120  double configDistance2(const std::vector<double>& config1, const std::vector<double>& config2) const;
122  void saveCache() const;
123 
125  unsigned int num_joints_;
126 
132  unsigned int max_cache_size_;
134  std::filesystem::path cache_file_name_;
135 
142  mutable std::vector<IKEntry> ik_cache_;
146  mutable unsigned int last_saved_cache_size_{ 0 };
148  mutable std::mutex lock_;
149 };
150 
152 class IKCacheMap : public std::unordered_map<std::string, IKCache*>
153 {
154 public:
157 
158  IKCacheMap(const std::string& robot_description, const std::string& group_name, unsigned int num_joints);
159  ~IKCacheMap();
164  const IKEntry& getBestApproximateIKSolution(const std::vector<std::string>& fixed,
165  const std::vector<std::string>& active,
166  const std::vector<Pose>& poses) const;
171  void updateCache(const IKEntry& nearest, const std::vector<std::string>& fixed,
172  const std::vector<std::string>& active, const std::vector<Pose>& poses,
173  const std::vector<double>& config);
174 
175 protected:
176  std::string getKey(const std::vector<std::string>& fixed, const std::vector<std::string>& active) const;
177  std::string robot_description_;
178  std::string group_name_;
179  unsigned int num_joints_;
180 };
181 
182 // Helper class to enable/disable initialize() methods with new/old API
183 // HasRobotModelApi<T>::value provides a true/false constexpr depending on KinematicsPlugin offers the new Api
184 // This uses SFINAE magic: https://jguegant.github.io/blogs/tech/sfinae-introduction.html
185 template <typename KinematicsPlugin, typename = bool>
186 struct HasRobotModelApi : std::false_type
187 {
188 };
189 
190 template <typename KinematicsPlugin>
191 struct HasRobotModelApi<KinematicsPlugin, decltype(std::declval<KinematicsPlugin&>().initialize(
192  std::declval<const rclcpp::Node::SharedPtr&>(),
193  std::declval<const moveit::core::RobotModel&>(), std::string(),
194  std::string(), std::vector<std::string>(), 0.0))> : std::true_type
195 {
196 };
197 
198 template <typename KinematicsPlugin, typename = bool>
199 struct HasRobotDescApi : std::false_type
200 {
201 };
202 
203 template <typename KinematicsPlugin>
204 struct HasRobotDescApi<KinematicsPlugin,
205  decltype(std::declval<KinematicsPlugin&>().KinematicsPlugin::initialize(
206  std::string(), std::string(), std::string(), std::vector<std::string>(), 0.0))>
207  : std::true_type
208 {
209 };
210 
212 template <class KinematicsPlugin>
213 class CachedIKKinematicsPlugin : public KinematicsPlugin
214 {
215 public:
220 
222 
223  ~CachedIKKinematicsPlugin() override;
224 
225  // virtual methods that need to be wrapped:
226 
227  bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
228  const std::string& group_name, const std::string& base_frame,
229  const std::vector<std::string>& tip_frames, double search_discretization) override
230  {
231  node_ = node;
232  return initializeImpl(node, robot_model, group_name, base_frame, tip_frames, search_discretization);
233  }
234 
235  bool getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
236  std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
237  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
238 
239  bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
240  double timeout, std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
241  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
242 
243  bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
244  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
245  moveit_msgs::msg::MoveItErrorCodes& error_code,
246  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
247 
248  bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
249  double timeout, std::vector<double>& solution, const IKCallbackFn& solution_callback,
250  moveit_msgs::msg::MoveItErrorCodes& error_code,
251  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
252 
253  bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
254  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
255  const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
256  const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override;
257 
258 private:
259  rclcpp::Node::SharedPtr node_;
260 
261  IKCache cache_;
262 
263  void initCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name);
264 
265  /* Using templates and SFINAE magic, we can selectively enable/disable methods depending on
266  availability of API in wrapped KinematicsPlugin class.
267  However, as templates and virtual functions cannot be combined, we need helpers initializeImpl(). */
268  template <class T = KinematicsPlugin>
269  typename std::enable_if<HasRobotModelApi<T>::value, bool>::type
270  initializeImpl(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
271  const std::string& group_name, const std::string& base_frame,
272  const std::vector<std::string>& tip_frames, double search_discretization)
273  {
274  if (tip_frames.size() != 1)
275  {
276  RCLCPP_ERROR(LOGGER, "This solver does not support multiple tip frames");
277  return false;
278  }
279 
280  // call initialize method of wrapped class
281  if (!KinematicsPlugin::initialize(node, robot_model, group_name, base_frame, tip_frames, search_discretization))
282  return false;
283  initCache(robot_model.getName(), group_name, base_frame + tip_frames[0]);
284  return true;
285  }
286 
287  template <class T = KinematicsPlugin>
288  typename std::enable_if<!HasRobotModelApi<T>::value, bool>::type
289  initializeImpl(const rclcpp::Node::SharedPtr& /*unused*/, const moveit::core::RobotModel& /*unused*/,
290  const std::string& /*unused*/, const std::string& /*unused*/,
291  const std::vector<std::string>& /*unused*/, double /*unused*/)
292  {
293  return false; // API not supported
294  }
295 
296  template <class T = KinematicsPlugin>
297  typename std::enable_if<HasRobotDescApi<T>::value, bool>::type
298  initializeImpl(const std::string& robot_description, const std::string& group_name, const std::string& base_frame,
299  const std::string& tip_frame, double search_discretization)
300  {
301  // call initialize method of wrapped class
302  if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frame, search_discretization))
303  return false;
304  initCache(robot_description, group_name, base_frame + tip_frame);
305  return true;
306  }
307 
308  template <class T = KinematicsPlugin>
309  typename std::enable_if<!HasRobotDescApi<T>::value, bool>::type
310  initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/,
311  const std::string& /*unused*/, double /*unused*/)
312  {
313  return false; // API not supported
314  }
315 };
316 
328 template <class KinematicsPlugin>
330 {
331 public:
336 
337  bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
338  const std::string& group_name, const std::string& base_frame,
339  const std::vector<std::string>& tip_frames, double search_discretization) override;
340 
341  bool searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
342  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
343  const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
345  const moveit::core::RobotState* context_state = nullptr) const override;
346 };
347 } // namespace cached_ik_kinematics_plugin
348 
bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions()) const override
bool initialize(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions()) const override
bool searchPositionIK(const std::vector< geometry_msgs::msg::Pose > &ik_poses, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const override
bool initialize(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
const IKEntry & getBestApproximateIKSolution(const std::vector< std::string > &fixed, const std::vector< std::string > &active, const std::vector< Pose > &poses) const
Definition: ik_cache.cpp:298
std::string getKey(const std::vector< std::string > &fixed, const std::vector< std::string > &active) const
Definition: ik_cache.cpp:330
void updateCache(const IKEntry &nearest, const std::vector< std::string > &fixed, const std::vector< std::string > &active, const std::vector< Pose > &poses, const std::vector< double > &config)
Definition: ik_cache.cpp:313
IKCacheMap(const std::string &robot_description, const std::string &group_name, unsigned int num_joints)
Definition: ik_cache.cpp:287
A cache of inverse kinematic solutions.
const IKEntry & getBestApproximateIKSolution(const Pose &pose) const
Definition: ik_cache.cpp:151
std::pair< std::vector< Pose >, std::vector< double > > IKEntry
void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin &fk) const
Definition: ik_cache.cpp:252
NearestNeighborsGNAT< IKEntry * > ik_nn_
double configDistance2(const std::vector< double > &config1, const std::vector< double > &config2) const
Definition: ik_cache.cpp:140
void initializeCache(const std::string &robot_id, const std::string &group_name, const std::string &cache_name, const unsigned int num_joints, const Options &opts=Options())
Definition: ik_cache.cpp:62
void updateCache(const IKEntry &nearest, const Pose &pose, const std::vector< double > &config) const
Definition: ik_cache.cpp:173
IKCache(const IKCache &)=delete
Specific implementation of kinematics using KDL. This version supports any kinematic chain,...
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const std::string & getName() const
Get the model name.
Definition: robot_model.h:85
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
double distance(const Pose &pose) const
Definition: ik_cache.cpp:282
A set of options for the kinematics solver.