kdl_kinematics_plugin.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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, David Lu!!, Ugo Cupcic */
36 
37 #ifndef MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_
38 #define MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_
39 
40 // ROS
41 #include <ros/ros.h>
43 
44 // ROS msgs
45 #include <geometry_msgs/PoseStamped.h>
46 #include <moveit_msgs/GetPositionFK.h>
47 #include <moveit_msgs/GetPositionIK.h>
48 #include <moveit_msgs/KinematicSolverInfo.h>
49 #include <moveit_msgs/MoveItErrorCodes.h>
50 
51 // KDL
52 #include <kdl/jntarray.hpp>
53 #include <kdl/chainiksolvervel_pinv.hpp>
54 #include <kdl/chainiksolverpos_nr_jl.hpp>
55 #include <kdl/chainfksolverpos_recursive.hpp>
59 
60 // MoveIt!
64 
65 namespace kdl_kinematics_plugin
66 {
71 {
72 public:
77 
78  virtual bool
79  getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
80  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
82 
83  virtual bool
84  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
85  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
87 
88  virtual bool
89  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
90  const std::vector<double>& consistency_limits, std::vector<double>& solution,
91  moveit_msgs::MoveItErrorCodes& error_code,
93 
94  virtual bool
95  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
96  std::vector<double>& solution, const IKCallbackFn& solution_callback,
97  moveit_msgs::MoveItErrorCodes& error_code,
99 
100  virtual bool
101  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
102  const std::vector<double>& consistency_limits, std::vector<double>& solution,
103  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
105 
106  virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
107  std::vector<geometry_msgs::Pose>& poses) const;
108 
109  virtual bool initialize(const std::string& robot_description, const std::string& group_name,
110  const std::string& base_name, const std::string& tip_name, double search_discretization);
111 
115  const std::vector<std::string>& getJointNames() const;
116 
120  const std::vector<std::string>& getLinkNames() const;
121 
122 protected:
139  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
140  std::vector<double>& solution, const IKCallbackFn& solution_callback,
141  moveit_msgs::MoveItErrorCodes& error_code, const std::vector<double>& consistency_limits,
143 
144  virtual bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
145 
146 private:
147  bool timedOut(const ros::WallTime& start_time, double duration) const;
148 
157  bool checkConsistency(const KDL::JntArray& seed_state, const std::vector<double>& consistency_limit,
158  const KDL::JntArray& solution) const;
159 
160  int getJointIndex(const std::string& name) const;
161 
162  int getKDLSegmentIndex(const std::string& name) const;
163 
164  void getRandomConfiguration(KDL::JntArray& jnt_array, bool lock_redundancy) const;
165 
173  void getRandomConfiguration(const KDL::JntArray& seed_state, const std::vector<double>& consistency_limits,
174  KDL::JntArray& jnt_array, bool lock_redundancy) const;
175 
176  bool isRedundantJoint(unsigned int index) const;
177 
178  bool active_;
180  moveit_msgs::KinematicSolverInfo ik_chain_info_;
182  moveit_msgs::KinematicSolverInfo fk_chain_info_;
185 
186  unsigned int dimension_;
191 
192  robot_model::RobotModelPtr robot_model_;
193 
194  robot_state::RobotStatePtr state_, state_2_;
195 
197  std::vector<unsigned int> redundant_joints_map_index_;
198 
199  // Storage required for when the set of redundant joints is reset
200  bool position_ik_; // whether this solver is only being used for position ik
201  robot_model::JointModelGroup* joint_model_group_;
203  double epsilon_;
204  std::vector<JointMimic> mimic_joints_;
205 };
206 }
207 
208 #endif
int getKDLSegmentIndex(const std::string &name) const
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
robot_model::JointModelGroup * joint_model_group_
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
random_numbers::RandomNumberGenerator random_number_generator_
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
unsigned int index
bool checkConsistency(const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
Check whether the solution lies within the consistency limit of the seed state.
Specific implementation of kinematics using KDL. This version can be used with any robot...
options
moveit_msgs::KinematicSolverInfo fk_chain_info_
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
virtual 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 kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
moveit_msgs::KinematicSolverInfo ik_chain_info_
bool isRedundantJoint(unsigned int index) const
std::vector< unsigned int > redundant_joints_map_index_
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
virtual 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 kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const


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