pr2_arm_ik_solver.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Sachin Chitta */
36 
37 #ifndef MOVEIT_PR2_ARM_IK_SOLVER_
38 #define MOVEIT_PR2_ARM_IK_SOLVER_
39 
40 #include <urdf/model.h>
41 #include <Eigen/Core>
42 #include <kdl/chainiksolver.hpp>
46 #include <moveit_msgs/PositionIKRequest.h>
47 #include <geometry_msgs/PoseStamped.h>
48 #include <tf_conversions/tf_kdl.h>
49 
50 namespace pr2_arm_kinematics
51 {
52 
53 static const int NO_IK_SOLUTION = -1;
54 static const int TIMED_OUT = -2;
55 
57  {
58  public:
59 
65  PR2ArmIKSolver(const urdf::Model &robot_model,
66  const std::string &root_frame_name,
67  const std::string &tip_frame_name,
68  const double &search_discretization_angle,
69  const int &free_angle);
70 
71  ~PR2ArmIKSolver(){ delete pr2_arm_ik_; };
72 
73  virtual void updateInternalDataStructures();
74 
79 
83  bool active_;
84 
97  int CartToJnt(const KDL::JntArray& q_init,
98  const KDL::Frame& p_in,
99  KDL::JntArray& q_out);
100 
113  int CartToJnt(const KDL::JntArray& q_init,
114  const KDL::Frame& p_in,
115  std::vector<KDL::JntArray> &q_out);
116 
128  int CartToJntSearch(const KDL::JntArray& q_in,
129  const KDL::Frame& p_in,
130  std::vector<KDL::JntArray> &q_out,
131  const double &timeout);
132 
145 //
146 // Undefined in pr2_arm_ik_solver.cpp, so commented out dash@clearpathrobotics.com
147  //int CartToJntSearch(const KDL::JntArray& q_in,
148  // const KDL::Frame& p_in,
149  // KDL::JntArray &q_out,
150  // const double &timeout);
151 
166  int CartToJntSearch(const KDL::JntArray& q_in,
167  const KDL::Frame& p_in,
168  KDL::JntArray &q_out,
169  const double &timeout,
170  const double& consistency_limit);
171 
185  int CartToJntSearch(const KDL::JntArray& q_in,
186  const KDL::Frame& p_in,
187  KDL::JntArray &q_out,
188  const double &timeout,
189  moveit_msgs::MoveItErrorCodes &error_code,
190  const kinematics::KinematicsBase::IKCallbackFn &solution_callback);
191 
206  int CartToJntSearch(const KDL::JntArray& q_in,
207  const KDL::Frame& p_in,
208  KDL::JntArray &q_out,
209  const double &timeout,
210  const double& consistency_limit,
211  moveit_msgs::MoveItErrorCodes &error_code,
212  const kinematics::KinematicsBase::IKCallbackFn &solution_callback);
213 
219  void getSolverInfo(moveit_msgs::KinematicSolverInfo &response);
220 
221  std::string getFrameId();
222 
223  unsigned int getFreeAngle() const {
224  return free_angle_;
225  }
226 
227  void setFreeAngle(const unsigned int& free_angle) {
228  free_angle_ = free_angle;
229  }
230 
231  private:
232 
248  int CartToJntSearch(const KDL::JntArray& q_in,
249  const KDL::Frame& p_in,
250  KDL::JntArray &q_out,
251  const double &timeout,
252  bool use_consistency_limit,
253  const double& consistency_limit,
254  moveit_msgs::MoveItErrorCodes &error_code,
255  const kinematics::KinematicsBase::IKCallbackFn &solution_callback);
256 
257  bool getCount(int &count, const int &max_count, const int &min_count);
258 
260 
261  int free_angle_;
262 
263  std::string root_frame_name_;
264  };
265 }
266 #endif// PR2_ARM_IK_SOLVER_H
Namespace for the PR2ArmKinematics.
bool getCount(int &count, const int &max_count, const int &min_count)
int CartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
static const int NO_IK_SOLUTION
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out)
bool active_
Indicates whether the solver has been successfully initialized.
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
PR2ArmIKSolver(const urdf::ModelInterface &robot_model, const std::string &root_frame_name, const std::string &tip_frame_name, const double &search_discretization_angle, const int &free_angle)
void getSolverInfo(moveit_msgs::KinematicSolverInfo &response)
static const int TIMED_OUT


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Sun May 9 2021 02:19:39