pr2_arm_kinematics_plugin.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 PR2_ARM_IK_NODE_H
38 #define PR2_ARM_IK_NODE_H
39 
40 #include <kdl/frames.hpp>
41 #include <kdl/jntarray.hpp>
42 #include <kdl/tree.hpp>
44 
45 #include <angles/angles.h>
46 #include <tf_conversions/tf_kdl.h>
47 
49 #include <moveit_msgs/GetPositionFK.h>
50 #include <moveit_msgs/GetPositionIK.h>
51 #include <moveit_msgs/KinematicSolverInfo.h>
52 #include <moveit_msgs/MoveItErrorCodes.h>
53 
54 #include <kdl/chainfksolverpos_recursive.hpp>
55 
56 #include <urdf/model.h>
57 
59 
60 #include <memory>
61 
62 #include "pr2_arm_ik.h"
63 
64 namespace pr2_arm_kinematics
65 {
66 static const int NO_IK_SOLUTION = -1;
67 static const int TIMED_OUT = -2;
68 
70 
71 // minimal stuff necessary
73 {
74 public:
76 
85  PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
86  const std::string& tip_frame_name, const double& search_discretization_angle, const int& free_angle);
87 
89 
94 
98  bool active_;
99 
100  int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out);
101 
102  int CartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, const double& timeout);
103 
104  void getSolverInfo(moveit_msgs::KinematicSolverInfo& response)
105  {
106  pr2_arm_ik_.getSolverInfo(response);
107  }
108 
109 private:
110  bool getCount(int& count, const int& max_count, const int& min_count);
111 
113 
115 
116  std::string root_frame_name_;
117 };
118 
119 Eigen::Affine3f KDLToEigenMatrix(const KDL::Frame& p);
120 double computeEuclideanDistance(const std::vector<double>& array_1, const KDL::JntArray& array_2);
121 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::KinematicSolverInfo& chain_info);
122 
124 
126 {
127 public:
132 
133  void setRobotModel(urdf::ModelInterfaceSharedPtr& robot_model);
134 
139  bool isActive();
140 
148  virtual bool
149  getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
150  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
152 
161  virtual bool
162  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
163  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
174  virtual bool
175  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
176  const std::vector<double>& consistency_limits, std::vector<double>& solution,
177  moveit_msgs::MoveItErrorCodes& error_code,
179 
188  virtual bool
189  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
190  std::vector<double>& solution, const IKCallbackFn& solution_callback,
191  moveit_msgs::MoveItErrorCodes& error_code,
193 
204  virtual bool
205  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
206  const std::vector<double>& consistency_limits, std::vector<double>& solution,
207  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
209 
217  virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
218  std::vector<geometry_msgs::Pose>& poses) const;
219 
224  virtual bool initialize(const std::string& robot_description, const std::string& group_name,
225  const std::string& base_name, const std::string& tip_name, double search_discretization);
226 
230  const std::vector<std::string>& getJointNames() const;
231 
235  const std::vector<std::string>& getLinkNames() const;
236 
237 protected:
238  bool active_;
240  urdf::ModelInterfaceSharedPtr robot_model_;
241  pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_;
242  std::string root_name_;
244  std::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_;
246  moveit_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
247 
250 
251  void desiredPoseCallback(const KDL::JntArray& jnt_array, const KDL::Frame& ik_pose,
252  moveit_msgs::MoveItErrorCodes& error_code) const;
253 
254  void jointSolutionCallback(const KDL::JntArray& jnt_array, const KDL::Frame& ik_pose,
255  moveit_msgs::MoveItErrorCodes& error_code) const;
256 };
257 }
258 
259 #endif
A set of options for the kinematics solver.
Core components of MoveIt!
MOVEIT_CLASS_FORWARD(PR2ArmIKSolver)
ROSCONSOLE_DECL void initialize()
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
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
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
Provides an interface for kinematics solvers.
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out)
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
Eigen::Affine3f KDLToEigenMatrix(const KDL::Frame &p)
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
The signature for a callback that can compute IK.
moveit_msgs::KinematicSolverInfo ik_solver_info_
void getSolverInfo(moveit_msgs::KinematicSolverInfo &info)
get chain information about the arm. This populates the IK query response, filling in joint level inf...
Definition: pr2_arm_ik.cpp:192
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)
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05