cached_ik_kinematics_plugin-inl.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 #include <chrono>
38 #include <cstdlib>
39 
41 {
42 template <class KinematicsPlugin>
44 {
45 }
46 
47 template <class KinematicsPlugin>
49 {
50 }
51 
52 template <class KinematicsPlugin>
53 bool CachedIKKinematicsPlugin<KinematicsPlugin>::initialize(const std::string& robot_description,
54  const std::string& group_name,
55  const std::string& base_frame, const std::string& tip_frame,
56  double search_discretization)
57 {
58  // call initialize method of wrapped class
59  if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frame, search_discretization))
60  {
61  ROS_ERROR_NAMED("cached_ik", "failed to initialized caching plugin");
62  return false;
63  }
64 
65  IKCache::Options opts;
66  int max_cache_size; // rosparam can't handle unsigned int
67  kinematics::KinematicsBase::lookupParam("max_cache_size", max_cache_size, static_cast<int>(opts.max_cache_size));
68  opts.max_cache_size = max_cache_size;
69  kinematics::KinematicsBase::lookupParam("min_pose_distance", opts.min_pose_distance, 1.0);
70  kinematics::KinematicsBase::lookupParam("min_joint_config_distance", opts.min_joint_config_distance, 1.0);
71  kinematics::KinematicsBase::lookupParam<std::string>("cached_ik_path", opts.cached_ik_path, "");
72 
73  cache_.initializeCache(robot_description, group_name, base_frame + tip_frame,
74  KinematicsPlugin::getJointNames().size(), opts);
75 
76  // for debugging purposes:
77  // kdl_kinematics_plugin::KDLKinematicsPlugin fk;
78  // fk.initialize(robot_description, group_name, base_frame, tip_frame, search_discretization);
79  // cache_.verifyCache(fk);
80 
81  return true;
82 }
83 
84 template <class KinematicsPlugin>
85 bool CachedMultiTipIKKinematicsPlugin<KinematicsPlugin>::initialize(const std::string& robot_description,
86  const std::string& group_name,
87  const std::string& base_frame,
88  const std::vector<std::string>& tip_frames,
89  double search_discretization)
90 {
91  // call initialize method of wrapped class
92  if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frames, search_discretization))
93  {
94  ROS_ERROR_NAMED("cached_ik", "failed to initialized caching plugin");
95  return false;
96  }
97 
98  std::string cache_name = base_frame;
99  std::accumulate(tip_frames.begin(), tip_frames.end(), cache_name);
100  CachedIKKinematicsPlugin<KinematicsPlugin>::cache_.initializeCache(robot_description, group_name, cache_name,
101  KinematicsPlugin::getJointNames().size());
102  return true;
103 }
104 
105 template <class KinematicsPlugin>
106 bool CachedIKKinematicsPlugin<KinematicsPlugin>::getPositionIK(const geometry_msgs::Pose& ik_pose,
107  const std::vector<double>& ik_seed_state,
108  std::vector<double>& solution,
109  moveit_msgs::MoveItErrorCodes& error_code,
110  const KinematicsQueryOptions& options) const
111 {
112  Pose pose(ik_pose);
113  const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
114  bool solution_found = KinematicsPlugin::getPositionIK(ik_pose, nearest.second, solution, error_code, options) ||
115  KinematicsPlugin::getPositionIK(ik_pose, ik_seed_state, solution, error_code, options);
116  if (solution_found)
117  cache_.updateCache(nearest, pose, solution);
118  return solution_found;
119 }
120 
121 template <class KinematicsPlugin>
122 bool CachedIKKinematicsPlugin<KinematicsPlugin>::searchPositionIK(const geometry_msgs::Pose& ik_pose,
123  const std::vector<double>& ik_seed_state,
124  double timeout, std::vector<double>& solution,
125  moveit_msgs::MoveItErrorCodes& error_code,
126  const KinematicsQueryOptions& options) const
127 {
128  std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
129  Pose pose(ik_pose);
130  const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
131  bool solution_found =
132  KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, error_code, options);
133  if (!solution_found)
134  {
135  std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
136  solution_found =
137  KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, error_code, options);
138  }
139  if (solution_found)
140  cache_.updateCache(nearest, pose, solution);
141  return solution_found;
142 }
143 
144 template <class KinematicsPlugin>
146  const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
147  const std::vector<double>& consistency_limits, std::vector<double>& solution,
148  moveit_msgs::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options) const
149 {
150  std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
151  Pose pose(ik_pose);
152  const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
153  bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
154  solution, error_code, options);
155  if (!solution_found)
156  {
157  std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
158  solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
159  solution, error_code, options);
160  }
161  if (solution_found)
162  cache_.updateCache(nearest, pose, solution);
163  return solution_found;
164 }
165 
166 template <class KinematicsPlugin>
167 bool CachedIKKinematicsPlugin<KinematicsPlugin>::searchPositionIK(const geometry_msgs::Pose& ik_pose,
168  const std::vector<double>& ik_seed_state,
169  double timeout, std::vector<double>& solution,
170  const IKCallbackFn& solution_callback,
171  moveit_msgs::MoveItErrorCodes& error_code,
172  const KinematicsQueryOptions& options) const
173 {
174  std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
175  Pose pose(ik_pose);
176  const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
177  bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution,
178  solution_callback, error_code, options);
179  if (!solution_found)
180  {
181  std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
182  solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution,
183  solution_callback, error_code, options);
184  }
185  if (solution_found)
186  cache_.updateCache(nearest, pose, solution);
187  return solution_found;
188 }
189 
190 template <class KinematicsPlugin>
192  const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
193  const std::vector<double>& consistency_limits, std::vector<double>& solution, const IKCallbackFn& solution_callback,
194  moveit_msgs::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options) const
195 {
196  std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
197  Pose pose(ik_pose);
198  const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
199  bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
200  solution, solution_callback, error_code, options);
201  if (!solution_found)
202  {
203  std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
204  solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
205  solution, solution_callback, error_code, options);
206  }
207  if (solution_found)
208  cache_.updateCache(nearest, pose, solution);
209  return solution_found;
210 }
211 
212 template <class KinematicsPlugin>
214  const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state, double timeout,
215  const std::vector<double>& consistency_limits, std::vector<double>& solution, const IKCallbackFn& solution_callback,
216  moveit_msgs::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options,
217  const moveit::core::RobotState* context_state) const
218 {
219  std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
220  std::vector<Pose> poses(ik_poses.size());
221  for (unsigned int i = 0; i < poses.size(); ++i)
222  poses[i] = Pose(ik_poses[i]);
223  const IKEntry& nearest = CachedIKKinematicsPlugin<KinematicsPlugin>::cache_.getBestApproximateIKSolution(poses);
224  bool solution_found =
225  KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution,
226  solution_callback, error_code, options, context_state);
227  if (!solution_found)
228  {
229  std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
230  solution_found =
231  KinematicsPlugin::searchPositionIK(ik_poses, ik_seed_state, diff.count(), consistency_limits, solution,
232  solution_callback, error_code, options, context_state);
233  }
234 
235  if (solution_found)
236  CachedIKKinematicsPlugin<KinematicsPlugin>::cache_.updateCache(nearest, poses, solution);
237  return solution_found;
238 }
239 }
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization) override
bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions()) const override
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
bool lookupParam(const std::string &param, T &val, const T &default_val) const
bool searchPositionIK(const std::vector< geometry_msgs::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::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const override
#define ROS_ERROR_NAMED(name,...)
bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions()) const override


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53