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 #pragma once
38 
39 #include <kdl/config.h>
40 #include <kdl/frames.hpp>
41 #include <kdl/jntarray.hpp>
42 #include <kdl/tree.hpp>
44 
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 #include <kdl/chainfksolverpos_recursive.hpp>
52 
53 #include <urdf/model.h>
54 
56 
57 #include <memory>
58 
59 #include "pr2_arm_ik.h"
60 
61 namespace pr2_arm_kinematics
62 {
63 static const int NO_IK_SOLUTION = -1;
64 static const int TIMED_OUT = -2;
65 
66 MOVEIT_CLASS_FORWARD(PR2ArmIKSolver);
67 
68 // minimal stuff necessary
69 class PR2ArmIKSolver : public KDL::ChainIkSolverPos
70 {
71 public:
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
81  PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
82  const std::string& tip_frame_name, const double& search_discretization_angle, const int& free_angle);
83 
84  ~PR2ArmIKSolver() override{};
85 
86 // TODO: simplify after kinetic support is dropped
87 #define KDL_VERSION_LESS(a, b, c) ((KDL_VERSION) < ((a << 16) | (b << 8) | c))
88 #if KDL_VERSION_LESS(1, 4, 0)
90 #else
91  void updateInternalDataStructures() override;
92 #endif
93 #undef KDL_VERSION_LESS
94 
98  PR2ArmIK pr2_arm_ik_;
99 
103  bool active_;
104 
105  int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out) override;
106 
107  int cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, const double& timeout);
108 
109  void getSolverInfo(moveit_msgs::KinematicSolverInfo& response)
110  {
111  pr2_arm_ik_.getSolverInfo(response);
112  }
113 
114 private:
115  bool getCount(int& count, const int& max_count, const int& min_count);
116 
118 
120 
121  std::string root_frame_name_;
122 };
123 
124 Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame& p);
125 double computeEuclideanDistance(const std::vector<double>& array_1, const KDL::JntArray& array_2);
126 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::KinematicSolverInfo& chain_info);
127 
128 MOVEIT_CLASS_FORWARD(PR2ArmKinematicsPlugin);
129 
131 {
132 public:
135 
140  bool isActive();
141 
148  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  bool searchPositionIK(
162  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  bool searchPositionIK(
175  const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
176  const std::vector<double>& consistency_limit, std::vector<double>& solution,
177  moveit_msgs::MoveItErrorCodes& error_code,
179 
188  bool searchPositionIK(
189  const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
190  std::vector<double>& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
192 
203  bool searchPositionIK(
204  const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
205  const std::vector<double>& consistency_limit, std::vector<double>& solution,
206  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
208 
213  bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
214  std::vector<geometry_msgs::Pose>& poses) const override;
215 
220  bool initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
221  const std::string& base_frame, const std::vector<std::string>& tip_frames,
222  double search_discretization) override;
223 
227  const std::vector<std::string>& getJointNames() const override;
228 
232  const std::vector<std::string>& getLinkNames() const override;
233 
234 protected:
235  bool active_;
236  int free_angle_;
237  pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_;
238  std::string root_name_;
239  int dimension_;
240  std::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_;
241  KDL::Chain kdl_chain_;
242  moveit_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
243 
246 
247  void desiredPoseCallback(const KDL::JntArray& jnt_array, const KDL::Frame& ik_pose,
248  moveit_msgs::MoveItErrorCodes& error_code) const;
249 
250  void jointSolutionCallback(const KDL::JntArray& jnt_array, const KDL::Frame& ik_pose,
251  moveit_msgs::MoveItErrorCodes& error_code) const;
252 };
253 } // namespace pr2_arm_kinematics
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getPositionFK
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
Definition: pr2_arm_kinematics_plugin.cpp:404
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
pr2_arm_kinematics::PR2ArmKinematicsPlugin::fk_solver_info_
moveit_msgs::KinematicSolverInfo fk_solver_info_
Definition: pr2_arm_kinematics_plugin.h:274
pr2_arm_kinematics
Definition: pr2_arm_ik.h:48
pr2_arm_kinematics::TIMED_OUT
static const int TIMED_OUT
Definition: pr2_arm_kinematics_plugin.h:96
pr2_arm_kinematics::PR2ArmIKSolver::getSolverInfo
void getSolverInfo(moveit_msgs::KinematicSolverInfo &response)
Definition: pr2_arm_kinematics_plugin.h:141
pr2_arm_kinematics::PR2ArmIKSolver::getCount
bool getCount(int &count, const int &max_count, const int &min_count)
Definition: pr2_arm_kinematics_plugin.cpp:50
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getPositionIK
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 override
Given a desired pose of the end-effector, compute the joint angles to reach it.
Definition: pr2_arm_kinematics_plugin.cpp:316
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:143
pr2_arm_kinematics::getKDLChainInfo
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
Definition: pr2_arm_kinematics_plugin.cpp:245
pr2_arm_kinematics::PR2ArmKinematicsPlugin::active_
bool active_
Definition: pr2_arm_kinematics_plugin.h:267
pr2_arm_kinematics::PR2ArmKinematicsPlugin::kdl_chain_
KDL::Chain kdl_chain_
Definition: pr2_arm_kinematics_plugin.h:273
kinematics_base.h
class_forward.h
pr2_arm_kinematics::PR2ArmKinematicsPlugin::isActive
bool isActive()
Specifies if the node is active or not.
Definition: pr2_arm_kinematics_plugin.cpp:259
kinematics::KinematicsBase::IKCallbackFn
boost::function< void(const geometry_msgs::Pose &, const std::vector< double > &, moveit_msgs::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition: kinematics_base.h:153
pr2_arm_kinematics::PR2ArmIKSolver::CartToJnt
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
Definition: pr2_arm_kinematics_plugin.cpp:103
pr2_arm_kinematics::PR2ArmKinematicsPlugin::dimension_
int dimension_
Definition: pr2_arm_kinematics_plugin.h:271
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getLinkNames
const std::vector< std::string > & getLinkNames() const override
Return all the link names in the order they are represented internally.
Definition: pr2_arm_kinematics_plugin.cpp:420
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getJointNames
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
Definition: pr2_arm_kinematics_plugin.cpp:411
pr2_arm_kinematics::PR2ArmKinematicsPlugin::solutionCallback_
IKCallbackFn solutionCallback_
Definition: pr2_arm_kinematics_plugin.h:277
pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback_
IKCallbackFn desiredPoseCallback_
Definition: pr2_arm_kinematics_plugin.h:276
pr2_arm_kinematics::PR2ArmIK::getSolverInfo
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
pr2_arm_kinematics::PR2ArmKinematicsPlugin::initialize
bool initialize(const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
Initialization function for the kinematics.
Definition: pr2_arm_kinematics_plugin.cpp:264
kinematics::KinematicsBase
Provides an interface for kinematics solvers.
Definition: kinematics_base.h:145
pr2_arm_kinematics::PR2ArmKinematicsPlugin::free_angle_
int free_angle_
Definition: pr2_arm_kinematics_plugin.h:268
pr2_arm_kinematics::PR2ArmIKSolver::PR2ArmIKSolver
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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)
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
Definition: pr2_arm_kinematics_plugin.cpp:86
pr2_arm_kinematics::PR2ArmKinematicsPlugin::pr2_arm_ik_solver_
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
Definition: pr2_arm_kinematics_plugin.h:269
pr2_arm_kinematics::computeEuclideanDistance
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
Definition: pr2_arm_kinematics_plugin.cpp:235
pr2_arm_kinematics::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(PR2ArmIKSolver)
model.h
pr2_arm_kinematics::PR2ArmKinematicsPlugin::root_name_
std::string root_name_
Definition: pr2_arm_kinematics_plugin.h:270
pr2_arm_kinematics::PR2ArmKinematicsPlugin::jnt_to_pose_solver_
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
Definition: pr2_arm_kinematics_plugin.h:272
pr2_arm_kinematics::NO_IK_SOLUTION
static const int NO_IK_SOLUTION
Definition: pr2_arm_kinematics_plugin.h:95
pr2_arm_kinematics::KDLToEigenMatrix
Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame &p)
Definition: pr2_arm_kinematics_plugin.cpp:221
kdl_parser.hpp
pr2_arm_kinematics::PR2ArmIKSolver::root_frame_name_
std::string root_frame_name_
Definition: pr2_arm_kinematics_plugin.h:153
pr2_arm_kinematics::PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin
PR2ArmKinematicsPlugin()
Plugin-able interface to the PR2 arm kinematics.
Definition: pr2_arm_kinematics_plugin.cpp:255
pr2_arm_kinematics::PR2ArmIKSolver::cartToJntSearch
int cartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
Definition: pr2_arm_kinematics_plugin.cpp:159
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:109
pr2_arm_kinematics::PR2ArmIKSolver::updateInternalDataStructures
void updateInternalDataStructures()
Definition: pr2_arm_kinematics_plugin.cpp:97
pr2_arm_kinematics::PR2ArmIKSolver::free_angle_
int free_angle_
Definition: pr2_arm_kinematics_plugin.h:151
pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback
void desiredPoseCallback(const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, moveit_msgs::MoveItErrorCodes &error_code) const
pr2_arm_kinematics::PR2ArmKinematicsPlugin::jointSolutionCallback
void jointSolutionCallback(const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, moveit_msgs::MoveItErrorCodes &error_code) const
pr2_arm_ik.h
pr2_arm_kinematics::PR2ArmIKSolver::active_
bool active_
Indicates whether the solver has been successfully initialized.
Definition: pr2_arm_kinematics_plugin.h:135
pr2_arm_kinematics::PR2ArmKinematicsPlugin
Definition: pr2_arm_kinematics_plugin.h:162
pr2_arm_kinematics::PR2ArmIKSolver::pr2_arm_ik_
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
Definition: pr2_arm_kinematics_plugin.h:130
pr2_arm_kinematics::PR2ArmKinematicsPlugin::ik_solver_info_
moveit_msgs::KinematicSolverInfo ik_solver_info_
Definition: pr2_arm_kinematics_plugin.h:274
pr2_arm_kinematics::PR2ArmKinematicsPlugin::searchPositionIK
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 override
Given a desired pose of the end-effector, search for the joint angles required to reach it....
Definition: pr2_arm_kinematics_plugin.cpp:325
pr2_arm_kinematics::PR2ArmIKSolver::search_discretization_angle_
double search_discretization_angle_
Definition: pr2_arm_kinematics_plugin.h:149
pr2_arm_kinematics::PR2ArmIKSolver::~PR2ArmIKSolver
~PR2ArmIKSolver() override
Definition: pr2_arm_kinematics_plugin.h:116


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 6 2024 02:26:32