srv_kinematics_plugin.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, JSK, The University of Tokyo.
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 JSK, The University of Tokyo 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: Dave Coleman, Masaki Murooka
36  Desc: Connects MoveIt! to any inverse kinematics solver via a ROS service call
37  Supports planning groups with multiple tip frames
38  \todo: better support for mimic joints
39  \todo: better support for redundant joints
40 */
41 
42 #ifndef MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_
43 #define MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_
44 
45 // ROS
46 #include <ros/ros.h>
47 
48 // System
49 #include <memory>
50 
51 // ROS msgs
52 #include <geometry_msgs/PoseStamped.h>
53 #include <moveit_msgs/GetPositionFK.h>
54 #include <moveit_msgs/GetPositionIK.h>
55 #include <moveit_msgs/KinematicSolverInfo.h>
56 #include <moveit_msgs/MoveItErrorCodes.h>
57 
58 // MoveIt!
62 
64 {
70 {
71 public:
76 
77  virtual bool
78  getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
79  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
81 
82  virtual bool
83  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
84  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
86 
87  virtual bool
88  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
89  const std::vector<double>& consistency_limits, std::vector<double>& solution,
90  moveit_msgs::MoveItErrorCodes& error_code,
92 
93  virtual bool
94  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
95  std::vector<double>& solution, const IKCallbackFn& solution_callback,
96  moveit_msgs::MoveItErrorCodes& error_code,
98 
99  virtual bool
100  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
101  const std::vector<double>& consistency_limits, std::vector<double>& solution,
102  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
104 
105  virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
106  std::vector<geometry_msgs::Pose>& poses) const;
107 
108  virtual bool initialize(const std::string& robot_description, const std::string& group_name,
109  const std::string& base_name, const std::string& tip_frame, double search_discretization)
110  {
111  std::vector<std::string> tip_frames;
112  tip_frames.push_back(tip_frame);
113  return initialize(robot_description, group_name, base_name, tip_frames, search_discretization);
114  }
115 
116  virtual bool initialize(const std::string& robot_description, const std::string& group_name,
117  const std::string& base_name, const std::vector<std::string>& tip_frames,
118  double search_discretization);
119 
123  const std::vector<std::string>& getJointNames() const;
124 
128  const std::vector<std::string>& getLinkNames() const;
129 
133  const std::vector<std::string>& getVariableNames() const;
134 
135 protected:
136  virtual bool
137  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
138  std::vector<double>& solution, const IKCallbackFn& solution_callback,
139  moveit_msgs::MoveItErrorCodes& error_code, const std::vector<double>& consistency_limits,
141 
142  virtual bool
143  searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
144  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
145  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
147 
148  virtual bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
149 
150 private:
151  bool timedOut(const ros::WallTime& start_time, double duration) const;
152 
153  int getJointIndex(const std::string& name) const;
154 
155  bool isRedundantJoint(unsigned int index) const;
156 
157  bool active_;
159  moveit_msgs::KinematicSolverInfo ik_group_info_;
161  unsigned int dimension_;
163  robot_model::RobotModelPtr robot_model_;
164  robot_model::JointModelGroup* joint_model_group_;
165 
166  robot_state::RobotStatePtr robot_state_;
167 
169 
170  std::shared_ptr<ros::ServiceClient> ik_service_client_;
171 };
172 }
173 
174 #endif
moveit_msgs::KinematicSolverInfo ik_group_info_
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
bool isRedundantJoint(unsigned int index) const
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_frame, double search_discretization)
std::shared_ptr< ros::ServiceClient > ik_service_client_
const std::vector< std::string > & getVariableNames() const
Return all the variable names in the order they are represented internally.
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
unsigned int index
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
robot_model::JointModelGroup * joint_model_group_
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
options
bool timedOut(const ros::WallTime &start_time, double duration) const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
int getJointIndex(const std::string &name) const
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
Specific implementation of kinematics using ROS service calls to communicate with external IK solvers...


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