moveit2
The MoveIt Motion Planning Framework for ROS 2.
pr2_arm_ik.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 <urdf_model/model.h>
40 #include <urdf/model.h>
41 #include <Eigen/Geometry>
42 #include <Eigen/LU> // provides LU decomposition
43 #include <kdl/chainiksolver.hpp>
44 #include <moveit_msgs/srv/get_position_fk.hpp>
45 #include <moveit_msgs/srv/get_position_ik.hpp>
46 #include <moveit_msgs/msg/kinematic_solver_info.hpp>
47 #include <rclcpp/rclcpp.hpp>
48 
49 namespace pr2_arm_kinematics
50 {
51 static const int NUM_JOINTS_ARM7DOF = 7;
52 
53 static const double IK_EPS = 1e-5;
54 
55 inline double distance(const urdf::Pose& transform)
56 {
57  return sqrt(transform.position.x * transform.position.x + transform.position.y * transform.position.y +
58  transform.position.z * transform.position.z);
59 }
60 
61 inline bool solveQuadratic(const double& a, const double& b, const double& c, double* x1, double* x2)
62 {
63  double discriminant = b * b - 4 * a * c;
64  if (fabs(a) < IK_EPS)
65  {
66  *x1 = -c / b;
67  *x2 = *x1;
68  return true;
69  }
70  // ROS_DEBUG("Discriminant: %f\n",discriminant);
71  if (discriminant >= 0)
72  {
73  *x1 = (-b + sqrt(discriminant)) / (2 * a);
74  *x2 = (-b - sqrt(discriminant)) / (2 * a);
75  return true;
76  }
77  else if (fabs(discriminant) < IK_EPS)
78  {
79  *x1 = -b / (2 * a);
80  *x2 = -b / (2 * a);
81  return true;
82  }
83  else
84  {
85  *x1 = -b / (2 * a);
86  *x2 = -b / (2 * a);
87  return false;
88  }
89 }
90 
91 inline bool solveCosineEqn(const double& a, const double& b, const double& c, double& soln1, double& soln2)
92 {
93  double theta1 = atan2(b, a);
94  double denom = sqrt(a * a + b * b);
95 
96  if (fabs(denom) < IK_EPS) // should never happen, wouldn't make sense but make sure it is checked nonetheless
97  {
98 #ifdef DEBUG
99  std::cout << "denom: " << denom << '\n';
100 #endif
101  return false;
102  }
103  double rhs_ratio = c / denom;
104  if (rhs_ratio < -1 || rhs_ratio > 1)
105  {
106 #ifdef DEBUG
107  std::cout << "rhs_ratio: " << rhs_ratio << '\n';
108 #endif
109  return false;
110  }
111  double acos_term = acos(rhs_ratio);
112  soln1 = theta1 + acos_term;
113  soln2 = theta1 - acos_term;
114 
115  return true;
116 }
117 
118 class PR2ArmIK
119 {
120 public:
126  PR2ArmIK();
128 
136  bool init(const urdf::ModelInterface& robot_model, const std::string& root_name, const std::string& tip_name);
137 
143  void computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& shoulder_pan_initial_guess,
144  std::vector<std::vector<double> >& solution) const;
145 
151  void computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double& shoulder_roll_initial_guess,
152  std::vector<std::vector<double> >& solution) const;
153 
154  // std::vector<std::vector<double> > solution_ik_;/// a vector of ik solutions
155 
161  void getSolverInfo(moveit_msgs::msg::KinematicSolverInfo& info);
162 
166  moveit_msgs::msg::KinematicSolverInfo solver_info_;
167 
169 
170 private:
171  void addJointToChainInfo(const urdf::JointConstSharedPtr& joint, moveit_msgs::msg::KinematicSolverInfo& info);
172 
173  bool checkJointLimits(const std::vector<double>& joint_values) const;
174 
175  bool checkJointLimits(const double& joint_value, const int& joint_num) const;
176 
177  Eigen::Isometry3f grhs_, gf_, home_inv_;
178 
179  std::vector<double> angle_multipliers_;
180 
181  std::vector<double> solution_;
182 
183  double shoulder_upperarm_offset_, upperarm_elbow_offset_, elbow_wrist_offset_, shoulder_wrist_offset_,
184  shoulder_elbow_offset_, torso_shoulder_offset_x_, torso_shoulder_offset_y_, torso_shoulder_offset_z_;
185 
186  std::vector<double> min_angles_;
187 
188  std::vector<double> max_angles_;
189 
190  std::vector<bool> continuous_joint_;
191 };
192 } // namespace pr2_arm_kinematics
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
void computeIKShoulderRoll(const Eigen::Isometry3f &g_in, const double &shoulder_roll_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder roll angle. h
Definition: pr2_arm_ik.cpp:466
bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name)
Initialize the solver by providing a urdf::Model and a root and tip name.
Definition: pr2_arm_ik.cpp:58
void computeIKShoulderPan(const Eigen::Isometry3f &g_in, const double &shoulder_pan_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder pan angle.
Definition: pr2_arm_ik.cpp:201
moveit_msgs::msg::KinematicSolverInfo solver_info_
get chain information about the arm.
Definition: pr2_arm_ik.h:166
void getSolverInfo(moveit_msgs::msg::KinematicSolverInfo &info)
get chain information about the arm. This populates the IK query response, filling in joint level inf...
Definition: pr2_arm_ik.cpp:196
a
Definition: plan.py:54
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
Definition: pr2_arm_ik.h:91
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
Definition: pr2_arm_ik.h:61